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