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
}