quickfirstprog.cpp

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