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@5:87cbce6dc969, 2014-06-05 (annotated)

Committer:
mmpeter
Date:
Thu Jun 05 14:30:58 2014 +0000
Revision:
5:87cbce6dc969
Parent:
4:9dd1477111da
Child:
6:53ea00c03b22
Cheater cheater pumpkin eater

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 3:b2d61294966e 86 turn_right();
mmpeter 3:b2d61294966e 87 thinggy.stop();
mmpeter 1:48163ae9fe4d 88
mmpeter 1:48163ae9fe4d 89 //Chill the fuck out
mmpeter 0:6b067dcbab9d 90 thinggy.cls();
mmpeter 1:48163ae9fe4d 91 thinggy.printf("Relax");
mmpeter 1:48163ae9fe4d 92 wait(1);
mmpeter 1:48163ae9fe4d 93 thinggy.cls();
mmpeter 1:48163ae9fe4d 94 thinggy.printf("I got it");
mmpeter 1:48163ae9fe4d 95 thinggy.stop();
mmpeter 1:48163ae9fe4d 96 wait(1);
mmpeter 0:6b067dcbab9d 97
mmpeter 0:6b067dcbab9d 98 //Determine which direction is darkest
mmpeter 5:87cbce6dc969 99
mmpeter 5:87cbce6dc969 100
mmpeter 1:48163ae9fe4d 101 //If the darkest spot is found, stop, play victory sound
mmpeter 1:48163ae9fe4d 102 if(v1==2000){
mmpeter 1:48163ae9fe4d 103 thinggy.stop();
mmpeter 1:48163ae9fe4d 104 thinggy.cls();
mmpeter 1:48163ae9fe4d 105 thinggy.printf("Found it");
mmpeter 5:87cbce6dc969 106 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 107 wait(0.1);
mmpeter 1:48163ae9fe4d 108 thinggy.stop();
mmpeter 1:48163ae9fe4d 109 wait(1);
mmpeter 1:48163ae9fe4d 110 victors();
mmpeter 1:48163ae9fe4d 111 return found=1;
mmpeter 1:48163ae9fe4d 112 }
mmpeter 1:48163ae9fe4d 113 if(v2==2000){
mmpeter 1:48163ae9fe4d 114 thinggy.stop();
mmpeter 1:48163ae9fe4d 115 thinggy.cls();
mmpeter 1:48163ae9fe4d 116 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 117 turn_right();
mmpeter 5:87cbce6dc969 118 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 119 wait(0.1);
mmpeter 1:48163ae9fe4d 120 thinggy.stop();
mmpeter 1:48163ae9fe4d 121 wait(1);
mmpeter 1:48163ae9fe4d 122 victors();
mmpeter 1:48163ae9fe4d 123 return found=1;
mmpeter 1:48163ae9fe4d 124 }
mmpeter 1:48163ae9fe4d 125
mmpeter 1:48163ae9fe4d 126 if(v3==2000){
mmpeter 1:48163ae9fe4d 127 thinggy.stop();
mmpeter 1:48163ae9fe4d 128 thinggy.cls();
mmpeter 1:48163ae9fe4d 129 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 130 turn_right();
mmpeter 1:48163ae9fe4d 131 turn_right();
mmpeter 5:87cbce6dc969 132 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 133 wait(0.1);
mmpeter 1:48163ae9fe4d 134 thinggy.stop();
mmpeter 1:48163ae9fe4d 135 wait(1);
mmpeter 1:48163ae9fe4d 136 victors();
mmpeter 1:48163ae9fe4d 137 return found=1;
mmpeter 1:48163ae9fe4d 138 }
mmpeter 1:48163ae9fe4d 139
mmpeter 1:48163ae9fe4d 140 if(v4==2000){
mmpeter 1:48163ae9fe4d 141 thinggy.stop();
mmpeter 1:48163ae9fe4d 142 thinggy.cls();
mmpeter 1:48163ae9fe4d 143 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 144 turn_right();
mmpeter 1:48163ae9fe4d 145 turn_right();
mmpeter 1:48163ae9fe4d 146 turn_right();
mmpeter 5:87cbce6dc969 147 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 148 wait(0.1);
mmpeter 1:48163ae9fe4d 149 thinggy.stop();
mmpeter 1:48163ae9fe4d 150 wait(1);
mmpeter 1:48163ae9fe4d 151 victors();
mmpeter 1:48163ae9fe4d 152 return found=1;
mmpeter 1:48163ae9fe4d 153 }
mmpeter 1:48163ae9fe4d 154
mmpeter 1:48163ae9fe4d 155 if(v5==2000){
mmpeter 1:48163ae9fe4d 156 thinggy.stop();
mmpeter 1:48163ae9fe4d 157 thinggy.cls();
mmpeter 1:48163ae9fe4d 158 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 159 turn_left();
mmpeter 1:48163ae9fe4d 160 turn_left();
mmpeter 5:87cbce6dc969 161 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 162 wait(0.1);
mmpeter 1:48163ae9fe4d 163 thinggy.stop();
mmpeter 1:48163ae9fe4d 164 wait(1);
mmpeter 1:48163ae9fe4d 165 victors();
mmpeter 1:48163ae9fe4d 166 return found=1;
mmpeter 1:48163ae9fe4d 167 }
mmpeter 1:48163ae9fe4d 168
mmpeter 1:48163ae9fe4d 169 if(v6==2000){
mmpeter 1:48163ae9fe4d 170 thinggy.stop();
mmpeter 1:48163ae9fe4d 171 thinggy.cls();
mmpeter 1:48163ae9fe4d 172 thinggy.printf("Found it");
mmpeter 1:48163ae9fe4d 173 turn_left();
mmpeter 5:87cbce6dc969 174 thinggy.forward(0.3);
mmpeter 1:48163ae9fe4d 175 wait(0.1);
mmpeter 1:48163ae9fe4d 176 thinggy.stop();
mmpeter 1:48163ae9fe4d 177 wait(1);
mmpeter 1:48163ae9fe4d 178 victors();
mmpeter 1:48163ae9fe4d 179 return found=1;
mmpeter 1:48163ae9fe4d 180 }
mmpeter 5:87cbce6dc969 181
mmpeter 5:87cbce6dc969 182
mmpeter 1:48163ae9fe4d 183 //Continue the search
mmpeter 0:6b067dcbab9d 184 //North is darkest
mmpeter 0:6b067dcbab9d 185 if(v1>v2 and v1>v3 and v1>v4 and v1>v5 and v1>v6){
mmpeter 0:6b067dcbab9d 186 x=1;
mmpeter 0:6b067dcbab9d 187 dark=v1;
mmpeter 0:6b067dcbab9d 188 thinggy.cls();
mmpeter 0:6b067dcbab9d 189 thinggy.printf("It's 1");
mmpeter 0:6b067dcbab9d 190 }
mmpeter 0:6b067dcbab9d 191 //60 degrees from north is darkest
mmpeter 0:6b067dcbab9d 192 else if(v2>v1 and v2>v3 and v2>v4 and v2>v5 and v2>v6){
mmpeter 0:6b067dcbab9d 193 x=2;
mmpeter 0:6b067dcbab9d 194 dark=v2;
mmpeter 0:6b067dcbab9d 195 thinggy.cls();
mmpeter 0:6b067dcbab9d 196 thinggy.printf("It's 2");
mmpeter 0:6b067dcbab9d 197 }
mmpeter 0:6b067dcbab9d 198 //120 degrees from north is darkest
mmpeter 0:6b067dcbab9d 199 else if(v3>v1 and v3>v2 and v3>v4 and v3>v5 and v3>v6){
mmpeter 0:6b067dcbab9d 200 x=3;
mmpeter 0:6b067dcbab9d 201 dark=v3;
mmpeter 0:6b067dcbab9d 202 thinggy.cls();
mmpeter 0:6b067dcbab9d 203 thinggy.printf("It's 3");
mmpeter 0:6b067dcbab9d 204 }
mmpeter 0:6b067dcbab9d 205 //180 degrees from north is darkest
mmpeter 0:6b067dcbab9d 206 else if(v4>v1 and v4>v2 and v4>v3 and v4>v5 and v4>v6){
mmpeter 0:6b067dcbab9d 207 x=4;
mmpeter 0:6b067dcbab9d 208 dark=v4;
mmpeter 0:6b067dcbab9d 209 thinggy.cls();
mmpeter 0:6b067dcbab9d 210 thinggy.printf("It's 4");
mmpeter 0:6b067dcbab9d 211 }
mmpeter 0:6b067dcbab9d 212 //240 degrees from north is darkest
mmpeter 0:6b067dcbab9d 213 else if(v5>v1 and v5>v2 and v5>v3 and v5>v4 and v5>v6){
mmpeter 0:6b067dcbab9d 214 x=5;
mmpeter 0:6b067dcbab9d 215 dark=v5;
mmpeter 0:6b067dcbab9d 216 thinggy.cls();
mmpeter 0:6b067dcbab9d 217 thinggy.printf("It's 5");
mmpeter 0:6b067dcbab9d 218 }
mmpeter 0:6b067dcbab9d 219 //300 degrees from north is darkest
mmpeter 0:6b067dcbab9d 220 else if(v6>v1 and v6>v2 and v6>v3 and v6>v4 and v6>v5){
mmpeter 0:6b067dcbab9d 221 x=6;
mmpeter 0:6b067dcbab9d 222 dark=v6;
mmpeter 0:6b067dcbab9d 223 thinggy.cls();
mmpeter 0:6b067dcbab9d 224 thinggy.printf("It's 6");
mmpeter 0:6b067dcbab9d 225 }
mmpeter 0:6b067dcbab9d 226
mmpeter 0:6b067dcbab9d 227 switch(x){
mmpeter 0:6b067dcbab9d 228 case 1:
mmpeter 0:6b067dcbab9d 229 //Go forward
mmpeter 4:9dd1477111da 230 wait(0.1);
mmpeter 1:48163ae9fe4d 231 thinggy.forward(0.3);
mmpeter 4:9dd1477111da 232 wait(0.2);
mmpeter 0:6b067dcbab9d 233 break;
mmpeter 0:6b067dcbab9d 234 case 2:
mmpeter 0:6b067dcbab9d 235 //Go 60 degrees from north
mmpeter 0:6b067dcbab9d 236 turn_right();
mmpeter 1:48163ae9fe4d 237 thinggy.forward(0.3);
mmpeter 4:9dd1477111da 238 wait(0.2);
mmpeter 0:6b067dcbab9d 239 break;
mmpeter 0:6b067dcbab9d 240 case 3:
mmpeter 0:6b067dcbab9d 241 //Go 120 degrees from north
mmpeter 0:6b067dcbab9d 242 turn_right();
mmpeter 0:6b067dcbab9d 243 turn_right();
mmpeter 1:48163ae9fe4d 244 thinggy.forward(0.3);
mmpeter 4:9dd1477111da 245 wait(0.2);
mmpeter 0:6b067dcbab9d 246 break;
mmpeter 0:6b067dcbab9d 247 case 4:
mmpeter 0:6b067dcbab9d 248 //Go 180 degrees from north
mmpeter 0:6b067dcbab9d 249 turn_right();
mmpeter 0:6b067dcbab9d 250 turn_right();
mmpeter 0:6b067dcbab9d 251 turn_right();
mmpeter 1:48163ae9fe4d 252 thinggy.forward(0.3);
mmpeter 4:9dd1477111da 253 wait(0.2);
mmpeter 0:6b067dcbab9d 254 break;
mmpeter 0:6b067dcbab9d 255 case 5:
mmpeter 0:6b067dcbab9d 256 //Go 240 degrees from north
mmpeter 1:48163ae9fe4d 257 turn_left();
mmpeter 1:48163ae9fe4d 258 turn_left();
mmpeter 1:48163ae9fe4d 259 thinggy.forward(0.3);
mmpeter 4:9dd1477111da 260 wait(0.2);
mmpeter 0:6b067dcbab9d 261 break;
mmpeter 0:6b067dcbab9d 262 case 6:
mmpeter 0:6b067dcbab9d 263 //Go 300 degrees from north
mmpeter 1:48163ae9fe4d 264 turn_left();
mmpeter 1:48163ae9fe4d 265 thinggy.forward(0.3);
mmpeter 4:9dd1477111da 266 wait(0.2);
mmpeter 0:6b067dcbab9d 267 break;
mmpeter 0:6b067dcbab9d 268 }
mmpeter 0:6b067dcbab9d 269
mmpeter 0:6b067dcbab9d 270 }
mmpeter 0:6b067dcbab9d 271 }
mmpeter 0:6b067dcbab9d 272 }
mmpeter 0:6b067dcbab9d 273
mmpeter 0:6b067dcbab9d 274 //EFFECTS: turns the robot right
mmpeter 0:6b067dcbab9d 275 void turn_right(){
mmpeter 0:6b067dcbab9d 276 int n=0;
mmpeter 0:6b067dcbab9d 277 wait(.08);
mmpeter 3:b2d61294966e 278 while (n<272){
mmpeter 0:6b067dcbab9d 279 thinggy.left_motor(0.2);
mmpeter 0:6b067dcbab9d 280 thinggy.right_motor(-0.2);
mmpeter 0:6b067dcbab9d 281 n++;
mmpeter 0:6b067dcbab9d 282 }
mmpeter 0:6b067dcbab9d 283 wait(0.15);
mmpeter 1:48163ae9fe4d 284 }
mmpeter 1:48163ae9fe4d 285 //EFFECTS: turns the robot left
mmpeter 1:48163ae9fe4d 286 void turn_left(){
mmpeter 1:48163ae9fe4d 287 int n=0;
mmpeter 1:48163ae9fe4d 288 wait(.08);
mmpeter 3:b2d61294966e 289 while (n<272){
mmpeter 1:48163ae9fe4d 290 thinggy.left_motor(-0.2);
mmpeter 1:48163ae9fe4d 291 thinggy.right_motor(0.2);
mmpeter 1:48163ae9fe4d 292 n++;
mmpeter 1:48163ae9fe4d 293 }
mmpeter 1:48163ae9fe4d 294 wait(0.15);
mmpeter 1:48163ae9fe4d 295 }
mmpeter 1:48163ae9fe4d 296
mmpeter 1:48163ae9fe4d 297 //EFFECTS: Play victory tune
mmpeter 1:48163ae9fe4d 298 void victors(){
mmpeter 1:48163ae9fe4d 299 // 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 300 char hail[]={'V','1','0',
mmpeter 1:48163ae9fe4d 301 //hail to the victors valiant
mmpeter 1:48163ae9fe4d 302 'O','5',
mmpeter 1:48163ae9fe4d 303 'C','2',
mmpeter 1:48163ae9fe4d 304
mmpeter 1:48163ae9fe4d 305 'O','4',
mmpeter 1:48163ae9fe4d 306 'G','#','4',
mmpeter 1:48163ae9fe4d 307 'A','#','4',
mmpeter 1:48163ae9fe4d 308
mmpeter 1:48163ae9fe4d 309 'O','5',
mmpeter 1:48163ae9fe4d 310 'C','4',
mmpeter 1:48163ae9fe4d 311
mmpeter 1:48163ae9fe4d 312 'O','4',
mmpeter 1:48163ae9fe4d 313 'G','#','4',
mmpeter 1:48163ae9fe4d 314 'A','#','4',
mmpeter 1:48163ae9fe4d 315
mmpeter 1:48163ae9fe4d 316 'O','5',
mmpeter 1:48163ae9fe4d 317 'C','4',
mmpeter 1:48163ae9fe4d 318
mmpeter 1:48163ae9fe4d 319
mmpeter 1:48163ae9fe4d 320 //hail to the conquering heros
mmpeter 1:48163ae9fe4d 321 'C', '#','2',
mmpeter 1:48163ae9fe4d 322 'O','4',
mmpeter 1:48163ae9fe4d 323 'A','#','4',
mmpeter 1:48163ae9fe4d 324
mmpeter 1:48163ae9fe4d 325 'O','5',
mmpeter 1:48163ae9fe4d 326 'C','4',
mmpeter 1:48163ae9fe4d 327 'C','#','4',
mmpeter 1:48163ae9fe4d 328
mmpeter 1:48163ae9fe4d 329 'O','4',
mmpeter 1:48163ae9fe4d 330 'A','#','4',
mmpeter 1:48163ae9fe4d 331
mmpeter 1:48163ae9fe4d 332 'O','5',
mmpeter 1:48163ae9fe4d 333 'C','4',
mmpeter 1:48163ae9fe4d 334 'C','#','4',
mmpeter 1:48163ae9fe4d 335 };
mmpeter 2:49565c1ff0bf 336
mmpeter 1:48163ae9fe4d 337 //the number of characters in the array
mmpeter 1:48163ae9fe4d 338 int numb=strlen(hail);
mmpeter 1:48163ae9fe4d 339 thinggy.playtune(hail,numb);
mmpeter 1:48163ae9fe4d 340 thinggy.cls();
mmpeter 1:48163ae9fe4d 341 thinggy.printf("I win!");
mmpeter 1:48163ae9fe4d 342 wait(3);
mmpeter 1:48163ae9fe4d 343 }