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@33:60bd40e9839e, 2015-03-24 (annotated)

Committer:
Reckstyle
Date:
Tue Mar 24 12:43:44 2015 +0000
Revision:
33:60bd40e9839e
Parent:
32:d1dc27f4a18d
Child:
34:9e7d192ee06c
ffsad

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 28:d1e7daeb240e 21 DigitalOut led(LED_RED);
Bartas 24:ecc3fbaf2824 22 Serial HC06(PTE0,PTE1);
Reckstyle 9:718987b106a8 23
Reckstyle 18:d277614084bc 24 Timer measureTimer; //Timer used for measurement
Reckstyle 29:307b45a52401 25 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
Reckstyle 9:718987b106a8 26
Reckstyle 11:9e56d52485d1 27 typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
Reckstyle 11:9e56d52485d1 28 mode driveMode; //declaring the variable for the states
Reckstyle 11:9e56d52485d1 29 int sensorsCheckSum; //varibale used for sensors mapping access
Reckstyle 18:d277614084bc 30 int passedTime1,passedTime2;
Reckstyle 25:bec5dc4c9f5e 31 int oldValues[5] = {0};
Bartas 27:fc0fd2c0eebb 32 int cursor = 0, k = 0;
Reckstyle 11:9e56d52485d1 33
Reckstyle 11:9e56d52485d1 34
Reckstyle 11:9e56d52485d1 35 void measureSensors () {
Bartas 24:ecc3fbaf2824 36 sensorsCheckSum = 0; //zero it when first going into the routine
Bartas 24:ecc3fbaf2824 37 int iterationNumber = NUMBER_SENSORS_REGULAR;
Bartas 24:ecc3fbaf2824 38 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 39 iterationNumber += NUMBER_SENSORS_SQUARE;
Bartas 24:ecc3fbaf2824 40 }
Bartas 24:ecc3fbaf2824 41 for (int i = 0; i < iterationNumber;i++){
Bartas 24:ecc3fbaf2824 42 //pc.printf("%i iteration%i ",i,iterationNumber);
Bartas 24:ecc3fbaf2824 43 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 44 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 45 }
Bartas 24:ecc3fbaf2824 46 }
Reckstyle 25:bec5dc4c9f5e 47 if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
Reckstyle 25:bec5dc4c9f5e 48 oldValues[cursor] = sensorsCheckSum;
Reckstyle 23:9b53c72fcd38 49 cursor = (cursor+1)%5;
Bartas 24:ecc3fbaf2824 50 }
Bartas 24:ecc3fbaf2824 51 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 52 }
Reckstyle 11:9e56d52485d1 53
Reckstyle 15:6453cd351452 54 void printBluetooth() { //for debugging
Reckstyle 29:307b45a52401 55 HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 29:307b45a52401 56 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 16:3649eb1a056d 57 //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 15:6453cd351452 58 //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 15:6453cd351452 59 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Reckstyle 29:307b45a52401 60 HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Reckstyle 18:d277614084bc 61 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 15:6453cd351452 62 }
Reckstyle 15:6453cd351452 63
Reckstyle 11:9e56d52485d1 64 int main() {
Reckstyle 21:9cf274ffe1de 65
Reckstyle 28:d1e7daeb240e 66
Reckstyle 29:307b45a52401 67 //setup_counter(1000, 0);
Reckstyle 22:902c3086394e 68 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 29:307b45a52401 69 led = 0;//start LED with beginning of main program
Reckstyle 29:307b45a52401 70
Reckstyle 22:902c3086394e 71 measureTimer.start();
Reckstyle 22:902c3086394e 72 driveMode = REGULAR; //initialise drivemoder
Reckstyle 22:902c3086394e 73 sensor_initialise(); // initialise sensor values
Reckstyle 22:902c3086394e 74 wait(1); //give time to set up the system
Reckstyle 22:902c3086394e 75
Reckstyle 22:902c3086394e 76 sensorTimer.start(); // start timer for sensors
Reckstyle 29:307b45a52401 77 float normalSpeed = 0.1f;
Reckstyle 32:d1dc27f4a18d 78 motors.setSteeringMode (NORMAL);
Reckstyle 29:307b45a52401 79 motors.forward ();
Reckstyle 29:307b45a52401 80 motors.setSpeed (normalSpeed);
Reckstyle 32:d1dc27f4a18d 81 motors.start ();
Reckstyle 32:d1dc27f4a18d 82 //motors.enableSlew();
Reckstyle 32:d1dc27f4a18d 83
Reckstyle 32:d1dc27f4a18d 84 // HC06.printf("heello");
Reckstyle 16:3649eb1a056d 85 while(1){
Reckstyle 32:d1dc27f4a18d 86 measureSensors();
Reckstyle 32:d1dc27f4a18d 87 // if (HC06.getc() == 'r') {
Reckstyle 32:d1dc27f4a18d 88 // measureSensors();
Reckstyle 32:d1dc27f4a18d 89 // printBluetooth();
Reckstyle 32:d1dc27f4a18d 90 // }
Reckstyle 32:d1dc27f4a18d 91 if (sensorsCheckSum == 94) {
Reckstyle 33:60bd40e9839e 92 motors.setSpeed (normalSpeed*2);
Reckstyle 33:60bd40e9839e 93 wait(0.4);
Reckstyle 29:307b45a52401 94 }
Reckstyle 29:307b45a52401 95 else if (sensorsCheckSum < 96) {
Reckstyle 29:307b45a52401 96 motors.setSpeed (0.0);
Reckstyle 32:d1dc27f4a18d 97 motors.setLeftSpeed(normalSpeed*2);
Reckstyle 33:60bd40e9839e 98 wait(0.2);
Reckstyle 28:d1e7daeb240e 99 }
Reckstyle 29:307b45a52401 100 else if (sensorsCheckSum > 96) {
Reckstyle 29:307b45a52401 101 motors.setSpeed (0.0);
Reckstyle 32:d1dc27f4a18d 102 motors.setRightSpeed(normalSpeed*2);
Reckstyle 33:60bd40e9839e 103 wait(0.2);
Reckstyle 28:d1e7daeb240e 104 }
Reckstyle 33:60bd40e9839e 105 motors.setSpeed(0.0);
Reckstyle 33:60bd40e9839e 106 wait(0.2);
Reckstyle 25:bec5dc4c9f5e 107 }
Reckstyle 28:d1e7daeb240e 108
Reckstyle 28:d1e7daeb240e 109 /*
Reckstyle 28:d1e7daeb240e 110 int testOtherCases = 0; //needed for control statements
Reckstyle 19:198dc13777eb 111 while (1) {
Reckstyle 19:198dc13777eb 112
Reckstyle 25:bec5dc4c9f5e 113 if (driveMode == REGULAR) {
Bartas 27:fc0fd2c0eebb 114 measureSensors();
Bartas 27:fc0fd2c0eebb 115 switch (sensorsCheckSum) {
Reckstyle 25:bec5dc4c9f5e 116 case 0: // all black, turn around by 180 degrees or a possible turn on the left
Bartas 27:fc0fd2c0eebb 117 for (k=0;k<5;k++) { //left turn situation >> stop, go backwards
Bartas 27:fc0fd2c0eebb 118 if (oldValues[k] == 194) {
Bartas 27:fc0fd2c0eebb 119 motors.stop();
Bartas 27:fc0fd2c0eebb 120 motors.setSteeringMode(NORMAL);
Bartas 27:fc0fd2c0eebb 121 motors.reverse();
Bartas 27:fc0fd2c0eebb 122 motors.setSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 123 motors.start();
Bartas 27:fc0fd2c0eebb 124 wait(2);
Bartas 27:fc0fd2c0eebb 125 motors.stop();
Bartas 27:fc0fd2c0eebb 126 } else { //dead end situation >> stop, go a bit backwards so there is space for turning and turn CCW
Bartas 27:fc0fd2c0eebb 127 motors.stop();
Bartas 27:fc0fd2c0eebb 128 motors.setSteeringMode(NORMAL);
Bartas 27:fc0fd2c0eebb 129 motors.reverse();
Bartas 27:fc0fd2c0eebb 130 motors.setSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 131 motors.start();
Bartas 27:fc0fd2c0eebb 132 wait(2);
Bartas 27:fc0fd2c0eebb 133 motors.stop();
Bartas 27:fc0fd2c0eebb 134 motors.setSteeringMode(PIVOT_CCW);
Bartas 27:fc0fd2c0eebb 135 motors.setSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 136 do
Bartas 27:fc0fd2c0eebb 137 {
Reckstyle 25:bec5dc4c9f5e 138 motors.start();
Reckstyle 25:bec5dc4c9f5e 139 measureSensors();
Bartas 27:fc0fd2c0eebb 140 } while (sensorsCheckSum != 96);
Reckstyle 25:bec5dc4c9f5e 141 motors.stop();
Bartas 27:fc0fd2c0eebb 142 }
Bartas 27:fc0fd2c0eebb 143 }
Bartas 27:fc0fd2c0eebb 144 break;
Bartas 27:fc0fd2c0eebb 145 case 30: //all right are white, left all black >> turn right(move left wheel faster)
Bartas 27:fc0fd2c0eebb 146 motors.setSteeringMode(NORMAL);
Bartas 27:fc0fd2c0eebb 147 motors.forward();
Bartas 27:fc0fd2c0eebb 148 motors.setRightSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 149 motors.setLeftSpeed(0.05f);
Bartas 27:fc0fd2c0eebb 150 do
Bartas 27:fc0fd2c0eebb 151 {
Bartas 27:fc0fd2c0eebb 152 motors.start();
Bartas 27:fc0fd2c0eebb 153 measureSensors();
Bartas 27:fc0fd2c0eebb 154 } while (sensorsCheckSum == 30);
Bartas 27:fc0fd2c0eebb 155 motors.stop();
Bartas 27:fc0fd2c0eebb 156 break;
Bartas 27:fc0fd2c0eebb 157
Bartas 27:fc0fd2c0eebb 158 case 94: //normal <<STARTING POSITION>>, half of right and half of left are white
Bartas 27:fc0fd2c0eebb 159 motors.setSteeringMode(NORMAL);
Bartas 27:fc0fd2c0eebb 160 motors.forward();
Bartas 27:fc0fd2c0eebb 161 motors.setSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 162 do
Bartas 27:fc0fd2c0eebb 163 {
Bartas 27:fc0fd2c0eebb 164 motors.start();
Bartas 27:fc0fd2c0eebb 165 measureSensors();
Bartas 27:fc0fd2c0eebb 166 } while (sensorsCheckSum == 94);
Bartas 27:fc0fd2c0eebb 167 motors.start();
Bartas 27:fc0fd2c0eebb 168 break;
Bartas 27:fc0fd2c0eebb 169 case 104: //right all white, left half white >> turn right
Bartas 27:fc0fd2c0eebb 170 motors.setSteeringMode(NORMAL);
Bartas 27:fc0fd2c0eebb 171 motors.forward();
Bartas 27:fc0fd2c0eebb 172 motors.setSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 173 motors.start();
Bartas 27:fc0fd2c0eebb 174 wait(1);
Bartas 27:fc0fd2c0eebb 175 motors.stop();
Bartas 30:60c424504a8b 176 motors.setSteeringMode(PIVOT_CW);
Bartas 30:60c424504a8b 177 motors.setRightSpeed(0.1f);
Bartas 30:60c424504a8b 178 motors.setLeftSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 179 do
Bartas 27:fc0fd2c0eebb 180 {
Bartas 27:fc0fd2c0eebb 181 motors.start();
Bartas 27:fc0fd2c0eebb 182 measureSensors();
Bartas 30:60c424504a8b 183 } while (sensorsCheckSum <= 104);
Bartas 27:fc0fd2c0eebb 184 motors.stop();
Bartas 27:fc0fd2c0eebb 185 break;
Bartas 27:fc0fd2c0eebb 186 case 174: //left all white, right all black >> turn left (move right wheel)
Bartas 27:fc0fd2c0eebb 187 motors.setSteeringMode(NORMAL);
Bartas 27:fc0fd2c0eebb 188 motors.forward();
Bartas 27:fc0fd2c0eebb 189 motors.setRightSpeed(0.05f);
Bartas 27:fc0fd2c0eebb 190 motors.setLeftSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 191 do
Bartas 27:fc0fd2c0eebb 192 {
Bartas 27:fc0fd2c0eebb 193 motors.start();
Bartas 27:fc0fd2c0eebb 194 measureSensors();
Bartas 27:fc0fd2c0eebb 195 } while (sensorsCheckSum == 174);
Bartas 27:fc0fd2c0eebb 196 break;
Bartas 27:fc0fd2c0eebb 197 case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
Bartas 27:fc0fd2c0eebb 198 motors.setSteeringMode(NORMAL);
Bartas 27:fc0fd2c0eebb 199 motors.forward();
Bartas 27:fc0fd2c0eebb 200 motors.setRightSpeed(0.05f);
Bartas 27:fc0fd2c0eebb 201 motors.setLeftSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 202 do
Bartas 27:fc0fd2c0eebb 203 {
Bartas 27:fc0fd2c0eebb 204 motors.start();
Bartas 27:fc0fd2c0eebb 205 measureSensors();
Bartas 27:fc0fd2c0eebb 206 } while (sensorsCheckSum == 194);
Bartas 27:fc0fd2c0eebb 207 motors.stop(); //do while left is white
Bartas 27:fc0fd2c0eebb 208 break;
Bartas 27:fc0fd2c0eebb 209 case 204 : //all sensors are white
Bartas 27:fc0fd2c0eebb 210 for (k=0;k<6;k++) { //situation when a square is entered, need to follow right line
Bartas 27:fc0fd2c0eebb 211 if (oldValues[k] == 194) { //checks whether black line on the right was present before
Bartas 27:fc0fd2c0eebb 212 motors.setSteeringMode(NORMAL);
Bartas 27:fc0fd2c0eebb 213 motors.setRightSpeed(0.05f);
Bartas 27:fc0fd2c0eebb 214 motors.setLeftSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 215 do
Bartas 27:fc0fd2c0eebb 216 {
Bartas 27:fc0fd2c0eebb 217 motors.start();
Bartas 27:fc0fd2c0eebb 218 measureSensors();
Bartas 27:fc0fd2c0eebb 219 } while (sensorsCheckSum == 204);
Bartas 27:fc0fd2c0eebb 220 motors.stop();
Reckstyle 25:bec5dc4c9f5e 221 }
Bartas 27:fc0fd2c0eebb 222 if (oldValues[k] == 104) { //right turn 90 situation
Bartas 27:fc0fd2c0eebb 223 motors.setSteeringMode(PIVOT_CW);
Bartas 27:fc0fd2c0eebb 224 motors.setRightSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 225 motors.setLeftSpeed(0.1f);
Bartas 27:fc0fd2c0eebb 226 do
Bartas 27:fc0fd2c0eebb 227 {
Bartas 27:fc0fd2c0eebb 228 motors.start();
Bartas 27:fc0fd2c0eebb 229 measureSensors();
Bartas 27:fc0fd2c0eebb 230 } while (sensorsCheckSum != 94);
Bartas 27:fc0fd2c0eebb 231 motors.stop();
Reckstyle 28:d1e7daeb240e 232 }
Reckstyle 28:d1e7daeb240e 233 }
Bartas 27:fc0fd2c0eebb 234 break;
Bartas 27:fc0fd2c0eebb 235 default:
Bartas 27:fc0fd2c0eebb 236 measureSensors();
Bartas 27:fc0fd2c0eebb 237 break;
Reckstyle 28:d1e7daeb240e 238 }
Reckstyle 28:d1e7daeb240e 239 } // if statement
Reckstyle 28:d1e7daeb240e 240 else {
Reckstyle 28:d1e7daeb240e 241 testOtherCases = 1;
Reckstyle 28:d1e7daeb240e 242 }//if driveMode == SQUARE
Reckstyle 28:d1e7daeb240e 243
Reckstyle 28:d1e7daeb240e 244 }//while loop
Reckstyle 28:d1e7daeb240e 245 */
Reckstyle 28:d1e7daeb240e 246 } //int main
Reckstyle 11:9e56d52485d1 247
Reckstyle 28:d1e7daeb240e 248