File content as of revision 37:3d14df6dec34:

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

Please consider that although it is an embedded envrionment we are NOT creating a HARD-TIME real time system - delays can be dealt with

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 ();


Timer measureTimer; //Timer used for measurement
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 oldValues[5] = {0};
int cursor = 0, k = 0;
static float normalSpeed = 0.15f;
int countRightTurns = 0;
int countLeftTurns = 0;


int main() {
        
   //setup_counter(1000, 0);
   // float frequency = measure_frequency(CHANNEL_1);
    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
    
    squareOut.period(0.01f);  // 0.01 second period
    squareOut.write(0.5);  // 50% duty cycle
//    for (int b = 1; b <7;b++) {
//                    shoot(0.4);
//                }
    shoot (0);
//    motors.setSteeringMode (NORMAL);
//    motors.disableHardBraking();
//    motors.forward ();
//    
//    motors.start (); 
//    
//    motors.setSpeed (normalSpeed);

    //motors.enableSlew();
 
    // HC06.printf("heello");
    int testOtherCases = 0; //used for control
    while(1){    
       
    // printBluetooth();
//        while (HC06.getc() != 'r') {
//            measureSensors();
//            printBluetooth();
//            wait(1);
//        }
        
        

        measureSensors();
        printBluetooth();
        switch (sensorsCheckSum) {
            
            
            case 94:
            motors.setSpeed (normalSpeed);
            wait(0.15);
            break;
            
            case 104:
                      if ( countRightTurns == 1)
                        turnRight();
                    else {
                        motors.setSpeed (normalSpeed);
                        wait(0.15);
                        measureSensors(); 
                        
                        if (sensorsCheckSum == 104) { //confirm the turn
                            
                            wait(0.15);
                            measureSensors();
                            if (sensorsCheckSum == 104)
                                turnRight ();
                        }
                    }
                
            break;
            case 158:
                        motors.setSpeed (normalSpeed);
                        wait(0.15);
                        measureSensors(); 
                        
                        if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
                         
                            turnLeft();
                        }
                break;
            case 168:
                if ( countRightTurns == 1)
                 turnRight();
                else {
                    motors.setSpeed (normalSpeed);
                    wait(0.15);
                    measureSensors(); 
                    
                    if (sensorsCheckSum == 168) { //confirm the turn
                        
                        wait(0.15);
                        measureSensors();
                        if (sensorsCheckSum == 168)
                            turnRight ();
                    }
                }
            break;
            case 194:
                        motors.setSpeed (normalSpeed);
                        wait(0.15);
                        measureSensors(); 
                        
                        if (sensorsCheckSum == 194) { //confirm the turn
                            
                            
                            turnLeft();
                        }
                break;
            default:
            testOtherCases =1;
            break;
        }
        if (testOtherCases == 1) {
            
            if (sensorsCheckSum < 96) {
                motors.setSpeed (0.0);
                motors.setLeftSpeed(normalSpeed);
                wait(0.075);
            }
            else if (sensorsCheckSum > 96) {
                motors.setSpeed (0.0);
                motors.setRightSpeed(normalSpeed);
                wait(0.1);
            }
            
        }   
        
        motors.setSpeed(0.0);
        wait(0.2);
        
        
    }
} //int main

void printBluetooth() { //for debugging
    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
    if (countRightTurns == 0 ) {
        motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
        wait(1);
        motors.setSteeringMode(PIVOT_CCW);
        motors.setSpeed (normalSpeed); //turn only the relevant wheel
        wait (1); 
    }
    else {
        motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
        wait(1);
        motors.setSteeringMode(PIVOT_CCW);
        motors.setSpeed (normalSpeed); //turn only the relevant wheel
        wait (0.8); 
    }

    int exit = 0; //used for exiting the function
    motors.setSpeed(0.0);
    wait(0.5);
    motors.setSteeringMode (NORMAL);
    wait(0.3);
    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){ // two sensors are considered on line
                motors.setSteeringMode(PIVOT_CCW);
                motors.setSpeed(normalSpeed);
                wait (0.3);
                motors.setSpeed(0.0);
                motors.setSteeringMode(NORMAL);
                while (1) {
                    int checkSumLocal =0;
                    for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {

                        if (measure(sensorArray[i]) == 0) 
                            checkSumLocal += i*i;
                    }     
                        printBluetooth();
                        if (checkSumLocal == 74) {
                            led = 0;
                            ledMode = 1; // turn off the LED
                            countRightTurns ++;
                            wait(1);
                            return;
                         }   
                         if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
                            motors.setSteeringMode(PIVOT_CCW);
                            motors.setSpeed(normalSpeed);
                            wait (0.6);
                            motors.setSpeed(0.0);
                            motors.setSteeringMode(NORMAL);
                         
                         }    
                        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.setSpeed(0.0);
                    wait(0.4);                      
                }

            }        

        }
//        motors.setSpeed(normalSpeed);
        motors.setRightSpeed (normalSpeed*1.7);
        motors.setLeftSpeed (normalSpeed);
        wait(0.2);
    }

}

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_CW);
    motors.setSpeed (normalSpeed); //turn only the relevant wheel
    wait (1); 
    int exit = 0; //used for exiting the function
    motors.setSpeed(0.0);
    wait(0.5);
    motors.setSteeringMode (NORMAL);
    wait(0.3);
    while (1) {   
        motors.setSpeed(0.0); //stop and check if the line is reached
        exit = 0;
        wait (0.4);
        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_CW);
                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);
                    }     
                        HC06.printf ("checkSumLocal is %i \n", checkSumLocal);
                        if (checkSumLocal == 20) {
                            led = 0;
                            ledMode = 1; // turn off the LED
                            countLeftTurns ++;
                            wait(1);
                            if (countLeftTurns == 1) {
                                //shootFind ();
                            }    
                            return;
                         }   
                         if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
                            motors.setSteeringMode(PIVOT_CCW);
                            motors.setSpeed(normalSpeed);
                            wait (0.6);
                            motors.setSpeed(0.0);
                            motors.setSteeringMode(NORMAL);
                         
                         }    
                        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.setSpeed(0.0);
                    wait(0.4);                      
                }

            }        

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

void shootFind () {
    motors.setSpeed(normalSpeed);
    wait (1);
    motors.setSteeringMode (PIVOT_CW);
    wait (0.8);
    motors.setSteeringMode (NORMAL);

    while (1) {
        motors.setSpeed(0.0);
        wait (0.3);
        for (int i = 8; i < 11 ; i++) {
            if (measure(sensorArray[i]) == 2){
                motors.setSteeringMode (PIVOT_CW);
                motors.setSpeed(normalSpeed);
                wait (1.6);
                motors.setSpeed(normalSpeed);
                for (int b = 1; b <7;b++) {
                    shoot(0.1*(float)b);
                }
                shoot (0);
                motors.setSteeringMode(NORMAL);
 //               while (1) {
//                    motors.setSpeed(0.0);
//                    wait (0.4);
//                    for (int c =4 ;c <8;c++) {
//                       if( measure(sensorArray[i]) == 0){
//                            return;
//                        }
//                    }
//                    motors.setSpeed(normalSpeed);
//                    motors.setLeftSpeed(normalSpeed*1.3);
//                    wait(0.2);
//                }
                return;
            }
                
        }
        motors.setSpeed (normalSpeed);
        wait (0.5);
        
    }
}

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++){
         //pc.printf("%i  iteration%i ",i,iterationNumber);
         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;
     }
     //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
}