MEGACARAZ / Mbed 2 deprecated Turning

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   btbee m3pi_ng mbed

main.cpp@17:4e63e8eacad7, 2015-06-08 (annotated)

Committer:
morganrose402
Date:
Mon Jun 08 11:27:29 2015 +0000
Revision:
17:4e63e8eacad7
Parent:
16:ee3b36038b6b
Child:
18:acc404d8488c
mario kart and timer;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
crmackey 0:f4302186f4cb 1 #include "mbed.h"
crmackey 0:f4302186f4cb 2 #include "m3pi_ng.h"
crmackey 0:f4302186f4cb 3 #include "time.h"
crmackey 0:f4302186f4cb 4 #include "btbee.h"
crmackey 5:0c2da538ada2 5 #include <vector>
crmackey 5:0c2da538ada2 6 #include <string>
crmackey 0:f4302186f4cb 7
crmackey 0:f4302186f4cb 8 m3pi m3pi;
crmackey 0:f4302186f4cb 9 btbee btbee;
crmackey 0:f4302186f4cb 10 DigitalIn m3pi_pb(p21);
crmackey 0:f4302186f4cb 11 DigitalIn m3pi_IN[] = {(p12)};
crmackey 0:f4302186f4cb 12 DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
crmackey 0:f4302186f4cb 13 DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
crmackey 0:f4302186f4cb 14
crmackey 0:f4302186f4cb 15 // Minimum and maximum motor speeds
crmackey 0:f4302186f4cb 16 #define MAX 0.5
crmackey 0:f4302186f4cb 17 #define MIN 0
crmackey 0:f4302186f4cb 18
crmackey 0:f4302186f4cb 19 // PID terms
crmackey 0:f4302186f4cb 20 #define P_TERM 1.5
crmackey 0:f4302186f4cb 21 #define I_TERM 0
crmackey 0:f4302186f4cb 22 #define D_TERM 25
crmackey 0:f4302186f4cb 23
crmackey 0:f4302186f4cb 24
crmackey 0:f4302186f4cb 25 float right; // initializes variables
crmackey 0:f4302186f4cb 26 float left;
crmackey 0:f4302186f4cb 27 float current_pos_of_line = 0.0;
crmackey 0:f4302186f4cb 28 float previous_pos_of_line = 0.0;
crmackey 0:f4302186f4cb 29 float derivative,proportional,integral = 0;
crmackey 0:f4302186f4cb 30 float power;
crmackey 0:f4302186f4cb 31 float speed = MAX;
crmackey 0:f4302186f4cb 32 int LapTest[5];
crmackey 4:a0baf6752548 33 float s1, s2, s3, s4, s5;
crmackey 0:f4302186f4cb 34 Timer TurnTime;
crmackey 3:711ac018d3ec 35 Timer LapTime;
morganrose402 17:4e63e8eacad7 36 Timer TotalTime;
crmackey 4:a0baf6752548 37 int counter = 0;
crmackey 5:0c2da538ada2 38 string path;
crmackey 5:0c2da538ada2 39 char dir;
crmackey 14:ca6283fecd05 40 int location;
crmackey 14:ca6283fecd05 41 int goal = 0;
crmackey 15:065889f0799b 42 vector < int > command;
crmackey 15:065889f0799b 43 int i = 0;
crmackey 15:065889f0799b 44 int orders;
crmackey 5:0c2da538ada2 45
crmackey 14:ca6283fecd05 46 vector < string > place(6, "");
crmackey 14:ca6283fecd05 47 vector < vector < string > > rga(6,place);
crmackey 14:ca6283fecd05 48
crmackey 0:f4302186f4cb 49
crmackey 0:f4302186f4cb 50 void RightTurn() {
crmackey 0:f4302186f4cb 51
crmackey 0:f4302186f4cb 52 m3pi.stop();
crmackey 0:f4302186f4cb 53 TurnTime.reset();
crmackey 0:f4302186f4cb 54 TurnTime.start();
crmackey 0:f4302186f4cb 55
crmackey 0:f4302186f4cb 56 while (1) {
crmackey 0:f4302186f4cb 57
crmackey 0:f4302186f4cb 58 m3pi.right_motor(0.45);
crmackey 0:f4302186f4cb 59 m3pi.left_motor(-0.1);
crmackey 0:f4302186f4cb 60
crmackey 0:f4302186f4cb 61 if( TurnTime.read() > 0.2375) {
crmackey 0:f4302186f4cb 62
crmackey 5:0c2da538ada2 63 LapTime.reset();
crmackey 5:0c2da538ada2 64 LapTime.start();
crmackey 5:0c2da538ada2 65
crmackey 0:f4302186f4cb 66 return;
crmackey 0:f4302186f4cb 67
crmackey 0:f4302186f4cb 68 }
crmackey 0:f4302186f4cb 69
crmackey 0:f4302186f4cb 70 }
crmackey 0:f4302186f4cb 71
crmackey 0:f4302186f4cb 72 }
crmackey 0:f4302186f4cb 73
crmackey 0:f4302186f4cb 74
crmackey 0:f4302186f4cb 75 void LeftTurn() {
crmackey 0:f4302186f4cb 76
crmackey 0:f4302186f4cb 77 m3pi.stop();
crmackey 0:f4302186f4cb 78 TurnTime.reset();
crmackey 0:f4302186f4cb 79 TurnTime.start();
crmackey 0:f4302186f4cb 80
crmackey 0:f4302186f4cb 81 while (1) {
crmackey 0:f4302186f4cb 82
crmackey 0:f4302186f4cb 83 m3pi.left_motor(0.45);
crmackey 0:f4302186f4cb 84 m3pi.right_motor(-0.1);
crmackey 0:f4302186f4cb 85
crmackey 0:f4302186f4cb 86 if( TurnTime.read() > 0.2375) {
crmackey 0:f4302186f4cb 87
crmackey 5:0c2da538ada2 88 LapTime.reset();
crmackey 5:0c2da538ada2 89 LapTime.start();
crmackey 5:0c2da538ada2 90
crmackey 0:f4302186f4cb 91 return;
crmackey 0:f4302186f4cb 92
crmackey 0:f4302186f4cb 93 }
crmackey 0:f4302186f4cb 94
crmackey 0:f4302186f4cb 95 }
crmackey 0:f4302186f4cb 96
crmackey 0:f4302186f4cb 97 }
crmackey 5:0c2da538ada2 98
crmackey 5:0c2da538ada2 99 void TurnAround() {
crmackey 5:0c2da538ada2 100
crmackey 5:0c2da538ada2 101 m3pi.stop();
crmackey 14:ca6283fecd05 102 wait(1.0);
crmackey 5:0c2da538ada2 103 TurnTime.reset();
crmackey 5:0c2da538ada2 104 TurnTime.start();
crmackey 5:0c2da538ada2 105
crmackey 5:0c2da538ada2 106 while (1) {
crmackey 5:0c2da538ada2 107
crmackey 14:ca6283fecd05 108 m3pi.left_motor(0.25);
crmackey 14:ca6283fecd05 109 m3pi.right_motor(-0.25);
crmackey 5:0c2da538ada2 110
crmackey 14:ca6283fecd05 111 if( TurnTime.read() > 0.50) {
crmackey 5:0c2da538ada2 112
crmackey 5:0c2da538ada2 113 m3pi.stop();
crmackey 5:0c2da538ada2 114
crmackey 5:0c2da538ada2 115 return;
crmackey 5:0c2da538ada2 116
crmackey 5:0c2da538ada2 117 }
crmackey 5:0c2da538ada2 118 }
crmackey 5:0c2da538ada2 119 }
crmackey 5:0c2da538ada2 120
crmackey 5:0c2da538ada2 121 void GoStraight() {
crmackey 5:0c2da538ada2 122
crmackey 5:0c2da538ada2 123 TurnTime.reset();
crmackey 5:0c2da538ada2 124 TurnTime.start();
crmackey 5:0c2da538ada2 125
crmackey 5:0c2da538ada2 126 while(1) {
crmackey 5:0c2da538ada2 127
crmackey 5:0c2da538ada2 128 m3pi.right_motor(0.5);
crmackey 5:0c2da538ada2 129 m3pi.left_motor(0.5);
crmackey 5:0c2da538ada2 130
crmackey 5:0c2da538ada2 131 if(TurnTime.read() > 0.02) {
crmackey 5:0c2da538ada2 132
crmackey 5:0c2da538ada2 133 LapTime.reset();
crmackey 5:0c2da538ada2 134 LapTime.start();
crmackey 5:0c2da538ada2 135
crmackey 5:0c2da538ada2 136 return;
crmackey 5:0c2da538ada2 137
crmackey 5:0c2da538ada2 138 }
crmackey 5:0c2da538ada2 139 }
crmackey 5:0c2da538ada2 140 }
crmackey 5:0c2da538ada2 141
crmackey 5:0c2da538ada2 142
crmackey 5:0c2da538ada2 143 bool CrossDetect() {
crmackey 5:0c2da538ada2 144
crmackey 5:0c2da538ada2 145 if(s1 > 500 and s2 > 500 and s3 > 500 and s4 > 500 and s5 > 500) {
crmackey 5:0c2da538ada2 146
crmackey 5:0c2da538ada2 147 return(1);
crmackey 5:0c2da538ada2 148
crmackey 5:0c2da538ada2 149 }
crmackey 5:0c2da538ada2 150
crmackey 5:0c2da538ada2 151 else if(s1 > 700 and s2 > 700) {
crmackey 5:0c2da538ada2 152
crmackey 5:0c2da538ada2 153 return(1);
crmackey 5:0c2da538ada2 154
crmackey 5:0c2da538ada2 155 }
crmackey 5:0c2da538ada2 156
crmackey 5:0c2da538ada2 157 else if(s5 > 700 and s4 > 700) {
crmackey 5:0c2da538ada2 158
crmackey 5:0c2da538ada2 159 return(1);
crmackey 5:0c2da538ada2 160
crmackey 5:0c2da538ada2 161 }
crmackey 5:0c2da538ada2 162
crmackey 5:0c2da538ada2 163 else {
crmackey 5:0c2da538ada2 164
crmackey 5:0c2da538ada2 165 return(0);
crmackey 5:0c2da538ada2 166
crmackey 5:0c2da538ada2 167 }
crmackey 5:0c2da538ada2 168
crmackey 14:ca6283fecd05 169 }
crmackey 14:ca6283fecd05 170
crmackey 15:065889f0799b 171 vector < int > GetLocation () {
crmackey 14:ca6283fecd05 172
crmackey 14:ca6283fecd05 173 char arr_read[30];
crmackey 14:ca6283fecd05 174 int chars_read1;
crmackey 15:065889f0799b 175 int stuff;
crmackey 15:065889f0799b 176 char things[1];
crmackey 15:065889f0799b 177 int k = 1;
crmackey 15:065889f0799b 178 btbee.printf("Enter a command string: \n");
crmackey 14:ca6283fecd05 179
crmackey 14:ca6283fecd05 180 while(1) {
crmackey 14:ca6283fecd05 181
crmackey 14:ca6283fecd05 182 if(btbee.readable()) {
crmackey 14:ca6283fecd05 183
crmackey 15:065889f0799b 184 btbee.read_line(arr_read, 30, &chars_read1);
crmackey 15:065889f0799b 185
crmackey 15:065889f0799b 186 for(int j = 0; j < chars_read1-1; j++) {
crmackey 15:065889f0799b 187
crmackey 15:065889f0799b 188 things[0] = arr_read[j];
crmackey 15:065889f0799b 189 stuff = atoi(things);
crmackey 15:065889f0799b 190
crmackey 15:065889f0799b 191 btbee.printf("%d",stuff);
crmackey 15:065889f0799b 192
crmackey 15:065889f0799b 193 if( j != 0 ) {
crmackey 15:065889f0799b 194
crmackey 15:065889f0799b 195 if( stuff == command.at(j-k) ) {
crmackey 15:065889f0799b 196
crmackey 15:065889f0799b 197 k++;
crmackey 15:065889f0799b 198
crmackey 15:065889f0799b 199 }
crmackey 15:065889f0799b 200
crmackey 15:065889f0799b 201 else {
crmackey 15:065889f0799b 202
crmackey 15:065889f0799b 203 command.push_back(stuff);
crmackey 15:065889f0799b 204
crmackey 15:065889f0799b 205 }
crmackey 15:065889f0799b 206
crmackey 15:065889f0799b 207 }
crmackey 15:065889f0799b 208
crmackey 15:065889f0799b 209 else if (j == 0) {
crmackey 15:065889f0799b 210
crmackey 15:065889f0799b 211 command.push_back(stuff);
crmackey 15:065889f0799b 212
crmackey 15:065889f0799b 213 }
crmackey 15:065889f0799b 214
crmackey 15:065889f0799b 215
crmackey 15:065889f0799b 216 }
crmackey 15:065889f0799b 217
crmackey 15:065889f0799b 218 command.push_back(7);
crmackey 15:065889f0799b 219
crmackey 15:065889f0799b 220 return(command);
crmackey 14:ca6283fecd05 221
crmackey 14:ca6283fecd05 222 }
crmackey 14:ca6283fecd05 223
crmackey 14:ca6283fecd05 224 }
crmackey 14:ca6283fecd05 225
crmackey 14:ca6283fecd05 226 }
crmackey 14:ca6283fecd05 227
crmackey 14:ca6283fecd05 228 bool LocTest() {
crmackey 14:ca6283fecd05 229
crmackey 15:065889f0799b 230 if( goal == 7 )
crmackey 15:065889f0799b 231 return 0;
crmackey 15:065889f0799b 232
crmackey 15:065889f0799b 233 if( path.at(0) == 'P' ) {
crmackey 14:ca6283fecd05 234
crmackey 14:ca6283fecd05 235 return 1;
crmackey 14:ca6283fecd05 236
crmackey 14:ca6283fecd05 237 }
crmackey 14:ca6283fecd05 238
crmackey 14:ca6283fecd05 239 else {
crmackey 14:ca6283fecd05 240 return 0;
crmackey 14:ca6283fecd05 241 }
crmackey 14:ca6283fecd05 242
crmackey 14:ca6283fecd05 243 }
crmackey 0:f4302186f4cb 244
crmackey 0:f4302186f4cb 245 int main() {
crmackey 0:f4302186f4cb 246
crmackey 14:ca6283fecd05 247 rga.at(0).at(0) = "P";
crmackey 14:ca6283fecd05 248 rga.at(0).at(1) = "LRLLRX";
crmackey 14:ca6283fecd05 249 rga.at(0).at(2) = "LRLSSX";
crmackey 14:ca6283fecd05 250 rga.at(0).at(3) = "LRLSRLX";
crmackey 14:ca6283fecd05 251 rga.at(0).at(4) = "LRRLSLRRRLX";
crmackey 14:ca6283fecd05 252 rga.at(0).at(5) = "LRRLRX";
crmackey 5:0c2da538ada2 253
crmackey 14:ca6283fecd05 254 rga.at(1).at(0) = "LRRLRX";
crmackey 14:ca6283fecd05 255 rga.at(1).at(1) = "P";
crmackey 5:0c2da538ada2 256 rga.at(1).at(2) = "LLSX";
crmackey 5:0c2da538ada2 257 rga.at(1).at(3) = "LLRLX";
crmackey 5:0c2da538ada2 258 rga.at(1).at(4) = "LSRLRRLX";
crmackey 5:0c2da538ada2 259 rga.at(1).at(5) = "LRSLRX";
crmackey 5:0c2da538ada2 260
crmackey 14:ca6283fecd05 261 rga.at(2).at(0) = "SSRLRX";
crmackey 5:0c2da538ada2 262 rga.at(2).at(1) = "SRRX";
crmackey 14:ca6283fecd05 263 rga.at(2).at(2) = "P";
crmackey 5:0c2da538ada2 264 rga.at(2).at(3) = "LLX";
crmackey 5:0c2da538ada2 265 rga.at(2).at(4) = "LRSRLX";
crmackey 5:0c2da538ada2 266 rga.at(2).at(5) = "SSSLRX";
crmackey 5:0c2da538ada2 267
crmackey 6:8bd154642fb4 268 rga.at(3).at(0) = "SRRLLRLRX";
crmackey 5:0c2da538ada2 269 rga.at(3).at(1) = "RLRRX";
crmackey 5:0c2da538ada2 270 rga.at(3).at(2) = "RRX";
crmackey 14:ca6283fecd05 271 rga.at(3).at(3) = "P";
crmackey 5:0c2da538ada2 272 rga.at(3).at(4) = "SSRLX";
crmackey 5:0c2da538ada2 273 rga.at(3).at(5) = "SRLRLX";
crmackey 5:0c2da538ada2 274
crmackey 14:ca6283fecd05 275 rga.at(4).at(0) = "RLLLRSRLLRX";
crmackey 5:0c2da538ada2 276 rga.at(4).at(1) = "RLLRLSRX";
crmackey 5:0c2da538ada2 277 rga.at(4).at(2) = "RLLRLRSX";
crmackey 5:0c2da538ada2 278 rga.at(4).at(3) = "RLSSX";
crmackey 14:ca6283fecd05 279 rga.at(4).at(4) = "P";
crmackey 5:0c2da538ada2 280 rga.at(4).at(5) = "RLLLRLX";
crmackey 5:0c2da538ada2 281
crmackey 14:ca6283fecd05 282 rga.at(5).at(0) = "LRLLRX";
crmackey 5:0c2da538ada2 283 rga.at(5).at(1) = "LSLX";
crmackey 5:0c2da538ada2 284 rga.at(5).at(2) = "LRSSSX";
crmackey 5:0c2da538ada2 285 rga.at(5).at(3) = "RLRLSX";
crmackey 5:0c2da538ada2 286 rga.at(5).at(4) = "RLRRRLX";
crmackey 14:ca6283fecd05 287 rga.at(5).at(5) = "P";
crmackey 5:0c2da538ada2 288
crmackey 5:0c2da538ada2 289 btbee.reset();
crmackey 0:f4302186f4cb 290 m3pi_pb.mode(PullUp);
crmackey 0:f4302186f4cb 291
crmackey 0:f4302186f4cb 292 m3pi.printf("Wait 4");
crmackey 0:f4302186f4cb 293 m3pi.locate(0,1);
crmackey 0:f4302186f4cb 294 m3pi.printf("PC");
crmackey 0:f4302186f4cb 295
crmackey 0:f4302186f4cb 296 while(m3pi_pb) { // Loop that runs until user presses button after bluetooth is connected.
crmackey 0:f4302186f4cb 297 m3pi_led[0]=!m3pi_led[0];
crmackey 0:f4302186f4cb 298 wait(3);
crmackey 0:f4302186f4cb 299 btbee.printf("\n");
crmackey 0:f4302186f4cb 300 btbee.printf("PC connected. Press the button \n");
crmackey 0:f4302186f4cb 301 }
crmackey 0:f4302186f4cb 302
crmackey 0:f4302186f4cb 303 m3pi.cls();
crmackey 0:f4302186f4cb 304
crmackey 3:711ac018d3ec 305 // reads battery voltage to screen on start up
crmackey 0:f4302186f4cb 306 char Bat[] = {'V','o','l','t',' ','i','s'};
crmackey 0:f4302186f4cb 307 m3pi.print(Bat,7);
crmackey 0:f4302186f4cb 308 wait(0.75);
crmackey 0:f4302186f4cb 309 m3pi.cls();
crmackey 0:f4302186f4cb 310 float batteryvoltage = m3pi.battery();
crmackey 0:f4302186f4cb 311 char* str = new char[30];
crmackey 0:f4302186f4cb 312 sprintf(str, "%.4g", batteryvoltage);
crmackey 0:f4302186f4cb 313 m3pi.print(str,6);
crmackey 0:f4302186f4cb 314 btbee.printf("Battery voltage is %f \n", batteryvoltage);
crmackey 0:f4302186f4cb 315 wait(0.75);
crmackey 0:f4302186f4cb 316 m3pi.cls();
crmackey 0:f4302186f4cb 317
crmackey 0:f4302186f4cb 318 if (batteryvoltage < 4.2) // exits program if voltage is less than 4.2 V
crmackey 0:f4302186f4cb 319 {
crmackey 0:f4302186f4cb 320 char low[] = {'L','o','w',' ','b','a','t'};
crmackey 0:f4302186f4cb 321 m3pi.print(low,7);
crmackey 0:f4302186f4cb 322 char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
crmackey 0:f4302186f4cb 323 m3pi.playtune(ExitSound,11);
crmackey 0:f4302186f4cb 324 btbee.printf("Battery voltage is too low. Stopping program");
crmackey 0:f4302186f4cb 325 exit(1);
crmackey 0:f4302186f4cb 326 }
crmackey 0:f4302186f4cb 327
crmackey 15:065889f0799b 328 command = GetLocation();
crmackey 15:065889f0799b 329 location = command.at(i);
crmackey 15:065889f0799b 330 i++;
crmackey 15:065889f0799b 331 goal = command.at(i);
crmackey 15:065889f0799b 332 i++;
crmackey 14:ca6283fecd05 333
crmackey 14:ca6283fecd05 334 btbee.printf("\n");
crmackey 14:ca6283fecd05 335
crmackey 0:f4302186f4cb 336 btbee.printf("Now calibrating \n"); // calibrating robot on the line.
crmackey 0:f4302186f4cb 337
crmackey 0:f4302186f4cb 338 m3pi.sensor_auto_calibrate();
crmackey 0:f4302186f4cb 339
crmackey 0:f4302186f4cb 340 btbee.printf("Finished calibrating \n");
crmackey 0:f4302186f4cb 341
crmackey 0:f4302186f4cb 342
crmackey 14:ca6283fecd05 343 btbee.printf("Robot is at location %d \n", location);
crmackey 14:ca6283fecd05 344 btbee.printf("Robot is going to %d \n", goal);
crmackey 14:ca6283fecd05 345
crmackey 14:ca6283fecd05 346
crmackey 0:f4302186f4cb 347 btbee.printf("\n");
crmackey 0:f4302186f4cb 348 btbee.printf("Now starting \n");
crmackey 0:f4302186f4cb 349
morganrose402 16:ee3b36038b6b 350 //Countdown
morganrose402 16:ee3b36038b6b 351 char StartTune[]={'V','1','5','O','5','E','4','R','4','E','4','R','4','E','4','R','4','O','6','E','4','R','4'};
morganrose402 16:ee3b36038b6b 352 m3pi.playtune(StartTune,23);
morganrose402 16:ee3b36038b6b 353 m3pi.printf("3");
morganrose402 16:ee3b36038b6b 354 wait(1);
morganrose402 16:ee3b36038b6b 355 m3pi.cls();
morganrose402 16:ee3b36038b6b 356 m3pi.printf("2");
morganrose402 16:ee3b36038b6b 357 wait(1);
morganrose402 16:ee3b36038b6b 358 m3pi.cls();
morganrose402 16:ee3b36038b6b 359 m3pi.printf("1");
morganrose402 16:ee3b36038b6b 360 wait(1);
morganrose402 16:ee3b36038b6b 361 m3pi.cls();
morganrose402 16:ee3b36038b6b 362 m3pi.printf("GO!");
morganrose402 16:ee3b36038b6b 363 wait(.5);
morganrose402 16:ee3b36038b6b 364
crmackey 3:711ac018d3ec 365 LapTime.start();
crmackey 3:711ac018d3ec 366
morganrose402 17:4e63e8eacad7 367 TotalTime.start();
morganrose402 17:4e63e8eacad7 368
crmackey 14:ca6283fecd05 369 path = rga.at(location-1).at(goal-1);
rzeer 11:254930ef37fb 370
rzeer 11:254930ef37fb 371 while (1) {
rzeer 7:0b4f136366c6 372
crmackey 5:0c2da538ada2 373 LapTime.start();
crmackey 0:f4302186f4cb 374
crmackey 0:f4302186f4cb 375 // Get the position of the line.
crmackey 0:f4302186f4cb 376 current_pos_of_line = m3pi.line_position();
crmackey 0:f4302186f4cb 377 proportional = current_pos_of_line;
crmackey 0:f4302186f4cb 378
crmackey 0:f4302186f4cb 379 // Compute the derivative
crmackey 0:f4302186f4cb 380 derivative = current_pos_of_line - previous_pos_of_line;
crmackey 0:f4302186f4cb 381
crmackey 0:f4302186f4cb 382 // Compute the integral
crmackey 0:f4302186f4cb 383 integral += proportional;
crmackey 0:f4302186f4cb 384
crmackey 0:f4302186f4cb 385 // Remember the last position.
crmackey 0:f4302186f4cb 386 previous_pos_of_line = current_pos_of_line;
crmackey 0:f4302186f4cb 387
crmackey 0:f4302186f4cb 388 // Compute the power
crmackey 0:f4302186f4cb 389 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
crmackey 0:f4302186f4cb 390
crmackey 0:f4302186f4cb 391 // Compute new speeds
crmackey 0:f4302186f4cb 392 right = speed+power;
crmackey 0:f4302186f4cb 393 left = speed-power;
crmackey 0:f4302186f4cb 394
crmackey 0:f4302186f4cb 395 // limit checks
crmackey 0:f4302186f4cb 396 if (right < MIN)
crmackey 0:f4302186f4cb 397 right = MIN;
crmackey 0:f4302186f4cb 398 else if (right > MAX)
crmackey 0:f4302186f4cb 399 right = MAX;
crmackey 0:f4302186f4cb 400
crmackey 0:f4302186f4cb 401 if (left < MIN)
crmackey 0:f4302186f4cb 402 left = MIN;
crmackey 0:f4302186f4cb 403 else if (left > MAX)
crmackey 0:f4302186f4cb 404 left = MAX;
crmackey 0:f4302186f4cb 405
crmackey 0:f4302186f4cb 406 // set speed
crmackey 0:f4302186f4cb 407 m3pi.left_motor(left);
crmackey 0:f4302186f4cb 408 m3pi.right_motor(right);
crmackey 0:f4302186f4cb 409
crmackey 0:f4302186f4cb 410
crmackey 0:f4302186f4cb 411 m3pi.calibrated_sensor(LapTest);
crmackey 0:f4302186f4cb 412
crmackey 0:f4302186f4cb 413 s1 = LapTest[0];
crmackey 0:f4302186f4cb 414 s2 = LapTest[1];
crmackey 0:f4302186f4cb 415 s3 = LapTest[2];
crmackey 0:f4302186f4cb 416 s4 = LapTest[3];
crmackey 0:f4302186f4cb 417 s5 = LapTest[4];
crmackey 0:f4302186f4cb 418
crmackey 4:a0baf6752548 419
crmackey 5:0c2da538ada2 420
crmackey 5:0c2da538ada2 421 if(CrossDetect() and LapTime > 0.1) {
crmackey 5:0c2da538ada2 422
crmackey 5:0c2da538ada2 423 btbee.printf("\n");
crmackey 5:0c2da538ada2 424 dir = path.at(counter);
crmackey 5:0c2da538ada2 425 btbee.printf("%c\n", dir);
crmackey 5:0c2da538ada2 426
crmackey 5:0c2da538ada2 427 switch(dir) {
crmackey 4:a0baf6752548 428
crmackey 5:0c2da538ada2 429 case 'R': {
crmackey 5:0c2da538ada2 430
crmackey 5:0c2da538ada2 431 RightTurn();
crmackey 5:0c2da538ada2 432 btbee.printf("Turning right\n");
crmackey 14:ca6283fecd05 433 counter++;
crmackey 5:0c2da538ada2 434 break;
crmackey 5:0c2da538ada2 435
crmackey 5:0c2da538ada2 436 }
crmackey 5:0c2da538ada2 437
crmackey 5:0c2da538ada2 438 case 'L': {
crmackey 5:0c2da538ada2 439
crmackey 5:0c2da538ada2 440 LeftTurn();
crmackey 5:0c2da538ada2 441 btbee.printf("Turning left\n");
crmackey 14:ca6283fecd05 442 counter++;
crmackey 5:0c2da538ada2 443 break;
crmackey 5:0c2da538ada2 444
crmackey 5:0c2da538ada2 445 }
crmackey 5:0c2da538ada2 446
crmackey 5:0c2da538ada2 447 case 'S': {
crmackey 5:0c2da538ada2 448
crmackey 5:0c2da538ada2 449 GoStraight();
crmackey 5:0c2da538ada2 450 btbee.printf("Going straight\n");
crmackey 14:ca6283fecd05 451 counter++;
crmackey 5:0c2da538ada2 452 break;
crmackey 5:0c2da538ada2 453
crmackey 5:0c2da538ada2 454 }
crmackey 5:0c2da538ada2 455
crmackey 5:0c2da538ada2 456 case 'X': {
crmackey 5:0c2da538ada2 457
crmackey 15:065889f0799b 458 m3pi.left_motor(0.5);
crmackey 15:065889f0799b 459 m3pi.right_motor(0.5);
crmackey 15:065889f0799b 460 wait(0.07);
crmackey 5:0c2da538ada2 461 btbee.printf("Reached goal. Turning around\n");
crmackey 5:0c2da538ada2 462 TurnAround();
crmackey 14:ca6283fecd05 463 wait(1.0);
crmackey 14:ca6283fecd05 464
crmackey 14:ca6283fecd05 465 location = goal;
crmackey 14:ca6283fecd05 466
crmackey 15:065889f0799b 467 goal = command.at(i);
crmackey 15:065889f0799b 468
morganrose402 17:4e63e8eacad7 469 if(goal == 7) {
morganrose402 17:4e63e8eacad7 470 TotalTime.stop();
morganrose402 17:4e63e8eacad7 471 m3pi.printf("%f",TotalTime.read());
morganrose402 17:4e63e8eacad7 472 command.clear();
morganrose402 17:4e63e8eacad7 473 i = 0;
morganrose402 17:4e63e8eacad7 474 command = GetLocation();
morganrose402 17:4e63e8eacad7 475 m3pi.cls();
morganrose402 17:4e63e8eacad7 476 location = command.at(i);
morganrose402 17:4e63e8eacad7 477 i++;
morganrose402 17:4e63e8eacad7 478 goal = command.at(i);
morganrose402 17:4e63e8eacad7 479
crmackey 15:065889f0799b 480
crmackey 15:065889f0799b 481 }
crmackey 14:ca6283fecd05 482
crmackey 14:ca6283fecd05 483 path = rga.at(location-1).at(goal-1);
crmackey 15:065889f0799b 484
crmackey 15:065889f0799b 485 while(LocTest()) {
crmackey 15:065889f0799b 486
crmackey 15:065889f0799b 487 i++;
crmackey 15:065889f0799b 488 goal = command.at(i);
crmackey 15:065889f0799b 489 path = rga.at(location-1).at(goal-1);
crmackey 15:065889f0799b 490
crmackey 15:065889f0799b 491 }
crmackey 15:065889f0799b 492
crmackey 15:065889f0799b 493
crmackey 14:ca6283fecd05 494 counter = 0;
crmackey 15:065889f0799b 495 i++;
crmackey 14:ca6283fecd05 496
crmackey 14:ca6283fecd05 497 break;
crmackey 14:ca6283fecd05 498
rzeer 8:86d31f2a7479 499 }
crmackey 14:ca6283fecd05 500
crmackey 5:0c2da538ada2 501
rzeer 8:86d31f2a7479 502 }
crmackey 14:ca6283fecd05 503
crmackey 4:a0baf6752548 504
crmackey 4:a0baf6752548 505 }
crmackey 5:0c2da538ada2 506
crmackey 0:f4302186f4cb 507
crmackey 0:f4302186f4cb 508
crmackey 0:f4302186f4cb 509 }
crmackey 0:f4302186f4cb 510
crmackey 5:0c2da538ada2 511 }