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@0:6b067dcbab9d, 2014-06-05 (annotated)

Committer:
mmpeter
Date:
Thu Jun 05 09:16:52 2014 +0000
Revision:
0:6b067dcbab9d
Child:
1:48163ae9fe4d
My shoddy attempt at finding the extreme

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 0:6b067dcbab9d 10 int darkest=0;
mmpeter 0:6b067dcbab9d 11 int dark;
mmpeter 0:6b067dcbab9d 12 int previousdark;
mmpeter 0:6b067dcbab9d 13 int v1=0,v2=0,v3=0,v4=0,v5=0,v6=0;
mmpeter 0:6b067dcbab9d 14 bool found;
mmpeter 0:6b067dcbab9d 15 int x;
mmpeter 0:6b067dcbab9d 16
mmpeter 0:6b067dcbab9d 17
mmpeter 0:6b067dcbab9d 18 using namespace std;
mmpeter 0:6b067dcbab9d 19 int sensor[5];
mmpeter 0:6b067dcbab9d 20
mmpeter 0:6b067dcbab9d 21 int main() {
mmpeter 0:6b067dcbab9d 22 thinggy.calibrated_sensor(sensor);
mmpeter 0:6b067dcbab9d 23 while(1){
mmpeter 0:6b067dcbab9d 24 //Initial sensor readings
mmpeter 0:6b067dcbab9d 25 while(found!=1){
mmpeter 0:6b067dcbab9d 26 v1=sensor[2];
mmpeter 0:6b067dcbab9d 27 thinggy.stop();
mmpeter 0:6b067dcbab9d 28 wait(0.5);
mmpeter 0:6b067dcbab9d 29 thinggy.cls();
mmpeter 0:6b067dcbab9d 30 thinggy.printf("Got 1");
mmpeter 0:6b067dcbab9d 31 turn_right();
mmpeter 0:6b067dcbab9d 32 //Turn 60 degrees from north
mmpeter 0:6b067dcbab9d 33 v2=sensor[2];
mmpeter 0:6b067dcbab9d 34 thinggy.stop();
mmpeter 0:6b067dcbab9d 35 wait(0.5);
mmpeter 0:6b067dcbab9d 36 thinggy.cls();
mmpeter 0:6b067dcbab9d 37 thinggy.printf("Got 2");
mmpeter 0:6b067dcbab9d 38 turn_right();
mmpeter 0:6b067dcbab9d 39 //120 degrees from north
mmpeter 0:6b067dcbab9d 40 v3=sensor[2];
mmpeter 0:6b067dcbab9d 41 thinggy.stop();
mmpeter 0:6b067dcbab9d 42 wait(0.5);
mmpeter 0:6b067dcbab9d 43 thinggy.cls();
mmpeter 0:6b067dcbab9d 44 thinggy.printf("Got 3");
mmpeter 0:6b067dcbab9d 45 turn_right();
mmpeter 0:6b067dcbab9d 46 //180 degrees from north
mmpeter 0:6b067dcbab9d 47 v4=sensor[2];
mmpeter 0:6b067dcbab9d 48 thinggy.stop();
mmpeter 0:6b067dcbab9d 49 wait(0.5);
mmpeter 0:6b067dcbab9d 50 thinggy.cls();
mmpeter 0:6b067dcbab9d 51 thinggy.printf("Got 4");
mmpeter 0:6b067dcbab9d 52 turn_right();
mmpeter 0:6b067dcbab9d 53 //240 degrees from north
mmpeter 0:6b067dcbab9d 54 v5=sensor[2];
mmpeter 0:6b067dcbab9d 55 thinggy.stop();
mmpeter 0:6b067dcbab9d 56 wait(0.5);
mmpeter 0:6b067dcbab9d 57 thinggy.cls();
mmpeter 0:6b067dcbab9d 58 thinggy.printf("Got 5");
mmpeter 0:6b067dcbab9d 59 turn_right();
mmpeter 0:6b067dcbab9d 60 //300 degrees from north
mmpeter 0:6b067dcbab9d 61 v6=sensor[2];
mmpeter 0:6b067dcbab9d 62 thinggy.stop();
mmpeter 0:6b067dcbab9d 63 wait(0.5);
mmpeter 0:6b067dcbab9d 64 thinggy.cls();
mmpeter 0:6b067dcbab9d 65 thinggy.printf("Got 6");
mmpeter 0:6b067dcbab9d 66 turn_right();
mmpeter 0:6b067dcbab9d 67
mmpeter 0:6b067dcbab9d 68 //Determine which direction is darkest
mmpeter 0:6b067dcbab9d 69 //North is darkest
mmpeter 0:6b067dcbab9d 70 if(v1>v2 and v1>v3 and v1>v4 and v1>v5 and v1>v6){
mmpeter 0:6b067dcbab9d 71 x=1;
mmpeter 0:6b067dcbab9d 72 dark=v1;
mmpeter 0:6b067dcbab9d 73 thinggy.cls();
mmpeter 0:6b067dcbab9d 74 thinggy.printf("It's 1");
mmpeter 0:6b067dcbab9d 75 }
mmpeter 0:6b067dcbab9d 76 //60 degrees from north is darkest
mmpeter 0:6b067dcbab9d 77 else if(v2>v1 and v2>v3 and v2>v4 and v2>v5 and v2>v6){
mmpeter 0:6b067dcbab9d 78 x=2;
mmpeter 0:6b067dcbab9d 79 dark=v2;
mmpeter 0:6b067dcbab9d 80 thinggy.cls();
mmpeter 0:6b067dcbab9d 81 thinggy.printf("It's 2");
mmpeter 0:6b067dcbab9d 82 }
mmpeter 0:6b067dcbab9d 83 //120 degrees from north is darkest
mmpeter 0:6b067dcbab9d 84 else if(v3>v1 and v3>v2 and v3>v4 and v3>v5 and v3>v6){
mmpeter 0:6b067dcbab9d 85 x=3;
mmpeter 0:6b067dcbab9d 86 dark=v3;
mmpeter 0:6b067dcbab9d 87 thinggy.cls();
mmpeter 0:6b067dcbab9d 88 thinggy.printf("It's 3");
mmpeter 0:6b067dcbab9d 89 }
mmpeter 0:6b067dcbab9d 90 //180 degrees from north is darkest
mmpeter 0:6b067dcbab9d 91 else if(v4>v1 and v4>v2 and v4>v3 and v4>v5 and v4>v6){
mmpeter 0:6b067dcbab9d 92 x=4;
mmpeter 0:6b067dcbab9d 93 dark=v4;
mmpeter 0:6b067dcbab9d 94 thinggy.cls();
mmpeter 0:6b067dcbab9d 95 thinggy.printf("It's 4");
mmpeter 0:6b067dcbab9d 96 }
mmpeter 0:6b067dcbab9d 97 //240 degrees from north is darkest
mmpeter 0:6b067dcbab9d 98 else if(v5>v1 and v5>v2 and v5>v3 and v5>v4 and v5>v6){
mmpeter 0:6b067dcbab9d 99 x=5;
mmpeter 0:6b067dcbab9d 100 dark=v5;
mmpeter 0:6b067dcbab9d 101 thinggy.cls();
mmpeter 0:6b067dcbab9d 102 thinggy.printf("It's 5");
mmpeter 0:6b067dcbab9d 103 }
mmpeter 0:6b067dcbab9d 104 //300 degrees from north is darkest
mmpeter 0:6b067dcbab9d 105 else if(v6>v1 and v6>v2 and v6>v3 and v6>v4 and v6>v5){
mmpeter 0:6b067dcbab9d 106 x=6;
mmpeter 0:6b067dcbab9d 107 dark=v6;
mmpeter 0:6b067dcbab9d 108 thinggy.cls();
mmpeter 0:6b067dcbab9d 109 thinggy.printf("It's 6");
mmpeter 0:6b067dcbab9d 110 }
mmpeter 0:6b067dcbab9d 111
mmpeter 0:6b067dcbab9d 112 switch(x){
mmpeter 0:6b067dcbab9d 113 case 1:
mmpeter 0:6b067dcbab9d 114 //Go forward
mmpeter 0:6b067dcbab9d 115 thinggy.forward(0.5);
mmpeter 0:6b067dcbab9d 116 break;
mmpeter 0:6b067dcbab9d 117 case 2:
mmpeter 0:6b067dcbab9d 118 //Go 60 degrees from north
mmpeter 0:6b067dcbab9d 119 turn_right();
mmpeter 0:6b067dcbab9d 120 thinggy.forward(0.5);
mmpeter 0:6b067dcbab9d 121 break;
mmpeter 0:6b067dcbab9d 122 case 3:
mmpeter 0:6b067dcbab9d 123 //Go 120 degrees from north
mmpeter 0:6b067dcbab9d 124 turn_right();
mmpeter 0:6b067dcbab9d 125 turn_right();
mmpeter 0:6b067dcbab9d 126 thinggy.forward(0.5);
mmpeter 0:6b067dcbab9d 127 break;
mmpeter 0:6b067dcbab9d 128 case 4:
mmpeter 0:6b067dcbab9d 129 //Go 180 degrees from north
mmpeter 0:6b067dcbab9d 130 turn_right();
mmpeter 0:6b067dcbab9d 131 turn_right();
mmpeter 0:6b067dcbab9d 132 turn_right();
mmpeter 0:6b067dcbab9d 133 thinggy.forward(0.5);
mmpeter 0:6b067dcbab9d 134 break;
mmpeter 0:6b067dcbab9d 135 case 5:
mmpeter 0:6b067dcbab9d 136 //Go 240 degrees from north
mmpeter 0:6b067dcbab9d 137 turn_right();
mmpeter 0:6b067dcbab9d 138 turn_right();
mmpeter 0:6b067dcbab9d 139 turn_right();
mmpeter 0:6b067dcbab9d 140 turn_right();
mmpeter 0:6b067dcbab9d 141 thinggy.forward(0.5);
mmpeter 0:6b067dcbab9d 142 break;
mmpeter 0:6b067dcbab9d 143 case 6:
mmpeter 0:6b067dcbab9d 144 //Go 300 degrees from north
mmpeter 0:6b067dcbab9d 145 turn_right();
mmpeter 0:6b067dcbab9d 146 turn_right();
mmpeter 0:6b067dcbab9d 147 turn_right();
mmpeter 0:6b067dcbab9d 148 turn_right();
mmpeter 0:6b067dcbab9d 149 turn_right();
mmpeter 0:6b067dcbab9d 150 thinggy.forward(0.5);
mmpeter 0:6b067dcbab9d 151 break;
mmpeter 0:6b067dcbab9d 152 }
mmpeter 0:6b067dcbab9d 153
mmpeter 0:6b067dcbab9d 154 if(dark>darkest){
mmpeter 0:6b067dcbab9d 155 darkest=dark;
mmpeter 0:6b067dcbab9d 156 }
mmpeter 0:6b067dcbab9d 157 //Condition for exiting the loop: darkest spot found
mmpeter 0:6b067dcbab9d 158 if(previousdark>dark){
mmpeter 0:6b067dcbab9d 159 //Go 180 degrees
mmpeter 0:6b067dcbab9d 160 found=1;
mmpeter 0:6b067dcbab9d 161 thinggy.printf("Got it!");
mmpeter 0:6b067dcbab9d 162 }
mmpeter 0:6b067dcbab9d 163 dark=previousdark;
mmpeter 0:6b067dcbab9d 164
mmpeter 0:6b067dcbab9d 165
mmpeter 0:6b067dcbab9d 166
mmpeter 0:6b067dcbab9d 167 }
mmpeter 0:6b067dcbab9d 168 }
mmpeter 0:6b067dcbab9d 169 }
mmpeter 0:6b067dcbab9d 170
mmpeter 0:6b067dcbab9d 171 //EFFECTS: turns the robot right
mmpeter 0:6b067dcbab9d 172 void turn_right(){
mmpeter 0:6b067dcbab9d 173 int n=0;
mmpeter 0:6b067dcbab9d 174 wait(.08);
mmpeter 0:6b067dcbab9d 175 while (n<467){
mmpeter 0:6b067dcbab9d 176 thinggy.left_motor(0.2);
mmpeter 0:6b067dcbab9d 177 thinggy.right_motor(-0.2);
mmpeter 0:6b067dcbab9d 178 n++;
mmpeter 0:6b067dcbab9d 179 }
mmpeter 0:6b067dcbab9d 180 wait(0.15);
mmpeter 0:6b067dcbab9d 181 }