First program to demonstrate a working robot. crude and ugly, basicly a smoke test.
Dependencies: mbed Motordriver Servo GP2xx
main.cpp
00001 #include <mbed.h> 00002 #include <motordriver.h> 00003 #include <Servo.h> 00004 #include <GP2xx.h> 00005 //pc interface 00006 Serial pc(USBTX, USBRX); // tx, rx 00007 //servos 00008 Servo LeftServo(p24); 00009 Servo RightServo(p23); 00010 //motors, left and right side 00011 Motor leftM(p22, p6, p5, 1); // pwm, fwd, rev, can break 00012 Motor rightM(p21, p7, p8, 1); // pwm, fwd, rev, can break 00013 //range finders, left and right side, and left and right front 00014 IRRangeFinder LS(p18,1); 00015 IRRangeFinder LF(p17,1); 00016 IRRangeFinder RF(p16,1); 00017 IRRangeFinder RS(p15,1); 00018 //debug leds. 00019 DigitalOut led1 (LED1); 00020 DigitalOut led2 (LED2); 00021 DigitalOut led3 (LED3); 00022 DigitalOut led4 (LED4); 00023 DigitalOut ledleft (p14); 00024 DigitalOut ledright (p13); 00025 DigitalOut ledfront (p12); 00026 00027 int quickfirstprog() {//initalisation 00028 led1 = led2 = led3 = led4 = 1;//lights 00029 leftM.speed(0.5); 00030 rightM.speed(-0.5);//shows that it works. 00031 wait(1); 00032 while (1) {//infinate loop to drive around 00033 switch ( RS.read() ) {//ugly horrible switch statements that implement a crude attempt at object avoidance 00034 case 4://really this exists to prove that the system drives, and the IR rangefinders have some sane values. 00035 leftM.speed(-0.9); 00036 break; 00037 case 5: 00038 leftM.speed(-0.8); 00039 break; 00040 case 7: 00041 leftM.speed(-0.7); 00042 break; 00043 case 8: 00044 leftM.speed(-0.6); 00045 break; 00046 case 10: 00047 leftM.speed(-0.4); 00048 break; 00049 case 12: 00050 leftM.speed(-0.2); 00051 break; 00052 case 14: 00053 leftM.speed(0.0); 00054 break; 00055 case 20: 00056 leftM.speed(0.4); 00057 break; 00058 case 25: 00059 leftM.speed(0.6); 00060 break; 00061 case 30: 00062 leftM.speed(0.8); 00063 break; 00064 } 00065 switch ( LF.read() ) { 00066 case 4: 00067 rightM.speed(-1); 00068 break; 00069 case 5: 00070 rightM.speed(-0.9); 00071 break; 00072 case 7: 00073 rightM.speed(-0.8); 00074 break; 00075 case 8: 00076 rightM.speed(-0.7); 00077 break; 00078 case 10: 00079 rightM.speed(-0.6); 00080 break; 00081 case 12: 00082 rightM.speed(-0.4); 00083 break; 00084 case 14: 00085 rightM.speed(0.0); 00086 break; 00087 case 20: 00088 rightM.speed(0.4); 00089 break; 00090 case 25: 00091 rightM.speed(0.6); 00092 break; 00093 case 30: 00094 rightM.speed(0.8); 00095 break; 00096 } 00097 wait(0.1); 00098 }//end of infinate loop to drive around 00099 }
Generated on Sun Jul 24 2022 02:57:23 by 1.7.2