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@20:198dc13777eb, 2015-03-20 (annotated)

Committer:
Reckstyle
Date:
Fri Mar 20 11:40:57 2015 +0000
Revision:
20:198dc13777eb
Parent:
19:d277614084bc
Child:
21:e8da3b351cd0
updated case statemnts;

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 11:9e56d52485d1 43
Reckstyle 11:9e56d52485d1 44
Reckstyle 11:9e56d52485d1 45 void measureSensors () {
Reckstyle 11:9e56d52485d1 46 sensorsCheckSum = 0; //zero it when first going into the routine
Reckstyle 11:9e56d52485d1 47 int iterationNumber = NUMBER_SENSORS_REGULAR;
Reckstyle 11:9e56d52485d1 48 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 49 iterationNumber += NUMBER_SENSORS_SQUARE;
Reckstyle 11:9e56d52485d1 50 }
Reckstyle 11:9e56d52485d1 51 for (int i = 0; i < iterationNumber;i++){
Reckstyle 17:de8b3111ddc5 52 //pc.printf("%i iteration%i ",i,iterationNumber);
Reckstyle 16:3649eb1a056d 53 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 54 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 55 }
Reckstyle 11:9e56d52485d1 56 }
Reckstyle 11:9e56d52485d1 57 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 58 }
Reckstyle 11:9e56d52485d1 59
Reckstyle 15:6453cd351452 60 void printBluetooth() { //for debugging
Reckstyle 16:3649eb1a056d 61
Reckstyle 19:d277614084bc 62 pc.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[1]->state,sensorArray[0]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 19:d277614084bc 63 pc.printf("LLD%i LRD%i rld%i rrd%i\n\n",sensorArray[3]->state,sensorArray[2]->state,sensorArray[3]->state,sensorArray[2]->state);
Reckstyle 16:3649eb1a056d 64 //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 65 //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 66 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Reckstyle 19:d277614084bc 67 pc.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Reckstyle 19:d277614084bc 68 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 15:6453cd351452 69 }
Reckstyle 15:6453cd351452 70
Reckstyle 11:9e56d52485d1 71 int main() {
Reckstyle 11:9e56d52485d1 72
orsharp 12:bb21b76b6375 73
Reckstyle 11:9e56d52485d1 74
Reckstyle 19:d277614084bc 75 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US);
Reckstyle 11:9e56d52485d1 76
Reckstyle 17:de8b3111ddc5 77 // motors.setSpeed(0.1f);
Reckstyle 17:de8b3111ddc5 78 // motors.forward();
Reckstyle 17:de8b3111ddc5 79 // motors.start();
Reckstyle 17:de8b3111ddc5 80 // wait(2);
Reckstyle 17:de8b3111ddc5 81 // float x=0.1f;
Reckstyle 17:de8b3111ddc5 82 // while (1) {
Reckstyle 17:de8b3111ddc5 83 // motors.setLeftSpeed(x);
Reckstyle 17:de8b3111ddc5 84 // x = x+0.05;
Reckstyle 17:de8b3111ddc5 85 // wait(3);
Reckstyle 17:de8b3111ddc5 86 // }
Reckstyle 17:de8b3111ddc5 87 // motors.setLeftSpeed(0.1f);
Reckstyle 17:de8b3111ddc5 88 // wait(5);
Reckstyle 17:de8b3111ddc5 89 // motors.setLeftSpeed(0.2f);
Reckstyle 17:de8b3111ddc5 90 // motors.setRightSpeed (0.2f);
Reckstyle 17:de8b3111ddc5 91 // wait(3);
Reckstyle 17:de8b3111ddc5 92 // motors.setRightSpeed (0.1f);
Reckstyle 17:de8b3111ddc5 93 // wait(5);
Reckstyle 17:de8b3111ddc5 94 // motors.stop();
Reckstyle 11:9e56d52485d1 95
Reckstyle 17:de8b3111ddc5 96 //wait(1);
Reckstyle 17:de8b3111ddc5 97 // motors.reverse();
Reckstyle 17:de8b3111ddc5 98 // wait(5);
Reckstyle 17:de8b3111ddc5 99 // motors.stop();
Reckstyle 17:de8b3111ddc5 100 // motors.setSpeed(0.5f);
Reckstyle 17:de8b3111ddc5 101 // motors.start();
Reckstyle 17:de8b3111ddc5 102 // wait(5);
Reckstyle 17:de8b3111ddc5 103 // motors.stop();
Reckstyle 17:de8b3111ddc5 104 // wait(1);
Reckstyle 17:de8b3111ddc5 105 // motors.reverse();
Reckstyle 11:9e56d52485d1 106
Reckstyle 17:de8b3111ddc5 107
Reckstyle 17:de8b3111ddc5 108
Reckstyle 17:de8b3111ddc5 109 //motors.start();
Reckstyle 17:de8b3111ddc5 110
Reckstyle 11:9e56d52485d1 111 // setup_counter(1000, 0);
Reckstyle 11:9e56d52485d1 112 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 19:d277614084bc 113 measureTimer.start();
Reckstyle 15:6453cd351452 114 driveMode = REGULAR; //initialise drivemoder
Reckstyle 11:9e56d52485d1 115 sensor_initialise(); // initialise sensor values
Reckstyle 11:9e56d52485d1 116 wait(1); //give time to set up the system
Reckstyle 11:9e56d52485d1 117
Reckstyle 11:9e56d52485d1 118 sensorTimer.start(); // start timer for sensors
Reckstyle 19:d277614084bc 119 float normalSpeed = 0.01f;
Reckstyle 15:6453cd351452 120 HC06.baud(9600);
Reckstyle 16:3649eb1a056d 121 HC06.printf("working..");
Reckstyle 19:d277614084bc 122 motors.setSpeed(normalSpeed);
Reckstyle 17:de8b3111ddc5 123 motors.forward();
Reckstyle 17:de8b3111ddc5 124 motors.start();
Reckstyle 19:d277614084bc 125
Reckstyle 19:d277614084bc 126
Reckstyle 19:d277614084bc 127
Reckstyle 19:d277614084bc 128 wait(3);
Reckstyle 16:3649eb1a056d 129 while(1){
Reckstyle 19:d277614084bc 130
Reckstyle 16:3649eb1a056d 131 measureSensors();
Reckstyle 19:d277614084bc 132 //measureTimer.reset();
Reckstyle 16:3649eb1a056d 133 printBluetooth();
Reckstyle 19:d277614084bc 134 //passedTime1 = measureTimer.read_ms();
Reckstyle 19:d277614084bc 135 if (sensorsCheckSum == 0) {
Reckstyle 19:d277614084bc 136 motors.setSpeed(normalSpeed);
Reckstyle 19:d277614084bc 137 }
Reckstyle 19:d277614084bc 138 else if (sensorsCheckSum == 1 || sensorsCheckSum == 9 || sensorsCheckSum == 10 || sensorsCheckSum == 14 || sensorsCheckSum==26){
Reckstyle 19:d277614084bc 139 motors.setLeftSpeed(normalSpeed/2);
Reckstyle 19:d277614084bc 140
Reckstyle 19:d277614084bc 141 motors.setRightSpeed(normalSpeed*6);
Reckstyle 16:3649eb1a056d 142 }
Reckstyle 19:d277614084bc 143 else if (sensorsCheckSum == 4 && sensorsCheckSum ==16 || sensorsCheckSum == 20 || sensorsCheckSum ==21 ||sensorsCheckSum== 29) {
Reckstyle 19:d277614084bc 144 motors.setRightSpeed(normalSpeed/2);
Reckstyle 19:d277614084bc 145 motors.setLeftSpeed(normalSpeed*9);
Reckstyle 17:de8b3111ddc5 146 }
Reckstyle 17:de8b3111ddc5 147 else {
Reckstyle 19:d277614084bc 148 motors.setSpeed(normalSpeed);
Reckstyle 17:de8b3111ddc5 149 }
Reckstyle 19:d277614084bc 150
Reckstyle 17:de8b3111ddc5 151 }
Reckstyle 16:3649eb1a056d 152
Reckstyle 15:6453cd351452 153
Reckstyle 15:6453cd351452 154 //HC06.printf("AT");
Reckstyle 15:6453cd351452 155 //HC06.printf("AT+PIN5555");
Reckstyle 15:6453cd351452 156
Reckstyle 15:6453cd351452 157
Reckstyle 14:3844d1dacece 158 // pc.printf("Start...");
Reckstyle 20:198dc13777eb 159 /*
Reckstyle 20:198dc13777eb 160 int testOtherCases = 0; //needed for control statements
Reckstyle 20:198dc13777eb 161 while (1) {
Reckstyle 20:198dc13777eb 162
Reckstyle 20:198dc13777eb 163 if (driveMode == REGULAR) {
Reckstyle 20:198dc13777eb 164 measureSensors();
Reckstyle 20:198dc13777eb 165 switch (sensorsCheckSum) {
Reckstyle 20:198dc13777eb 166 case 94: //go straight
Reckstyle 20:198dc13777eb 167 break;
Reckstyle 20:198dc13777eb 168 case 95: //Turn right seriously
Reckstyle 20:198dc13777eb 169 break;
Reckstyle 20:198dc13777eb 170 case 104: //Turn right as well
Reckstyle 20:198dc13777eb 171 break;
Reckstyle 20:198dc13777eb 172
Reckstyle 20:198dc13777eb 173 case 158: //Turn left seriously
Reckstyle 20:198dc13777eb 174 pc.printf ("only Right is white \n");
Reckstyle 20:198dc13777eb 175 break;
Reckstyle 20:198dc13777eb 176 case 194 : //Turn left seriously again
Reckstyle 20:198dc13777eb 177 pc.printf ("only Left is white \n");
Reckstyle 20:198dc13777eb 178 break;
Reckstyle 20:198dc13777eb 179 case 103 : //RRD is white all else normal
Reckstyle 20:198dc13777eb 180 pc.printf ("both are white \n");
Reckstyle 20:198dc13777eb 181 break;
Reckstyle 20:198dc13777eb 182
Reckstyle 20:198dc13777eb 183 // case 204:
Reckstyle 20:198dc13777eb 184 // driveMode = SQUARE; //if all sensors are white you're in the square
Reckstyle 11:9e56d52485d1 185 // break;
Reckstyle 20:198dc13777eb 186 default: //checksum is zero , all are black
Reckstyle 20:198dc13777eb 187 testOtherCases = 1;
Reckstyle 20:198dc13777eb 188 break;
Reckstyle 20:198dc13777eb 189 }
Reckstyle 20:198dc13777eb 190 if (testOtherCases == 1) {
Reckstyle 20:198dc13777eb 191 if (sensorsCheckSum < 96){ // adjust right
Reckstyle 20:198dc13777eb 192 }
Reckstyle 20:198dc13777eb 193 else {//adjust left
Reckstyle 20:198dc13777eb 194 }
Reckstyle 20:198dc13777eb 195 testOtherCases = 0;
Reckstyle 20:198dc13777eb 196 }
Reckstyle 20:198dc13777eb 197 */
Reckstyle 14:3844d1dacece 198 //
Reckstyle 14:3844d1dacece 199 //
Reckstyle 14:3844d1dacece 200 //
Reckstyle 14:3844d1dacece 201 // }
Reckstyle 14:3844d1dacece 202 // else { //if (driveMode == SQUARE}
Reckstyle 14:3844d1dacece 203 // //implement the square searching thing..
Reckstyle 14:3844d1dacece 204 //
Reckstyle 14:3844d1dacece 205 // }
Reckstyle 14:3844d1dacece 206 //
Reckstyle 14:3844d1dacece 207 //
Reckstyle 14:3844d1dacece 208 // }
Reckstyle 14:3844d1dacece 209 //
Reckstyle 11:9e56d52485d1 210
Reckstyle 11:9e56d52485d1 211 }
Reckstyle 11:9e56d52485d1 212
Reckstyle 11:9e56d52485d1 213
Reckstyle 11:9e56d52485d1 214
Reckstyle 11:9e56d52485d1 215
Reckstyle 11:9e56d52485d1 216
Reckstyle 9:718987b106a8 217