File content as of revision 27:fc0fd2c0eebb:

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

Serial HC06(PTE0,PTE1);

Timer measureTimer; //Timer used for measurement

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

typedef enum  mode {REGULAR,SQUARE} mode; //enumeration for different states
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;


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

void printBluetooth() { //for debugging
    pc.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
    pc.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 %i",sensorArray[NUMBER_SENSORS_REGULAR-3]->state,sensorArray[NUMBER_SENSORS_REGULAR-4]->state,sensorArray[3]->state,sensorArray[2]->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());
    pc.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
    //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
}

int main() {
        
    Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US);
    
   // setup_counter(1000, 0);
   // float frequency = measure_frequency(CHANNEL_1);
    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
//    float normalSpeed = 0.01f;
/*
    while(1){
        if (pc.getc() == 'r') {
        measureSensors();
        //measureTimer.reset();
        printBluetooth();
        //passedTime1 = measureTimer.read_ms();
        //if (sensorsCheckSum == 0) {
//            motors.setSpeed(normalSpeed);
//            }
//        else if (sensorsCheckSum == 1 || sensorsCheckSum == 9 || sensorsCheckSum == 10 || sensorsCheckSum == 14 || sensorsCheckSum==26){
//            motors.setLeftSpeed(normalSpeed/2);
//            
//            motors.setRightSpeed(normalSpeed*6);
//        }
//        else if (sensorsCheckSum == 4 && sensorsCheckSum ==16 || sensorsCheckSum == 20 || sensorsCheckSum ==21 ||sensorsCheckSum== 29) {
//            motors.setRightSpeed(normalSpeed/2);
//            motors.setLeftSpeed(normalSpeed*9);
//        }
//        else {
//            motors.setSpeed(normalSpeed);
//        }
    }
*/
//    int testOtherCases = 0; //needed for control statements
    while (1) {
       
        if (driveMode == REGULAR) {
                measureSensors();
                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.setSpeed(0.1f);
                            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.setSpeed(0.1f);
                            motors.start();
                            wait(2);
                            motors.stop();
                            motors.setSteeringMode(PIVOT_CCW);
                            motors.setSpeed(0.1f);
                            do
                            {   
                            motors.start();
                            measureSensors();
                            } while (sensorsCheckSum != 96);
                            motors.stop();
                        }
                    } 
            break;
          case 30:                                                  //all right are white, left all black >> turn right(move left wheel faster)
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.1f);
            motors.setLeftSpeed(0.05f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 30);
            motors.stop();
            break;

        case 94: //normal <<STARTING POSITION>>, half of right and half of left are white
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setSpeed(0.1f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 94);
            motors.start();
            break;
        case 104: //right all white, left half white >> turn right
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setSpeed(0.1f);
            motors.start();
            wait(1);
            motors.stop();
            motors.setRightSpeed(0.2f);
            motors.setLeftSpeed(0.0f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum <= 94);
            motors.stop();
            break;
        case 174: //left all white, right all black >> turn left (move right wheel)
            motors.setSteeringMode(NORMAL);
            motors.forward();
            motors.setRightSpeed(0.05f);
            motors.setLeftSpeed(0.1f);
            do
            {   
            motors.start();
            measureSensors();
            } while (sensorsCheckSum == 174);
            break;
        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.05f);
            motors.setLeftSpeed(0.1f);
            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.05f);
                    motors.setLeftSpeed(0.1f);
                    do
                    {   
                    motors.start();
                    measureSensors();
                    } while (sensorsCheckSum == 204);
                    motors.stop();
                }
                if (oldValues[k] == 104) {          //right turn 90 situation
                    motors.setSteeringMode(PIVOT_CW);
                    motors.setRightSpeed(0.1f);
                    motors.setLeftSpeed(0.1f);
                    do
                    {
                    motors.start();
                    measureSensors();
                    } while (sensorsCheckSum != 94);
                    motors.stop();
                }            
        break;              
        default:
            measureSensors(); 
            break;
        }
    }
}
}
}