File content as of revision 20:e1a78ee68726:

//This will rotate the wheels so that after 5 seconds the robot counterclockwise and then clockwise
#include "mbed.h"
#include "rtos.h"
#include "motordriver.h"
#include "MultiModalRobot.h"
#include "uLCD_4DGL.h"

uLCD_4DGL uLCD(p9,p10,p11); // serial tx, serial rx, reset pin;

Motor lw(p26, p29, p30, 1); // pwm, fwd, rev   LEFT WHEEL
Motor rw(p25, p28, p27, 1); // pwm, fwd, rev   RIGHT WHEEL
MultiModalRobot robot(lw, rw);

bool light = true;

Serial blue(p13, p14);
Serial py(USBTX, USBRX);

BusOut leds(LED1,LED2,LED3,LED4);


//Class to control an RGB LED using three PWM pins
class RGBLed
{
public:
    RGBLed(PinName redpin, PinName greenpin, PinName bluepin);
    void write(float red,float green, float blue);
private:
    PwmOut _redpin;
    PwmOut _greenpin;
    PwmOut _bluepin;
};

RGBLed::RGBLed (PinName redpin, PinName greenpin, PinName bluepin)
    : _redpin(redpin), _greenpin(greenpin), _bluepin(bluepin)
{
    //50Hz PWM clock default a bit too low, go to 2000Hz (less flicker)
    _redpin.period(0.0005);
}

void RGBLed::write(float red,float green, float blue)
{
    _redpin = red;
    _greenpin = green;
    _bluepin = blue;
}
//class could be moved to include file

RGBLed myRGBled(p21,p22,p23); //RGB PWM pins

bool visible;
char lastSeen;

int getYpos(){
    int ypos;
    char buffer[4];
    char y;
    while(1){
      char start = 0;
      start = py.getc();
      if(start == 's' || start == 'v'){

      } else {
        uLCD.printf("\nUnexpected char %c\n", start);
        continue;
      }
      if (start=='s') {
          if(py.getc()=='t') {
            if (py.getc() == 'a') {
                if (py.getc() == 'r') {
                    if (py.getc() == 't') {
                        if (py.getc() == 'c') {
                            for (int count = 0; count < 4; count++) {
                                y = py.getc();
                                buffer[count] = y;
                                if (count % 3 == 0) {
                                    //char snum[5];
                                    memcpy(&ypos, buffer, 4);
                                    if(ypos>120){
                                      lastSeen = 'r';
                                    } else {
                                      lastSeen = 'l';
                                    }
                                    if (light) { //Cop lights
                                      myRGBled.write(1.0,0.0,0.0);
                                      light = false;
                                    } else {
                                      myRGBled.write(0.0,0.0,1.0);
                                      light = true;
                                    }
                                }
                            }
                            break;
                        }
                  }
              }
          }
        }
      } else if(start=='v'){
          if(py.getc()=='w'){
            if(py.getc()=='x'){
              if(py.getc()=='y'){
                if(py.getc()=='z'){
                  ypos = -1;
                  break;
                }
              }
            }
          }
      } else {
        uLCD.printf("\nUNEXPECTED\n"); //Default Green on black text
      }
    }
    uLCD.cls();
    uLCD.printf("\nReceived %d\n", ypos); //Default Green on black text
    //uLCD.freeBUFFER();
    return ypos;
}


