SUCA project

#include <SoftwareSerial.h>

//Constants
#define DEF_FORWARD 'f'
#define DEF_BACK 'b'
#define DEF_STOP 's'
#define DEF_LEFT 'l'
#define DEF_RIGHT 'r'
#define DEF_STRAIGHT '$'

#define STATE_STOP 1
#define STATE_FORWARD 2
#define STATE_BACK 3

#define DIR_STRAIGHT 1
#define DIR_RIGHT 2
#define DIR_LEFT 3

#define SPEED_STOP 0
#define SPEED_ONE 100
#define SPEED_TWO 180
#define SPEED_THREE 255


//Decl widgets
char data;
short state;
short iDirection;
short iSpeed;

short leftOutForward= 3;
short leftOutBack= 5;

short rightOutForward= 6;
short rightOutBack= 9;


SoftwareSerial mySerial(10, 11);


void setup() {

    //Setting the Outputs
    pinMode(leftOutForward, OUTPUT);
    pinMode(leftOutBack, OUTPUT);
    pinMode(rightOutForward, OUTPUT);
    pinMode(rightOutBack, OUTPUT);

    // Default communication rate of the Bluetooth module
    mySerial.begin(9600);
    Serial.begin(9600);

    //Init variables
    data=' ';
    state=STATE_STOP;
    iDirection=DIR_STRAIGHT;
    iSpeed=SPEED_THREE;

    Serial.print("Decl is it ok");

}


void goForward(){

    int lSpeed=iSpeed;
    int rSpeed=iSpeed;

    switch(iDirection){
        case DIR_LEFT:
            lSpeed/=2;
            break;

        case DIR_RIGHT:
            rSpeed/=2;
            break;
    }


    //Serial.println("Forward");

    analogWrite(leftOutForward, lSpeed);
    analogWrite(leftOutBack, LOW);
    analogWrite(rightOutForward, rSpeed);
    analogWrite(rightOutBack, LOW);
}
void goBack(){

    int lSpeed=iSpeed;
    int rSpeed=iSpeed;

    switch(iDirection){
        case DIR_LEFT:
            lSpeed/=2;
            break;

        case DIR_RIGHT:
            rSpeed/=2;
            break;
    }

    //Serial.println("Back");

    analogWrite(leftOutForward, LOW);
    analogWrite(leftOutBack, lSpeed);
    analogWrite(rightOutForward, LOW);
    analogWrite(rightOutBack, rSpeed);
}
void goStop(){

    //Serial.println("Stop");

    analogWrite(leftOutForward, LOW);
    analogWrite(leftOutBack, LOW);
    analogWrite(rightOutForward, LOW);
    analogWrite(rightOutBack, LOW);

    switch(iDirection){
        case DIR_LEFT:
            goLeft();
            break;

        case DIR_RIGHT:
            goRight();
            break;
    }

}
void goLeft(){

    //Serial.println("Left");

    analogWrite(leftOutForward, LOW);
    analogWrite(leftOutBack, iSpeed);
    analogWrite(rightOutForward, iSpeed);
    analogWrite(rightOutBack, LOW);

}
void goRight(){

    //Serial.println("Right");

    analogWrite(leftOutForward, iSpeed);
    analogWrite(leftOutBack, LOW);
    analogWrite(rightOutForward, LOW);
    analogWrite(rightOutBack, iSpeed);
}

void loop() {

    //Setting the state to ' '
    data=' ';

    // Checks whether data is comming from the serial port
    if(mySerial.available() > 0){

        // Reads the data from the serial port
        data = mySerial.read();

    Serial.println(data);

    }


    switch(data){
        case DEF_FORWARD:
            state=STATE_FORWARD;
            break;
        case DEF_BACK:
            state=STATE_BACK;
            break;

        case DEF_LEFT:
            iDirection=DIR_LEFT;
            break;
        case DEF_RIGHT:
            iDirection= DIR_RIGHT;
            break;

        case DEF_STRAIGHT:
            iDirection=DIR_STRAIGHT;
            break;

        case DEF_STOP:
            state=STATE_STOP;
            break;

    case '0':
      iSpeed=SPEED_STOP;
      break;

        case '1':
            iSpeed=SPEED_ONE;
            break;

        case '2':
            iSpeed=SPEED_TWO;
            break;

        case '3':
            iSpeed=SPEED_THREE;
            break;


    }

    switch(state){
        case STATE_FORWARD:
            goForward();
            break;

        case STATE_BACK:
            goBack();
            break;

        case STATE_STOP:
            goStop();
            break;
    }

    /*
    Serial.print(state);
    Serial.print("  ");
    Serial.print(iDirection);
    Serial.println();
    */


}