Bartas Abaravicius / Mbed 2 deprecated TDP_main

Lol smth

Dependencies:   LocalPositionSystem MMA8451Q Motor_Driver Sensors mbed

Fork of TDP_main_BartFork by Yelfie

main.cpp@25:8be440e10126, 2015-03-22 (annotated)

Committer:
Bartas
Date:
Sun Mar 22 23:55:20 2015 +0000
Revision:
25:8be440e10126
Parent:
24:c1b1b0ea0cb9
Child:
26:cbbfe012a757
All the cases and turns theoretical code. Some could be missing and needs experimentation. Got rid of "goto" call.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joseph_Penikis 3:0274e082582e 1 // TESTING REPO COMMIT
Joseph_Penikis 3:0274e082582e 2
Reckstyle 2:e2adb7ab2947 3 /*
Reckstyle 2:e2adb7ab2947 4 ****** MAIN PROGRAM ******
Reckstyle 2:e2adb7ab2947 5
Reckstyle 11:9e56d52485d1 6
Reckstyle 2:e2adb7ab2947 7
Reckstyle 2:e2adb7ab2947 8 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 9
Reckstyle 11:9e56d52485d1 10
Reckstyle 11:9e56d52485d1 11 Sensors are mapped on the global variable sensorsCheckSum,
Reckstyle 11:9e56d52485d1 12 which multiplies the sensor number by itself to then decode,
Reckstyle 11:9e56d52485d1 13 which sensors are off and which are on
Reckstyle 11:9e56d52485d1 14 ie. if sensor rightright - sensorChecksum = 1*1 = 1
Reckstyle 11:9e56d52485d1 15 if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
Reckstyle 11:9e56d52485d1 16 ...
Reckstyle 2:e2adb7ab2947 17 */
Reckstyle 2:e2adb7ab2947 18
Reckstyle 2:e2adb7ab2947 19
Reckstyle 2:e2adb7ab2947 20
Reckstyle 0:5ca0450111f3 21 #include "mbed.h"
Reckstyle 11:9e56d52485d1 22 #include "sensor_measure.h"
Joseph_Penikis 5:2fc7bf0135d4 23 #include "Motor_Driver.h"
orsharp 12:bb21b76b6375 24 #include "shooter.h"
Reckstyle 9:718987b106a8 25 //#include "Sensors.h"
Reckstyle 0:5ca0450111f3 26
Joseph_Penikis 5:2fc7bf0135d4 27 #define PWM_PERIOD_US 10000
Reckstyle 0:5ca0450111f3 28
Joseph_Penikis 7:14af656b721f 29
orsharp 12:bb21b76b6375 30 //DigitalOut led(LED1);
orsharp 12:bb21b76b6375 31
Reckstyle 16:3649eb1a056d 32 Serial HC06(PTE0,PTE1); //TX,RX
Joseph_Penikis 10:c9212bbfeae6 33 //Serial pc(USBTX, USBRX);
Reckstyle 9:718987b106a8 34
Reckstyle 19:d277614084bc 35 Timer measureTimer; //Timer used for measurement
Joseph_Penikis 10:c9212bbfeae6 36
orsharp 12:bb21b76b6375 37 //Motor_Driver motors(PTD4, PTA12, PTA4, PTA5, PTC9, PTC8, PWM_PERIOD_US);
Reckstyle 9:718987b106a8 38
Reckstyle 11:9e56d52485d1 39 typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
Reckstyle 11:9e56d52485d1 40 mode driveMode; //declaring the variable for the states
Reckstyle 11:9e56d52485d1 41 int sensorsCheckSum; //varibale used for sensors mapping access
Reckstyle 19:d277614084bc 42 int passedTime1,passedTime2;
Bartas 25:8be440e10126 43 int oldValues[5] = {0};
Reckstyle 22:9cf274ffe1de 44 int k = 0;
Reckstyle 11:9e56d52485d1 45
Reckstyle 11:9e56d52485d1 46
Reckstyle 11:9e56d52485d1 47 void measureSensors () {
Reckstyle 11:9e56d52485d1 48 sensorsCheckSum = 0; //zero it when first going into the routine
Reckstyle 11:9e56d52485d1 49 int iterationNumber = NUMBER_SENSORS_REGULAR;
Reckstyle 11:9e56d52485d1 50 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 51 iterationNumber += NUMBER_SENSORS_SQUARE;
Reckstyle 11:9e56d52485d1 52 }
Reckstyle 11:9e56d52485d1 53 for (int i = 0; i < iterationNumber;i++){
Reckstyle 17:de8b3111ddc5 54 //pc.printf("%i iteration%i ",i,iterationNumber);
Reckstyle 16:3649eb1a056d 55 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 56 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 57 }
Reckstyle 22:9cf274ffe1de 58 }
Bartas 25:8be440e10126 59 if (oldValues[0] != sensorsCheckSum) {
Reckstyle 22:9cf274ffe1de 60 for (k = 5; k > 0; k--) {
Bartas 25:8be440e10126 61 oldValues[k] = oldValues[k-1];
Reckstyle 22:9cf274ffe1de 62 }
Bartas 25:8be440e10126 63 oldValues[0] = sensorsCheckSum;
Reckstyle 22:9cf274ffe1de 64 }
Reckstyle 11:9e56d52485d1 65 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 66 }
Reckstyle 11:9e56d52485d1 67
Reckstyle 15:6453cd351452 68 void printBluetooth() { //for debugging
Reckstyle 21:e8da3b351cd0 69 pc.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 21:e8da3b351cd0 70 pc.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 71 //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 72 //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 73 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Reckstyle 19:d277614084bc 74 pc.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Reckstyle 19:d277614084bc 75 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 15:6453cd351452 76 }
Reckstyle 15:6453cd351452 77
Reckstyle 11:9e56d52485d1 78 int main() {
Reckstyle 22:9cf274ffe1de 79
Reckstyle 19:d277614084bc 80 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US);
Reckstyle 11:9e56d52485d1 81
Reckstyle 17:de8b3111ddc5 82 // motors.setSpeed(0.1f);
Reckstyle 17:de8b3111ddc5 83 // motors.forward();
Reckstyle 17:de8b3111ddc5 84 // motors.start();
Reckstyle 17:de8b3111ddc5 85 // wait(2);
Reckstyle 17:de8b3111ddc5 86 // float x=0.1f;
Reckstyle 17:de8b3111ddc5 87 // while (1) {
Reckstyle 17:de8b3111ddc5 88 // motors.setLeftSpeed(x);
Reckstyle 17:de8b3111ddc5 89 // x = x+0.05;
Reckstyle 17:de8b3111ddc5 90 // wait(3);
Reckstyle 17:de8b3111ddc5 91 // }
Reckstyle 17:de8b3111ddc5 92 // motors.setLeftSpeed(0.1f);
Reckstyle 17:de8b3111ddc5 93 // wait(5);
Reckstyle 17:de8b3111ddc5 94 // motors.setLeftSpeed(0.2f);
Reckstyle 17:de8b3111ddc5 95 // motors.setRightSpeed (0.2f);
Reckstyle 17:de8b3111ddc5 96 // wait(3);
Reckstyle 17:de8b3111ddc5 97 // motors.setRightSpeed (0.1f);
Reckstyle 17:de8b3111ddc5 98 // wait(5);
Reckstyle 17:de8b3111ddc5 99 // motors.stop();
Reckstyle 11:9e56d52485d1 100
Reckstyle 17:de8b3111ddc5 101 //wait(1);
Reckstyle 17:de8b3111ddc5 102 // motors.reverse();
Reckstyle 17:de8b3111ddc5 103 // wait(5);
Reckstyle 17:de8b3111ddc5 104 // motors.stop();
Reckstyle 17:de8b3111ddc5 105 // motors.setSpeed(0.5f);
Reckstyle 17:de8b3111ddc5 106 // motors.start();
Reckstyle 17:de8b3111ddc5 107 // wait(5);
Reckstyle 17:de8b3111ddc5 108 // motors.stop();
Reckstyle 17:de8b3111ddc5 109 // wait(1);
Reckstyle 17:de8b3111ddc5 110 // motors.reverse();
Reckstyle 11:9e56d52485d1 111
Reckstyle 17:de8b3111ddc5 112
Reckstyle 22:9cf274ffe1de 113 //motors.start();
Reckstyle 11:9e56d52485d1 114
Reckstyle 23:902c3086394e 115 // setup_counter(1000, 0);
Reckstyle 23:902c3086394e 116 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 23:902c3086394e 117 measureTimer.start();
Reckstyle 23:902c3086394e 118 driveMode = REGULAR; //initialise drivemoder
Reckstyle 23:902c3086394e 119 sensor_initialise(); // initialise sensor values
Reckstyle 23:902c3086394e 120 wait(1); //give time to set up the system
Reckstyle 23:902c3086394e 121
Reckstyle 23:902c3086394e 122 sensorTimer.start(); // start timer for sensors
Bartas 25:8be440e10126 123 // float normalSpeed = 0.01f;
Reckstyle 22:9cf274ffe1de 124 // HC06.baud(9600);
Reckstyle 22:9cf274ffe1de 125 // HC06.printf("working..");
Reckstyle 22:9cf274ffe1de 126 // motors.setSpeed(normalSpeed);
Reckstyle 22:9cf274ffe1de 127 // motors.forward();
Reckstyle 22:9cf274ffe1de 128 // motors.start();
Reckstyle 22:9cf274ffe1de 129 //
Reckstyle 22:9cf274ffe1de 130 //
Reckstyle 22:9cf274ffe1de 131 //
Reckstyle 22:9cf274ffe1de 132 // wait(3);
Bartas 25:8be440e10126 133 // while(1){
Bartas 25:8be440e10126 134 // if (pc.getc() == 'r') {
Bartas 25:8be440e10126 135 // measureSensors();
Reckstyle 19:d277614084bc 136 //measureTimer.reset();
Bartas 25:8be440e10126 137 // printBluetooth();
Reckstyle 19:d277614084bc 138 //passedTime1 = measureTimer.read_ms();
Reckstyle 23:902c3086394e 139 //if (sensorsCheckSum == 0) {
Reckstyle 23:902c3086394e 140 // motors.setSpeed(normalSpeed);
Reckstyle 23:902c3086394e 141 // }
Reckstyle 23:902c3086394e 142 // else if (sensorsCheckSum == 1 || sensorsCheckSum == 9 || sensorsCheckSum == 10 || sensorsCheckSum == 14 || sensorsCheckSum==26){
Reckstyle 23:902c3086394e 143 // motors.setLeftSpeed(normalSpeed/2);
Reckstyle 23:902c3086394e 144 //
Reckstyle 23:902c3086394e 145 // motors.setRightSpeed(normalSpeed*6);
Reckstyle 23:902c3086394e 146 // }
Reckstyle 23:902c3086394e 147 // else if (sensorsCheckSum == 4 && sensorsCheckSum ==16 || sensorsCheckSum == 20 || sensorsCheckSum ==21 ||sensorsCheckSum== 29) {
Reckstyle 23:902c3086394e 148 // motors.setRightSpeed(normalSpeed/2);
Reckstyle 23:902c3086394e 149 // motors.setLeftSpeed(normalSpeed*9);
Reckstyle 23:902c3086394e 150 // }
Reckstyle 23:902c3086394e 151 // else {
Reckstyle 23:902c3086394e 152 // motors.setSpeed(normalSpeed);
Reckstyle 23:902c3086394e 153 // }
Bartas 25:8be440e10126 154 // }
Bartas 25:8be440e10126 155 // }
Reckstyle 21:e8da3b351cd0 156 //HC06.printf("AT");
Reckstyle 21:e8da3b351cd0 157 //HC06.printf("AT+PIN5555");
Reckstyle 15:6453cd351452 158
Reckstyle 15:6453cd351452 159
Reckstyle 14:3844d1dacece 160 // pc.printf("Start...");
Bartas 24:c1b1b0ea0cb9 161
Bartas 24:c1b1b0ea0cb9 162 // int testOtherCases = 0; //needed for control statements
Bartas 24:c1b1b0ea0cb9 163
Bartas 24:c1b1b0ea0cb9 164
Bartas 24:c1b1b0ea0cb9 165
Reckstyle 20:198dc13777eb 166 while (1) {
Reckstyle 20:198dc13777eb 167
Reckstyle 20:198dc13777eb 168 if (driveMode == REGULAR) {
Reckstyle 20:198dc13777eb 169 measureSensors();
Bartas 24:c1b1b0ea0cb9 170 switch (sensorsCheckSum) {
Reckstyle 21:e8da3b351cd0 171 case 0: // all black, turn around by 180 degrees
Bartas 25:8be440e10126 172 for (k=0;k<6;k++) { //right turn situation
Bartas 25:8be440e10126 173 if (oldValues[k] == 194) {
Bartas 25:8be440e10126 174 motors.stop();
Bartas 25:8be440e10126 175 motors.setSteeringMode(NORMAL);
Bartas 25:8be440e10126 176 motors.reverse();
Bartas 25:8be440e10126 177 motors.setSpeed(0.1f);
Bartas 25:8be440e10126 178 motors.start();
Bartas 25:8be440e10126 179 wait(1);
Bartas 25:8be440e10126 180 } else {
Bartas 25:8be440e10126 181 motors.stop();
Bartas 25:8be440e10126 182 motors.setSteeringMode(NORMAL);
Bartas 25:8be440e10126 183 motors.reverse();
Bartas 25:8be440e10126 184 motors.setSpeed(0.1f);
Bartas 25:8be440e10126 185 motors.start();
Bartas 25:8be440e10126 186 wait(2);
Bartas 25:8be440e10126 187 motors.stop();
Bartas 25:8be440e10126 188 motors.setSteeringMode(PIVOT_CCW);
Bartas 25:8be440e10126 189 motors.setSpeed(0.1f);
Bartas 25:8be440e10126 190 do
Bartas 25:8be440e10126 191 {
Bartas 25:8be440e10126 192 motors.start();
Bartas 25:8be440e10126 193 measureSensors();
Bartas 25:8be440e10126 194 } while (sensorsCheckSum != 96);
Bartas 25:8be440e10126 195 motors.stop();
Bartas 25:8be440e10126 196 motors.setSteeringMode(NORMAL);
Bartas 25:8be440e10126 197 }
Reckstyle 21:e8da3b351cd0 198 break;
Reckstyle 21:e8da3b351cd0 199 case 30: //all right are white, left all black >> turn right(move left wheel)
Bartas 24:c1b1b0ea0cb9 200 motors.setRightSpeed(0.15f);
Bartas 24:c1b1b0ea0cb9 201 motors.setLeftSpeed(0.5f);
Reckstyle 21:e8da3b351cd0 202 break;
Bartas 25:8be440e10126 203 // case 46: //left 5 white, right only 3 black >> turn right
Bartas 25:8be440e10126 204 // motors.setRightSpeed(0.15f);
Bartas 25:8be440e10126 205 // motors.setLeftSpeed(0.1f);
Bartas 25:8be440e10126 206 // break;
Bartas 24:c1b1b0ea0cb9 207 case 94: //normal starting position, half of right and half of left are white, (move right wheel)
Bartas 25:8be440e10126 208 motors.setSteeringMode(NORMAL);
Bartas 24:c1b1b0ea0cb9 209 motors.setSpeed(0.1f);
Bartas 25:8be440e10126 210 motors.forward();
Bartas 25:8be440e10126 211 motors.start();
Reckstyle 21:e8da3b351cd0 212 break;
Bartas 25:8be440e10126 213 case 104: //right all white, left half white >> turn right
Bartas 25:8be440e10126 214 motors.setRightSpeed(0.1f);
Bartas 25:8be440e10126 215 motors.setLeftSpeed(0.15f);
Reckstyle 21:e8da3b351cd0 216 break;
Bartas 25:8be440e10126 217 // case 154: //right 4 white, left only 6 black >> turn left
Bartas 25:8be440e10126 218 // break;
Reckstyle 21:e8da3b351cd0 219 case 174: //left all white, right all black >> turn left (move right wheel)
Bartas 25:8be440e10126 220 motors.setRightSpeed(0.05f);
Bartas 25:8be440e10126 221 motors.setLeftSpeed(0.15f);
Reckstyle 21:e8da3b351cd0 222 break;
Bartas 25:8be440e10126 223 case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
Reckstyle 21:e8da3b351cd0 224 break;
Reckstyle 21:e8da3b351cd0 225 case 204 : //all sensors are white
Bartas 25:8be440e10126 226
Bartas 25:8be440e10126 227 for (k=0;k<6;k++) { //situation when a square is entered, need to follow right line
Bartas 25:8be440e10126 228 if (oldValues[k] == 194) { //checks whether black line on the right was present before
Bartas 25:8be440e10126 229 motors.setRightSpeed(0.15f);
Bartas 25:8be440e10126 230 motors.setLeftSpeed(0.05f);
Bartas 25:8be440e10126 231 }
Bartas 25:8be440e10126 232
Bartas 25:8be440e10126 233 if (oldValues[k] == 104) { //right turn 90 situation
Bartas 25:8be440e10126 234 motors.stop();
Bartas 25:8be440e10126 235 motors.setSteeringMode(PIVOT_CW);
Bartas 25:8be440e10126 236 motors.setRightSpeed(0.1f);
Bartas 25:8be440e10126 237 motors.setLeftSpeed(0.1f);
Bartas 25:8be440e10126 238 do
Bartas 25:8be440e10126 239 {
Bartas 25:8be440e10126 240 motors.start();
Bartas 25:8be440e10126 241 measureSensors();
Bartas 25:8be440e10126 242 } while (sensorsCheckSum != 94);
Bartas 25:8be440e10126 243 motors.stop();
Bartas 25:8be440e10126 244 motors.setSteeringMode(NORMAL);
Bartas 25:8be440e10126 245 }
Bartas 25:8be440e10126 246 break;
Bartas 25:8be440e10126 247
Bartas 24:c1b1b0ea0cb9 248 default: //checksum is zero , all are black
Bartas 24:c1b1b0ea0cb9 249 measureSensors();
Reckstyle 20:198dc13777eb 250 break;
Bartas 24:c1b1b0ea0cb9 251 }
Bartas 25:8be440e10126 252 }
Bartas 25:8be440e10126 253 }
Bartas 25:8be440e10126 254 }
Bartas 25:8be440e10126 255 }
Bartas 25:8be440e10126 256 }
Bartas 24:c1b1b0ea0cb9 257
Bartas 24:c1b1b0ea0cb9 258 // if (testOtherCases == 1) {
Bartas 24:c1b1b0ea0cb9 259 // if (sensorsCheckSum < 96){ // adjust right
Bartas 24:c1b1b0ea0cb9 260 // }
Bartas 24:c1b1b0ea0cb9 261 // else {//adjust left
Bartas 24:c1b1b0ea0cb9 262 // }
Bartas 24:c1b1b0ea0cb9 263 // testOtherCases = 0;
Bartas 25:8be440e10126 264 // }
Reckstyle 14:3844d1dacece 265 //
Reckstyle 14:3844d1dacece 266 //
Reckstyle 14:3844d1dacece 267 //
Reckstyle 14:3844d1dacece 268 // }
Reckstyle 14:3844d1dacece 269 // else { //if (driveMode == SQUARE}
Reckstyle 14:3844d1dacece 270 // //implement the square searching thing..
Reckstyle 14:3844d1dacece 271 //
Reckstyle 14:3844d1dacece 272 // }
Reckstyle 14:3844d1dacece 273 //
Reckstyle 14:3844d1dacece 274 //
Reckstyle 14:3844d1dacece 275 // }
Reckstyle 14:3844d1dacece 276 //