File content as of revision 38:02ef89edd828:

/*
****** 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

DigitalOut led(LED_RED);
Serial HC06(PTE0,PTE1);
Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object

Timer measureTimer; //Timer used for measurements

typedef enum mode {REGULAR,SQUARE} mode;
mode driveMode; //enumeration for different states
typedef enum flag {USE, SKIP} flag;
flag oldValuesFlag;

int sensorsCheckSum; //variable used for sensors mapping access
int passedTime1,passedTime2;
int oldValues[5] = {0};
int cursor = 0, k = 0;
float normalSpeed = 0.15f;

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

int main() {
    
    led = 0;        //start LED with beginning of main program    
    measureTimer.start();
    sensorTimer.start();        // start timer for sensors
    sensor_initialise();
    driveMode = REGULAR;        // initialise sensor values
    oldValuesFlag = USE;
    wait(1);        //give time to set up the system

    motors.setSteeringMode(NORMAL);
    motors.disableHardBraking();
    motors.forward();
    motors.setSpeed (0.1f);
    motors.start();
    wait(1); 
    //motors.enableSlew();
    
    while (1) {
        measureSensors();
        oldValuesFlag = USE;                
        switch (sensorsCheckSum) {
/*        case 0:     //all black >> turn around by 180 degrees or a possible turn on the left
                for (k=0;k<5;k++) {                             //left turn situation >> stop, go backwards
                    if (oldValues[k] == 194) {
                        motors.stop();
                        motors.setSteeringMode(NORMAL);
                        motors.reverse();
                        motors.setRightSpeed(0.1f);
                        motors.setLeftSpeed(0.2f);
                        motors.start();
                        wait(2);
                        motors.stop();
                    } else {       //dead end situation >> stop, go a bit backwards so there is space for turning and turn CCW
                        motors.stop();
                        motors.setSteeringMode(NORMAL);
                        motors.reverse();
                        motors.setRightSpeed(0.1f);
                        motors.setLeftSpeed(0.2f);
                        motors.start();
                        wait(2);
                        motors.stop();
                        motors.setSteeringMode(PIVOT_CCW);
                        motors.setRightSpeed(0.1f);
                        motors.setLeftSpeed(0.2f);
                        do
                        {   
                        motors.start();
                        measureSensors();
                        } while (sensorsCheckSum != 96);
                        motors.stop();
                    }
                } 
            break;  */
        case 30:    //all right sensors are white, left all black >> turn right
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.02f);
            motors.setLeftSpeed(0.04f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 30);
            motors.stop();
            break;
        case 46:    //robot is facing north-west >> turn right
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.02f);
            motors.setLeftSpeed(0.05f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 46);
            motors.stop();
            break;            
        case 94:    //normal <<STARTING POSITION>>, half of right and half of left are white
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.08f);
            motors.setLeftSpeed(0.08f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 94);
            motors.stop();
            break;
//        case 95:        //right turn is present always followed by case 104           
        case 104:   //right all white, left half white >> crossroads, make a right turn
            motors.stop();
            wait(0.5);
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.01f);
            motors.setLeftSpeed(0.09f);
//            motors.start();
//            wait(0.5);
//            motors.stop();
//            motors.setSteeringMode(PIVOT_CCW);
//            motors.setRightSpeed(0.07f);
//            motors.setLeftSpeed(0.07f);
            do
            {   
            motors.start();
            measureSensors();
            } while (measure(sensorArray[0]) == 0);
            motors.stop();
            break;
        case 154:   //robot is facing north-east >> turn left
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.05f);
            motors.setLeftSpeed(0.02f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 154);
            motors.stop();
            break;            
        case 174:   //left all white, right all black >> turn left (move right wheel)
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.06f);
            motors.setLeftSpeed(0.02f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 174);
            motors.stop();
            break;
//        case 179: //right turn, followed by case 204
        case 194:   //left all white, right half white >> go straight, turn right if 194 goes to 204
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.01f);
            motors.setLeftSpeed(0.02f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 194);
            motors.stop();       //do while left is white
            break;
/*        case 204:   //all sensors are white                 
            for (k=0;k<6;k++) {                 //situation when a square is entered, need to follow right line
                if (oldValues[k] == 194) {      //checks whether  black line on the right was present before
                    motors.setSteeringMode(NORMAL);
                    motors.setRightSpeed(0.01f);
                    motors.setLeftSpeed(0.05f);
                    do
                    {   
                    motors.start();
                    measureSensors();
                    } while (sensorsCheckSum == 204);
                    motors.stop();
                }
                if ((oldValues[k] == 30) or (oldValues[k] == 104))  {          //right turn 90 situation
                    motors.stop();
                    wait(0.5);
                    motors.setSteeringMode(NORMAL);
                    motors.forward();
                    motors.setRightSpeed(0.01f);
                    motors.setLeftSpeed(0.09f);
//                  motors.start();
//                  wait(0.5);
//                  motors.stop();
//                  motors.setSteeringMode(PIVOT_CCW);
//                  motors.setRightSpeed(0.07f);
//                  motors.setLeftSpeed(0.07f);
                    do
                    {   
                    motors.start();
                    measureSensors();
                    } while (measure(sensorArray[0]) == 0);
                    motors.stop();
                    break;
                    }    
                }        
            break;  */            
        default:            
 /*          if (sensorsCheckSum < 96) {
                motors.setSpeed(0.0);
                motors.setLeftSpeed(0.02);
            }
            else if (sensorsCheckSum > 96) {
                 motors.setSpeed (0.0);
                 motors.setRightSpeed(0.02);
                 }
            } */
            //motors.start();  
            oldValuesFlag = SKIP;
            break;

        }   // switch statement
    }      //while loop
}   // main
/*    
    motors.setSpeed (normalSpeed);
    while(1){    
       measureSensors();
//        if (HC06.getc() == 'r') {
//            measureSensors();
//            printBluetooth();
//        }
        
} //int main

void turnRight () {
    motors.setSpeed (normalSpeed);
    wait(0.7);
    motors.setSpeed(0.0);
    motors.setLeftSpeed (normalSpeed);
    wait (1.4);  
    int exit = 0; //used for exiting the function
    motors.setSpeed(0.0);
    wait(0.2);
    motors.setSpeed (normalSpeed);
    for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
        if (measure(sensorArray[i]) == 0) {//if sensor is black 
             exit ++;
        }
        if (exit > 1){
            
            return;
        }
        exit = 0;
    }
} */

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("%f                      %f",motor.getLeftSpeed(),motor.getRightSpeed());
    HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
    //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
}


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) and (oldValuesFlag == USE)) {
        oldValues[cursor] = sensorsCheckSum;
        cursor = (cursor+1)%5;
     }*/
     if ((oldValues[cursor] != sensorsCheckSum) and (oldValuesFlag == USE)) {       // priority queu, ideas to make it faster/shorter?
        int i = 0;
        while ((oldValues[cursor] != sensorsCheckSum)) {
            for (k = 0; k < 5; k++) {
                if (oldValues[k] == sensorsCheckSum) {
                    for (i = k; i > 0; i--) {                           
                       oldValues[i] = oldValues[i-1];
                    }
                } else {
                    oldValues[k] = oldValues[k-1];
                }
            oldValues[0] = sensorsCheckSum;    
            }
        }
     }
}