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@26:cbbfe012a757, 2015-03-23 (annotated)

Committer:
Bartas
Date:
Mon Mar 23 12:32:32 2015 +0000
Revision:
26:cbbfe012a757
Parent:
25:8be440e10126 
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 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 16:3649eb1a056d 21 Serial HC06(PTE0,PTE1); //TX,RX
Reckstyle 9:718987b106a8 22
Reckstyle 19:d277614084bc 23 Timer measureTimer; //Timer used for measurement
Joseph_Penikis 10:c9212bbfeae6 24
orsharp 12:bb21b76b6375 25 //Motor_Driver motors(PTD4, PTA12, PTA4, PTA5, PTC9, PTC8, PWM_PERIOD_US);
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 19:d277614084bc 30 int passedTime1,passedTime2;
Bartas 25:8be440e10126 31 int oldValues[5] = {0};
Reckstyle 22:9cf274ffe1de 32 int k = 0;
Reckstyle 11:9e56d52485d1 33
Reckstyle 11:9e56d52485d1 34
Reckstyle 11:9e56d52485d1 35 void measureSensors () {
Bartas 26:cbbfe012a757 36 sensorsCheckSum = 0; //zero it when first going into the routine
Bartas 26:cbbfe012a757 37 int iterationNumber = NUMBER_SENSORS_REGULAR;
Bartas 26:cbbfe012a757 38 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 39 iterationNumber += NUMBER_SENSORS_SQUARE;
Bartas 26:cbbfe012a757 40 }
Bartas 26:cbbfe012a757 41 for (int i = 0; i < iterationNumber;i++){
Reckstyle 17:de8b3111ddc5 42 //pc.printf("%i iteration%i ",i,iterationNumber);
Bartas 26:cbbfe012a757 43 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 44 sensorsCheckSum += (i+1)*(i+1);
Bartas 26:cbbfe012a757 45 }
Bartas 26:cbbfe012a757 46 }
Bartas 26:cbbfe012a757 47 if (oldValues[0] != sensorsCheckSum) {
Reckstyle 22:9cf274ffe1de 48 for (k = 5; k > 0; k--) {
Bartas 25:8be440e10126 49 oldValues[k] = oldValues[k-1];
Reckstyle 22:9cf274ffe1de 50 }
Bartas 25:8be440e10126 51 oldValues[0] = sensorsCheckSum;
Bartas 26:cbbfe012a757 52 }
Reckstyle 11:9e56d52485d1 53 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 54 }
Reckstyle 11:9e56d52485d1 55
Reckstyle 15:6453cd351452 56 void printBluetooth() { //for debugging
Bartas 26:cbbfe012a757 57 HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Bartas 26:cbbfe012a757 58 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 59 //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 60 //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 61 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Bartas 26:cbbfe012a757 62 HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Reckstyle 19:d277614084bc 63 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 15:6453cd351452 64 }
Reckstyle 15:6453cd351452 65
Reckstyle 11:9e56d52485d1 66 int main() {
Reckstyle 22:9cf274ffe1de 67
Reckstyle 19:d277614084bc 68 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US);
Reckstyle 11:9e56d52485d1 69
Bartas 26:cbbfe012a757 70 // motors.setSpeed(0.1f);
Reckstyle 17:de8b3111ddc5 71 // motors.forward();
Reckstyle 17:de8b3111ddc5 72 // motors.start();
Reckstyle 17:de8b3111ddc5 73 // wait(2);
Reckstyle 17:de8b3111ddc5 74 // float x=0.1f;
Reckstyle 17:de8b3111ddc5 75 // while (1) {
Reckstyle 17:de8b3111ddc5 76 // motors.setLeftSpeed(x);
Reckstyle 17:de8b3111ddc5 77 // x = x+0.05;
Reckstyle 17:de8b3111ddc5 78 // wait(3);
Reckstyle 17:de8b3111ddc5 79 // }
Reckstyle 17:de8b3111ddc5 80 // motors.setLeftSpeed(0.1f);
Reckstyle 17:de8b3111ddc5 81 // wait(5);
Reckstyle 17:de8b3111ddc5 82 // motors.setLeftSpeed(0.2f);
Reckstyle 17:de8b3111ddc5 83 // motors.setRightSpeed (0.2f);
Reckstyle 17:de8b3111ddc5 84 // wait(3);
Reckstyle 17:de8b3111ddc5 85 // motors.setRightSpeed (0.1f);
Reckstyle 17:de8b3111ddc5 86 // wait(5);
Reckstyle 17:de8b3111ddc5 87 // motors.stop();
Bartas 26:cbbfe012a757 88 // wait(1);
Reckstyle 17:de8b3111ddc5 89 // motors.reverse();
Reckstyle 17:de8b3111ddc5 90 // wait(5);
Reckstyle 17:de8b3111ddc5 91 // motors.stop();
Reckstyle 17:de8b3111ddc5 92 // motors.setSpeed(0.5f);
Reckstyle 17:de8b3111ddc5 93 // motors.start();
Reckstyle 17:de8b3111ddc5 94 // wait(5);
Reckstyle 17:de8b3111ddc5 95 // motors.stop();
Reckstyle 17:de8b3111ddc5 96 // wait(1);
Bartas 26:cbbfe012a757 97 // motors.reverse();
Bartas 26:cbbfe012a757 98 // motors.start();
Bartas 26:cbbfe012a757 99
Bartas 26:cbbfe012a757 100 // setup_counter(1000, 0);
Bartas 26:cbbfe012a757 101 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 23:902c3086394e 102 measureTimer.start();
Reckstyle 23:902c3086394e 103 driveMode = REGULAR; //initialise drivemoder
Reckstyle 23:902c3086394e 104 sensor_initialise(); // initialise sensor values
Reckstyle 23:902c3086394e 105 wait(1); //give time to set up the system
Reckstyle 23:902c3086394e 106 sensorTimer.start(); // start timer for sensors
Bartas 26:cbbfe012a757 107 pc.printf("Hello");
Bartas 26:cbbfe012a757 108 while(1){
Bartas 26:cbbfe012a757 109 if (HC06.getc() == 'r') {
Bartas 26:cbbfe012a757 110 measureSensors();
Bartas 26:cbbfe012a757 111 measureTimer.reset();
Bartas 26:cbbfe012a757 112 printBluetooth();
Bartas 26:cbbfe012a757 113 wait(1);
Bartas 26:cbbfe012a757 114 }
Reckstyle 19:d277614084bc 115 //passedTime1 = measureTimer.read_ms();
Reckstyle 23:902c3086394e 116 //if (sensorsCheckSum == 0) {
Reckstyle 23:902c3086394e 117 // motors.setSpeed(normalSpeed);
Reckstyle 23:902c3086394e 118 // }
Reckstyle 23:902c3086394e 119 // else if (sensorsCheckSum == 1 || sensorsCheckSum == 9 || sensorsCheckSum == 10 || sensorsCheckSum == 14 || sensorsCheckSum==26){
Reckstyle 23:902c3086394e 120 // motors.setLeftSpeed(normalSpeed/2);
Reckstyle 23:902c3086394e 121 //
Reckstyle 23:902c3086394e 122 // motors.setRightSpeed(normalSpeed*6);
Reckstyle 23:902c3086394e 123 // }
Reckstyle 23:902c3086394e 124 // else if (sensorsCheckSum == 4 && sensorsCheckSum ==16 || sensorsCheckSum == 20 || sensorsCheckSum ==21 ||sensorsCheckSum== 29) {
Reckstyle 23:902c3086394e 125 // motors.setRightSpeed(normalSpeed/2);
Reckstyle 23:902c3086394e 126 // motors.setLeftSpeed(normalSpeed*9);
Reckstyle 23:902c3086394e 127 // }
Reckstyle 23:902c3086394e 128 // else {
Reckstyle 23:902c3086394e 129 // motors.setSpeed(normalSpeed);
Reckstyle 23:902c3086394e 130 // }
Bartas 25:8be440e10126 131 // }
Bartas 25:8be440e10126 132 // }
Reckstyle 21:e8da3b351cd0 133 //HC06.printf("AT");
Reckstyle 21:e8da3b351cd0 134 //HC06.printf("AT+PIN5555");
Bartas 24:c1b1b0ea0cb9 135 // int testOtherCases = 0; //needed for control statements
Bartas 26:cbbfe012a757 136 // while (1) {
Bartas 26:cbbfe012a757 137 //
Reckstyle 20:198dc13777eb 138 if (driveMode == REGULAR) {
Reckstyle 20:198dc13777eb 139 measureSensors();
Bartas 26:cbbfe012a757 140 switch (sensorsCheckSum) {
Bartas 26:cbbfe012a757 141 case 0: // all black, turn around by 180 degrees or a possible turn on the left
Bartas 26:cbbfe012a757 142 for (k=0;k<6;k++) { //left turn situation
Bartas 25:8be440e10126 143 if (oldValues[k] == 194) {
Bartas 25:8be440e10126 144 motors.stop();
Bartas 25:8be440e10126 145 motors.setSteeringMode(NORMAL);
Bartas 25:8be440e10126 146 motors.reverse();
Bartas 25:8be440e10126 147 motors.setSpeed(0.1f);
Bartas 25:8be440e10126 148 motors.start();
Bartas 25:8be440e10126 149 wait(1);
Bartas 26:cbbfe012a757 150 motors.stop();
Bartas 25:8be440e10126 151 } else {
Bartas 25:8be440e10126 152 motors.stop();
Bartas 25:8be440e10126 153 motors.setSteeringMode(NORMAL);
Bartas 25:8be440e10126 154 motors.reverse();
Bartas 25:8be440e10126 155 motors.setSpeed(0.1f);
Bartas 25:8be440e10126 156 motors.start();
Bartas 25:8be440e10126 157 wait(2);
Bartas 25:8be440e10126 158 motors.stop();
Bartas 25:8be440e10126 159 motors.setSteeringMode(PIVOT_CCW);
Bartas 25:8be440e10126 160 motors.setSpeed(0.1f);
Bartas 25:8be440e10126 161 do
Bartas 25:8be440e10126 162 {
Bartas 25:8be440e10126 163 motors.start();
Bartas 25:8be440e10126 164 measureSensors();
Bartas 25:8be440e10126 165 } while (sensorsCheckSum != 96);
Bartas 25:8be440e10126 166 motors.stop();
Bartas 25:8be440e10126 167 motors.setSteeringMode(NORMAL);
Bartas 25:8be440e10126 168 }
Reckstyle 21:e8da3b351cd0 169 break;
Reckstyle 21:e8da3b351cd0 170 case 30: //all right are white, left all black >> turn right(move left wheel)
Bartas 26:cbbfe012a757 171 motors.setSteeringMode(NORMAL);
Bartas 26:cbbfe012a757 172 motors.forward();
Bartas 24:c1b1b0ea0cb9 173 motors.setRightSpeed(0.15f);
Bartas 24:c1b1b0ea0cb9 174 motors.setLeftSpeed(0.5f);
Reckstyle 21:e8da3b351cd0 175 break;
Bartas 25:8be440e10126 176 // case 46: //left 5 white, right only 3 black >> turn right
Bartas 25:8be440e10126 177 // motors.setRightSpeed(0.15f);
Bartas 25:8be440e10126 178 // motors.setLeftSpeed(0.1f);
Bartas 25:8be440e10126 179 // break;
Bartas 26:cbbfe012a757 180 case 94: //normal <<STARTING POSITION>>, half of right and half of left are white
Bartas 25:8be440e10126 181 motors.setSteeringMode(NORMAL);
Bartas 26:cbbfe012a757 182 motors.forward();
Bartas 24:c1b1b0ea0cb9 183 motors.setSpeed(0.1f);
Bartas 25:8be440e10126 184 motors.start();
Reckstyle 21:e8da3b351cd0 185 break;
Bartas 25:8be440e10126 186 case 104: //right all white, left half white >> turn right
Bartas 25:8be440e10126 187 motors.setRightSpeed(0.1f);
Bartas 25:8be440e10126 188 motors.setLeftSpeed(0.15f);
Reckstyle 21:e8da3b351cd0 189 break;
Bartas 25:8be440e10126 190 // case 154: //right 4 white, left only 6 black >> turn left
Bartas 25:8be440e10126 191 // break;
Reckstyle 21:e8da3b351cd0 192 case 174: //left all white, right all black >> turn left (move right wheel)
Bartas 25:8be440e10126 193 motors.setRightSpeed(0.05f);
Bartas 25:8be440e10126 194 motors.setLeftSpeed(0.15f);
Reckstyle 21:e8da3b351cd0 195 break;
Bartas 25:8be440e10126 196 case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
Reckstyle 21:e8da3b351cd0 197 break;
Reckstyle 21:e8da3b351cd0 198 case 204 : //all sensors are white
Bartas 25:8be440e10126 199
Bartas 25:8be440e10126 200 for (k=0;k<6;k++) { //situation when a square is entered, need to follow right line
Bartas 25:8be440e10126 201 if (oldValues[k] == 194) { //checks whether black line on the right was present before
Bartas 25:8be440e10126 202 motors.setRightSpeed(0.15f);
Bartas 25:8be440e10126 203 motors.setLeftSpeed(0.05f);
Bartas 25:8be440e10126 204 }
Bartas 25:8be440e10126 205
Bartas 25:8be440e10126 206 if (oldValues[k] == 104) { //right turn 90 situation
Bartas 25:8be440e10126 207 motors.stop();
Bartas 25:8be440e10126 208 motors.setSteeringMode(PIVOT_CW);
Bartas 25:8be440e10126 209 motors.setRightSpeed(0.1f);
Bartas 25:8be440e10126 210 motors.setLeftSpeed(0.1f);
Bartas 25:8be440e10126 211 do
Bartas 25:8be440e10126 212 {
Bartas 25:8be440e10126 213 motors.start();
Bartas 25:8be440e10126 214 measureSensors();
Bartas 25:8be440e10126 215 } while (sensorsCheckSum != 94);
Bartas 25:8be440e10126 216 motors.stop();
Bartas 25:8be440e10126 217 motors.setSteeringMode(NORMAL);
Bartas 25:8be440e10126 218 }
Bartas 25:8be440e10126 219 break;
Bartas 25:8be440e10126 220
Bartas 24:c1b1b0ea0cb9 221 default: //checksum is zero , all are black
Bartas 24:c1b1b0ea0cb9 222 measureSensors();
Reckstyle 20:198dc13777eb 223 break;
Bartas 24:c1b1b0ea0cb9 224 }
Bartas 25:8be440e10126 225 }
Bartas 24:c1b1b0ea0cb9 226 // if (testOtherCases == 1) {
Bartas 24:c1b1b0ea0cb9 227 // if (sensorsCheckSum < 96){ // adjust right
Bartas 26:cbbfe012a757 228 // } else {//adjust left
Bartas 26:cbbfe012a757 229 // }
Bartas 24:c1b1b0ea0cb9 230 // testOtherCases = 0;
Bartas 26:cbbfe012a757 231 // } else { //if (driveMode == SQUARE} //implement the square searching thing.
Bartas 26:cbbfe012a757 232 }
Bartas 26:cbbfe012a757 233 }