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

copy.cpp@37:3d14df6dec34, 2015-03-26 (annotated)

Committer:
Reckstyle
Date:
Thu Mar 26 20:13:49 2015 +0000
Revision:
37:3d14df6dec34
fdsa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Reckstyle 37:3d14df6dec34 1 /***
Reckstyle 37:3d14df6dec34 2 ********** MAIN PROGRAM **********
Reckstyle 37:3d14df6dec34 3
Reckstyle 37:3d14df6dec34 4 Sensors are mapped on the global variable sensorsCheckSum,
Reckstyle 37:3d14df6dec34 5 which multiplies the sensor number by itself to then decode,
Reckstyle 37:3d14df6dec34 6 which sensors are off and which are on
Reckstyle 37:3d14df6dec34 7 ie. if sensor rightright - sensorChecksum = 1*1 = 1
Reckstyle 37:3d14df6dec34 8 if rightright and rightcentre - sensorChecksum = 1*1 + 2*2 = 5
Reckstyle 37:3d14df6dec34 9 ...
Reckstyle 37:3d14df6dec34 10 ***/
Reckstyle 37:3d14df6dec34 11
Reckstyle 37:3d14df6dec34 12 #include "mbed.h"
Reckstyle 37:3d14df6dec34 13 #include "sensor_measure.h"
Reckstyle 37:3d14df6dec34 14 #include "Motor_Driver.h"
Reckstyle 37:3d14df6dec34 15 #include "shooter.h"
Reckstyle 37:3d14df6dec34 16
Reckstyle 37:3d14df6dec34 17 #define PWM_PERIOD_US 10000
Reckstyle 37:3d14df6dec34 18
Reckstyle 37:3d14df6dec34 19 typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
Reckstyle 37:3d14df6dec34 20 DigitalOut led(LED_RED);
Reckstyle 37:3d14df6dec34 21 DigitalOut ledMode(LED_GREEN);
Reckstyle 37:3d14df6dec34 22 Serial HC06(PTE0,PTE1);
Reckstyle 37:3d14df6dec34 23
Reckstyle 37:3d14df6dec34 24 void turnLeft ();
Reckstyle 37:3d14df6dec34 25 void turnRight ();
Reckstyle 37:3d14df6dec34 26 void measureSensors ();
Reckstyle 37:3d14df6dec34 27 void printBluetooth();
Reckstyle 37:3d14df6dec34 28 void shootFind ();
Reckstyle 37:3d14df6dec34 29
Reckstyle 37:3d14df6dec34 30
Reckstyle 37:3d14df6dec34 31 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
Reckstyle 37:3d14df6dec34 32
Reckstyle 37:3d14df6dec34 33 mode driveMode; //declaring the variable for the states
Reckstyle 37:3d14df6dec34 34 int sensorsCheckSum; //varibale used for sensors mapping access
Reckstyle 37:3d14df6dec34 35 int passedTime1,passedTime2;
Reckstyle 37:3d14df6dec34 36 int cursor = 0, k = 0;
Reckstyle 37:3d14df6dec34 37 static float normalSpeed = 0.15f;
Reckstyle 37:3d14df6dec34 38
Reckstyle 37:3d14df6dec34 39
Reckstyle 37:3d14df6dec34 40
Reckstyle 37:3d14df6dec34 41 int main() {
Reckstyle 37:3d14df6dec34 42 led = 1;//start LED with beginning of main program
Reckstyle 37:3d14df6dec34 43 measureTimer.start();
Reckstyle 37:3d14df6dec34 44 driveMode = REGULAR; //initialise drivemoder
Reckstyle 37:3d14df6dec34 45 sensor_initialise(); // initialise sensor values
Reckstyle 37:3d14df6dec34 46 wait(1); //give time to set up the system
Reckstyle 37:3d14df6dec34 47
Reckstyle 37:3d14df6dec34 48 sensorTimer.start(); // start timer for sensors
Reckstyle 37:3d14df6dec34 49
Reckstyle 37:3d14df6dec34 50 motors.setSteeringMode (NORMAL);
Reckstyle 37:3d14df6dec34 51 motors.disableHardBraking();
Reckstyle 37:3d14df6dec34 52 motors.forward ();
Reckstyle 37:3d14df6dec34 53
Reckstyle 37:3d14df6dec34 54 motors.start ();
Reckstyle 37:3d14df6dec34 55
Reckstyle 37:3d14df6dec34 56 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 57
Reckstyle 37:3d14df6dec34 58 motors.enableSlew();
Reckstyle 37:3d14df6dec34 59 int testOtherCases = 0; //used for control
Reckstyle 37:3d14df6dec34 60 while(1){
Reckstyle 37:3d14df6dec34 61
Reckstyle 37:3d14df6dec34 62 measureSensors();
Reckstyle 37:3d14df6dec34 63 printBluetooth();
Reckstyle 37:3d14df6dec34 64 switch (sensorsCheckSum) {
Reckstyle 37:3d14df6dec34 65 case 94: //continue straight
Reckstyle 37:3d14df6dec34 66 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 67 break;
Reckstyle 37:3d14df6dec34 68 case 104:
Reckstyle 37:3d14df6dec34 69 motors.setSpeed (normalSpeed); //move a little forward
Reckstyle 37:3d14df6dec34 70 measureSensors(); //measure again
Reckstyle 37:3d14df6dec34 71 if (sensorsCheckSum == 104 || sensorsCheckSum == 168) { //confirm the turn
Reckstyle 37:3d14df6dec34 72 turnRight();
Reckstyle 37:3d14df6dec34 73 break;
Reckstyle 37:3d14df6dec34 74 case 158:
Reckstyle 37:3d14df6dec34 75 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 76 measureSensors();
Reckstyle 37:3d14df6dec34 77
Reckstyle 37:3d14df6dec34 78 if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
Reckstyle 37:3d14df6dec34 79 turnLeft();
Reckstyle 37:3d14df6dec34 80 }
Reckstyle 37:3d14df6dec34 81 break;
Reckstyle 37:3d14df6dec34 82 case 168:
Reckstyle 37:3d14df6dec34 83 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 84 measureSensors();
Reckstyle 37:3d14df6dec34 85
Reckstyle 37:3d14df6dec34 86 if (sensorsCheckSum == 168) { //confirm the turn
Reckstyle 37:3d14df6dec34 87 turnRight ();
Reckstyle 37:3d14df6dec34 88 }
Reckstyle 37:3d14df6dec34 89
Reckstyle 37:3d14df6dec34 90 break;
Reckstyle 37:3d14df6dec34 91 case 194:
Reckstyle 37:3d14df6dec34 92 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 93 measureSensors();
Reckstyle 37:3d14df6dec34 94
Reckstyle 37:3d14df6dec34 95 if (sensorsCheckSum == 194) { //confirm the turn
Reckstyle 37:3d14df6dec34 96 turnLeft();
Reckstyle 37:3d14df6dec34 97 }
Reckstyle 37:3d14df6dec34 98 break;
Reckstyle 37:3d14df6dec34 99 default:
Reckstyle 37:3d14df6dec34 100 testOtherCases =1;
Reckstyle 37:3d14df6dec34 101 break;
Reckstyle 37:3d14df6dec34 102 }
Reckstyle 37:3d14df6dec34 103 if (testOtherCases == 1) {
Reckstyle 37:3d14df6dec34 104
Reckstyle 37:3d14df6dec34 105 if (sensorsCheckSum < 96) { //turn left
Reckstyle 37:3d14df6dec34 106 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 107 motors.setLeftSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 108 wait(0.075);
Reckstyle 37:3d14df6dec34 109 }
Reckstyle 37:3d14df6dec34 110 else if (sensorsCheckSum > 96) { // turn right
Reckstyle 37:3d14df6dec34 111 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 112 motors.setRightSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 113 wait(0.1);
Reckstyle 37:3d14df6dec34 114 }
Reckstyle 37:3d14df6dec34 115
Reckstyle 37:3d14df6dec34 116 }
Reckstyle 37:3d14df6dec34 117
Reckstyle 37:3d14df6dec34 118 motors.setSpeed(0.0); //give time for the system to settle
Reckstyle 37:3d14df6dec34 119 wait(0.2);
Reckstyle 37:3d14df6dec34 120 }
Reckstyle 37:3d14df6dec34 121 } //int main
Reckstyle 37:3d14df6dec34 122
Reckstyle 37:3d14df6dec34 123 //print statements for debugging
Reckstyle 37:3d14df6dec34 124 void printBluetooth() { /
Reckstyle 37:3d14df6dec34 125 //HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 37:3d14df6dec34 126 //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 37:3d14df6dec34 127 //HC06.printf("%i %i %i \n",sensorArray[8]->state,sensorArray[9]->state,sensorArray[3]->state,sensorArray[10]->state);
Reckstyle 37:3d14df6dec34 128 //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 37:3d14df6dec34 129 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Reckstyle 37:3d14df6dec34 130 //HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Reckstyle 37:3d14df6dec34 131 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 37:3d14df6dec34 132 }
Reckstyle 37:3d14df6dec34 133
Reckstyle 37:3d14df6dec34 134
Reckstyle 37:3d14df6dec34 135
Reckstyle 37:3d14df6dec34 136 void turnRight () {
Reckstyle 37:3d14df6dec34 137
Reckstyle 37:3d14df6dec34 138 ledMode = 0; // put on LED to know we're in this state
Reckstyle 37:3d14df6dec34 139 motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
Reckstyle 37:3d14df6dec34 140 wait(1);
Reckstyle 37:3d14df6dec34 141 motors.setSteeringMode(PIVOT_CW); //pivot for the turn
Reckstyle 37:3d14df6dec34 142 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 143 wait (1);
Reckstyle 37:3d14df6dec34 144 int exit = 0; //used for exiting the function
Reckstyle 37:3d14df6dec34 145 motors.setSteeringMode (NORMAL);
Reckstyle 37:3d14df6dec34 146 while (1) {
Reckstyle 37:3d14df6dec34 147 motors.setSpeed(0.0); //stop and check if the line is reached
Reckstyle 37:3d14df6dec34 148 exit = 0;
Reckstyle 37:3d14df6dec34 149 wait (0.4);
Reckstyle 37:3d14df6dec34 150 for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
Reckstyle 37:3d14df6dec34 151 if (measure(sensorArray[i]) == 0) {//if sensor is black
Reckstyle 37:3d14df6dec34 152 exit ++;
Reckstyle 37:3d14df6dec34 153 }
Reckstyle 37:3d14df6dec34 154 if (exit > 2){ // if two sensors are black you're on the line
Reckstyle 37:3d14df6dec34 155 motors.setSteeringMode(PIVOT_CW); //rotate back to realign
Reckstyle 37:3d14df6dec34 156 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 157 wait (0.3);
Reckstyle 37:3d14df6dec34 158 motors.setSteeringMode(NORMAL); //go back to normal
Reckstyle 37:3d14df6dec34 159 while (1) {
Reckstyle 37:3d14df6dec34 160 int checkSumLocal =0;
Reckstyle 37:3d14df6dec34 161 for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
Reckstyle 37:3d14df6dec34 162 if (measure(sensorArray[i]) == 0)
Reckstyle 37:3d14df6dec34 163 checkSumLocal += i*i;
Reckstyle 37:3d14df6dec34 164 }
Reckstyle 37:3d14df6dec34 165 if (checkSumLocal == 74) {
Reckstyle 37:3d14df6dec34 166 ledMode = 1; // turn off the LED to signify going out of this state
Reckstyle 37:3d14df6dec34 167 return;
Reckstyle 37:3d14df6dec34 168 }
Reckstyle 37:3d14df6dec34 169 else if (checkSumLocal < 74) {
Reckstyle 37:3d14df6dec34 170 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 171 motors.setRightSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 172 wait(0.2);
Reckstyle 37:3d14df6dec34 173 }
Reckstyle 37:3d14df6dec34 174 else if (checkSumLocal > 74) {
Reckstyle 37:3d14df6dec34 175 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 176 motors.setLeftSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 177 wait(0.2);
Reckstyle 37:3d14df6dec34 178 }
Reckstyle 37:3d14df6dec34 179 }
Reckstyle 37:3d14df6dec34 180
Reckstyle 37:3d14df6dec34 181 }
Reckstyle 37:3d14df6dec34 182
Reckstyle 37:3d14df6dec34 183 }
Reckstyle 37:3d14df6dec34 184 motors.setRightSpeed (normalSpeed*1.7); //increase of the motors to bank left
Reckstyle 37:3d14df6dec34 185 motors.setLeftSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 186 }
Reckstyle 37:3d14df6dec34 187
Reckstyle 37:3d14df6dec34 188 }
Reckstyle 37:3d14df6dec34 189
Reckstyle 37:3d14df6dec34 190 void turnLeft () {
Reckstyle 37:3d14df6dec34 191
Reckstyle 37:3d14df6dec34 192 ledMode = 0; // put on LED to know we're in this state
Reckstyle 37:3d14df6dec34 193 motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
Reckstyle 37:3d14df6dec34 194 wait(1.1);
Reckstyle 37:3d14df6dec34 195 motors.setSteeringMode(PIVOT_CCW);
Reckstyle 37:3d14df6dec34 196 motors.setSpeed (normalSpeed); //turn only the relevant wheel
Reckstyle 37:3d14df6dec34 197 wait (1);
Reckstyle 37:3d14df6dec34 198 int exit = 0; //used for exiting the function
Reckstyle 37:3d14df6dec34 199 motors.setSteeringMode (NORMAL);
Reckstyle 37:3d14df6dec34 200 while (1) {
Reckstyle 37:3d14df6dec34 201 motors.setSpeed(0.0); //stop and check if the line is reached
Reckstyle 37:3d14df6dec34 202 exit = 0;
Reckstyle 37:3d14df6dec34 203 for (int i = 0; i < 4; i++) {
Reckstyle 37:3d14df6dec34 204 if (measure(sensorArray[i]) == 0) {//if sensor is black
Reckstyle 37:3d14df6dec34 205 exit ++;
Reckstyle 37:3d14df6dec34 206 }
Reckstyle 37:3d14df6dec34 207 if (exit > 2){ // two sensors are considered on line
Reckstyle 37:3d14df6dec34 208 motors.setSteeringMode(PIVOT_CCW); //pivot
Reckstyle 37:3d14df6dec34 209 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 210 wait (0.3);
Reckstyle 37:3d14df6dec34 211 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 212 motors.setSteeringMode(NORMAL);
Reckstyle 37:3d14df6dec34 213 while (1) {
Reckstyle 37:3d14df6dec34 214 int checkSumLocal =0;
Reckstyle 37:3d14df6dec34 215 for (int i = 0; i < 4; i++) {
Reckstyle 37:3d14df6dec34 216 if (measure(sensorArray[i]) == 1)
Reckstyle 37:3d14df6dec34 217 checkSumLocal += (i+1)*(i+1);
Reckstyle 37:3d14df6dec34 218 }
Reckstyle 37:3d14df6dec34 219 if (checkSumLocal == 20) {
Reckstyle 37:3d14df6dec34 220 led = 0;
Reckstyle 37:3d14df6dec34 221 ledMode = 1; // turn off the LED
Reckstyle 37:3d14df6dec34 222 return;
Reckstyle 37:3d14df6dec34 223 }
Reckstyle 37:3d14df6dec34 224 else if (checkSumLocal > 20) {
Reckstyle 37:3d14df6dec34 225 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 226 motors.setRightSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 227 wait(0.2);
Reckstyle 37:3d14df6dec34 228 }
Reckstyle 37:3d14df6dec34 229 else if (checkSumLocal < 20) {
Reckstyle 37:3d14df6dec34 230 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 231 motors.setLeftSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 232 wait(0.2);
Reckstyle 37:3d14df6dec34 233 }
Reckstyle 37:3d14df6dec34 234
Reckstyle 37:3d14df6dec34 235 }
Reckstyle 37:3d14df6dec34 236
Reckstyle 37:3d14df6dec34 237 }
Reckstyle 37:3d14df6dec34 238
Reckstyle 37:3d14df6dec34 239 }
Reckstyle 37:3d14df6dec34 240 motors.setRightSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 241 motors.setLeftSpeed (normalSpeed*1.5);
Reckstyle 37:3d14df6dec34 242 wait(0.2);
Reckstyle 37:3d14df6dec34 243 }
Reckstyle 37:3d14df6dec34 244 }
Reckstyle 37:3d14df6dec34 245
Reckstyle 37:3d14df6dec34 246 void shootFind () { //needs to be implemeted
Reckstyle 37:3d14df6dec34 247 }
Reckstyle 37:3d14df6dec34 248
Reckstyle 37:3d14df6dec34 249 void measureSensors () {
Reckstyle 37:3d14df6dec34 250 sensorsCheckSum = 0; //zero it when first going into the routine
Reckstyle 37:3d14df6dec34 251 int iterationNumber = NUMBER_SENSORS_REGULAR;
Reckstyle 37:3d14df6dec34 252 if (driveMode == SQUARE) {
Reckstyle 37:3d14df6dec34 253 iterationNumber += NUMBER_SENSORS_SQUARE;
Reckstyle 37:3d14df6dec34 254 }
Reckstyle 37:3d14df6dec34 255 for (int i = 0; i < iterationNumber;i++){
Reckstyle 37:3d14df6dec34 256 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 37:3d14df6dec34 257 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 37:3d14df6dec34 258 }
Reckstyle 37:3d14df6dec34 259 }
Reckstyle 37:3d14df6dec34 260 if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
Reckstyle 37:3d14df6dec34 261 oldValues[cursor] = sensorsCheckSum;
Reckstyle 37:3d14df6dec34 262 cursor = (cursor+1)%5;
Reckstyle 37:3d14df6dec34 263 }
Reckstyle 37:3d14df6dec34 264 }