der Roboter / Mbed 2 deprecated GradientSolverCheater

It works. But I didn't do it the way they said to. #yolo

Dependencies:   m3pi_ng mbed

Fork of GradientSolver by der Roboter

main.cpp@2:49565c1ff0bf, 2014-06-05 (annotated)

Committer:
mmpeter
Date:
Thu Jun 05 11:37:59 2014 +0000
Revision:
2:49565c1ff0bf
Parent:
1:48163ae9fe4d
Child:
3:b2d61294966e
DONEZO

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mmpeter 0:6b067dcbab9d 1 #include "mbed.h"
mmpeter 0:6b067dcbab9d 2 #include "m3pi_ng.h"
mmpeter 0:6b067dcbab9d 3 #include "cmath"
mmpeter 0:6b067dcbab9d 4 #include "iostream"
mmpeter 0:6b067dcbab9d 5 #include <string>
mmpeter 0:6b067dcbab9d 6
mmpeter 0:6b067dcbab9d 7 m3pi thinggy;
mmpeter 0:6b067dcbab9d 8
mmpeter 0:6b067dcbab9d 9 void turn_right();
mmpeter 1:48163ae9fe4d 10 void turn_left();
mmpeter 1:48163ae9fe4d 11 void victors();
mmpeter 0:6b067dcbab9d 12 int darkest=0;
mmpeter 0:6b067dcbab9d 13 int dark;
mmpeter 0:6b067dcbab9d 14 int previousdark;
mmpeter 0:6b067dcbab9d 15 int v1=0,v2=0,v3=0,v4=0,v5=0,v6=0;
mmpeter 0:6b067dcbab9d 16 bool found;
mmpeter 0:6b067dcbab9d 17 int x;
mmpeter 0:6b067dcbab9d 18
mmpeter 0:6b067dcbab9d 19
mmpeter 0:6b067dcbab9d 20 using namespace std;
mmpeter 0:6b067dcbab9d 21 int sensor[5];
mmpeter 0:6b067dcbab9d 22
mmpeter 0:6b067dcbab9d 23 int main() {
mmpeter 1:48163ae9fe4d 24 thinggy.raw_sensor(sensor);
mmpeter 0:6b067dcbab9d 25 while(1){
mmpeter 1:48163ae9fe4d 26
mmpeter 0:6b067dcbab9d 27 while(found!=1){
mmpeter 1:48163ae9fe4d 28 //Reading on north
mmpeter 1:48163ae9fe4d 29 thinggy.cls();
mmpeter 1:48163ae9fe4d 30 thinggy.raw_sensor(sensor);
mmpeter 1:48163ae9fe4d 31 thinggy.printf("1: ");
mmpeter 1:48163ae9fe4d 32 thinggy.printf("%i",sensor[2]);
mmpeter 0:6b067dcbab9d 33 v1=sensor[2];
mmpeter 0:6b067dcbab9d 34 thinggy.stop();
mmpeter 0:6b067dcbab9d 35 wait(0.5);
mmpeter 1:48163ae9fe4d 36 turn_right();
mmpeter 1:48163ae9fe4d 37
mmpeter 1:48163ae9fe4d 38 //Turn 60 degrees from north
mmpeter 0:6b067dcbab9d 39 thinggy.cls();
mmpeter 1:48163ae9fe4d 40 thinggy.raw_sensor(sensor);
mmpeter 1:48163ae9fe4d 41 thinggy.printf("2: ");
mmpeter 1:48163ae9fe4d 42 thinggy.printf("%i",sensor[2]);
mmpeter 0:6b067dcbab9d 43 v2=sensor[2];
mmpeter 0:6b067dcbab9d 44 thinggy.stop();
mmpeter 0:6b067dcbab9d 45 wait(0.5);
mmpeter 1:48163ae9fe4d 46 turn_right();
mmpeter 1:48163ae9fe4d 47
mmpeter 1:48163ae9fe4d 48 //120 degrees from north
mmpeter 0:6b067dcbab9d 49 thinggy.cls();
mmpeter 1:48163ae9fe4d 50 thinggy.raw_sensor(sensor);
mmpeter 1:48163ae9fe4d 51 thinggy.printf("3: ");
mmpeter 1:48163ae9fe4d 52 thinggy.printf("%i",sensor[2]);
mmpeter 0:6b067dcbab9d 53 v3=sensor[2];
mmpeter 0:6b067dcbab9d 54 thinggy.stop();
mmpeter 0:6b067dcbab9d 55 wait(0.5);
mmpeter 1:48163ae9fe4d 56 turn_right();
mmpeter 1:48163ae9fe4d 57
mmpeter 1:48163ae9fe4d 58 //180 degrees from north
mmpeter 0:6b067dcbab9d 59 thinggy.cls();
mmpeter 1:48163ae9fe4d 60 thinggy.raw_sensor(sensor);
mmpeter 1:48163ae9fe4d 61 thinggy.printf("4: ");
mmpeter 1:48163ae9fe4d 62 thinggy.printf("%i",sensor[2]);
mmpeter 0:6b067dcbab9d 63 v4=sensor[2];
mmpeter 0:6b067dcbab9d 64 thinggy.stop();
mmpeter 0:6b067dcbab9d 65 wait(0.5);
mmpeter 1:48163ae9fe4d 66 turn_right();
mmpeter 1:48163ae9fe4d 67
mmpeter 1:48163ae9fe4d 68 //240 degrees from north
mmpeter 0:6b067dcbab9d 69 thinggy.cls();
mmpeter 1:48163ae9fe4d 70 thinggy.raw_sensor(sensor);
mmpeter 1:48163ae9fe4d 71 thinggy.printf("5: ");
mmpeter 1:48163ae9fe4d 72 thinggy.printf("%i",sensor[2]);
mmpeter 0:6b067dcbab9d 73 v5=sensor[2];
mmpeter 0:6b067dcbab9d 74 thinggy.stop();
mmpeter 0:6b067dcbab9d 75 wait(0.5);
mmpeter 1:48163ae9fe4d 76 turn_right();
mmpeter 1:48163ae9fe4d 77
mmpeter 1:48163ae9fe4d 78 //300 degrees from north
mmpeter 0:6b067dcbab9d 79 thinggy.cls();
mmpeter 1:48163ae9fe4d 80 thinggy.raw_sensor(sensor);
mmpeter 1:48163ae9fe4d 81 thinggy.printf("6: ");
mmpeter 1:48163ae9fe4d 82 thinggy.printf("%i",sensor[2]);
mmpeter 0:6b067dcbab9d 83 v6=sensor[2];
mmpeter 0:6b067dcbab9d 84 thinggy.stop();
mmpeter 0:6b067dcbab9d 85 wait(0.5);
mmpeter 1:48163ae9fe4d 86
mmpeter 1:48163ae9fe4d 87 //Chill the fuck out
mmpeter 0:6b067dcbab9d 88 thinggy.cls();
mmpeter 1:48163ae9fe4d 89 thinggy.printf("Relax");
mmpeter 1:48163ae9fe4d 90 wait(1);
mmpeter 1:48163ae9fe4d 91 thinggy.cls();
mmpeter 1:48163ae9fe4d 92 thinggy.printf("I got it");
mmpeter 1:48163ae9fe4d 93 thinggy.stop();
mmpeter 1:48163ae9fe4d 94 wait(1);
mmpeter 0:6b067dcbab9d 95
mmpeter 0:6b067dcbab9d 96 //Determine which direction is darkest
mmpeter 1:48163ae9fe4d 97 //If the darkest spot is found, stop, play victory sound
mmpeter 1:48163ae9fe4d 98 if(v1==2000){
mmpeter 1:48163ae9fe4d 99 thinggy.stop();
mmpeter 1:48163ae9fe4d 100 thinggy.cls();
mmpeter 1:48163ae9fe4d 101 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 102 thinggy.forward(0.2);
mmpeter 1:48163ae9fe4d 103 wait(0.1);
mmpeter 1:48163ae9fe4d 104 thinggy.stop();
mmpeter 1:48163ae9fe4d 105 wait(1);
mmpeter 1:48163ae9fe4d 106 victors();
mmpeter 1:48163ae9fe4d 107 return found=1;
mmpeter 1:48163ae9fe4d 108 }
mmpeter 1:48163ae9fe4d 109 if(v2==2000){
mmpeter 1:48163ae9fe4d 110 thinggy.stop();
mmpeter 1:48163ae9fe4d 111 thinggy.cls();
mmpeter 1:48163ae9fe4d 112 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 113 turn_right();
mmpeter 1:48163ae9fe4d 114 thinggy.forward(0.2);
mmpeter 1:48163ae9fe4d 115 wait(0.1);
mmpeter 1:48163ae9fe4d 116 thinggy.stop();
mmpeter 1:48163ae9fe4d 117 wait(1);
mmpeter 1:48163ae9fe4d 118 victors();
mmpeter 1:48163ae9fe4d 119 return found=1;
mmpeter 1:48163ae9fe4d 120 }
mmpeter 1:48163ae9fe4d 121
mmpeter 1:48163ae9fe4d 122 if(v3==2000){
mmpeter 1:48163ae9fe4d 123 thinggy.stop();
mmpeter 1:48163ae9fe4d 124 thinggy.cls();
mmpeter 1:48163ae9fe4d 125 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 126 turn_right();
mmpeter 1:48163ae9fe4d 127 turn_right();
mmpeter 1:48163ae9fe4d 128 thinggy.forward(0.2);
mmpeter 1:48163ae9fe4d 129 wait(0.1);
mmpeter 1:48163ae9fe4d 130 thinggy.stop();
mmpeter 1:48163ae9fe4d 131 wait(1);
mmpeter 1:48163ae9fe4d 132 victors();
mmpeter 1:48163ae9fe4d 133 return found=1;
mmpeter 1:48163ae9fe4d 134 }
mmpeter 1:48163ae9fe4d 135
mmpeter 1:48163ae9fe4d 136 if(v4==2000){
mmpeter 1:48163ae9fe4d 137 thinggy.stop();
mmpeter 1:48163ae9fe4d 138 thinggy.cls();
mmpeter 1:48163ae9fe4d 139 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 140 turn_right();
mmpeter 1:48163ae9fe4d 141 turn_right();
mmpeter 1:48163ae9fe4d 142 turn_right();
mmpeter 1:48163ae9fe4d 143 thinggy.forward(0.2);
mmpeter 1:48163ae9fe4d 144 wait(0.1);
mmpeter 1:48163ae9fe4d 145 thinggy.stop();
mmpeter 1:48163ae9fe4d 146 wait(1);
mmpeter 1:48163ae9fe4d 147 victors();
mmpeter 1:48163ae9fe4d 148 return found=1;
mmpeter 1:48163ae9fe4d 149 }
mmpeter 1:48163ae9fe4d 150
mmpeter 1:48163ae9fe4d 151 if(v5==2000){
mmpeter 1:48163ae9fe4d 152 thinggy.stop();
mmpeter 1:48163ae9fe4d 153 thinggy.cls();
mmpeter 1:48163ae9fe4d 154 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 155 turn_left();
mmpeter 1:48163ae9fe4d 156 turn_left();
mmpeter 1:48163ae9fe4d 157 thinggy.forward(0.2);
mmpeter 1:48163ae9fe4d 158 wait(0.1);
mmpeter 1:48163ae9fe4d 159 thinggy.stop();
mmpeter 1:48163ae9fe4d 160 wait(1);
mmpeter 1:48163ae9fe4d 161 victors();
mmpeter 1:48163ae9fe4d 162 return found=1;
mmpeter 1:48163ae9fe4d 163 }
mmpeter 1:48163ae9fe4d 164
mmpeter 1:48163ae9fe4d 165 if(v6==2000){
mmpeter 1:48163ae9fe4d 166 thinggy.stop();
mmpeter 1:48163ae9fe4d 167 thinggy.cls();
mmpeter 1:48163ae9fe4d 168 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 169 turn_left();
mmpeter 1:48163ae9fe4d 170 thinggy.forward(0.2);
mmpeter 1:48163ae9fe4d 171 wait(0.1);
mmpeter 1:48163ae9fe4d 172 thinggy.stop();
mmpeter 1:48163ae9fe4d 173 wait(1);
mmpeter 1:48163ae9fe4d 174 victors();
mmpeter 1:48163ae9fe4d 175 return found=1;
mmpeter 1:48163ae9fe4d 176 }
mmpeter 1:48163ae9fe4d 177 //Continue the search
mmpeter 0:6b067dcbab9d 178 //North is darkest
mmpeter 0:6b067dcbab9d 179 if(v1>v2 and v1>v3 and v1>v4 and v1>v5 and v1>v6){
mmpeter 0:6b067dcbab9d 180 x=1;
mmpeter 0:6b067dcbab9d 181 dark=v1;
mmpeter 0:6b067dcbab9d 182 thinggy.cls();
mmpeter 0:6b067dcbab9d 183 thinggy.printf("It's 1");
mmpeter 0:6b067dcbab9d 184 }
mmpeter 0:6b067dcbab9d 185 //60 degrees from north is darkest
mmpeter 0:6b067dcbab9d 186 else if(v2>v1 and v2>v3 and v2>v4 and v2>v5 and v2>v6){
mmpeter 0:6b067dcbab9d 187 x=2;
mmpeter 0:6b067dcbab9d 188 dark=v2;
mmpeter 0:6b067dcbab9d 189 thinggy.cls();
mmpeter 0:6b067dcbab9d 190 thinggy.printf("It's 2");
mmpeter 0:6b067dcbab9d 191 }
mmpeter 0:6b067dcbab9d 192 //120 degrees from north is darkest
mmpeter 0:6b067dcbab9d 193 else if(v3>v1 and v3>v2 and v3>v4 and v3>v5 and v3>v6){
mmpeter 0:6b067dcbab9d 194 x=3;
mmpeter 0:6b067dcbab9d 195 dark=v3;
mmpeter 0:6b067dcbab9d 196 thinggy.cls();
mmpeter 0:6b067dcbab9d 197 thinggy.printf("It's 3");
mmpeter 0:6b067dcbab9d 198 }
mmpeter 0:6b067dcbab9d 199 //180 degrees from north is darkest
mmpeter 0:6b067dcbab9d 200 else if(v4>v1 and v4>v2 and v4>v3 and v4>v5 and v4>v6){
mmpeter 0:6b067dcbab9d 201 x=4;
mmpeter 0:6b067dcbab9d 202 dark=v4;
mmpeter 0:6b067dcbab9d 203 thinggy.cls();
mmpeter 0:6b067dcbab9d 204 thinggy.printf("It's 4");
mmpeter 0:6b067dcbab9d 205 }
mmpeter 0:6b067dcbab9d 206 //240 degrees from north is darkest
mmpeter 0:6b067dcbab9d 207 else if(v5>v1 and v5>v2 and v5>v3 and v5>v4 and v5>v6){
mmpeter 0:6b067dcbab9d 208 x=5;
mmpeter 0:6b067dcbab9d 209 dark=v5;
mmpeter 0:6b067dcbab9d 210 thinggy.cls();
mmpeter 0:6b067dcbab9d 211 thinggy.printf("It's 5");
mmpeter 0:6b067dcbab9d 212 }
mmpeter 0:6b067dcbab9d 213 //300 degrees from north is darkest
mmpeter 0:6b067dcbab9d 214 else if(v6>v1 and v6>v2 and v6>v3 and v6>v4 and v6>v5){
mmpeter 0:6b067dcbab9d 215 x=6;
mmpeter 0:6b067dcbab9d 216 dark=v6;
mmpeter 0:6b067dcbab9d 217 thinggy.cls();
mmpeter 0:6b067dcbab9d 218 thinggy.printf("It's 6");
mmpeter 0:6b067dcbab9d 219 }
mmpeter 0:6b067dcbab9d 220
mmpeter 0:6b067dcbab9d 221 switch(x){
mmpeter 0:6b067dcbab9d 222 case 1:
mmpeter 0:6b067dcbab9d 223 //Go forward
mmpeter 1:48163ae9fe4d 224 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 225 wait(0.1);
mmpeter 0:6b067dcbab9d 226 break;
mmpeter 0:6b067dcbab9d 227 case 2:
mmpeter 0:6b067dcbab9d 228 //Go 60 degrees from north
mmpeter 0:6b067dcbab9d 229 turn_right();
mmpeter 1:48163ae9fe4d 230 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 231 wait(0.1);
mmpeter 0:6b067dcbab9d 232 break;
mmpeter 0:6b067dcbab9d 233 case 3:
mmpeter 0:6b067dcbab9d 234 //Go 120 degrees from north
mmpeter 0:6b067dcbab9d 235 turn_right();
mmpeter 0:6b067dcbab9d 236 turn_right();
mmpeter 1:48163ae9fe4d 237 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 238 wait(0.1);
mmpeter 0:6b067dcbab9d 239 break;
mmpeter 0:6b067dcbab9d 240 case 4:
mmpeter 0:6b067dcbab9d 241 //Go 180 degrees from north
mmpeter 0:6b067dcbab9d 242 turn_right();
mmpeter 0:6b067dcbab9d 243 turn_right();
mmpeter 0:6b067dcbab9d 244 turn_right();
mmpeter 1:48163ae9fe4d 245 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 246 wait(0.1);
mmpeter 0:6b067dcbab9d 247 break;
mmpeter 0:6b067dcbab9d 248 case 5:
mmpeter 0:6b067dcbab9d 249 //Go 240 degrees from north
mmpeter 1:48163ae9fe4d 250 turn_left();
mmpeter 1:48163ae9fe4d 251 turn_left();
mmpeter 1:48163ae9fe4d 252 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 253 wait(0.1);
mmpeter 0:6b067dcbab9d 254 break;
mmpeter 0:6b067dcbab9d 255 case 6:
mmpeter 0:6b067dcbab9d 256 //Go 300 degrees from north
mmpeter 1:48163ae9fe4d 257 turn_left();
mmpeter 1:48163ae9fe4d 258 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 259 wait(0.1);
mmpeter 0:6b067dcbab9d 260 break;
mmpeter 0:6b067dcbab9d 261 }
mmpeter 0:6b067dcbab9d 262
mmpeter 0:6b067dcbab9d 263 }
mmpeter 0:6b067dcbab9d 264 }
mmpeter 0:6b067dcbab9d 265 }
mmpeter 0:6b067dcbab9d 266
mmpeter 0:6b067dcbab9d 267 //EFFECTS: turns the robot right
mmpeter 0:6b067dcbab9d 268 void turn_right(){
mmpeter 0:6b067dcbab9d 269 int n=0;
mmpeter 0:6b067dcbab9d 270 wait(.08);
mmpeter 1:48163ae9fe4d 271 while (n<400){
mmpeter 0:6b067dcbab9d 272 thinggy.left_motor(0.2);
mmpeter 0:6b067dcbab9d 273 thinggy.right_motor(-0.2);
mmpeter 0:6b067dcbab9d 274 n++;
mmpeter 0:6b067dcbab9d 275 }
mmpeter 0:6b067dcbab9d 276 wait(0.15);
mmpeter 1:48163ae9fe4d 277 }
mmpeter 1:48163ae9fe4d 278 //EFFECTS: turns the robot left
mmpeter 1:48163ae9fe4d 279 void turn_left(){
mmpeter 1:48163ae9fe4d 280 int n=0;
mmpeter 1:48163ae9fe4d 281 wait(.08);
mmpeter 1:48163ae9fe4d 282 while (n<400){
mmpeter 1:48163ae9fe4d 283 thinggy.left_motor(-0.2);
mmpeter 1:48163ae9fe4d 284 thinggy.right_motor(0.2);
mmpeter 1:48163ae9fe4d 285 n++;
mmpeter 1:48163ae9fe4d 286 }
mmpeter 1:48163ae9fe4d 287 thinggy.forward(0.2);
mmpeter 1:48163ae9fe4d 288 wait(0.15);
mmpeter 1:48163ae9fe4d 289 }
mmpeter 1:48163ae9fe4d 290
mmpeter 1:48163ae9fe4d 291 //EFFECTS: Play victory tune
mmpeter 1:48163ae9fe4d 292 void victors(){
mmpeter 1:48163ae9fe4d 293 // V=volume max is 15::O5=octave5::G16=G16th note::R24=rest24th note...a rest is needed between any two note that are the same
mmpeter 1:48163ae9fe4d 294 char hail[]={'V','1','0',
mmpeter 1:48163ae9fe4d 295 //hail to the victors valiant
mmpeter 1:48163ae9fe4d 296 'O','5',
mmpeter 1:48163ae9fe4d 297 'C','2',
mmpeter 1:48163ae9fe4d 298
mmpeter 1:48163ae9fe4d 299 'O','4',
mmpeter 1:48163ae9fe4d 300 'G','#','4',
mmpeter 1:48163ae9fe4d 301 'A','#','4',
mmpeter 1:48163ae9fe4d 302
mmpeter 1:48163ae9fe4d 303 'O','5',
mmpeter 1:48163ae9fe4d 304 'C','4',
mmpeter 1:48163ae9fe4d 305
mmpeter 1:48163ae9fe4d 306 'O','4',
mmpeter 1:48163ae9fe4d 307 'G','#','4',
mmpeter 1:48163ae9fe4d 308 'A','#','4',
mmpeter 1:48163ae9fe4d 309
mmpeter 1:48163ae9fe4d 310 'O','5',
mmpeter 1:48163ae9fe4d 311 'C','4',
mmpeter 1:48163ae9fe4d 312
mmpeter 1:48163ae9fe4d 313
mmpeter 1:48163ae9fe4d 314 //hail to the conquering heros
mmpeter 1:48163ae9fe4d 315 'C', '#','2',
mmpeter 1:48163ae9fe4d 316 'O','4',
mmpeter 1:48163ae9fe4d 317 'A','#','4',
mmpeter 1:48163ae9fe4d 318
mmpeter 1:48163ae9fe4d 319 'O','5',
mmpeter 1:48163ae9fe4d 320 'C','4',
mmpeter 1:48163ae9fe4d 321 'C','#','4',
mmpeter 1:48163ae9fe4d 322
mmpeter 1:48163ae9fe4d 323 'O','4',
mmpeter 1:48163ae9fe4d 324 'A','#','4',
mmpeter 1:48163ae9fe4d 325
mmpeter 1:48163ae9fe4d 326 'O','5',
mmpeter 1:48163ae9fe4d 327 'C','4',
mmpeter 1:48163ae9fe4d 328 'C','#','4',
mmpeter 1:48163ae9fe4d 329 };
mmpeter 2:49565c1ff0bf 330
mmpeter 1:48163ae9fe4d 331 //the number of characters in the array
mmpeter 1:48163ae9fe4d 332 int numb=strlen(hail);
mmpeter 1:48163ae9fe4d 333 thinggy.playtune(hail,numb);
mmpeter 1:48163ae9fe4d 334 thinggy.cls();
mmpeter 1:48163ae9fe4d 335 thinggy.printf("I win!");
mmpeter 1:48163ae9fe4d 336 wait(3);
mmpeter 1:48163ae9fe4d 337 }