File content as of revision 37:3d14df6dec34:

/***
********** MAIN PROGRAM **********

Sensors are mapped on the global variable sensorsCheckSum,
which multiplies the sensor number by itself to then decode,
 which sensors are off and which are on
ie. if sensor rightright - sensorChecksum = 1*1 = 1
    if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
    ...
***/

#include "mbed.h"
#include "sensor_measure.h"
#include "Motor_Driver.h"
#include "shooter.h"

#define PWM_PERIOD_US 10000

typedef enum  mode {REGULAR,SQUARE} mode; //enumeration for different states
DigitalOut led(LED_RED);
DigitalOut ledMode(LED_GREEN);
Serial HC06(PTE0,PTE1);

void turnLeft ();
void turnRight ();
void measureSensors ();
void printBluetooth();
void shootFind ();


Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object

mode driveMode; //declaring the variable for the states
int sensorsCheckSum; //varibale used for sensors mapping access
int passedTime1,passedTime2;
int cursor = 0, k = 0;
static float normalSpeed = 0.15f;



int main() {
    led = 1;//start LED with beginning of main program   
    measureTimer.start();
    driveMode = REGULAR; //initialise drivemoder
    sensor_initialise(); // initialise sensor values
    wait(1); //give time to set up the system
    
    sensorTimer.start(); // start timer for sensors
    
    motors.setSteeringMode (NORMAL);
    motors.disableHardBraking();
    motors.forward ();
    
    motors.start (); 
    
    motors.setSpeed (normalSpeed);

    motors.enableSlew();
    int testOtherCases = 0; //used for control
    while(1){       
        
        measureSensors();
        printBluetooth();
        switch (sensorsCheckSum) {    
            case 94: //continue straight
            motors.setSpeed (normalSpeed);
            break;
            case 104:
                motors.setSpeed (normalSpeed); //move a little forward
                measureSensors(); //measure again
                if (sensorsCheckSum == 104 || sensorsCheckSum == 168) { //confirm the turn                
                    turnRight();
                break;
            case 158:
                motors.setSpeed (normalSpeed);
                measureSensors(); 
                        
                if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
                    turnLeft();
                }
                break;
            case 168:
                motors.setSpeed (normalSpeed);
                measureSensors(); 
                    
                if (sensorsCheckSum == 168) { //confirm the turn
                    turnRight ();
                }
                
                break;
            case 194:
                    motors.setSpeed (normalSpeed);
                    measureSensors(); 
                        
                    if (sensorsCheckSum == 194) { //confirm the turn      
                        turnLeft();
                    }
                    break;
            default:
                testOtherCases =1;
            break;
        }
        if (testOtherCases == 1) {
            
            if (sensorsCheckSum < 96) { //turn left
                motors.setSpeed (0.0);
                motors.setLeftSpeed(normalSpeed);
                wait(0.075);
            }
            else if (sensorsCheckSum > 96) { // turn right
                motors.setSpeed (0.0);
                motors.setRightSpeed(normalSpeed);
                wait(0.1);
            }
            
        }   
        
        motors.setSpeed(0.0); //give time for the system to settle
        wait(0.2);
    }
} //int main

//print statements for debugging
void printBluetooth() { /
    //HC06.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
    //HC06.printf("LLD%i  LRD%i               rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
    //HC06.printf("%i  %i  %i \n",sensorArray[8]->state,sensorArray[9]->state,sensorArray[3]->state,sensorArray[10]->state);
    //HC06.printf("%i      %i/n%i      %i,sensorArray[NUMBER_SENSORS_REGULAR]->state,sensorArray[NUMBER_SENSORS_REGULAR+1]->state,sensorArray[NUMBER_SENSORS_REGULAR+2]->state,sensorArray[NUMBER_SENSORS_REGULAR+3]->state);        
    //HC06.printf("%f                      %f",motor.getLeftSpeed(),motor.getRightSpeed());
    //HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
    //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
}



