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@24:c1b1b0ea0cb9, 2015-03-22 (annotated)

Committer:
Bartas
Date:
Sun Mar 22 14:57:21 2015 +0000
Revision:
24:c1b1b0ea0cb9
Parent:
23:902c3086394e
Child:
25:8be440e10126
Added some of the cases an goto for turning around

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;
Reckstyle 22:9cf274ffe1de 43 int values_old[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 }
Reckstyle 22:9cf274ffe1de 59 if (values_old[0] != sensorsCheckSum) {
Reckstyle 22:9cf274ffe1de 60 for (k = 5; k > 0; k--) {
Reckstyle 22:9cf274ffe1de 61 values_old[k] = values_old[k-1];
Reckstyle 22:9cf274ffe1de 62 }
Reckstyle 22:9cf274ffe1de 63 values_old[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
Reckstyle 19:d277614084bc 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);
Reckstyle 16:3649eb1a056d 133 while(1){
Reckstyle 23:902c3086394e 134 if (pc.getc() == 'r') {
Reckstyle 16:3649eb1a056d 135 measureSensors();
Reckstyle 19:d277614084bc 136 //measureTimer.reset();
Reckstyle 16:3649eb1a056d 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 // }
Reckstyle 16:3649eb1a056d 154 }
Reckstyle 17:de8b3111ddc5 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 24:c1b1b0ea0cb9 172 goto turnAround;
Reckstyle 21:e8da3b351cd0 173 break;
Reckstyle 21:e8da3b351cd0 174 case 30: //all right are white, left all black >> turn right(move left wheel)
Bartas 24:c1b1b0ea0cb9 175 motors.setRightSpeed(0.15f);
Bartas 24:c1b1b0ea0cb9 176 motors.setLeftSpeed(0.5f);
Reckstyle 21:e8da3b351cd0 177 break;
Reckstyle 21:e8da3b351cd0 178 case 46: //left 5 white, right only 3 black >> turn right
Bartas 24:c1b1b0ea0cb9 179 motors.setRightSpeed(0.15f);
Bartas 24:c1b1b0ea0cb9 180 motors.setLeftSpeed(0.5f);
Reckstyle 21:e8da3b351cd0 181 break;
Bartas 24:c1b1b0ea0cb9 182 case 94: //normal starting position, half of right and half of left are white, (move right wheel)
Bartas 24:c1b1b0ea0cb9 183 motors.setSpeed(0.1f);
Bartas 24:c1b1b0ea0cb9 184 motors.forward;
Bartas 24:c1b1b0ea0cb9 185 motors.start;
Reckstyle 21:e8da3b351cd0 186 break;
Reckstyle 21:e8da3b351cd0 187 case 104: //right all white, left half white >> turn right 90 degrees
Bartas 24:c1b1b0ea0cb9 188
Reckstyle 21:e8da3b351cd0 189 break;
Reckstyle 21:e8da3b351cd0 190 case 154: //right 4 white, left only 6 black >> turn left
Bartas 24:c1b1b0ea0cb9 191
Reckstyle 21:e8da3b351cd0 192 break;
Reckstyle 21:e8da3b351cd0 193 case 174: //left all white, right all black >> turn left (move right wheel)
Bartas 24:c1b1b0ea0cb9 194
Bartas 24:c1b1b0ea0cb9 195
Reckstyle 21:e8da3b351cd0 196 break;
Reckstyle 21:e8da3b351cd0 197 case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
Bartas 24:c1b1b0ea0cb9 198
Reckstyle 21:e8da3b351cd0 199 break;
Reckstyle 21:e8da3b351cd0 200 case 204 : //all sensors are white
Bartas 24:c1b1b0ea0cb9 201 default: //checksum is zero , all are black
Bartas 24:c1b1b0ea0cb9 202 measureSensors();
Reckstyle 20:198dc13777eb 203 break;
Bartas 24:c1b1b0ea0cb9 204 }
Bartas 24:c1b1b0ea0cb9 205
Bartas 24:c1b1b0ea0cb9 206 // if (testOtherCases == 1) {
Bartas 24:c1b1b0ea0cb9 207 // if (sensorsCheckSum < 96){ // adjust right
Bartas 24:c1b1b0ea0cb9 208 // }
Bartas 24:c1b1b0ea0cb9 209 // else {//adjust left
Bartas 24:c1b1b0ea0cb9 210 // }
Bartas 24:c1b1b0ea0cb9 211 // testOtherCases = 0;
Bartas 24:c1b1b0ea0cb9 212 // }
Bartas 24:c1b1b0ea0cb9 213
Reckstyle 14:3844d1dacece 214 //
Reckstyle 14:3844d1dacece 215 //
Reckstyle 14:3844d1dacece 216 //
Reckstyle 14:3844d1dacece 217 // }
Reckstyle 14:3844d1dacece 218 // else { //if (driveMode == SQUARE}
Reckstyle 14:3844d1dacece 219 // //implement the square searching thing..
Reckstyle 14:3844d1dacece 220 //
Reckstyle 14:3844d1dacece 221 // }
Reckstyle 14:3844d1dacece 222 //
Reckstyle 14:3844d1dacece 223 //
Reckstyle 14:3844d1dacece 224 // }
Reckstyle 14:3844d1dacece 225 //
Reckstyle 11:9e56d52485d1 226
Reckstyle 11:9e56d52485d1 227 }
Reckstyle 11:9e56d52485d1 228
Bartas 24:c1b1b0ea0cb9 229 turnAround:
Bartas 24:c1b1b0ea0cb9 230 motors.reverse;
Bartas 24:c1b1b0ea0cb9 231 motors.setSpeed(0.1f);
Bartas 24:c1b1b0ea0cb9 232 motors.start;
Bartas 24:c1b1b0ea0cb9 233 wait(2);
Bartas 24:c1b1b0ea0cb9 234 motors.stop;
Bartas 24:c1b1b0ea0cb9 235 motors.setSteeringMode(PIVOT_CCW);
Bartas 24:c1b1b0ea0cb9 236 motors.setSpeed(0.1f);
Bartas 24:c1b1b0ea0cb9 237 do
Bartas 24:c1b1b0ea0cb9 238 {
Bartas 24:c1b1b0ea0cb9 239 motors.start;
Bartas 24:c1b1b0ea0cb9 240 } while (sensorsCheckSum != 96);
Bartas 24:c1b1b0ea0cb9 241 return 0;
Bartas 24:c1b1b0ea0cb9 242 }
Bartas 24:c1b1b0ea0cb9 243
Reckstyle 11:9e56d52485d1 244
Reckstyle 11:9e56d52485d1 245
Reckstyle 11:9e56d52485d1 246
Reckstyle 11:9e56d52485d1 247
Reckstyle 9:718987b106a8 248
Bartas 24:c1b1b0ea0cb9 249