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@37:3d14df6dec34, 2015-03-26 (annotated)

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

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 36:a48d57d63630 21 typedef enum mode {REGULAR,SQUARE} mode; //enumeration for different states
Reckstyle 28:d1e7daeb240e 22 DigitalOut led(LED_RED);
Reckstyle 36:a48d57d63630 23 DigitalOut ledMode(LED_GREEN);
Bartas 24:ecc3fbaf2824 24 Serial HC06(PTE0,PTE1);
Reckstyle 9:718987b106a8 25
Reckstyle 35:1819c5a8254a 26 void turnLeft ();
Reckstyle 35:1819c5a8254a 27 void turnRight ();
Reckstyle 35:1819c5a8254a 28 void measureSensors ();
Reckstyle 35:1819c5a8254a 29 void printBluetooth();
Reckstyle 37:3d14df6dec34 30 void shootFind ();
Reckstyle 35:1819c5a8254a 31
Reckstyle 35:1819c5a8254a 32
Reckstyle 18:d277614084bc 33 Timer measureTimer; //Timer used for measurement
Reckstyle 29:307b45a52401 34 Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
Reckstyle 9:718987b106a8 35
Reckstyle 36:a48d57d63630 36
Reckstyle 11:9e56d52485d1 37 mode driveMode; //declaring the variable for the states
Reckstyle 11:9e56d52485d1 38 int sensorsCheckSum; //varibale used for sensors mapping access
Reckstyle 18:d277614084bc 39 int passedTime1,passedTime2;
Reckstyle 25:bec5dc4c9f5e 40 int oldValues[5] = {0};
Bartas 27:fc0fd2c0eebb 41 int cursor = 0, k = 0;
Reckstyle 36:a48d57d63630 42 static float normalSpeed = 0.15f;
Reckstyle 37:3d14df6dec34 43 int countRightTurns = 0;
Reckstyle 37:3d14df6dec34 44 int countLeftTurns = 0;
Reckstyle 11:9e56d52485d1 45
Reckstyle 11:9e56d52485d1 46
Reckstyle 35:1819c5a8254a 47 int main() {
Reckstyle 35:1819c5a8254a 48
Reckstyle 35:1819c5a8254a 49 //setup_counter(1000, 0);
Reckstyle 35:1819c5a8254a 50 // float frequency = measure_frequency(CHANNEL_1);
Reckstyle 37:3d14df6dec34 51 led = 1;//start LED with beginning of main program
Reckstyle 35:1819c5a8254a 52
Reckstyle 35:1819c5a8254a 53 measureTimer.start();
Reckstyle 35:1819c5a8254a 54 driveMode = REGULAR; //initialise drivemoder
Reckstyle 35:1819c5a8254a 55 sensor_initialise(); // initialise sensor values
Reckstyle 35:1819c5a8254a 56 wait(1); //give time to set up the system
Reckstyle 35:1819c5a8254a 57
Reckstyle 35:1819c5a8254a 58 sensorTimer.start(); // start timer for sensors
Reckstyle 35:1819c5a8254a 59
Reckstyle 37:3d14df6dec34 60 squareOut.period(0.01f); // 0.01 second period
Reckstyle 37:3d14df6dec34 61 squareOut.write(0.5); // 50% duty cycle
Reckstyle 37:3d14df6dec34 62 // for (int b = 1; b <7;b++) {
Reckstyle 37:3d14df6dec34 63 // shoot(0.4);
Reckstyle 37:3d14df6dec34 64 // }
Reckstyle 37:3d14df6dec34 65 shoot (0);
Reckstyle 37:3d14df6dec34 66 // motors.setSteeringMode (NORMAL);
Reckstyle 37:3d14df6dec34 67 // motors.disableHardBraking();
Reckstyle 37:3d14df6dec34 68 // motors.forward ();
Reckstyle 37:3d14df6dec34 69 //
Reckstyle 37:3d14df6dec34 70 // motors.start ();
Reckstyle 37:3d14df6dec34 71 //
Reckstyle 37:3d14df6dec34 72 // motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 73
Reckstyle 35:1819c5a8254a 74 //motors.enableSlew();
Reckstyle 37:3d14df6dec34 75
Reckstyle 35:1819c5a8254a 76 // HC06.printf("heello");
Reckstyle 36:a48d57d63630 77 int testOtherCases = 0; //used for control
Reckstyle 35:1819c5a8254a 78 while(1){
Reckstyle 37:3d14df6dec34 79
Reckstyle 37:3d14df6dec34 80 // printBluetooth();
Reckstyle 36:a48d57d63630 81 // while (HC06.getc() != 'r') {
Reckstyle 35:1819c5a8254a 82 // measureSensors();
Reckstyle 35:1819c5a8254a 83 // printBluetooth();
Reckstyle 36:a48d57d63630 84 // wait(1);
Reckstyle 35:1819c5a8254a 85 // }
Reckstyle 35:1819c5a8254a 86
Reckstyle 37:3d14df6dec34 87
Reckstyle 37:3d14df6dec34 88
Reckstyle 37:3d14df6dec34 89 measureSensors();
Reckstyle 37:3d14df6dec34 90 printBluetooth();
Reckstyle 35:1819c5a8254a 91 switch (sensorsCheckSum) {
Reckstyle 37:3d14df6dec34 92
Reckstyle 37:3d14df6dec34 93
Reckstyle 35:1819c5a8254a 94 case 94:
Reckstyle 36:a48d57d63630 95 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 96 wait(0.15);
Reckstyle 35:1819c5a8254a 97 break;
Reckstyle 35:1819c5a8254a 98
Reckstyle 35:1819c5a8254a 99 case 104:
Reckstyle 37:3d14df6dec34 100 if ( countRightTurns == 1)
Reckstyle 37:3d14df6dec34 101 turnRight();
Reckstyle 37:3d14df6dec34 102 else {
Reckstyle 37:3d14df6dec34 103 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 104 wait(0.15);
Reckstyle 37:3d14df6dec34 105 measureSensors();
Reckstyle 37:3d14df6dec34 106
Reckstyle 37:3d14df6dec34 107 if (sensorsCheckSum == 104) { //confirm the turn
Reckstyle 37:3d14df6dec34 108
Reckstyle 37:3d14df6dec34 109 wait(0.15);
Reckstyle 37:3d14df6dec34 110 measureSensors();
Reckstyle 37:3d14df6dec34 111 if (sensorsCheckSum == 104)
Reckstyle 37:3d14df6dec34 112 turnRight ();
Reckstyle 37:3d14df6dec34 113 }
Reckstyle 37:3d14df6dec34 114 }
Reckstyle 36:a48d57d63630 115
Reckstyle 36:a48d57d63630 116 break;
Reckstyle 37:3d14df6dec34 117 case 158:
Reckstyle 37:3d14df6dec34 118 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 119 wait(0.15);
Reckstyle 37:3d14df6dec34 120 measureSensors();
Reckstyle 37:3d14df6dec34 121
Reckstyle 37:3d14df6dec34 122 if (sensorsCheckSum == 194 || sensorsCheckSum == 158) { //confirm the turn
Reckstyle 37:3d14df6dec34 123
Reckstyle 37:3d14df6dec34 124 turnLeft();
Reckstyle 37:3d14df6dec34 125 }
Reckstyle 37:3d14df6dec34 126 break;
Reckstyle 36:a48d57d63630 127 case 168:
Reckstyle 37:3d14df6dec34 128 if ( countRightTurns == 1)
Reckstyle 37:3d14df6dec34 129 turnRight();
Reckstyle 37:3d14df6dec34 130 else {
Reckstyle 37:3d14df6dec34 131 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 132 wait(0.15);
Reckstyle 37:3d14df6dec34 133 measureSensors();
Reckstyle 36:a48d57d63630 134
Reckstyle 37:3d14df6dec34 135 if (sensorsCheckSum == 168) { //confirm the turn
Reckstyle 37:3d14df6dec34 136
Reckstyle 37:3d14df6dec34 137 wait(0.15);
Reckstyle 37:3d14df6dec34 138 measureSensors();
Reckstyle 37:3d14df6dec34 139 if (sensorsCheckSum == 168)
Reckstyle 37:3d14df6dec34 140 turnRight ();
Reckstyle 37:3d14df6dec34 141 }
Reckstyle 36:a48d57d63630 142 }
Reckstyle 35:1819c5a8254a 143 break;
Reckstyle 37:3d14df6dec34 144 case 194:
Reckstyle 37:3d14df6dec34 145 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 146 wait(0.15);
Reckstyle 37:3d14df6dec34 147 measureSensors();
Reckstyle 37:3d14df6dec34 148
Reckstyle 37:3d14df6dec34 149 if (sensorsCheckSum == 194) { //confirm the turn
Reckstyle 37:3d14df6dec34 150
Reckstyle 37:3d14df6dec34 151
Reckstyle 37:3d14df6dec34 152 turnLeft();
Reckstyle 37:3d14df6dec34 153 }
Reckstyle 37:3d14df6dec34 154 break;
Reckstyle 35:1819c5a8254a 155 default:
Reckstyle 35:1819c5a8254a 156 testOtherCases =1;
Reckstyle 35:1819c5a8254a 157 break;
Reckstyle 35:1819c5a8254a 158 }
Reckstyle 35:1819c5a8254a 159 if (testOtherCases == 1) {
Reckstyle 37:3d14df6dec34 160
Reckstyle 35:1819c5a8254a 161 if (sensorsCheckSum < 96) {
Reckstyle 35:1819c5a8254a 162 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 163 motors.setLeftSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 164 wait(0.075);
Reckstyle 35:1819c5a8254a 165 }
Reckstyle 35:1819c5a8254a 166 else if (sensorsCheckSum > 96) {
Reckstyle 35:1819c5a8254a 167 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 168 motors.setRightSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 169 wait(0.1);
Reckstyle 35:1819c5a8254a 170 }
Reckstyle 35:1819c5a8254a 171
Reckstyle 37:3d14df6dec34 172 }
Reckstyle 37:3d14df6dec34 173
Reckstyle 35:1819c5a8254a 174 motors.setSpeed(0.0);
Reckstyle 35:1819c5a8254a 175 wait(0.2);
Reckstyle 35:1819c5a8254a 176
Reckstyle 37:3d14df6dec34 177
Reckstyle 35:1819c5a8254a 178 }
Reckstyle 35:1819c5a8254a 179 } //int main
Reckstyle 35:1819c5a8254a 180
Reckstyle 35:1819c5a8254a 181 void printBluetooth() { //for debugging
Reckstyle 35:1819c5a8254a 182 HC06.printf("LLU%i LRU%i rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
Reckstyle 35:1819c5a8254a 183 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 184 //HC06.printf("%i %i %i \n",sensorArray[8]->state,sensorArray[9]->state,sensorArray[3]->state,sensorArray[10]->state);
Reckstyle 35:1819c5a8254a 185 //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 35:1819c5a8254a 186 //HC06.printf("%f %f",motor.getLeftSpeed(),motor.getRightSpeed());
Reckstyle 35:1819c5a8254a 187 HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
Reckstyle 35:1819c5a8254a 188 //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
Reckstyle 35:1819c5a8254a 189 }
Reckstyle 35:1819c5a8254a 190
Reckstyle 36:a48d57d63630 191
Reckstyle 36:a48d57d63630 192
Reckstyle 35:1819c5a8254a 193 void turnRight () {
Reckstyle 37:3d14df6dec34 194
Reckstyle 36:a48d57d63630 195 ledMode = 0; // put on LED to know we're in this state
Reckstyle 37:3d14df6dec34 196 if (countRightTurns == 0 ) {
Reckstyle 37:3d14df6dec34 197 motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
Reckstyle 37:3d14df6dec34 198 wait(1);
Reckstyle 37:3d14df6dec34 199 motors.setSteeringMode(PIVOT_CCW);
Reckstyle 37:3d14df6dec34 200 motors.setSpeed (normalSpeed); //turn only the relevant wheel
Reckstyle 37:3d14df6dec34 201 wait (1);
Reckstyle 37:3d14df6dec34 202 }
Reckstyle 37:3d14df6dec34 203 else {
Reckstyle 37:3d14df6dec34 204 motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
Reckstyle 37:3d14df6dec34 205 wait(1);
Reckstyle 37:3d14df6dec34 206 motors.setSteeringMode(PIVOT_CCW);
Reckstyle 37:3d14df6dec34 207 motors.setSpeed (normalSpeed); //turn only the relevant wheel
Reckstyle 37:3d14df6dec34 208 wait (0.8);
Reckstyle 37:3d14df6dec34 209 }
Reckstyle 37:3d14df6dec34 210
Reckstyle 37:3d14df6dec34 211 int exit = 0; //used for exiting the function
Reckstyle 35:1819c5a8254a 212 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 213 wait(0.5);
Reckstyle 37:3d14df6dec34 214 motors.setSteeringMode (NORMAL);
Reckstyle 37:3d14df6dec34 215 wait(0.3);
Reckstyle 37:3d14df6dec34 216 while (1) {
Reckstyle 36:a48d57d63630 217 motors.setSpeed(0.0); //stop and check if the line is reached
Reckstyle 36:a48d57d63630 218 exit = 0;
Reckstyle 37:3d14df6dec34 219 wait (0.4);
Reckstyle 37:3d14df6dec34 220 for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
Reckstyle 37:3d14df6dec34 221
Reckstyle 37:3d14df6dec34 222 if (measure(sensorArray[i]) == 0) {//if sensor is black
Reckstyle 37:3d14df6dec34 223 exit ++;
Reckstyle 37:3d14df6dec34 224 }
Reckstyle 37:3d14df6dec34 225 if (exit > 2){ // two sensors are considered on line
Reckstyle 37:3d14df6dec34 226 motors.setSteeringMode(PIVOT_CCW);
Reckstyle 37:3d14df6dec34 227 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 228 wait (0.3);
Reckstyle 37:3d14df6dec34 229 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 230 motors.setSteeringMode(NORMAL);
Reckstyle 37:3d14df6dec34 231 while (1) {
Reckstyle 37:3d14df6dec34 232 int checkSumLocal =0;
Reckstyle 37:3d14df6dec34 233 for (int i = 4; i < NUMBER_SENSORS_REGULAR; i++) {
Reckstyle 37:3d14df6dec34 234
Reckstyle 37:3d14df6dec34 235 if (measure(sensorArray[i]) == 0)
Reckstyle 37:3d14df6dec34 236 checkSumLocal += i*i;
Reckstyle 37:3d14df6dec34 237 }
Reckstyle 37:3d14df6dec34 238 printBluetooth();
Reckstyle 37:3d14df6dec34 239 if (checkSumLocal == 74) {
Reckstyle 37:3d14df6dec34 240 led = 0;
Reckstyle 37:3d14df6dec34 241 ledMode = 1; // turn off the LED
Reckstyle 37:3d14df6dec34 242 countRightTurns ++;
Reckstyle 37:3d14df6dec34 243 wait(1);
Reckstyle 37:3d14df6dec34 244 return;
Reckstyle 37:3d14df6dec34 245 }
Reckstyle 37:3d14df6dec34 246 if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
Reckstyle 37:3d14df6dec34 247 motors.setSteeringMode(PIVOT_CCW);
Reckstyle 37:3d14df6dec34 248 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 249 wait (0.6);
Reckstyle 37:3d14df6dec34 250 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 251 motors.setSteeringMode(NORMAL);
Reckstyle 37:3d14df6dec34 252
Reckstyle 37:3d14df6dec34 253 }
Reckstyle 37:3d14df6dec34 254 else if (checkSumLocal < 74) {
Reckstyle 37:3d14df6dec34 255 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 256 motors.setRightSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 257 wait(0.2);
Reckstyle 37:3d14df6dec34 258 }
Reckstyle 37:3d14df6dec34 259 else if (checkSumLocal > 74) {
Reckstyle 37:3d14df6dec34 260 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 261 motors.setLeftSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 262 wait(0.2);
Reckstyle 37:3d14df6dec34 263 }
Reckstyle 37:3d14df6dec34 264 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 265 wait(0.4);
Reckstyle 37:3d14df6dec34 266 }
Reckstyle 37:3d14df6dec34 267
Reckstyle 37:3d14df6dec34 268 }
Reckstyle 37:3d14df6dec34 269
Reckstyle 35:1819c5a8254a 270 }
Reckstyle 37:3d14df6dec34 271 // motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 272 motors.setRightSpeed (normalSpeed*1.7);
Reckstyle 37:3d14df6dec34 273 motors.setLeftSpeed (normalSpeed);
Reckstyle 36:a48d57d63630 274 wait(0.2);
Reckstyle 35:1819c5a8254a 275 }
Reckstyle 35:1819c5a8254a 276
Reckstyle 35:1819c5a8254a 277 }
Reckstyle 35:1819c5a8254a 278
Reckstyle 37:3d14df6dec34 279 void turnLeft () {
Reckstyle 37:3d14df6dec34 280
Reckstyle 37:3d14df6dec34 281 ledMode = 0; // put on LED to know we're in this state
Reckstyle 37:3d14df6dec34 282 motors.setSpeed (normalSpeed); //go a bit to the front to compensate for sensors arms
Reckstyle 37:3d14df6dec34 283 wait(1.1);
Reckstyle 37:3d14df6dec34 284 motors.setSteeringMode(PIVOT_CW);
Reckstyle 37:3d14df6dec34 285 motors.setSpeed (normalSpeed); //turn only the relevant wheel
Reckstyle 37:3d14df6dec34 286 wait (1);
Reckstyle 37:3d14df6dec34 287 int exit = 0; //used for exiting the function
Reckstyle 37:3d14df6dec34 288 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 289 wait(0.5);
Reckstyle 37:3d14df6dec34 290 motors.setSteeringMode (NORMAL);
Reckstyle 37:3d14df6dec34 291 wait(0.3);
Reckstyle 37:3d14df6dec34 292 while (1) {
Reckstyle 37:3d14df6dec34 293 motors.setSpeed(0.0); //stop and check if the line is reached
Reckstyle 37:3d14df6dec34 294 exit = 0;
Reckstyle 37:3d14df6dec34 295 wait (0.4);
Reckstyle 37:3d14df6dec34 296 for (int i = 0; i < 4; i++) {
Reckstyle 37:3d14df6dec34 297
Reckstyle 37:3d14df6dec34 298 if (measure(sensorArray[i]) == 0) {//if sensor is black
Reckstyle 37:3d14df6dec34 299 exit ++;
Reckstyle 37:3d14df6dec34 300 }
Reckstyle 37:3d14df6dec34 301 if (exit > 2){ // two sensors are considered on line
Reckstyle 37:3d14df6dec34 302 motors.setSteeringMode(PIVOT_CW);
Reckstyle 37:3d14df6dec34 303 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 304 wait (0.3);
Reckstyle 37:3d14df6dec34 305 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 306 motors.setSteeringMode(NORMAL);
Reckstyle 37:3d14df6dec34 307 while (1) {
Reckstyle 37:3d14df6dec34 308 int checkSumLocal =0;
Reckstyle 37:3d14df6dec34 309 for (int i = 0; i < 4; i++) {
Reckstyle 37:3d14df6dec34 310
Reckstyle 37:3d14df6dec34 311 if (measure(sensorArray[i]) == 1)
Reckstyle 37:3d14df6dec34 312 checkSumLocal += (i+1)*(i+1);
Reckstyle 37:3d14df6dec34 313 }
Reckstyle 37:3d14df6dec34 314 HC06.printf ("checkSumLocal is %i \n", checkSumLocal);
Reckstyle 37:3d14df6dec34 315 if (checkSumLocal == 20) {
Reckstyle 37:3d14df6dec34 316 led = 0;
Reckstyle 37:3d14df6dec34 317 ledMode = 1; // turn off the LED
Reckstyle 37:3d14df6dec34 318 countLeftTurns ++;
Reckstyle 37:3d14df6dec34 319 wait(1);
Reckstyle 37:3d14df6dec34 320 if (countLeftTurns == 1) {
Reckstyle 37:3d14df6dec34 321 //shootFind ();
Reckstyle 37:3d14df6dec34 322 }
Reckstyle 37:3d14df6dec34 323 return;
Reckstyle 37:3d14df6dec34 324 }
Reckstyle 37:3d14df6dec34 325 if (checkSumLocal == 64 || checkSumLocal ==36 || checkSumLocal ==100 || checkSumLocal == 125 || checkSumLocal == 149) {
Reckstyle 37:3d14df6dec34 326 motors.setSteeringMode(PIVOT_CCW);
Reckstyle 37:3d14df6dec34 327 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 328 wait (0.6);
Reckstyle 37:3d14df6dec34 329 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 330 motors.setSteeringMode(NORMAL);
Reckstyle 37:3d14df6dec34 331
Reckstyle 37:3d14df6dec34 332 }
Reckstyle 37:3d14df6dec34 333 else if (checkSumLocal > 20) {
Reckstyle 37:3d14df6dec34 334 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 335 motors.setRightSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 336 wait(0.2);
Reckstyle 37:3d14df6dec34 337 }
Reckstyle 37:3d14df6dec34 338 else if (checkSumLocal < 20) {
Reckstyle 37:3d14df6dec34 339 motors.setSpeed (0.0);
Reckstyle 37:3d14df6dec34 340 motors.setLeftSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 341 wait(0.2);
Reckstyle 37:3d14df6dec34 342 }
Reckstyle 37:3d14df6dec34 343 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 344 wait(0.4);
Reckstyle 37:3d14df6dec34 345 }
Reckstyle 37:3d14df6dec34 346
Reckstyle 37:3d14df6dec34 347 }
Reckstyle 37:3d14df6dec34 348
Reckstyle 37:3d14df6dec34 349 }
Reckstyle 37:3d14df6dec34 350 // motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 351 motors.setRightSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 352 motors.setLeftSpeed (normalSpeed*1.5);
Reckstyle 37:3d14df6dec34 353 wait(0.2);
Reckstyle 37:3d14df6dec34 354 }
Reckstyle 37:3d14df6dec34 355 }
Reckstyle 37:3d14df6dec34 356
Reckstyle 37:3d14df6dec34 357 void shootFind () {
Reckstyle 37:3d14df6dec34 358 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 359 wait (1);
Reckstyle 37:3d14df6dec34 360 motors.setSteeringMode (PIVOT_CW);
Reckstyle 37:3d14df6dec34 361 wait (0.8);
Reckstyle 37:3d14df6dec34 362 motors.setSteeringMode (NORMAL);
Reckstyle 37:3d14df6dec34 363
Reckstyle 37:3d14df6dec34 364 while (1) {
Reckstyle 37:3d14df6dec34 365 motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 366 wait (0.3);
Reckstyle 37:3d14df6dec34 367 for (int i = 8; i < 11 ; i++) {
Reckstyle 37:3d14df6dec34 368 if (measure(sensorArray[i]) == 2){
Reckstyle 37:3d14df6dec34 369 motors.setSteeringMode (PIVOT_CW);
Reckstyle 37:3d14df6dec34 370 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 371 wait (1.6);
Reckstyle 37:3d14df6dec34 372 motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 373 for (int b = 1; b <7;b++) {
Reckstyle 37:3d14df6dec34 374 shoot(0.1*(float)b);
Reckstyle 37:3d14df6dec34 375 }
Reckstyle 37:3d14df6dec34 376 shoot (0);
Reckstyle 37:3d14df6dec34 377 motors.setSteeringMode(NORMAL);
Reckstyle 37:3d14df6dec34 378 // while (1) {
Reckstyle 37:3d14df6dec34 379 // motors.setSpeed(0.0);
Reckstyle 37:3d14df6dec34 380 // wait (0.4);
Reckstyle 37:3d14df6dec34 381 // for (int c =4 ;c <8;c++) {
Reckstyle 37:3d14df6dec34 382 // if( measure(sensorArray[i]) == 0){
Reckstyle 37:3d14df6dec34 383 // return;
Reckstyle 37:3d14df6dec34 384 // }
Reckstyle 37:3d14df6dec34 385 // }
Reckstyle 37:3d14df6dec34 386 // motors.setSpeed(normalSpeed);
Reckstyle 37:3d14df6dec34 387 // motors.setLeftSpeed(normalSpeed*1.3);
Reckstyle 37:3d14df6dec34 388 // wait(0.2);
Reckstyle 37:3d14df6dec34 389 // }
Reckstyle 37:3d14df6dec34 390 return;
Reckstyle 37:3d14df6dec34 391 }
Reckstyle 37:3d14df6dec34 392
Reckstyle 37:3d14df6dec34 393 }
Reckstyle 37:3d14df6dec34 394 motors.setSpeed (normalSpeed);
Reckstyle 37:3d14df6dec34 395 wait (0.5);
Reckstyle 37:3d14df6dec34 396
Reckstyle 37:3d14df6dec34 397 }
Reckstyle 36:a48d57d63630 398 }
Reckstyle 36:a48d57d63630 399
Reckstyle 11:9e56d52485d1 400 void measureSensors () {
Bartas 24:ecc3fbaf2824 401 sensorsCheckSum = 0; //zero it when first going into the routine
Bartas 24:ecc3fbaf2824 402 int iterationNumber = NUMBER_SENSORS_REGULAR;
Bartas 24:ecc3fbaf2824 403 if (driveMode == SQUARE) {
orsharp 12:bb21b76b6375 404 iterationNumber += NUMBER_SENSORS_SQUARE;
Bartas 24:ecc3fbaf2824 405 }
Bartas 24:ecc3fbaf2824 406 for (int i = 0; i < iterationNumber;i++){
Bartas 24:ecc3fbaf2824 407 //pc.printf("%i iteration%i ",i,iterationNumber);
Bartas 24:ecc3fbaf2824 408 if (measure(sensorArray[i]) == 1) {//if sensor is white
Reckstyle 11:9e56d52485d1 409 sensorsCheckSum += (i+1)*(i+1);
Reckstyle 11:9e56d52485d1 410 }
Bartas 24:ecc3fbaf2824 411 }
Reckstyle 25:bec5dc4c9f5e 412 if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
Reckstyle 25:bec5dc4c9f5e 413 oldValues[cursor] = sensorsCheckSum;
Reckstyle 23:9b53c72fcd38 414 cursor = (cursor+1)%5;
Bartas 24:ecc3fbaf2824 415 }
Bartas 24:ecc3fbaf2824 416 //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
Reckstyle 11:9e56d52485d1 417 }