void turnRight () {
    
    ledMode = 0; // put on LED to know we're in this state
    motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
    wait(1);
    motors.setSteeringMode(PIVOT_CW); //pivot for the turn
    motors.setSpeed (normalSpeed); 
    wait (1); 
    int exit = 0; //used for exiting the function
    motors.setSteeringMode (NORMAL);
    while (1) {   
        motors.setSpeed(0.0); //stop and check if the line is reached
        exit = 0;
        wait (0.4);
        for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
            if (measure(sensorArray[i]) == 0) {//if sensor is black 
                 exit ++;
            }
            if (exit > 2){ // if two sensors are black you're on the line
                motors.setSteeringMode(PIVOT_CW); //rotate back to realign
                motors.setSpeed(normalSpeed);
                wait (0.3);
                motors.setSteeringMode(NORMAL); //go back to normal
                while (1) {
                    int checkSumLocal =0;
                    for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
                        if (measure(sensorArray[i]) == 0) 
                            checkSumLocal += i*i;
                    }     
                    if (checkSumLocal == 74) {
                        ledMode = 1; // turn off the LED to signify going out of this state
                        return;
                    }       
                    else if (checkSumLocal < 74) {
                         motors.setSpeed (0.0);
                         motors.setRightSpeed(normalSpeed);
                         wait(0.2);
                    }
                    else if (checkSumLocal > 74) {
                         motors.setSpeed (0.0);
                            motors.setLeftSpeed(normalSpeed);
                            wait(0.2);
                        }                 
                }

            }        

        }
        motors.setRightSpeed (normalSpeed*1.7); //increase of the motors to bank left
        motors.setLeftSpeed (normalSpeed);
    }

}

void turnLeft () { 

    ledMode = 0; // put on LED to know we're in this state
    motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
    wait(1.1);
    motors.setSteeringMode(PIVOT_CCW);
    motors.setSpeed (normalSpeed); //turn only the relevant wheel
    wait (1); 
    int exit = 0; //used for exiting the function
    motors.setSteeringMode (NORMAL);
    while (1) {   
        motors.setSpeed(0.0); //stop and check if the line is reached
        exit = 0;
        for (int i = 0; i < 4; i++) {
            if (measure(sensorArray[i]) == 0) {//if sensor is black 
                exit ++;
            }
            if (exit > 2){ // two sensors are considered on line
                motors.setSteeringMode(PIVOT_CCW); //pivot
                motors.setSpeed(normalSpeed);
                wait (0.3);
                motors.setSpeed(0.0);
                motors.setSteeringMode(NORMAL);
                while (1) {
                    int checkSumLocal =0;
                    for (int i = 0; i < 4; i++) {
                        if (measure(sensorArray[i]) == 1) 
                            checkSumLocal += (i+1)*(i+1);
                    }     
                    if (checkSumLocal == 20) {
                        led = 0;
                        ledMode = 1; // turn off the LED   
                        return;
                    }    
                    else if (checkSumLocal > 20) {
                         motors.setSpeed (0.0);
                         motors.setRightSpeed(normalSpeed);
                         wait(0.2);
                    }
                    else if (checkSumLocal < 20) {
                         motors.setSpeed (0.0);
                         motors.setLeftSpeed(normalSpeed);
                         wait(0.2);
                    }
                     
                }

            }        

        }
        motors.setRightSpeed (normalSpeed);
        motors.setLeftSpeed (normalSpeed*1.5);
        wait(0.2);
    }
}

void shootFind () { //needs to be implemeted
}

void measureSensors () {
     sensorsCheckSum = 0; //zero it when first going into the routine
     int iterationNumber = NUMBER_SENSORS_REGULAR;
     if (driveMode == SQUARE) {
          iterationNumber += NUMBER_SENSORS_SQUARE;
     }
     for (int i = 0; i < iterationNumber;i++){
         if (measure(sensorArray[i]) == 1) {//if sensor is white 
            sensorsCheckSum += (i+1)*(i+1);
        }
     }
     if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
        oldValues[cursor] = sensorsCheckSum;
        cursor = (cursor+1)%5;
     }
}