vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Sources/Robot.cpp@15:915f8839fe48, 2017-05-11 (annotated)
- Committer:
- EpicG10
- Date:
- Thu May 11 18:57:45 2017 +0000
- Revision:
- 15:915f8839fe48
- Parent:
- 13:6c1f8e35b242
- Child:
- 16:f157e5ccd7d3
- Child:
- 17:4e1be70bdedb
c
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
beacon | 0:d267b248eff4 | 1 | #include "Robot.h" |
beacon | 4:67d7177c213f | 2 | #include "Declarations.h" |
beacon | 0:d267b248eff4 | 3 | |
beacon | 0:d267b248eff4 | 4 | /* Work in progress -------------------------------------------- */ |
beacon | 0:d267b248eff4 | 5 | |
EpicG10 | 15:915f8839fe48 | 6 | Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste ) |
beacon | 0:d267b248eff4 | 7 | { |
beacon | 1:388c915756f5 | 8 | this->left = left; |
beacon | 1:388c915756f5 | 9 | this->right = right; |
beacon | 10:f76476943a6c | 10 | this->powerSignal = powerSignal; |
beacon | 10:f76476943a6c | 11 | //this->errorMotor = errorMotor; |
beacon | 0:d267b248eff4 | 12 | |
beacon | 0:d267b248eff4 | 13 | this->left->period(0.00005f); |
beacon | 0:d267b248eff4 | 14 | this->right->period(0.00005f); |
beacon | 1:388c915756f5 | 15 | |
beacon | 1:388c915756f5 | 16 | this->leds = leds; |
beacon | 1:388c915756f5 | 17 | |
beacon | 1:388c915756f5 | 18 | this->FarbVoltage = FarbVoltage; |
beacon | 4:67d7177c213f | 19 | this->frontS = frontS; |
beacon | 4:67d7177c213f | 20 | this->leftS = leftS; |
beacon | 4:67d7177c213f | 21 | this->rightS = rightS; |
beacon | 4:67d7177c213f | 22 | |
beacon | 6:ba26dd3251b3 | 23 | this->Arm = Arm; |
beacon | 10:f76476943a6c | 24 | this->Greifer = Greifer; |
EpicG10 | 15:915f8839fe48 | 25 | this->Leiste = Leiste; |
beacon | 1:388c915756f5 | 26 | |
beacon | 0:d267b248eff4 | 27 | } |
beacon | 0:d267b248eff4 | 28 | |
beacon | 4:67d7177c213f | 29 | //Drive functions |
beacon | 0:d267b248eff4 | 30 | void Robot::drive() |
beacon | 0:d267b248eff4 | 31 | { |
beacon | 0:d267b248eff4 | 32 | //pwm determine what direction it goes. |
beacon | 0:d267b248eff4 | 33 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 34 | *left= 0.6f; |
beacon | 1:388c915756f5 | 35 | *right= 0.4f; |
beacon | 1:388c915756f5 | 36 | } |
beacon | 1:388c915756f5 | 37 | |
beacon | 4:67d7177c213f | 38 | void Robot::driveB() |
beacon | 4:67d7177c213f | 39 | { |
beacon | 4:67d7177c213f | 40 | //pwm determine what direction it goes. |
beacon | 4:67d7177c213f | 41 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 42 | *left= 0.4f; |
beacon | 4:67d7177c213f | 43 | *right= 0.6f; |
beacon | 4:67d7177c213f | 44 | } |
beacon | 4:67d7177c213f | 45 | |
beacon | 1:388c915756f5 | 46 | void Robot::turnLeft() |
beacon | 1:388c915756f5 | 47 | { |
beacon | 1:388c915756f5 | 48 | |
beacon | 1:388c915756f5 | 49 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 50 | *left= 0.4f; |
beacon | 1:388c915756f5 | 51 | *right= 0.4f; |
beacon | 1:388c915756f5 | 52 | |
beacon | 0:d267b248eff4 | 53 | } |
beacon | 0:d267b248eff4 | 54 | |
beacon | 4:67d7177c213f | 55 | void Robot::turnLeftS() |
beacon | 4:67d7177c213f | 56 | { |
beacon | 4:67d7177c213f | 57 | |
beacon | 4:67d7177c213f | 58 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 59 | *left= 0.45f; |
beacon | 4:67d7177c213f | 60 | *right= 0.45f; |
beacon | 4:67d7177c213f | 61 | |
beacon | 4:67d7177c213f | 62 | } |
beacon | 4:67d7177c213f | 63 | |
beacon | 1:388c915756f5 | 64 | void Robot::turnRight() |
beacon | 1:388c915756f5 | 65 | { |
beacon | 1:388c915756f5 | 66 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 67 | *left= 0.6f; |
beacon | 1:388c915756f5 | 68 | *right= 0.6f; |
beacon | 1:388c915756f5 | 69 | } |
beacon | 1:388c915756f5 | 70 | |
beacon | 4:67d7177c213f | 71 | void Robot::turnRightS() |
beacon | 4:67d7177c213f | 72 | { |
beacon | 4:67d7177c213f | 73 | |
beacon | 4:67d7177c213f | 74 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 75 | *left= 0.55f; |
beacon | 4:67d7177c213f | 76 | *right= 0.55f; |
beacon | 4:67d7177c213f | 77 | |
beacon | 4:67d7177c213f | 78 | } |
beacon | 4:67d7177c213f | 79 | |
beacon | 1:388c915756f5 | 80 | void Robot::turnAround(int left) |
beacon | 1:388c915756f5 | 81 | { |
beacon | 0:d267b248eff4 | 82 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 83 | |
beacon | 1:388c915756f5 | 84 | if (left) { |
beacon | 1:388c915756f5 | 85 | turnLeft(); |
beacon | 1:388c915756f5 | 86 | } |
beacon | 1:388c915756f5 | 87 | |
beacon | 1:388c915756f5 | 88 | else { |
beacon | 1:388c915756f5 | 89 | turnRight(); |
beacon | 1:388c915756f5 | 90 | } |
beacon | 1:388c915756f5 | 91 | } |
beacon | 1:388c915756f5 | 92 | |
beacon | 1:388c915756f5 | 93 | void Robot::stop() |
beacon | 1:388c915756f5 | 94 | { |
beacon | 1:388c915756f5 | 95 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 96 | *left= 0.5f; |
beacon | 1:388c915756f5 | 97 | *right= 0.5f; |
beacon | 0:d267b248eff4 | 98 | } |
beacon | 0:d267b248eff4 | 99 | |
beacon | 4:67d7177c213f | 100 | |
beacon | 4:67d7177c213f | 101 | //Functions that use the drive functions |
beacon | 4:67d7177c213f | 102 | void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){ |
beacon | 4:67d7177c213f | 103 | if (*lastAct != 0){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 104 | *timer = 0; |
beacon | 4:67d7177c213f | 105 | *lastAct = 0; |
beacon | 4:67d7177c213f | 106 | } |
beacon | 4:67d7177c213f | 107 | |
beacon | 4:67d7177c213f | 108 | if (*rando == -1){ //If rando was unused, set a new number. |
beacon | 4:67d7177c213f | 109 | *rando = rand() % 2; |
beacon | 4:67d7177c213f | 110 | } |
beacon | 4:67d7177c213f | 111 | |
beacon | 4:67d7177c213f | 112 | if (this->sensors[FWD] < NEAR){ //While something is seen turn around. |
beacon | 4:67d7177c213f | 113 | this->turnAround(*rando); |
beacon | 4:67d7177c213f | 114 | } |
beacon | 4:67d7177c213f | 115 | |
beacon | 4:67d7177c213f | 116 | else{ |
beacon | 4:67d7177c213f | 117 | *rando = -1; |
beacon | 4:67d7177c213f | 118 | *counter = 0; |
beacon | 4:67d7177c213f | 119 | } |
beacon | 4:67d7177c213f | 120 | } |
beacon | 4:67d7177c213f | 121 | |
beacon | 10:f76476943a6c | 122 | void Robot::wallRight(int* counter, int* timer, int* lastAct){ |
beacon | 10:f76476943a6c | 123 | *counter += 1; |
beacon | 10:f76476943a6c | 124 | |
beacon | 10:f76476943a6c | 125 | if (*lastAct != 1){ //If this wasn't the last called action, reset the timer. |
beacon | 10:f76476943a6c | 126 | *timer = 0; |
beacon | 10:f76476943a6c | 127 | *lastAct = 1; |
beacon | 10:f76476943a6c | 128 | } |
beacon | 10:f76476943a6c | 129 | |
beacon | 10:f76476943a6c | 130 | this->turnLeft(); |
beacon | 10:f76476943a6c | 131 | } |
beacon | 10:f76476943a6c | 132 | |
beacon | 4:67d7177c213f | 133 | void Robot::wallLeft(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 134 | *counter += 1; |
beacon | 4:67d7177c213f | 135 | |
beacon | 4:67d7177c213f | 136 | if (*lastAct != 2){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 137 | *timer = 0; |
beacon | 4:67d7177c213f | 138 | *lastAct = 2; |
beacon | 4:67d7177c213f | 139 | } |
beacon | 4:67d7177c213f | 140 | |
beacon | 4:67d7177c213f | 141 | this->turnRight(); |
beacon | 4:67d7177c213f | 142 | } |
beacon | 4:67d7177c213f | 143 | |
beacon | 4:67d7177c213f | 144 | void Robot::wallFront(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 145 | if (*lastAct != 3){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 146 | *timer = 0; |
beacon | 4:67d7177c213f | 147 | *lastAct = 3; |
beacon | 4:67d7177c213f | 148 | } |
beacon | 4:67d7177c213f | 149 | |
beacon | 4:67d7177c213f | 150 | *counter = MAX; //By setting the counter to MAX, next time it will go into the first if-statement (action 0). |
beacon | 4:67d7177c213f | 151 | } |
beacon | 4:67d7177c213f | 152 | |
beacon | 4:67d7177c213f | 153 | |
beacon | 5:1aaf5de776ff | 154 | void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound, int* found){ |
beacon | 4:67d7177c213f | 155 | //*counter += 1; |
beacon | 4:67d7177c213f | 156 | *legoFound = 0; |
beacon | 4:67d7177c213f | 157 | |
beacon | 5:1aaf5de776ff | 158 | if (*lastAct != 4){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 159 | *timer = 0; |
beacon | 4:67d7177c213f | 160 | *lastAct = 4; |
beacon | 4:67d7177c213f | 161 | } |
beacon | 4:67d7177c213f | 162 | |
beacon | 5:1aaf5de776ff | 163 | if (this->sensors[FWD] < NEAR){ //If Sam sees a wall turn around |
beacon | 4:67d7177c213f | 164 | *legoFound = -1; |
beacon | 5:1aaf5de776ff | 165 | *counter = MAX; //setting counter to MAX will couse sam to turnAround |
beacon | 4:67d7177c213f | 166 | } |
beacon | 4:67d7177c213f | 167 | |
beacon | 5:1aaf5de776ff | 168 | if (this->sensors[FWD_L] > 0.16f){ |
beacon | 4:67d7177c213f | 169 | this->drive(); |
beacon | 4:67d7177c213f | 170 | } |
beacon | 4:67d7177c213f | 171 | else{ |
beacon | 11:292bdbd85a9c | 172 | this->stop(); |
beacon | 5:1aaf5de776ff | 173 | *found = 1; |
beacon | 4:67d7177c213f | 174 | } |
beacon | 4:67d7177c213f | 175 | } |
beacon | 4:67d7177c213f | 176 | |
beacon | 4:67d7177c213f | 177 | void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){ |
beacon | 4:67d7177c213f | 178 | //*counter += 1; |
beacon | 4:67d7177c213f | 179 | *legoFound = 1; |
beacon | 4:67d7177c213f | 180 | |
beacon | 4:67d7177c213f | 181 | if (*lastAct != 5){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 182 | *timer = 0; |
beacon | 4:67d7177c213f | 183 | *lastAct = 5; |
beacon | 4:67d7177c213f | 184 | } |
beacon | 4:67d7177c213f | 185 | |
beacon | 5:1aaf5de776ff | 186 | if (this->sensors[FWD_L] > 0.22f){ |
beacon | 4:67d7177c213f | 187 | this->turnRight(); |
beacon | 4:67d7177c213f | 188 | } |
beacon | 4:67d7177c213f | 189 | else{ |
beacon | 4:67d7177c213f | 190 | this->stop(); |
beacon | 5:1aaf5de776ff | 191 | *legoFound = -1; |
beacon | 4:67d7177c213f | 192 | } |
beacon | 4:67d7177c213f | 193 | } |
beacon | 4:67d7177c213f | 194 | |
beacon | 4:67d7177c213f | 195 | void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){ |
beacon | 4:67d7177c213f | 196 | //*counter += 1; |
beacon | 4:67d7177c213f | 197 | *legoFound = 2; |
beacon | 0:d267b248eff4 | 198 | |
beacon | 4:67d7177c213f | 199 | if (*lastAct != 6){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 200 | *timer = 0; |
beacon | 4:67d7177c213f | 201 | *lastAct = 6; |
beacon | 4:67d7177c213f | 202 | } |
beacon | 4:67d7177c213f | 203 | |
beacon | 5:1aaf5de776ff | 204 | if (this->sensors[FWD_L] > 0.22f){ |
beacon | 4:67d7177c213f | 205 | this->turnLeft(); |
beacon | 4:67d7177c213f | 206 | } |
beacon | 4:67d7177c213f | 207 | else{ |
beacon | 4:67d7177c213f | 208 | this->stop(); |
beacon | 5:1aaf5de776ff | 209 | *legoFound = -1; |
beacon | 4:67d7177c213f | 210 | } |
beacon | 4:67d7177c213f | 211 | } |
beacon | 4:67d7177c213f | 212 | |
beacon | 4:67d7177c213f | 213 | |
beacon | 4:67d7177c213f | 214 | void Robot::nothingFound(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 215 | *counter = 0; |
beacon | 4:67d7177c213f | 216 | if (*lastAct != 7){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 217 | *timer = 0; |
beacon | 4:67d7177c213f | 218 | *lastAct = 7; |
beacon | 4:67d7177c213f | 219 | } |
beacon | 4:67d7177c213f | 220 | |
beacon | 4:67d7177c213f | 221 | this->drive(); |
beacon | 4:67d7177c213f | 222 | } |
beacon | 4:67d7177c213f | 223 | |
beacon | 4:67d7177c213f | 224 | |
beacon | 11:292bdbd85a9c | 225 | int Robot::search(int* timer){ |
EpicG10 | 13:6c1f8e35b242 | 226 | //return 1; |
beacon | 11:292bdbd85a9c | 227 | enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR }; |
beacon | 4:67d7177c213f | 228 | |
beacon | 11:292bdbd85a9c | 229 | static int state = neutral; |
beacon | 4:67d7177c213f | 230 | |
beacon | 11:292bdbd85a9c | 231 | static int counter = 0; //counter is used blabla |
beacon | 4:67d7177c213f | 232 | |
beacon | 11:292bdbd85a9c | 233 | static int rando = -1; |
beacon | 10:f76476943a6c | 234 | |
beacon | 11:292bdbd85a9c | 235 | //static int lastAct = 0; |
beacon | 4:67d7177c213f | 236 | |
beacon | 11:292bdbd85a9c | 237 | switch( state ){ |
beacon | 11:292bdbd85a9c | 238 | case neutral: |
beacon | 11:292bdbd85a9c | 239 | if( counter > MAX ){ |
beacon | 11:292bdbd85a9c | 240 | state = max; |
beacon | 11:292bdbd85a9c | 241 | } |
EpicG10 | 13:6c1f8e35b242 | 242 | else if( this->sensors[FWD] < NEAR ){ |
beacon | 11:292bdbd85a9c | 243 | state = wallF; |
EpicG10 | 13:6c1f8e35b242 | 244 | } |
beacon | 11:292bdbd85a9c | 245 | else if( this->sensors[LEFT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 246 | state = wallL; |
beacon | 11:292bdbd85a9c | 247 | } |
beacon | 11:292bdbd85a9c | 248 | else if( this->sensors[RIGHT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 249 | state = wallR; |
beacon | 11:292bdbd85a9c | 250 | } |
beacon | 11:292bdbd85a9c | 251 | else if( this->sensors[FWD_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 252 | state = legoF; |
beacon | 11:292bdbd85a9c | 253 | } |
beacon | 11:292bdbd85a9c | 254 | else if( this->sensors[LEFT_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 255 | state = legoL; |
beacon | 11:292bdbd85a9c | 256 | } |
beacon | 11:292bdbd85a9c | 257 | else if( this->sensors[RIGHT_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 258 | state = legoR; |
beacon | 11:292bdbd85a9c | 259 | } |
beacon | 11:292bdbd85a9c | 260 | else{ |
beacon | 11:292bdbd85a9c | 261 | this->drive(); |
beacon | 11:292bdbd85a9c | 262 | counter = 0; |
beacon | 11:292bdbd85a9c | 263 | } |
beacon | 11:292bdbd85a9c | 264 | break; |
beacon | 11:292bdbd85a9c | 265 | |
beacon | 11:292bdbd85a9c | 266 | case max: { |
beacon | 11:292bdbd85a9c | 267 | int time = 0; |
beacon | 11:292bdbd85a9c | 268 | if( time < 15 && this->sensors[FWD] > NEAR ){ |
beacon | 11:292bdbd85a9c | 269 | rando == -1 ? rando = rand() % 2 : rando = rando; |
beacon | 11:292bdbd85a9c | 270 | this->turnAround(rando); |
beacon | 11:292bdbd85a9c | 271 | } |
beacon | 11:292bdbd85a9c | 272 | else{ |
beacon | 11:292bdbd85a9c | 273 | state = neutral; |
beacon | 11:292bdbd85a9c | 274 | counter = 0; |
beacon | 11:292bdbd85a9c | 275 | rando = -1; |
beacon | 11:292bdbd85a9c | 276 | } |
beacon | 11:292bdbd85a9c | 277 | break; |
beacon | 11:292bdbd85a9c | 278 | } |
beacon | 11:292bdbd85a9c | 279 | |
beacon | 11:292bdbd85a9c | 280 | case wallF: |
beacon | 11:292bdbd85a9c | 281 | counter++; |
beacon | 11:292bdbd85a9c | 282 | if( this->sensors[FWD] < NEAR ){ |
beacon | 11:292bdbd85a9c | 283 | rando == -1 ? rando = rand() % 2 : rando = rando; |
beacon | 11:292bdbd85a9c | 284 | this->turnAround(rando); |
beacon | 11:292bdbd85a9c | 285 | } |
beacon | 11:292bdbd85a9c | 286 | else{ |
beacon | 11:292bdbd85a9c | 287 | state = neutral; |
beacon | 11:292bdbd85a9c | 288 | rando = -1; |
beacon | 11:292bdbd85a9c | 289 | } |
beacon | 11:292bdbd85a9c | 290 | break; |
beacon | 11:292bdbd85a9c | 291 | |
beacon | 11:292bdbd85a9c | 292 | case wallL: |
beacon | 11:292bdbd85a9c | 293 | counter++; |
beacon | 11:292bdbd85a9c | 294 | if( this->sensors[LEFT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 295 | this->turnRight(); |
beacon | 11:292bdbd85a9c | 296 | } |
beacon | 11:292bdbd85a9c | 297 | else{ |
beacon | 11:292bdbd85a9c | 298 | state = neutral; |
beacon | 11:292bdbd85a9c | 299 | } |
beacon | 11:292bdbd85a9c | 300 | break; |
beacon | 11:292bdbd85a9c | 301 | |
beacon | 11:292bdbd85a9c | 302 | case wallR: |
beacon | 11:292bdbd85a9c | 303 | counter++; |
beacon | 11:292bdbd85a9c | 304 | if( this->sensors[RIGHT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 305 | this->turnLeft(); |
beacon | 11:292bdbd85a9c | 306 | } |
beacon | 11:292bdbd85a9c | 307 | else{ |
beacon | 11:292bdbd85a9c | 308 | state = neutral; |
beacon | 11:292bdbd85a9c | 309 | } |
beacon | 11:292bdbd85a9c | 310 | break; |
beacon | 11:292bdbd85a9c | 311 | |
beacon | 11:292bdbd85a9c | 312 | case legoF: |
beacon | 11:292bdbd85a9c | 313 | //counter++; |
EpicG10 | 13:6c1f8e35b242 | 314 | if( this->sensors[FWD_L] > 0.17f ){ |
beacon | 11:292bdbd85a9c | 315 | this->drive(); |
beacon | 11:292bdbd85a9c | 316 | } |
beacon | 11:292bdbd85a9c | 317 | else{ |
beacon | 11:292bdbd85a9c | 318 | state = neutral; |
beacon | 11:292bdbd85a9c | 319 | counter = 0; |
beacon | 11:292bdbd85a9c | 320 | this->stop(); |
beacon | 11:292bdbd85a9c | 321 | return 1; |
beacon | 11:292bdbd85a9c | 322 | } |
beacon | 11:292bdbd85a9c | 323 | break; |
beacon | 11:292bdbd85a9c | 324 | |
beacon | 11:292bdbd85a9c | 325 | case legoL: |
beacon | 11:292bdbd85a9c | 326 | counter++; |
EpicG10 | 13:6c1f8e35b242 | 327 | if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){ |
beacon | 11:292bdbd85a9c | 328 | this->turnLeft(); |
beacon | 11:292bdbd85a9c | 329 | } |
beacon | 11:292bdbd85a9c | 330 | else{ |
beacon | 11:292bdbd85a9c | 331 | state = legoF; |
beacon | 11:292bdbd85a9c | 332 | } |
beacon | 11:292bdbd85a9c | 333 | break; |
beacon | 11:292bdbd85a9c | 334 | |
beacon | 11:292bdbd85a9c | 335 | case legoR: |
beacon | 11:292bdbd85a9c | 336 | counter++; |
EpicG10 | 13:6c1f8e35b242 | 337 | if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){ |
beacon | 11:292bdbd85a9c | 338 | this->turnRight(); |
beacon | 11:292bdbd85a9c | 339 | } |
beacon | 11:292bdbd85a9c | 340 | else{ |
beacon | 11:292bdbd85a9c | 341 | state = legoF; |
beacon | 11:292bdbd85a9c | 342 | } |
beacon | 11:292bdbd85a9c | 343 | break; |
beacon | 11:292bdbd85a9c | 344 | |
beacon | 4:67d7177c213f | 345 | } |
beacon | 11:292bdbd85a9c | 346 | return 0; |
beacon | 4:67d7177c213f | 347 | } |
beacon | 4:67d7177c213f | 348 | |
beacon | 10:f76476943a6c | 349 | int Robot::getErrorMotor(){ |
beacon | 10:f76476943a6c | 350 | return 0; //errorMotor; |
beacon | 0:d267b248eff4 | 351 | } |
beacon | 0:d267b248eff4 | 352 | |
beacon | 11:292bdbd85a9c | 353 | /*int Robot::searchh(int* timer){ |
beacon | 11:292bdbd85a9c | 354 | enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR }; |
beacon | 11:292bdbd85a9c | 355 | |
beacon | 11:292bdbd85a9c | 356 | static int state = neutral; |
beacon | 11:292bdbd85a9c | 357 | |
beacon | 11:292bdbd85a9c | 358 | static int counter = 0; //counter is used blabla |
beacon | 11:292bdbd85a9c | 359 | |
beacon | 11:292bdbd85a9c | 360 | //static int lastAct = 0; |
beacon | 11:292bdbd85a9c | 361 | |
beacon | 11:292bdbd85a9c | 362 | switch( state ){ |
beacon | 11:292bdbd85a9c | 363 | case neutral: |
beacon | 11:292bdbd85a9c | 364 | if( counter > MAX ){ |
beacon | 11:292bdbd85a9c | 365 | state = max; |
beacon | 11:292bdbd85a9c | 366 | } |
beacon | 11:292bdbd85a9c | 367 | else if( this->sensora[FWD] < NEAR ){ |
beacon | 11:292bdbd85a9c | 368 | state = wallF; |
beacon | 11:292bdbd85a9c | 369 | counter = 0; |
beacon | 11:292bdbd85a9c | 370 | } |
beacon | 11:292bdbd85a9c | 371 | else if( this->sensors[LEFT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 372 | state = wallL; |
beacon | 11:292bdbd85a9c | 373 | counter = 0; |
beacon | 11:292bdbd85a9c | 374 | } |
beacon | 11:292bdbd85a9c | 375 | else if( this->sensors[RIGHT] < NEAR ){ |
beacon | 11:292bdbd85a9c | 376 | state = wallR; |
beacon | 11:292bdbd85a9c | 377 | counter = 0; |
beacon | 11:292bdbd85a9c | 378 | } |
beacon | 11:292bdbd85a9c | 379 | else if( this->sensors[FWD_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 380 | state = legoF; |
beacon | 11:292bdbd85a9c | 381 | counter = 0; |
beacon | 11:292bdbd85a9c | 382 | } |
beacon | 11:292bdbd85a9c | 383 | else if( this->sensors[LEFT_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 384 | state = legoL; |
beacon | 11:292bdbd85a9c | 385 | counter = 0; |
beacon | 11:292bdbd85a9c | 386 | } |
beacon | 11:292bdbd85a9c | 387 | else if( this->sensors[RIGHT_L] < NEAR ){ |
beacon | 11:292bdbd85a9c | 388 | state = legoR; |
beacon | 11:292bdbd85a9c | 389 | counter = 0; |
beacon | 11:292bdbd85a9c | 390 | } |
beacon | 11:292bdbd85a9c | 391 | break; |
beacon | 11:292bdbd85a9c | 392 | |
beacon | 11:292bdbd85a9c | 393 | case max: |
beacon | 11:292bdbd85a9c | 394 | |
beacon | 11:292bdbd85a9c | 395 | break; |
beacon | 11:292bdbd85a9c | 396 | */ |
beacon | 11:292bdbd85a9c | 397 | |
beacon | 0:d267b248eff4 | 398 | //reset |
beacon | 0:d267b248eff4 | 399 | //} |
beacon | 0:d267b248eff4 | 400 | |
beacon | 0:d267b248eff4 | 401 | //Add management for Overpower!! Pin PB_15 |