Ivelin Kozarev / Mbed 2 deprecated TDP_main_fork

Team Design project 3 main file

Dependencies:   LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed

Fork of TDP_main by Yelfie

main.cpp@36:a48d57d63630, 2015-03-25 (annotated)

Committer:
Reckstyle
Date:
Wed Mar 25 11:50:44 2015 +0000
Revision:
36:a48d57d63630
Parent:
35:1819c5a8254a
Child:
37:3d14df6dec34
gd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reckstyle 2:e2adb7ab2947 1 /*
Reckstyle 2:e2adb7ab2947 2 ****** MAIN PROGRAM ******
Reckstyle 2:e2adb7ab2947 3
Reckstyle 2:e2adb7ab2947 4 Please consider that although it is an embedded envrionment we are NOT creating a HARD-TIME real time system - delays can be dealt with
Reckstyle 11:9e56d52485d1 5
Reckstyle 11:9e56d52485d1 6 Sensors are mapped on the global variable sensorsCheckSum,
Reckstyle 11:9e56d52485d1 7 which multiplies the sensor number by itself to then decode,
Reckstyle 11:9e56d52485d1 8 which sensors are off and which are on
Reckstyle 11:9e56d52485d1 9 ie. if sensor rightright - sensorChecksum = 1*1 = 1
Reckstyle 11:9e56d52485d1 10 if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
Reckstyle 11:9e56d52485d1 11 ...
Reckstyle 2:e2adb7ab2947 12 */
Reckstyle 2:e2adb7ab2947 13
Reckstyle 0:5ca0450111f3 14 #include "mbed.h"
Reckstyle 11:9e56d52485d1 15 #include "sensor_measure.h"
Joseph_Penikis 5:2fc7bf0135d4 16 #include "Motor_Driver.h"
orsharp 12:bb21b76b6375 17 #include "shooter.h"
Reckstyle 0:5ca0450111f3 18
Joseph_Penikis 5:2fc7bf0135d4 19 #define PWM_PERIOD_US 10000
Reckstyle 0:5ca0450111f3 20
Reckstyle 36:a48d57d63630 21 typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
Reckstyle 28:d1e7daeb240e 22 DigitalOut led(LED_RED);
Reckstyle 36:a48d57d63630 23 DigitalOut ledMode(LED_GREEN);
Bartas 24:ecc3fbaf2824 24 Serial HC06(PTE0,PTE1);
Reckstyle 9:718987b106a8 25
Reckstyle 35:1819c5a8254a 26 void turnLeft ();
Reckstyle 35:1819c5a8254a 27 void turnRight ();
Reckstyle 35:1819c5a8254a 28 void measureSensors ();
Reckstyle 35:1819c5a8254a 29 void printBluetooth();
Reckstyle 35:1819c5a8254a 30
Reckstyle 35:1819c5a8254a 31
Reckstyle 18:d277614084bc 32 Timer measureTimer; //Timer used for measurement
Reckstyle 29:307b45a52401 33 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
Reckstyle 9:718987b106a8 34
Reckstyle 36:a48d57d63630 35
Reckstyle 11:9e56d52485d1 36 mode driveMode; //declaring the variable for the states
Reckstyle 11:9e56d52485d1 37 int sensorsCheckSum; //varibale used for sensors mapping access
Reckstyle 18:d277614084bc 38 int passedTime1,passedTime2;
Reckstyle 25:bec5dc4c9f5e 39 int oldValues[5] = {0};
Bartas 27:fc0fd2c0eebb 40 int cursor = 0, k = 0;
Reckstyle 36:a48d57d63630 41 static float normalSpeed = 0.15f;
Reckstyle 11:9e56d52485d1 42
Reckstyle 11:9e56d52485d1 43
Reckstyle 35:1819c5a8254a 44 int main() {
Reckstyle 35:1819c5a8254a 45
Reckstyle 35:1819c5a8254a 46 //setup_counter(1000, 0);
Reckstyle 35:1819c5a8254a 47 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 35:1819c5a8254a 48 led = 0;//start LED with beginning of main program
Reckstyle 35:1819c5a8254a 49
Reckstyle 35:1819c5a8254a 50 measureTimer.start();
Reckstyle 35:1819c5a8254a 51 driveMode = REGULAR; //initialise drivemoder
Reckstyle 35:1819c5a8254a 52 sensor_initialise(); // initialise sensor values
Reckstyle 35:1819c5a8254a 53 wait(1); //give time to set up the system
Reckstyle 35:1819c5a8254a 54
Reckstyle 35:1819c5a8254a 55 sensorTimer.start(); // start timer for sensors
Reckstyle 35:1819c5a8254a 56
Reckstyle 35:1819c5a8254a 57 motors.setSteeringMode (NORMAL);
Reckstyle 35:1819c5a8254a 58 motors.disableHardBraking();
Reckstyle 35:1819c5a8254a 59 motors.forward ();
Reckstyle 35:1819c5a8254a 60
Reckstyle 35:1819c5a8254a 61 motors.start ();
Reckstyle 35:1819c5a8254a 62 motors.setSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 63
Reckstyle 35:1819c5a8254a 64 //motors.enableSlew();
Reckstyle 35:1819c5a8254a 65
Reckstyle 35:1819c5a8254a 66 // HC06.printf("heello");
Reckstyle 36:a48d57d63630 67 int testOtherCases = 0; //used for control
Reckstyle 35:1819c5a8254a 68 while(1){
Reckstyle 35:1819c5a8254a 69 measureSensors();
Reckstyle 36:a48d57d63630 70 //printBluetooth();
Reckstyle 36:a48d57d63630 71 // while (HC06.getc() != 'r') {
Reckstyle 35:1819c5a8254a 72 // measureSensors();
Reckstyle 35:1819c5a8254a 73 // printBluetooth();
Reckstyle 36:a48d57d63630 74 // wait(1);
Reckstyle 35:1819c5a8254a 75 // }
Reckstyle 35:1819c5a8254a 76
Reckstyle 35:1819c5a8254a 77 switch (sensorsCheckSum) {
Reckstyle 35:1819c5a8254a 78 case 94:
Reckstyle 36:a48d57d63630 79 motors.setSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 80 wait(0.4);
Reckstyle 35:1819c5a8254a 81 break;
Reckstyle 35:1819c5a8254a 82
Reckstyle 35:1819c5a8254a 83 case 104:
Reckstyle 36:a48d57d63630 84 motors.setSpeed (normalSpeed);
Reckstyle 36:a48d57d63630 85 wait(0.15);
Reckstyle 36:a48d57d63630 86 measureSensors();
Reckstyle 36:a48d57d63630 87
Reckstyle 36:a48d57d63630 88 if (sensorsCheckSum == 104) { //confirm the turn
Reckstyle 36:a48d57d63630 89
Reckstyle 36:a48d57d63630 90 wait(0.15);
Reckstyle 36:a48d57d63630 91 measureSensors();
Reckstyle 36:a48d57d63630 92 if (sensorsCheckSum == 104)
Reckstyle 36:a48d57d63630 93 turnRight ();
Reckstyle 36:a48d57d63630 94 }
Reckstyle 36:a48d57d63630 95
Reckstyle 36:a48d57d63630 96 break;
Reckstyle 36:a48d57d63630 97 case 168:
Reckstyle 36:a48d57d63630 98 motors.setSpeed (normalSpeed);
Reckstyle 36:a48d57d63630 99 wait(0.15);
Reckstyle 36:a48d57d63630 100 measureSensors();
Reckstyle 36:a48d57d63630 101
Reckstyle 36:a48d57d63630 102 if (sensorsCheckSum == 104) { //confirm the turn
Reckstyle 36:a48d57d63630 103
Reckstyle 36:a48d57d63630 104 wait(0.15);
Reckstyle 36:a48d57d63630 105 measureSensors();
Reckstyle 36:a48d57d63630 106 if (sensorsCheckSum == 104)
Reckstyle 36:a48d57d63630 107 turnRight ();
Reckstyle 36:a48d57d63630 108 }
Reckstyle 35:1819c5a8254a 109 break;
Reckstyle 35:1819c5a8254a 110 default:
Reckstyle 35:1819c5a8254a 111 testOtherCases =1;
Reckstyle 35:1819c5a8254a 112 break;
Reckstyle 35:1819c5a8254a 113 }
Reckstyle 35:1819c5a8254a 114 if (testOtherCases == 1) {
Reckstyle 35:1819c5a8254a 115 if (sensorsCheckSum < 96) {
Reckstyle 35:1819c5a8254a 116 motors.setSpeed (0.0);
Reckstyle 35:1819c5a8254a 117 motors.setLeftSpeed(normalSpeed*2);
Reckstyle 36:a48d57d63630 118 wait(0.2);
Reckstyle 35:1819c5a8254a 119 }
Reckstyle 35:1819c5a8254a 120 else if (sensorsCheckSum > 96) {
Reckstyle 35:1819c5a8254a 121 motors.setSpeed (0.0);
Reckstyle 35:1819c5a8254a 122 motors.setRightSpeed(normalSpeed*2);
Reckstyle 36:a48d57d63630 123 wait(0.2);
Reckstyle 35:1819c5a8254a 124 }
Reckstyle 35:1819c5a8254a 125
Reckstyle 35:1819c5a8254a 126 }
Reckstyle 35:1819c5a8254a 127 motors.setSpeed(0.0);
Reckstyle 35:1819c5a8254a 128 wait(0.2);
Reckstyle 35:1819c5a8254a 129
Reckstyle 35:1819c5a8254a 130 }
Reckstyle 35:1819c5a8254a 131 } //int main
Reckstyle 35:1819c5a8254a 132
Reckstyle 35:1819c5a8254a 133 void printBluetooth() { //for debugging
Reckstyle 35:1819c5a8254a 134 HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 35:1819c5a8254a 135 HC06.printf("LLD%i LRD%i rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
Reckstyle 35:1819c5a8254a 136 //HC06.printf("%i %i %i %i",sensorArray[NUMBER_SENSORS_REGULAR-3]->state,sensorArray[NUMBER_SENSORS_REGULAR-4]->state,sensorArray[3]->state,sensorArray[2]->state);
Reckstyle 35:1819c5a8254a 137 //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);
Reckstyle 35:1819c5a8254a 138 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Reckstyle 35:1819c5a8254a 139 HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Reckstyle 35:1819c5a8254a 140 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 35:1819c5a8254a 141 }
Reckstyle 35:1819c5a8254a 142
Reckstyle 36:a48d57d63630 143
Reckstyle 36:a48d57d63630 144
Reckstyle 35:1819c5a8254a 145 void turnRight () {
Reckstyle 36:a48d57d63630 146 ledMode = 0; // put on LED to know we're in this state
Reckstyle 36:a48d57d63630 147 motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
Reckstyle 36:a48d57d63630 148 wait(0.3);
Reckstyle 35:1819c5a8254a 149 motors.setSpeed(0.0);
Reckstyle 36:a48d57d63630 150 motors.setLeftSpeed (normalSpeed); //turn only the relevant wheel
Reckstyle 36:a48d57d63630 151 wait (1.7);
Reckstyle 35:1819c5a8254a 152 int exit = 0; //used for exiting the function
Reckstyle 36:a48d57d63630 153
Reckstyle 35:1819c5a8254a 154 for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
Reckstyle 36:a48d57d63630 155 motors.setSpeed(0.0); //stop and check if the line is reached
Reckstyle 36:a48d57d63630 156 exit = 0;
Reckstyle 35:1819c5a8254a 157 if (measure(sensorArray[i]) == 0) {//if sensor is black
Reckstyle 35:1819c5a8254a 158 exit ++;
Reckstyle 35:1819c5a8254a 159 }
Reckstyle 36:a48d57d63630 160 if (exit > 1){ // two sensors are considered on line
Reckstyle 36:a48d57d63630 161 ledMode = 1; // turn off the LED
Reckstyle 35:1819c5a8254a 162 return;
Reckstyle 36:a48d57d63630 163 }
Reckstyle 36:a48d57d63630 164 motors.setSpeed (normalSpeed);
Reckstyle 36:a48d57d63630 165 wait(0.2);
Reckstyle 35:1819c5a8254a 166 }
Reckstyle 35:1819c5a8254a 167
Reckstyle 35:1819c5a8254a 168 }
Reckstyle 35:1819c5a8254a 169
Reckstyle 36:a48d57d63630 170 void turnLeft () { //TOBE implemented
Reckstyle 36:a48d57d63630 171 }
Reckstyle 36:a48d57d63630 172
Reckstyle 11:9e56d52485d1 173 void measureSensors () {
Bartas 24:ecc3fbaf2824 174 sensorsCheckSum = 0; //zero it when first going into the routine
Bartas 24:ecc3fbaf2824 175 int iterationNumber = NUMBER_SENSORS_REGULAR;
Bartas 24:ecc3fbaf2824 176 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 177 iterationNumber += NUMBER_SENSORS_SQUARE;
Bartas 24:ecc3fbaf2824 178 }
Bartas 24:ecc3fbaf2824 179 for (int i = 0; i < iterationNumber;i++){
Bartas 24:ecc3fbaf2824 180 //pc.printf("%i iteration%i ",i,iterationNumber);
Bartas 24:ecc3fbaf2824 181 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 182 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 183 }
Bartas 24:ecc3fbaf2824 184 }
Reckstyle 25:bec5dc4c9f5e 185 if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
Reckstyle 25:bec5dc4c9f5e 186 oldValues[cursor] = sensorsCheckSum;
Reckstyle 23:9b53c72fcd38 187 cursor = (cursor+1)%5;
Bartas 24:ecc3fbaf2824 188 }
Bartas 24:ecc3fbaf2824 189 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 190 }