Yelfie / Mbed 2 deprecated TDP_main

Team Design project 3 main file

Dependencies:   mbed Motor_Driver Sensors MMA8451Q LocalPositionSystem

Fork of TDP_main by Ivelin Kozarev

main.cpp@38:02ef89edd828, 2015-03-25 (annotated)

Committer:
Bartas
Date:
Wed Mar 25 10:45:08 2015 +0000
Revision:
38:02ef89edd828
Parent:
37:c0fddc75c862 
asd

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 11:9e56d52485d1 4 Sensors are mapped on the global variable sensorsCheckSum,
Reckstyle 11:9e56d52485d1 5 which multiplies the sensor number by itself to then decode,
Reckstyle 11:9e56d52485d1 6 which sensors are off and which are on
Reckstyle 11:9e56d52485d1 7 ie. if sensor rightright - sensorChecksum = 1*1 = 1
Reckstyle 11:9e56d52485d1 8 if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
Reckstyle 11:9e56d52485d1 9 ...
Reckstyle 2:e2adb7ab2947 10 */
Reckstyle 2:e2adb7ab2947 11
Reckstyle 0:5ca0450111f3 12 #include "mbed.h"
Reckstyle 11:9e56d52485d1 13 #include "sensor_measure.h"
Joseph_Penikis 5:2fc7bf0135d4 14 #include "Motor_Driver.h"
orsharp 12:bb21b76b6375 15 #include "shooter.h"
Reckstyle 0:5ca0450111f3 16
Joseph_Penikis 5:2fc7bf0135d4 17 #define PWM_PERIOD_US 10000
Reckstyle 0:5ca0450111f3 18
Reckstyle 28:d1e7daeb240e 19 DigitalOut led(LED_RED);
Bartas 24:ecc3fbaf2824 20 Serial HC06(PTE0,PTE1);
Bartas 37:c0fddc75c862 21 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
Bartas 37:c0fddc75c862 22
Bartas 37:c0fddc75c862 23 Timer measureTimer; //Timer used for measurements
Bartas 37:c0fddc75c862 24
Bartas 37:c0fddc75c862 25 typedef enum mode {REGULAR,SQUARE} mode;
Bartas 37:c0fddc75c862 26 mode driveMode; //enumeration for different states
Bartas 37:c0fddc75c862 27 typedef enum flag {USE, SKIP} flag;
Bartas 37:c0fddc75c862 28 flag oldValuesFlag;
Bartas 37:c0fddc75c862 29
Bartas 37:c0fddc75c862 30 int sensorsCheckSum; //variable used for sensors mapping access
Bartas 37:c0fddc75c862 31 int passedTime1,passedTime2;
Bartas 37:c0fddc75c862 32 int oldValues[5] = {0};
Bartas 37:c0fddc75c862 33 int cursor = 0, k = 0;
Bartas 37:c0fddc75c862 34 float normalSpeed = 0.15f;
Reckstyle 9:718987b106a8 35
Reckstyle 35:1819c5a8254a 36 void turnLeft ();
Reckstyle 35:1819c5a8254a 37 void turnRight ();
Reckstyle 35:1819c5a8254a 38 void measureSensors ();
Reckstyle 35:1819c5a8254a 39 void printBluetooth();
Reckstyle 35:1819c5a8254a 40
Bartas 37:c0fddc75c862 41 int main() {
Bartas 37:c0fddc75c862 42
Bartas 37:c0fddc75c862 43 led = 0; //start LED with beginning of main program
Bartas 37:c0fddc75c862 44 measureTimer.start();
Bartas 37:c0fddc75c862 45 sensorTimer.start(); // start timer for sensors
Bartas 37:c0fddc75c862 46 sensor_initialise();
Bartas 37:c0fddc75c862 47 driveMode = REGULAR; // initialise sensor values
Bartas 37:c0fddc75c862 48 oldValuesFlag = USE;
Bartas 37:c0fddc75c862 49 wait(1); //give time to set up the system
Reckstyle 9:718987b106a8 50
Bartas 37:c0fddc75c862 51 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 52 motors.disableHardBraking();
Bartas 37:c0fddc75c862 53 motors.forward();
Bartas 38:02ef89edd828 54 motors.setSpeed (0.1f);
Bartas 38:02ef89edd828 55 motors.start();
Bartas 38:02ef89edd828 56 wait(1);
Bartas 37:c0fddc75c862 57 //motors.enableSlew();
Reckstyle 35:1819c5a8254a 58
Bartas 37:c0fddc75c862 59 while (1) {
Bartas 37:c0fddc75c862 60 measureSensors();
Bartas 37:c0fddc75c862 61 oldValuesFlag = USE;
Bartas 37:c0fddc75c862 62 switch (sensorsCheckSum) {
Bartas 38:02ef89edd828 63 /* case 0: //all black >> turn around by 180 degrees or a possible turn on the left
Bartas 37:c0fddc75c862 64 for (k=0;k<5;k++) { //left turn situation >> stop, go backwards
Bartas 37:c0fddc75c862 65 if (oldValues[k] == 194) {
Bartas 37:c0fddc75c862 66 motors.stop();
Bartas 37:c0fddc75c862 67 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 68 motors.reverse();
Bartas 37:c0fddc75c862 69 motors.setRightSpeed(0.1f);
Bartas 37:c0fddc75c862 70 motors.setLeftSpeed(0.2f);
Bartas 37:c0fddc75c862 71 motors.start();
Bartas 37:c0fddc75c862 72 wait(2);
Bartas 37:c0fddc75c862 73 motors.stop();
Bartas 37:c0fddc75c862 74 } else { //dead end situation >> stop, go a bit backwards so there is space for turning and turn CCW
Bartas 37:c0fddc75c862 75 motors.stop();
Bartas 37:c0fddc75c862 76 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 77 motors.reverse();
Bartas 37:c0fddc75c862 78 motors.setRightSpeed(0.1f);
Bartas 37:c0fddc75c862 79 motors.setLeftSpeed(0.2f);
Bartas 37:c0fddc75c862 80 motors.start();
Bartas 37:c0fddc75c862 81 wait(2);
Bartas 37:c0fddc75c862 82 motors.stop();
Bartas 37:c0fddc75c862 83 motors.setSteeringMode(PIVOT_CCW);
Bartas 37:c0fddc75c862 84 motors.setRightSpeed(0.1f);
Bartas 37:c0fddc75c862 85 motors.setLeftSpeed(0.2f);
Bartas 37:c0fddc75c862 86 do
Bartas 37:c0fddc75c862 87 {
Bartas 37:c0fddc75c862 88 motors.start();
Bartas 37:c0fddc75c862 89 measureSensors();
Bartas 37:c0fddc75c862 90 } while (sensorsCheckSum != 96);
Bartas 37:c0fddc75c862 91 motors.stop();
Bartas 37:c0fddc75c862 92 }
Bartas 37:c0fddc75c862 93 }
Bartas 38:02ef89edd828 94 break; */
Bartas 37:c0fddc75c862 95 case 30: //all right sensors are white, left all black >> turn right
Bartas 37:c0fddc75c862 96 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 97 motors.forward();
Bartas 37:c0fddc75c862 98 motors.setRightSpeed(0.02f);
Bartas 38:02ef89edd828 99 motors.setLeftSpeed(0.04f);
Bartas 37:c0fddc75c862 100 do
Bartas 37:c0fddc75c862 101 {
Bartas 37:c0fddc75c862 102 motors.start();
Bartas 37:c0fddc75c862 103 measureSensors();
Bartas 37:c0fddc75c862 104 } while (sensorsCheckSum == 30);
Bartas 37:c0fddc75c862 105 motors.stop();
Bartas 37:c0fddc75c862 106 break;
Bartas 38:02ef89edd828 107 case 46: //robot is facing north-west >> turn right
Bartas 37:c0fddc75c862 108 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 109 motors.forward();
Bartas 38:02ef89edd828 110 motors.setRightSpeed(0.02f);
Bartas 38:02ef89edd828 111 motors.setLeftSpeed(0.05f);
Bartas 37:c0fddc75c862 112 do
Bartas 37:c0fddc75c862 113 {
Bartas 37:c0fddc75c862 114 motors.start();
Bartas 37:c0fddc75c862 115 measureSensors();
Bartas 37:c0fddc75c862 116 } while (sensorsCheckSum == 46);
Bartas 37:c0fddc75c862 117 motors.stop();
Bartas 37:c0fddc75c862 118 break;
Bartas 37:c0fddc75c862 119 case 94: //normal <<STARTING POSITION>>, half of right and half of left are white
Bartas 37:c0fddc75c862 120 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 121 motors.forward();
Bartas 38:02ef89edd828 122 motors.setRightSpeed(0.08f);
Bartas 38:02ef89edd828 123 motors.setLeftSpeed(0.08f);
Bartas 37:c0fddc75c862 124 do
Bartas 37:c0fddc75c862 125 {
Bartas 37:c0fddc75c862 126 motors.start();
Bartas 37:c0fddc75c862 127 measureSensors();
Bartas 37:c0fddc75c862 128 } while (sensorsCheckSum == 94);
Bartas 37:c0fddc75c862 129 motors.stop();
Bartas 37:c0fddc75c862 130 break;
Bartas 37:c0fddc75c862 131 // case 95: //right turn is present always followed by case 104
Bartas 37:c0fddc75c862 132 case 104: //right all white, left half white >> crossroads, make a right turn
Bartas 37:c0fddc75c862 133 motors.stop();
Bartas 37:c0fddc75c862 134 wait(0.5);
Bartas 37:c0fddc75c862 135 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 136 motors.forward();
Bartas 37:c0fddc75c862 137 motors.setRightSpeed(0.01f);
Bartas 37:c0fddc75c862 138 motors.setLeftSpeed(0.09f);
Bartas 37:c0fddc75c862 139 // motors.start();
Bartas 37:c0fddc75c862 140 // wait(0.5);
Bartas 37:c0fddc75c862 141 // motors.stop();
Bartas 37:c0fddc75c862 142 // motors.setSteeringMode(PIVOT_CCW);
Bartas 37:c0fddc75c862 143 // motors.setRightSpeed(0.07f);
Bartas 37:c0fddc75c862 144 // motors.setLeftSpeed(0.07f);
Bartas 37:c0fddc75c862 145 do
Bartas 37:c0fddc75c862 146 {
Bartas 37:c0fddc75c862 147 motors.start();
Bartas 37:c0fddc75c862 148 measureSensors();
Bartas 37:c0fddc75c862 149 } while (measure(sensorArray[0]) == 0);
Bartas 37:c0fddc75c862 150 motors.stop();
Bartas 37:c0fddc75c862 151 break;
Bartas 38:02ef89edd828 152 case 154: //robot is facing north-east >> turn left
Bartas 37:c0fddc75c862 153 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 154 motors.forward();
Bartas 38:02ef89edd828 155 motors.setRightSpeed(0.05f);
Bartas 38:02ef89edd828 156 motors.setLeftSpeed(0.02f);
Bartas 37:c0fddc75c862 157 do
Bartas 37:c0fddc75c862 158 {
Bartas 37:c0fddc75c862 159 motors.start();
Bartas 37:c0fddc75c862 160 measureSensors();
Bartas 37:c0fddc75c862 161 } while (sensorsCheckSum == 154);
Bartas 37:c0fddc75c862 162 motors.stop();
Bartas 37:c0fddc75c862 163 break;
Bartas 37:c0fddc75c862 164 case 174: //left all white, right all black >> turn left (move right wheel)
Bartas 37:c0fddc75c862 165 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 166 motors.forward();
Bartas 38:02ef89edd828 167 motors.setRightSpeed(0.06f);
Bartas 38:02ef89edd828 168 motors.setLeftSpeed(0.02f);
Bartas 37:c0fddc75c862 169 do
Bartas 37:c0fddc75c862 170 {
Bartas 37:c0fddc75c862 171 motors.start();
Bartas 37:c0fddc75c862 172 measureSensors();
Bartas 37:c0fddc75c862 173 } while (sensorsCheckSum == 174);
Bartas 37:c0fddc75c862 174 motors.stop();
Bartas 37:c0fddc75c862 175 break;
Bartas 37:c0fddc75c862 176 // case 179: //right turn, followed by case 204
Bartas 37:c0fddc75c862 177 case 194: //left all white, right half white >> go straight, turn right if 194 goes to 204
Bartas 37:c0fddc75c862 178 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 179 motors.forward();
Bartas 38:02ef89edd828 180 motors.setRightSpeed(0.01f);
Bartas 38:02ef89edd828 181 motors.setLeftSpeed(0.02f);
Bartas 37:c0fddc75c862 182 do
Bartas 37:c0fddc75c862 183 {
Bartas 37:c0fddc75c862 184 motors.start();
Bartas 37:c0fddc75c862 185 measureSensors();
Bartas 37:c0fddc75c862 186 } while (sensorsCheckSum == 194);
Bartas 37:c0fddc75c862 187 motors.stop(); //do while left is white
Bartas 37:c0fddc75c862 188 break;
Bartas 38:02ef89edd828 189 /* case 204: //all sensors are white
Bartas 37:c0fddc75c862 190 for (k=0;k<6;k++) { //situation when a square is entered, need to follow right line
Bartas 37:c0fddc75c862 191 if (oldValues[k] == 194) { //checks whether black line on the right was present before
Bartas 37:c0fddc75c862 192 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 193 motors.setRightSpeed(0.01f);
Bartas 37:c0fddc75c862 194 motors.setLeftSpeed(0.05f);
Bartas 37:c0fddc75c862 195 do
Bartas 37:c0fddc75c862 196 {
Bartas 37:c0fddc75c862 197 motors.start();
Bartas 37:c0fddc75c862 198 measureSensors();
Bartas 37:c0fddc75c862 199 } while (sensorsCheckSum == 204);
Bartas 37:c0fddc75c862 200 motors.stop();
Bartas 37:c0fddc75c862 201 }
Bartas 37:c0fddc75c862 202 if ((oldValues[k] == 30) or (oldValues[k] == 104)) { //right turn 90 situation
Bartas 37:c0fddc75c862 203 motors.stop();
Bartas 37:c0fddc75c862 204 wait(0.5);
Bartas 37:c0fddc75c862 205 motors.setSteeringMode(NORMAL);
Bartas 37:c0fddc75c862 206 motors.forward();
Bartas 37:c0fddc75c862 207 motors.setRightSpeed(0.01f);
Bartas 37:c0fddc75c862 208 motors.setLeftSpeed(0.09f);
Bartas 37:c0fddc75c862 209 // motors.start();
Bartas 37:c0fddc75c862 210 // wait(0.5);
Bartas 37:c0fddc75c862 211 // motors.stop();
Bartas 37:c0fddc75c862 212 // motors.setSteeringMode(PIVOT_CCW);
Bartas 37:c0fddc75c862 213 // motors.setRightSpeed(0.07f);
Bartas 37:c0fddc75c862 214 // motors.setLeftSpeed(0.07f);
Bartas 37:c0fddc75c862 215 do
Bartas 37:c0fddc75c862 216 {
Bartas 37:c0fddc75c862 217 motors.start();
Bartas 37:c0fddc75c862 218 measureSensors();
Bartas 37:c0fddc75c862 219 } while (measure(sensorArray[0]) == 0);
Bartas 37:c0fddc75c862 220 motors.stop();
Bartas 37:c0fddc75c862 221 break;
Bartas 37:c0fddc75c862 222 }
Bartas 37:c0fddc75c862 223 }
Bartas 38:02ef89edd828 224 break; */
Bartas 38:02ef89edd828 225 default:
Bartas 38:02ef89edd828 226 /* if (sensorsCheckSum < 96) {
Bartas 38:02ef89edd828 227 motors.setSpeed(0.0);
Bartas 38:02ef89edd828 228 motors.setLeftSpeed(0.02);
Bartas 38:02ef89edd828 229 }
Bartas 38:02ef89edd828 230 else if (sensorsCheckSum > 96) {
Bartas 38:02ef89edd828 231 motors.setSpeed (0.0);
Bartas 38:02ef89edd828 232 motors.setRightSpeed(0.02);
Bartas 38:02ef89edd828 233 }
Bartas 38:02ef89edd828 234 } */
Bartas 38:02ef89edd828 235 //motors.start();
Bartas 37:c0fddc75c862 236 oldValuesFlag = SKIP;
Bartas 37:c0fddc75c862 237 break;
Bartas 37:c0fddc75c862 238
Bartas 38:02ef89edd828 239 } // switch statement
Bartas 38:02ef89edd828 240 } //while loop
Bartas 38:02ef89edd828 241 } // main
Bartas 37:c0fddc75c862 242 /*
Reckstyle 35:1819c5a8254a 243 motors.setSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 244 while(1){
Reckstyle 35:1819c5a8254a 245 measureSensors();
Reckstyle 35:1819c5a8254a 246 // if (HC06.getc() == 'r') {
Reckstyle 35:1819c5a8254a 247 // measureSensors();
Reckstyle 35:1819c5a8254a 248 // printBluetooth();
Reckstyle 35:1819c5a8254a 249 // }
Reckstyle 35:1819c5a8254a 250
Reckstyle 35:1819c5a8254a 251 } //int main
Reckstyle 35:1819c5a8254a 252
Reckstyle 35:1819c5a8254a 253 void turnRight () {
Reckstyle 35:1819c5a8254a 254 motors.setSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 255 wait(0.7);
Reckstyle 35:1819c5a8254a 256 motors.setSpeed(0.0);
Reckstyle 35:1819c5a8254a 257 motors.setLeftSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 258 wait (1.4);
Reckstyle 35:1819c5a8254a 259 int exit = 0; //used for exiting the function
Reckstyle 35:1819c5a8254a 260 motors.setSpeed(0.0);
Reckstyle 35:1819c5a8254a 261 wait(0.2);
Reckstyle 35:1819c5a8254a 262 motors.setSpeed (normalSpeed);
Reckstyle 35:1819c5a8254a 263 for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
Reckstyle 35:1819c5a8254a 264 if (measure(sensorArray[i]) == 0) {//if sensor is black
Reckstyle 35:1819c5a8254a 265 exit ++;
Reckstyle 35:1819c5a8254a 266 }
Reckstyle 35:1819c5a8254a 267 if (exit > 1){
Reckstyle 35:1819c5a8254a 268
Reckstyle 35:1819c5a8254a 269 return;
Reckstyle 35:1819c5a8254a 270 }
Reckstyle 35:1819c5a8254a 271 exit = 0;
Reckstyle 35:1819c5a8254a 272 }
Bartas 37:c0fddc75c862 273 } */
Reckstyle 35:1819c5a8254a 274
Bartas 37:c0fddc75c862 275 void printBluetooth() { //for debugging
Bartas 37:c0fddc75c862 276 HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Bartas 37:c0fddc75c862 277 HC06.printf("LLD%i LRD%i rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
Bartas 37:c0fddc75c862 278 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Bartas 37:c0fddc75c862 279 HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Bartas 37:c0fddc75c862 280 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 35:1819c5a8254a 281 }
Reckstyle 35:1819c5a8254a 282
Bartas 37:c0fddc75c862 283
Reckstyle 11:9e56d52485d1 284 void measureSensors () {
Bartas 24:ecc3fbaf2824 285 sensorsCheckSum = 0; //zero it when first going into the routine
Bartas 24:ecc3fbaf2824 286 int iterationNumber = NUMBER_SENSORS_REGULAR;
Bartas 24:ecc3fbaf2824 287 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 288 iterationNumber += NUMBER_SENSORS_SQUARE;
Bartas 24:ecc3fbaf2824 289 }
Bartas 24:ecc3fbaf2824 290 for (int i = 0; i < iterationNumber;i++){
Bartas 24:ecc3fbaf2824 291 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 292 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 293 }
Bartas 24:ecc3fbaf2824 294 }
Bartas 37:c0fddc75c862 295 /* if ((oldValues[cursor] != sensorsCheckSum) and (oldValuesFlag == USE)) {
Reckstyle 25:bec5dc4c9f5e 296 oldValues[cursor] = sensorsCheckSum;
Reckstyle 23:9b53c72fcd38 297 cursor = (cursor+1)%5;
Bartas 37:c0fddc75c862 298 }*/
Bartas 37:c0fddc75c862 299 if ((oldValues[cursor] != sensorsCheckSum) and (oldValuesFlag == USE)) { // priority queu, ideas to make it faster/shorter?
Bartas 37:c0fddc75c862 300 int i = 0;
Bartas 37:c0fddc75c862 301 while ((oldValues[cursor] != sensorsCheckSum)) {
Bartas 37:c0fddc75c862 302 for (k = 0; k < 5; k++) {
Bartas 37:c0fddc75c862 303 if (oldValues[k] == sensorsCheckSum) {
Bartas 37:c0fddc75c862 304 for (i = k; i > 0; i--) {
Bartas 37:c0fddc75c862 305 oldValues[i] = oldValues[i-1];
Bartas 38:02ef89edd828 306 }
Bartas 37:c0fddc75c862 307 } else {
Bartas 37:c0fddc75c862 308 oldValues[k] = oldValues[k-1];
Bartas 37:c0fddc75c862 309 }
Bartas 37:c0fddc75c862 310 oldValues[0] = sensorsCheckSum;
Bartas 37:c0fddc75c862 311 }
Bartas 37:c0fddc75c862 312 }
Bartas 24:ecc3fbaf2824 313 }
Bartas 38:02ef89edd828 314 }