Diff: copy.cpp

Revision:

37:3d14df6dec34

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/copy.cpp	Thu Mar 26 20:13:49 2015 +0000
@@ -0,0 +1,264 @@
+/***
+********** 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;
+     }
+}