File content as of revision 35:1819c5a8254a:

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

DigitalOut led(LED_RED);
Serial HC06(PTE0,PTE1);

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


Timer measureTimer; //Timer used for measurement
Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object

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;
float normalSpeed = 0.15f;


int main() {
        
   //setup_counter(1000, 0);
   // float frequency = measure_frequency(CHANNEL_1);
    led = 0;//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();

    // HC06.printf("heello");
    int testOtherCases = 0;
    while(1){    
       measureSensors();
      //printBluetooth();
//        if (HC06.getc() == 'r') {
//            measureSensors();
//            printBluetooth();
//        }
        
        switch (sensorsCheckSum) {
            case 94:
            motors.setSpeed (normalSpeed*2);
            wait(0.4);
            break;
            
            case 104:
            turnRight ();
            break;
            default:
            testOtherCases =1;
            break;
        }
        if (testOtherCases == 1) {
            if (sensorsCheckSum < 96) {
                motors.setSpeed (0.0);
                motors.setLeftSpeed(normalSpeed*2);
                wait(0.1);
            }
            else if (sensorsCheckSum > 96) {
                motors.setSpeed (0.0);
                motors.setRightSpeed(normalSpeed*2);
                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 %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());
    HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
    //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
}

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