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

Committer:
Reckstyle
Date:
Mon Mar 23 14:03:29 2015 +0000
Revision:
25:bec5dc4c9f5e
Parent:
24:ecc3fbaf2824
Child:
27:fc0fd2c0eebb
sda

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
Bartas 24:ecc3fbaf2824 21 Serial HC06(PTE0,PTE1);
Reckstyle 9:718987b106a8 22
Reckstyle 18: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 18:d277614084bc 30 int passedTime1,passedTime2;
Reckstyle 25:bec5dc4c9f5e 31 int oldValues[5] = {0};
Reckstyle 23:9b53c72fcd38 32 int cursor = 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 20:e8da3b351cd0 55 pc.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 20:e8da3b351cd0 56 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 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 18:d277614084bc 60 pc.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 18:d277614084bc 66 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US);
Reckstyle 11:9e56d52485d1 67
Reckstyle 22:902c3086394e 68 // setup_counter(1000, 0);
Reckstyle 22:902c3086394e 69 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 22:902c3086394e 70 measureTimer.start();
Reckstyle 22:902c3086394e 71 driveMode = REGULAR; //initialise drivemoder
Reckstyle 22:902c3086394e 72 sensor_initialise(); // initialise sensor values
Reckstyle 22:902c3086394e 73 wait(1); //give time to set up the system
Reckstyle 22:902c3086394e 74
Reckstyle 22:902c3086394e 75 sensorTimer.start(); // start timer for sensors
Reckstyle 18:d277614084bc 76 float normalSpeed = 0.01f;
Reckstyle 25:bec5dc4c9f5e 77 /*
Reckstyle 16:3649eb1a056d 78 while(1){
Reckstyle 22:902c3086394e 79 if (pc.getc() == 'r') {
Reckstyle 16:3649eb1a056d 80 measureSensors();
Reckstyle 18:d277614084bc 81 //measureTimer.reset();
Reckstyle 16:3649eb1a056d 82 printBluetooth();
Reckstyle 18:d277614084bc 83 //passedTime1 = measureTimer.read_ms();
Reckstyle 22:902c3086394e 84 //if (sensorsCheckSum == 0) {
Reckstyle 22:902c3086394e 85 // motors.setSpeed(normalSpeed);
Reckstyle 22:902c3086394e 86 // }
Reckstyle 22:902c3086394e 87 // else if (sensorsCheckSum == 1 || sensorsCheckSum == 9 || sensorsCheckSum == 10 || sensorsCheckSum == 14 || sensorsCheckSum==26){
Reckstyle 22:902c3086394e 88 // motors.setLeftSpeed(normalSpeed/2);
Reckstyle 22:902c3086394e 89 //
Reckstyle 22:902c3086394e 90 // motors.setRightSpeed(normalSpeed*6);
Reckstyle 22:902c3086394e 91 // }
Reckstyle 22:902c3086394e 92 // else if (sensorsCheckSum == 4 && sensorsCheckSum ==16 || sensorsCheckSum == 20 || sensorsCheckSum ==21 ||sensorsCheckSum== 29) {
Reckstyle 22:902c3086394e 93 // motors.setRightSpeed(normalSpeed/2);
Reckstyle 22:902c3086394e 94 // motors.setLeftSpeed(normalSpeed*9);
Reckstyle 22:902c3086394e 95 // }
Reckstyle 22:902c3086394e 96 // else {
Reckstyle 22:902c3086394e 97 // motors.setSpeed(normalSpeed);
Reckstyle 22:902c3086394e 98 // }
Reckstyle 25:bec5dc4c9f5e 99 }
Reckstyle 25:bec5dc4c9f5e 100 */
Reckstyle 19:198dc13777eb 101 int testOtherCases = 0; //needed for control statements
Reckstyle 19:198dc13777eb 102 while (1) {
Reckstyle 19:198dc13777eb 103
Reckstyle 25:bec5dc4c9f5e 104 if (driveMode == REGULAR) {
Reckstyle 25:bec5dc4c9f5e 105 measureSensors();
Reckstyle 25:bec5dc4c9f5e 106 switch (sensorsCheckSum) {
Reckstyle 25:bec5dc4c9f5e 107 case 0: // all black, turn around by 180 degrees or a possible turn on the left
Reckstyle 25:bec5dc4c9f5e 108 for (int k=0;k<5;k++) { //left turn situation
Reckstyle 25:bec5dc4c9f5e 109 if (oldValues[k] == 194) {
Reckstyle 25:bec5dc4c9f5e 110 motors.stop();
Reckstyle 25:bec5dc4c9f5e 111 motors.setSteeringMode(NORMAL);
Reckstyle 25:bec5dc4c9f5e 112 motors.reverse();
Reckstyle 25:bec5dc4c9f5e 113 motors.setSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 114 motors.start();
Reckstyle 25:bec5dc4c9f5e 115 wait(1);
Reckstyle 25:bec5dc4c9f5e 116 motors.stop();
Reckstyle 25:bec5dc4c9f5e 117 }
Reckstyle 25:bec5dc4c9f5e 118 else {
Reckstyle 25:bec5dc4c9f5e 119 motors.stop();
Reckstyle 25:bec5dc4c9f5e 120 motors.setSteeringMode(NORMAL);
Reckstyle 25:bec5dc4c9f5e 121 motors.reverse();
Reckstyle 25:bec5dc4c9f5e 122 motors.setSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 123 motors.start();
Reckstyle 25:bec5dc4c9f5e 124 wait(2);
Reckstyle 25:bec5dc4c9f5e 125 motors.stop();
Reckstyle 25:bec5dc4c9f5e 126 motors.setSteeringMode(PIVOT_CCW);
Reckstyle 25:bec5dc4c9f5e 127 motors.setSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 128 do
Reckstyle 25:bec5dc4c9f5e 129 {
Reckstyle 25:bec5dc4c9f5e 130 motors.start();
Reckstyle 25:bec5dc4c9f5e 131 measureSensors();
Reckstyle 25:bec5dc4c9f5e 132 } while (sensorsCheckSum != 96);
Reckstyle 25:bec5dc4c9f5e 133 motors.stop();
Reckstyle 25:bec5dc4c9f5e 134 motors.setSteeringMode(NORMAL);
Reckstyle 25:bec5dc4c9f5e 135 }
Reckstyle 25:bec5dc4c9f5e 136 }
Reckstyle 25:bec5dc4c9f5e 137 break;
Reckstyle 25:bec5dc4c9f5e 138 case 30: //all right are white, left all black >> turn right(move left wheel)
Reckstyle 25:bec5dc4c9f5e 139 motors.setSteeringMode(NORMAL);
Reckstyle 25:bec5dc4c9f5e 140 motors.forward();
Reckstyle 25:bec5dc4c9f5e 141 motors.setRightSpeed(0.15f);
Reckstyle 25:bec5dc4c9f5e 142 motors.setLeftSpeed(0.5f);
Reckstyle 25:bec5dc4c9f5e 143 break;
Reckstyle 25:bec5dc4c9f5e 144 // case 46: //left 5 white, right only 3 black >> turn right
Reckstyle 25:bec5dc4c9f5e 145 // motors.setRightSpeed(0.15f);
Reckstyle 25:bec5dc4c9f5e 146 // motors.setLeftSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 147 // break;
Reckstyle 25:bec5dc4c9f5e 148 case 94: //normal <<STARTING POSITION>>, half of right and half of left are white
Reckstyle 25:bec5dc4c9f5e 149 motors.setSteeringMode(NORMAL);
Reckstyle 25:bec5dc4c9f5e 150 motors.forward();
Reckstyle 25:bec5dc4c9f5e 151 motors.setSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 152 motors.start();
Reckstyle 25:bec5dc4c9f5e 153 break;
Reckstyle 25:bec5dc4c9f5e 154 case 104: //right all white, left half white >> turn right
Reckstyle 25:bec5dc4c9f5e 155 motors.setRightSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 156 motors.setLeftSpeed(0.15f);
Reckstyle 25:bec5dc4c9f5e 157 break;
Reckstyle 25:bec5dc4c9f5e 158 // case 154: //right 4 white, left only 6 black >> turn left
Reckstyle 25:bec5dc4c9f5e 159 // break;
Reckstyle 25:bec5dc4c9f5e 160 case 174: //left all white, right all black >> turn left (move right wheel)
Reckstyle 25:bec5dc4c9f5e 161 motors.setRightSpeed(0.05f);
Reckstyle 25:bec5dc4c9f5e 162 motors.setLeftSpeed(0.15f);
Reckstyle 25:bec5dc4c9f5e 163 break;
Reckstyle 25:bec5dc4c9f5e 164 case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
Reckstyle 25:bec5dc4c9f5e 165 break;
Reckstyle 25:bec5dc4c9f5e 166 case 204 : //all sensors are white
Reckstyle 25:bec5dc4c9f5e 167
Reckstyle 25:bec5dc4c9f5e 168 for (int k=0;k<5;k++) { //situation when a square is entered, need to follow right line
Reckstyle 25:bec5dc4c9f5e 169 if (oldValues[k] == 194) { //checks whether black line on the right was present before
Reckstyle 25:bec5dc4c9f5e 170 motors.setRightSpeed(0.15f);
Reckstyle 25:bec5dc4c9f5e 171 motors.setLeftSpeed(0.05f);
Reckstyle 25:bec5dc4c9f5e 172 }
Reckstyle 25:bec5dc4c9f5e 173
Reckstyle 25:bec5dc4c9f5e 174 if (oldValues[k] == 104) { //right turn 90 situation
Reckstyle 25:bec5dc4c9f5e 175 motors.stop();
Reckstyle 25:bec5dc4c9f5e 176 motors.setSteeringMode(PIVOT_CW);
Reckstyle 25:bec5dc4c9f5e 177 motors.setRightSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 178 motors.setLeftSpeed(0.1f);
Reckstyle 25:bec5dc4c9f5e 179 do
Reckstyle 25:bec5dc4c9f5e 180 {
Reckstyle 25:bec5dc4c9f5e 181 motors.start();
Reckstyle 25:bec5dc4c9f5e 182 measureSensors();
Reckstyle 25:bec5dc4c9f5e 183 } while (sensorsCheckSum != 94);
Reckstyle 25:bec5dc4c9f5e 184 motors.stop();
Reckstyle 25:bec5dc4c9f5e 185 motors.setSteeringMode(NORMAL);
Reckstyle 25:bec5dc4c9f5e 186 }
Reckstyle 25:bec5dc4c9f5e 187 break;
Reckstyle 25:bec5dc4c9f5e 188 default: //checksum is zero , all are black
Reckstyle 25:bec5dc4c9f5e 189 testOtherCases = 1;
Reckstyle 25:bec5dc4c9f5e 190 break;
Reckstyle 25:bec5dc4c9f5e 191 }
Bartas 24:ecc3fbaf2824 192
Reckstyle 25:bec5dc4c9f5e 193 if (testOtherCases == 1) {
Reckstyle 25:bec5dc4c9f5e 194 if (sensorsCheckSum < 96){ // adjust right
Reckstyle 25:bec5dc4c9f5e 195 }
Reckstyle 25:bec5dc4c9f5e 196 else {//adjust left
Reckstyle 25:bec5dc4c9f5e 197 }
Reckstyle 25:bec5dc4c9f5e 198 testOtherCases = 0;
Reckstyle 25:bec5dc4c9f5e 199 }
Reckstyle 25:bec5dc4c9f5e 200
Reckstyle 25:bec5dc4c9f5e 201 }
Reckstyle 25:bec5dc4c9f5e 202 else {// if driveMode == SQUARE)
Bartas 24:ecc3fbaf2824 203 }
Bartas 24:ecc3fbaf2824 204 }
Reckstyle 25:bec5dc4c9f5e 205 }
Reckstyle 11:9e56d52485d1 206