int main() {
    lastSeen = 'n';
    py.baud(9600);
    char bnum = 0;
    char bhit = 0;
    char needToStopRobot = 0;
    float leftSpeed = 0;
    float rightSpeed = 0;
    float DEFAULT_SPEED = 1.0;
    float ROTATE_SPEED = 0.25;

    while(1) {
        wait(0.1);
        if(blue.readable() && blue.getc() == '!') {
            if(blue.readable() && blue.getc() == 'B') {
                bnum = blue.getc();
                bhit = blue.getc();
                blue.getc();
                switch(bnum) {
                    case '1':
                        needToStopRobot = 1;
                        break;
                    case '2': // become follower
                        py.putc('s');
                        int ypos;
                        while (1) {
                            ypos = getYpos();
                            if (ypos == 251) { //exit condition since camera is 240 wide
                              myRGBled.write(0.0,1.0,0.0);
                              robot.driveWheels(0, 0);
                              break;
                            }
                            visible = (ypos>=0);
                            if (visible) { // CAMERA CAN SEE A CIRCLE
                                //leds[0] = 0;
//                                leds[1] = 0;
//                                leds[2] = 0;
                                if (light) { //Cop lights
                                  myRGBled.write(1.0,0.0,0.0);
                                  light = false;
                                } else {
                                  myRGBled.write(0.0,0.0,1.0);
                                  light = true;
                                }
                                leds[3] = 1;
                                if(ypos<159 && ypos>80) {
                                    leftSpeed = DEFAULT_SPEED;
                                    rightSpeed = DEFAULT_SPEED;
                                    robot.driveWheels(leftSpeed, rightSpeed);
                                    leds[0] = 1;
                                    leds[1] = 0;
                                    leds[2] = 0;
                                } else if(ypos>159) {
                                    while(ypos>120){
                                        ypos = getYpos();
                                        if(ypos<0){
                                         break;
                                        }
                                        leds[0] = 0;
                                        leds[1] = 1;
                                        leds[2] = 0;
                                        leftSpeed = ROTATE_SPEED;
                                        rightSpeed = -ROTATE_SPEED;
                                        robot.driveWheels(leftSpeed, rightSpeed);
                                    }
                                    robot.stop(0.5);
                                } else {
                                    while(ypos<120){
                                        ypos = getYpos();
                                        if(ypos<0){
                                         break;
                                        }
                                        leds[0] = 0;
                                        leds[1] = 0;
                                        leds[2] = 1;
                                        leftSpeed = -ROTATE_SPEED;
                                        rightSpeed = ROTATE_SPEED;
                                        robot.driveWheels(leftSpeed, rightSpeed);
                                    }
                                    robot.stop(0.5);
                                }


                            } else { // CIRCLE NOT VISIBLE
                                leds[0] = 0;
                                leds[1] = 0;
                                leds[2] = 0;
                                leds[3] = 0;
                                if(lastSeen=='r'){
                                  leftSpeed = ROTATE_SPEED;
                                  rightSpeed = -ROTATE_SPEED;
                                } else {
                                  leftSpeed = -ROTATE_SPEED;
                                  rightSpeed = ROTATE_SPEED;
                                }
                                robot.driveWheels(leftSpeed, rightSpeed);
                            }
                        }
                        // stop thread
                    case '5': //up
                        if(bhit=='1'){
                            leftSpeed = rightSpeed = DEFAULT_SPEED;
                        } else {
                            needToStopRobot = 1;
                        }
                        break;
                    case '6': //down
                        if(bhit=='1') {
                            leftSpeed = rightSpeed = -DEFAULT_SPEED;
                        } else {
                            needToStopRobot = 1;
                        }
                        break;
                    case '7': //left
                        if(bhit=='1') {
                            leftSpeed = -DEFAULT_SPEED;
                            rightSpeed = DEFAULT_SPEED;
                        } else {
                            needToStopRobot = 1;
                        }
                        break;
                    case '8': //right
                        if(bhit=='1') {
                            leftSpeed = DEFAULT_SPEED;
                            rightSpeed = -DEFAULT_SPEED;
                        } else {
                            needToStopRobot = 1;
                        }
                        break;
                    default:
                        robot.stop(0.5);
                        break;
                }
            }
        }
        if(needToStopRobot){
            robot.stop(0.5);
            needToStopRobot = 0;
            leftSpeed = 0;
            rightSpeed = 0;
            robot.driveWheels(leftSpeed, rightSpeed);
        } else {
            robot.driveWheels(leftSpeed, rightSpeed);
        }
    }
}