vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Sources/Robot.cpp@10:f76476943a6c, 2017-05-03 (annotated)
- Committer:
- beacon
- Date:
- Wed May 03 13:54:51 2017 +0000
- Revision:
- 10:f76476943a6c
- Parent:
- 6:ba26dd3251b3
- Child:
- 11:292bdbd85a9c
oi
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 | 10:f76476943a6c | 6 | Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer ) |
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; |
beacon | 1:388c915756f5 | 25 | |
beacon | 0:d267b248eff4 | 26 | } |
beacon | 0:d267b248eff4 | 27 | |
beacon | 4:67d7177c213f | 28 | //Drive functions |
beacon | 0:d267b248eff4 | 29 | void Robot::drive() |
beacon | 0:d267b248eff4 | 30 | { |
beacon | 0:d267b248eff4 | 31 | //pwm determine what direction it goes. |
beacon | 0:d267b248eff4 | 32 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 33 | *left= 0.6f; |
beacon | 1:388c915756f5 | 34 | *right= 0.4f; |
beacon | 1:388c915756f5 | 35 | } |
beacon | 1:388c915756f5 | 36 | |
beacon | 4:67d7177c213f | 37 | void Robot::driveB() |
beacon | 4:67d7177c213f | 38 | { |
beacon | 4:67d7177c213f | 39 | //pwm determine what direction it goes. |
beacon | 4:67d7177c213f | 40 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 41 | *left= 0.4f; |
beacon | 4:67d7177c213f | 42 | *right= 0.6f; |
beacon | 4:67d7177c213f | 43 | } |
beacon | 4:67d7177c213f | 44 | |
beacon | 1:388c915756f5 | 45 | void Robot::turnLeft() |
beacon | 1:388c915756f5 | 46 | { |
beacon | 1:388c915756f5 | 47 | |
beacon | 1:388c915756f5 | 48 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 49 | *left= 0.4f; |
beacon | 1:388c915756f5 | 50 | *right= 0.4f; |
beacon | 1:388c915756f5 | 51 | |
beacon | 0:d267b248eff4 | 52 | } |
beacon | 0:d267b248eff4 | 53 | |
beacon | 4:67d7177c213f | 54 | void Robot::turnLeftS() |
beacon | 4:67d7177c213f | 55 | { |
beacon | 4:67d7177c213f | 56 | |
beacon | 4:67d7177c213f | 57 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 58 | *left= 0.45f; |
beacon | 4:67d7177c213f | 59 | *right= 0.45f; |
beacon | 4:67d7177c213f | 60 | |
beacon | 4:67d7177c213f | 61 | } |
beacon | 4:67d7177c213f | 62 | |
beacon | 1:388c915756f5 | 63 | void Robot::turnRight() |
beacon | 1:388c915756f5 | 64 | { |
beacon | 1:388c915756f5 | 65 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 66 | *left= 0.6f; |
beacon | 1:388c915756f5 | 67 | *right= 0.6f; |
beacon | 1:388c915756f5 | 68 | } |
beacon | 1:388c915756f5 | 69 | |
beacon | 4:67d7177c213f | 70 | void Robot::turnRightS() |
beacon | 4:67d7177c213f | 71 | { |
beacon | 4:67d7177c213f | 72 | |
beacon | 4:67d7177c213f | 73 | *powerSignal = 1; |
beacon | 4:67d7177c213f | 74 | *left= 0.55f; |
beacon | 4:67d7177c213f | 75 | *right= 0.55f; |
beacon | 4:67d7177c213f | 76 | |
beacon | 4:67d7177c213f | 77 | } |
beacon | 4:67d7177c213f | 78 | |
beacon | 1:388c915756f5 | 79 | void Robot::turnAround(int left) |
beacon | 1:388c915756f5 | 80 | { |
beacon | 0:d267b248eff4 | 81 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 82 | |
beacon | 1:388c915756f5 | 83 | if (left) { |
beacon | 1:388c915756f5 | 84 | turnLeft(); |
beacon | 1:388c915756f5 | 85 | } |
beacon | 1:388c915756f5 | 86 | |
beacon | 1:388c915756f5 | 87 | else { |
beacon | 1:388c915756f5 | 88 | turnRight(); |
beacon | 1:388c915756f5 | 89 | } |
beacon | 1:388c915756f5 | 90 | } |
beacon | 1:388c915756f5 | 91 | |
beacon | 1:388c915756f5 | 92 | void Robot::stop() |
beacon | 1:388c915756f5 | 93 | { |
beacon | 1:388c915756f5 | 94 | *powerSignal = 1; |
beacon | 1:388c915756f5 | 95 | *left= 0.5f; |
beacon | 1:388c915756f5 | 96 | *right= 0.5f; |
beacon | 0:d267b248eff4 | 97 | } |
beacon | 0:d267b248eff4 | 98 | |
beacon | 4:67d7177c213f | 99 | |
beacon | 4:67d7177c213f | 100 | //Functions that use the drive functions |
beacon | 4:67d7177c213f | 101 | void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){ |
beacon | 4:67d7177c213f | 102 | if (*lastAct != 0){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 103 | *timer = 0; |
beacon | 4:67d7177c213f | 104 | *lastAct = 0; |
beacon | 4:67d7177c213f | 105 | } |
beacon | 4:67d7177c213f | 106 | |
beacon | 4:67d7177c213f | 107 | if (*rando == -1){ //If rando was unused, set a new number. |
beacon | 4:67d7177c213f | 108 | *rando = rand() % 2; |
beacon | 4:67d7177c213f | 109 | } |
beacon | 4:67d7177c213f | 110 | |
beacon | 4:67d7177c213f | 111 | if (this->sensors[FWD] < NEAR){ //While something is seen turn around. |
beacon | 4:67d7177c213f | 112 | this->turnAround(*rando); |
beacon | 4:67d7177c213f | 113 | } |
beacon | 4:67d7177c213f | 114 | |
beacon | 4:67d7177c213f | 115 | else{ |
beacon | 4:67d7177c213f | 116 | *rando = -1; |
beacon | 4:67d7177c213f | 117 | *counter = 0; |
beacon | 4:67d7177c213f | 118 | } |
beacon | 4:67d7177c213f | 119 | } |
beacon | 4:67d7177c213f | 120 | |
beacon | 10:f76476943a6c | 121 | void Robot::wallRight(int* counter, int* timer, int* lastAct){ |
beacon | 10:f76476943a6c | 122 | *counter += 1; |
beacon | 10:f76476943a6c | 123 | |
beacon | 10:f76476943a6c | 124 | if (*lastAct != 1){ //If this wasn't the last called action, reset the timer. |
beacon | 10:f76476943a6c | 125 | *timer = 0; |
beacon | 10:f76476943a6c | 126 | *lastAct = 1; |
beacon | 10:f76476943a6c | 127 | } |
beacon | 10:f76476943a6c | 128 | |
beacon | 10:f76476943a6c | 129 | this->turnLeft(); |
beacon | 10:f76476943a6c | 130 | } |
beacon | 10:f76476943a6c | 131 | |
beacon | 4:67d7177c213f | 132 | void Robot::wallLeft(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 133 | *counter += 1; |
beacon | 4:67d7177c213f | 134 | |
beacon | 4:67d7177c213f | 135 | if (*lastAct != 2){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 136 | *timer = 0; |
beacon | 4:67d7177c213f | 137 | *lastAct = 2; |
beacon | 4:67d7177c213f | 138 | } |
beacon | 4:67d7177c213f | 139 | |
beacon | 4:67d7177c213f | 140 | this->turnRight(); |
beacon | 4:67d7177c213f | 141 | } |
beacon | 4:67d7177c213f | 142 | |
beacon | 4:67d7177c213f | 143 | void Robot::wallFront(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 144 | if (*lastAct != 3){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 145 | *timer = 0; |
beacon | 4:67d7177c213f | 146 | *lastAct = 3; |
beacon | 4:67d7177c213f | 147 | } |
beacon | 4:67d7177c213f | 148 | |
beacon | 4:67d7177c213f | 149 | *counter = MAX; //By setting the counter to MAX, next time it will go into the first if-statement (action 0). |
beacon | 4:67d7177c213f | 150 | } |
beacon | 4:67d7177c213f | 151 | |
beacon | 4:67d7177c213f | 152 | |
beacon | 5:1aaf5de776ff | 153 | void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound, int* found){ |
beacon | 4:67d7177c213f | 154 | //*counter += 1; |
beacon | 4:67d7177c213f | 155 | *legoFound = 0; |
beacon | 4:67d7177c213f | 156 | |
beacon | 5:1aaf5de776ff | 157 | if (*lastAct != 4){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 158 | *timer = 0; |
beacon | 4:67d7177c213f | 159 | *lastAct = 4; |
beacon | 4:67d7177c213f | 160 | } |
beacon | 4:67d7177c213f | 161 | |
beacon | 5:1aaf5de776ff | 162 | if (this->sensors[FWD] < NEAR){ //If Sam sees a wall turn around |
beacon | 4:67d7177c213f | 163 | *legoFound = -1; |
beacon | 5:1aaf5de776ff | 164 | *counter = MAX; //setting counter to MAX will couse sam to turnAround |
beacon | 4:67d7177c213f | 165 | } |
beacon | 4:67d7177c213f | 166 | |
beacon | 5:1aaf5de776ff | 167 | if (this->sensors[FWD_L] > 0.16f){ |
beacon | 4:67d7177c213f | 168 | this->drive(); |
beacon | 4:67d7177c213f | 169 | } |
beacon | 4:67d7177c213f | 170 | else{ |
beacon | 5:1aaf5de776ff | 171 | *found = 1; |
beacon | 4:67d7177c213f | 172 | } |
beacon | 4:67d7177c213f | 173 | } |
beacon | 4:67d7177c213f | 174 | |
beacon | 4:67d7177c213f | 175 | void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){ |
beacon | 4:67d7177c213f | 176 | //*counter += 1; |
beacon | 4:67d7177c213f | 177 | *legoFound = 1; |
beacon | 4:67d7177c213f | 178 | |
beacon | 4:67d7177c213f | 179 | if (*lastAct != 5){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 180 | *timer = 0; |
beacon | 4:67d7177c213f | 181 | *lastAct = 5; |
beacon | 4:67d7177c213f | 182 | } |
beacon | 4:67d7177c213f | 183 | |
beacon | 5:1aaf5de776ff | 184 | if (this->sensors[FWD_L] > 0.22f){ |
beacon | 4:67d7177c213f | 185 | this->turnRight(); |
beacon | 4:67d7177c213f | 186 | } |
beacon | 4:67d7177c213f | 187 | else{ |
beacon | 4:67d7177c213f | 188 | this->stop(); |
beacon | 5:1aaf5de776ff | 189 | *legoFound = -1; |
beacon | 4:67d7177c213f | 190 | } |
beacon | 4:67d7177c213f | 191 | } |
beacon | 4:67d7177c213f | 192 | |
beacon | 4:67d7177c213f | 193 | void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){ |
beacon | 4:67d7177c213f | 194 | //*counter += 1; |
beacon | 4:67d7177c213f | 195 | *legoFound = 2; |
beacon | 0:d267b248eff4 | 196 | |
beacon | 4:67d7177c213f | 197 | if (*lastAct != 6){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 198 | *timer = 0; |
beacon | 4:67d7177c213f | 199 | *lastAct = 6; |
beacon | 4:67d7177c213f | 200 | } |
beacon | 4:67d7177c213f | 201 | |
beacon | 5:1aaf5de776ff | 202 | if (this->sensors[FWD_L] > 0.22f){ |
beacon | 4:67d7177c213f | 203 | this->turnLeft(); |
beacon | 4:67d7177c213f | 204 | } |
beacon | 4:67d7177c213f | 205 | else{ |
beacon | 4:67d7177c213f | 206 | this->stop(); |
beacon | 5:1aaf5de776ff | 207 | *legoFound = -1; |
beacon | 4:67d7177c213f | 208 | } |
beacon | 4:67d7177c213f | 209 | } |
beacon | 4:67d7177c213f | 210 | |
beacon | 4:67d7177c213f | 211 | |
beacon | 4:67d7177c213f | 212 | void Robot::nothingFound(int* counter, int* timer, int* lastAct){ |
beacon | 4:67d7177c213f | 213 | *counter = 0; |
beacon | 4:67d7177c213f | 214 | if (*lastAct != 7){ //If this wasn't the last called action, reset the timer. |
beacon | 4:67d7177c213f | 215 | *timer = 0; |
beacon | 4:67d7177c213f | 216 | *lastAct = 7; |
beacon | 4:67d7177c213f | 217 | } |
beacon | 4:67d7177c213f | 218 | |
beacon | 4:67d7177c213f | 219 | this->drive(); |
beacon | 4:67d7177c213f | 220 | } |
beacon | 4:67d7177c213f | 221 | |
beacon | 4:67d7177c213f | 222 | |
beacon | 4:67d7177c213f | 223 | void Robot::search(int* counter, int* timer, int* found){ |
beacon | 4:67d7177c213f | 224 | |
beacon | 4:67d7177c213f | 225 | static int rando = -1; //Rando will be used, to randomize turnAround() |
beacon | 4:67d7177c213f | 226 | //-1 := unused => set; 0 := turnRight(); 1 := turnLeft(); |
beacon | 4:67d7177c213f | 227 | |
beacon | 4:67d7177c213f | 228 | static int lastAct = -1; //Is used, to check if the same action in Robot::search() was made multiple times. |
beacon | 4:67d7177c213f | 229 | //-1 := unused; x != -1 := action x was called last. |
beacon | 4:67d7177c213f | 230 | |
beacon | 4:67d7177c213f | 231 | static int legoFound = -1; //Is used to determine, on what side the lego was found. |
beacon | 4:67d7177c213f | 232 | //-1 := unused; 0:= front; 1:= right, 2:= left. |
beacon | 4:67d7177c213f | 233 | |
beacon | 4:67d7177c213f | 234 | *timer += 1; //timer holds the time in 0.1s |
beacon | 4:67d7177c213f | 235 | |
beacon | 4:67d7177c213f | 236 | |
beacon | 5:1aaf5de776ff | 237 | if (*counter >= MAX) { //Robot is stuck turning left & right |
beacon | 4:67d7177c213f | 238 | counterMax(counter, timer, &lastAct, &rando); |
beacon | 4:67d7177c213f | 239 | } |
beacon | 4:67d7177c213f | 240 | |
beacon | 10:f76476943a6c | 241 | //Wall actions: |
beacon | 4:67d7177c213f | 242 | else if (this->sensors[RIGHT] < NEAR && legoFound == -1){ //Robot has spotted an obstacle on the right. |
beacon | 4:67d7177c213f | 243 | wallRight(counter, timer, &lastAct); |
beacon | 4:67d7177c213f | 244 | } |
beacon | 10:f76476943a6c | 245 | |
beacon | 4:67d7177c213f | 246 | else if (this->sensors[LEFT] < NEAR && legoFound == -1) { //Robot has spotted an obstacle on the left. |
beacon | 4:67d7177c213f | 247 | wallLeft(counter, timer, &lastAct); |
beacon | 4:67d7177c213f | 248 | } |
beacon | 4:67d7177c213f | 249 | |
beacon | 4:67d7177c213f | 250 | else if (this->sensors[FWD] < NEAR && legoFound == -1) { //Robot has spotted an obstacle infront |
beacon | 4:67d7177c213f | 251 | wallFront(counter, timer, &lastAct); |
beacon | 4:67d7177c213f | 252 | } |
beacon | 4:67d7177c213f | 253 | |
beacon | 4:67d7177c213f | 254 | //Lego actions: |
beacon | 10:f76476943a6c | 255 | else if (this->sensors[FWD_L] < NEAR_LEGO + 0.03f && legoFound == -1 || legoFound == 0){ //There's a Lego in front. |
beacon | 5:1aaf5de776ff | 256 | legoFront(counter, timer, &lastAct, &legoFound, found); |
beacon | 4:67d7177c213f | 257 | } |
beacon | 4:67d7177c213f | 258 | |
beacon | 10:f76476943a6c | 259 | else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){ //There's a Lego on the right. |
beacon | 4:67d7177c213f | 260 | legoRight(counter, timer, &lastAct, &legoFound); |
beacon | 4:67d7177c213f | 261 | } |
beacon | 4:67d7177c213f | 262 | |
beacon | 10:f76476943a6c | 263 | else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){ //There's a Lego on the left. |
beacon | 4:67d7177c213f | 264 | legoLeft(counter, timer, &lastAct, &legoFound); |
beacon | 4:67d7177c213f | 265 | } |
beacon | 4:67d7177c213f | 266 | |
beacon | 4:67d7177c213f | 267 | //Lego is ready to be grabbed. |
beacon | 4:67d7177c213f | 268 | else if(legoFound == 3){ |
beacon | 4:67d7177c213f | 269 | *found = 1; //When found is set to 1, main will call arms functions |
beacon | 4:67d7177c213f | 270 | *counter = 0; |
beacon | 4:67d7177c213f | 271 | *timer = 0; |
beacon | 4:67d7177c213f | 272 | legoFound = -1; |
beacon | 4:67d7177c213f | 273 | } |
beacon | 4:67d7177c213f | 274 | |
beacon | 4:67d7177c213f | 275 | |
beacon | 4:67d7177c213f | 276 | //Nothing found |
beacon | 4:67d7177c213f | 277 | else { |
beacon | 4:67d7177c213f | 278 | nothingFound(counter, timer, &lastAct); |
beacon | 4:67d7177c213f | 279 | } |
beacon | 4:67d7177c213f | 280 | } |
beacon | 4:67d7177c213f | 281 | |
beacon | 10:f76476943a6c | 282 | int Robot::getErrorMotor(){ |
beacon | 10:f76476943a6c | 283 | return 0; //errorMotor; |
beacon | 0:d267b248eff4 | 284 | } |
beacon | 0:d267b248eff4 | 285 | |
beacon | 0:d267b248eff4 | 286 | //reset |
beacon | 0:d267b248eff4 | 287 | //} |
beacon | 0:d267b248eff4 | 288 | |
beacon | 0:d267b248eff4 | 289 | //Add management for Overpower!! Pin PB_15 |