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