/********************************************************************/ #include #include #include // non-standard I/O #include "main.h" #include "timer.h" #include "test_button.h" #include "uart.h" #include "servo.h" // Default user flash optionbits. FLASH_OPTION1 = 0xFF; FLASH_OPTION2 = 0xFF; /////////////////////////////////////////////////////// // Global - Variable char subtmr=10; char t1=0; char t2=0; char t3=0; char t4=0; char t5=0; int lt1; char unsigned ircode=0; char unsigned oldir=0; #define LEG1a 1 #define LEG1b 2 #define LEG2a 3 #define LEG2b 4 #define LEG3a 11 #define LEG3b 12 #define LEG4a 7 #define LEG4b 8 #define LEG5a 5 #define LEG5b 6 #define LEG6a 13 #define LEG6b 14 // IR Codes, connected in PC7..PC4 #define FWD 0x80 #define REV 0x40 #define TRIGHT 0x60 #define TLEFT 0xa0 #define STOP 0x90 #define SIT 0xc0 #define STAND 0x20 void walk(int angle) { servo_out(0,8,0xFF); // turn on servos servo_out(9,8,0xFF); move(LEG1a,55,2); move(LEG5a,-55,2); move(LEG3a,55,2); move(LEG1b,angle,2); if(wait(6)!=0) return; move(LEG5b,-angle,2); move(LEG1a,0,2); if(wait(6)!=0) return; move(LEG3b,angle,2); move(LEG5a,0,2); if(wait(6)!=0) return; move(LEG3a,0,2); if(wait(6)!=0) return; move(LEG1b,-angle,3); move(LEG5b,angle,3); move(LEG3b,-angle,3); // Other side move(LEG2a,55,2); move(LEG4a,-55,2); move(LEG6a,-55,2); move(LEG4b,-angle,2); if(wait(6)!=0) return; move(LEG2b,angle,2); move(LEG4a,0,2); if(wait(6)!=0) return; move(LEG6b,-angle,2); move(LEG2a,0,2); if(wait(6)!=0) return; move(LEG6a,0,2); if(wait(6)!=0) return; move(LEG2b,-angle,4); move(LEG4b,angle,4); move(LEG6b,angle,4); } void forward(void) { walk(-35); } void reverse(void) { walk(35); } void tenhut(void) { servo_out(0,8,0xFF); // turn on servos servo_out(9,8,0xFF); // all legs stand up move(LEG1a,0,3); move(LEG2a,0,3); move(LEG3a,0,3); move(LEG4a,0,3); move(LEG5a,0,3); move(LEG6a,0,3); if(wait(6)!=0) return; move(LEG1a,55,2); move(LEG5a,-55,2); move(LEG3a,55,2); if(wait(6)!=0) return; move(LEG1b,30,2); move(LEG5b,0,2); move(LEG3b,-30,2); if(wait(6)!=0) return; move(LEG1a,0,2); move(LEG5a,0,2); move(LEG3a,0,2); if(wait(6)!=0) return; move(LEG2a,55,2); move(LEG4a,-55,2); move(LEG6a,-55,2); if(wait(6)!=0) return; move(LEG2b,0,2); move(LEG4b,-30,2); move(LEG6b,30,2); if(wait(6)!=0) return; move(LEG2a,0,2); move(LEG4a,0,2); move(LEG6a,0,2); wait(30); servo_out(0,8,0x00); // turn off servos servo_out(9,8,0x00); oldir=0; // to make sure this wont repeat } void turn(int angle) { servo_out(0,8,0xFF); // turn on servos servo_out(9,8,0xFF); move(LEG1a,45,2); move(LEG5a,-45,2); move(LEG3a,45,2); if(wait(5)!=0) return; move(LEG1b,angle,2); move(LEG5b,angle,2); move(LEG3b,angle,2); if(wait(8)!=0) return; move(LEG1a,0,2); move(LEG5a,0,2); move(LEG3a,0,2); if(wait(5)!=0) return; move(LEG2a,45,2); move(LEG4a,-45,2); move(LEG6a,-45,2); if(wait(5)!=0) return; move(LEG2b,angle,2); move(LEG4b,angle,2); move(LEG6b,angle,2); if(wait(8)!=0) return; move(LEG2a,0,2); move(LEG4a,0,2); move(LEG6a,0,2); if(wait(5)!=0) return; move(LEG1b,0,3); move(LEG2b,0,3); move(LEG3b,0,3); move(LEG4b,0,3); move(LEG5b,0,3); move(LEG6b,0,3); wait(8); } void left(void) { turn(-70); } void right(void) { turn(70); } void sit(void) { servo_out(0,8,0xFF); // turn on servos servo_out(9,8,0xFF); move(LEG3a,45,2); move(LEG3b,80,2); if(wait(5)!=0) return; move(LEG3a,0,1); if(wait(5)!=0) return; // move(LEG6a,-45,2); move(LEG6b,-80,2); if(wait(5)!=0) return; move(LEG6a,0,1); if(wait(5)!=0) return; move(LEG2a,45,2); move(LEG2b,80,2); if(wait(5)!=0) return; move(LEG2a,0,1); if(wait(5)!=0) return; // move(LEG5a,-45,2); move(LEG5b,-80,2); if(wait(5)!=0) return; move(LEG5a,0,1); if(wait(10)!=0) return; // move(LEG3a,60,5); move(LEG6a,-60,5); wait(10); while(1) { move(LEG1b,80,2); move(LEG4b,-80,2); if(wait(5)!=0) return; move(LEG1a,45,2); move(LEG4a,-45,2); if(wait(5)!=0) return; move(LEG1a,0,2); move(LEG4a,-0,2); if(wait(5)!=0) return; move(LEG1a,45,2); move(LEG4a,-45,2); if(wait(5)!=0) return; move(LEG1a,0,2); move(LEG4a,-0,2); if(wait(5)!=0) return; move(LEG1b,0,2); move(LEG4b,0,2); if(wait(5)!=0) return; move(LEG1a,45,2); move(LEG4a,-45,2); if(wait(5)!=0) return; move(LEG1a,0,2); move(LEG4a,-0,2); if(wait(5)!=0) return; move(LEG1a,45,2); move(LEG4a,-45,2); if(wait(5)!=0) return; move(LEG1a,0,2); move(LEG4a,-0,2); if(wait(5)!=0) return; } } void stand(void) { servo_out(0,8,0xFF); // turn on servos servo_out(9,8,0xFF); move(LEG1a,45,2); move(LEG1b,80,2); if(wait(5)!=0) return; move(LEG1a,0,1); if(wait(5)!=0) return; // move(LEG4a,-45,2); move(LEG4b,-80,2); if(wait(5)!=0) return; move(LEG4a,0,1); if(wait(5)!=0) return; move(LEG3a,0,5); move(LEG6a,0,5); wait(20); oldir=0; } //////////////////////////////////////////////////////// // Main program beings here // This program blinks LED-3 on the evaluation board void main () { init_led_gpio(); // Initializes LED ports (Port A and C) DI(); // Disable Interrupts init_timer0(); // Intialize Timer-0 EI(); // Enable Interrupts // allow the servo driver to wake up wait(10); tenhut(); // switch on servos while(1) // Infinite while loop { if(t1==0) { PAOUT >>=1; if(PAOUT==0) PAOUT=0x80; PCOUT <<= 1; if(PCOUT==0) PCOUT= 0x01; t1=4; } // This is the main sequence program if(oldir==STAND) stand(); if(oldir==SIT) sit(); if(oldir==FWD) forward(); if(oldir==REV) reverse(); if(oldir==TRIGHT) right(); if(oldir==TLEFT) left(); if(oldir==STOP) tenhut(); wait(1); } } // End of main program