First program to demonstrate a working robot. crude and ugly, basicly a smoke test.

Dependencies:   mbed Motordriver Servo GP2xx

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }