First program to demonstrate a working robot. crude and ugly, basicly a smoke test.
Dependencies: mbed Motordriver Servo GP2xx
main.cpp
- Committer:
- littlexc
- Date:
- 2010-12-01
- Revision:
- 0:0bc8408a55f8
File content as of revision 0:0bc8408a55f8:
#include <mbed.h> #include <motordriver.h> #include <Servo.h> #include <GP2xx.h> //pc interface Serial pc(USBTX, USBRX); // tx, rx //servos Servo LeftServo(p24); Servo RightServo(p23); //motors, left and right side Motor leftM(p22, p6, p5, 1); // pwm, fwd, rev, can break Motor rightM(p21, p7, p8, 1); // pwm, fwd, rev, can break //range finders, left and right side, and left and right front IRRangeFinder LS(p18,1); IRRangeFinder LF(p17,1); IRRangeFinder RF(p16,1); IRRangeFinder RS(p15,1); //debug leds. DigitalOut led1 (LED1); DigitalOut led2 (LED2); DigitalOut led3 (LED3); DigitalOut led4 (LED4); DigitalOut ledleft (p14); DigitalOut ledright (p13); DigitalOut ledfront (p12); int quickfirstprog() {//initalisation led1 = led2 = led3 = led4 = 1;//lights leftM.speed(0.5); rightM.speed(-0.5);//shows that it works. wait(1); while (1) {//infinate loop to drive around switch ( RS.read() ) {//ugly horrible switch statements that implement a crude attempt at object avoidance case 4://really this exists to prove that the system drives, and the IR rangefinders have some sane values. leftM.speed(-0.9); break; case 5: leftM.speed(-0.8); break; case 7: leftM.speed(-0.7); break; case 8: leftM.speed(-0.6); break; case 10: leftM.speed(-0.4); break; case 12: leftM.speed(-0.2); break; case 14: leftM.speed(0.0); break; case 20: leftM.speed(0.4); break; case 25: leftM.speed(0.6); break; case 30: leftM.speed(0.8); break; } switch ( LF.read() ) { case 4: rightM.speed(-1); break; case 5: rightM.speed(-0.9); break; case 7: rightM.speed(-0.8); break; case 8: rightM.speed(-0.7); break; case 10: rightM.speed(-0.6); break; case 12: rightM.speed(-0.4); break; case 14: rightM.speed(0.0); break; case 20: rightM.speed(0.4); break; case 25: rightM.speed(0.6); break; case 30: rightM.speed(0.8); break; } wait(0.1); }//end of infinate loop to drive around }