zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official-US

a

Dependencies:   Servo ServoArm mbed

Fork of PES_Official-TestF by zhaw_st16b_pes2_10

Sources/Robot.cpp@5:1aaf5de776ff, 2017-04-26 (annotated)

Committer:
beacon
Date:
Wed Apr 26 08:05:25 2017 +0000
Revision:
5:1aaf5de776ff
Parent:
4:67d7177c213f
Child:
6:ba26dd3251b3
k

Who changed what in which revision?

UserRevisionLine numberNew 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 4:67d7177c213f 6 Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS )
beacon 0:d267b248eff4 7 {
beacon 0:d267b248eff4 8 this->powerSignal = enableSignal;
beacon 1:388c915756f5 9 this->left = left;
beacon 1:388c915756f5 10 this->right = right;
beacon 0:d267b248eff4 11
beacon 0:d267b248eff4 12 this->left->period(0.00005f);
beacon 0:d267b248eff4 13 this->right->period(0.00005f);
beacon 1:388c915756f5 14
beacon 1:388c915756f5 15 this->leds = leds;
beacon 1:388c915756f5 16
beacon 1:388c915756f5 17 this->FarbVoltage = FarbVoltage;
beacon 4:67d7177c213f 18 this->frontS = frontS;
beacon 4:67d7177c213f 19 this->leftS = leftS;
beacon 4:67d7177c213f 20 this->rightS = rightS;
beacon 4:67d7177c213f 21
beacon 4:67d7177c213f 22 this->arm = arm;
beacon 1:388c915756f5 23
beacon 0:d267b248eff4 24 }
beacon 0:d267b248eff4 25
beacon 4:67d7177c213f 26 //Drive functions
beacon 0:d267b248eff4 27 void Robot::drive()
beacon 0:d267b248eff4 28 {
beacon 0:d267b248eff4 29 //pwm determine what direction it goes.
beacon 0:d267b248eff4 30 *powerSignal = 1;
beacon 1:388c915756f5 31 *left= 0.6f;
beacon 1:388c915756f5 32 *right= 0.4f;
beacon 1:388c915756f5 33 }
beacon 1:388c915756f5 34
beacon 4:67d7177c213f 35 void Robot::driveB()
beacon 4:67d7177c213f 36 {
beacon 4:67d7177c213f 37 //pwm determine what direction it goes.
beacon 4:67d7177c213f 38 *powerSignal = 1;
beacon 4:67d7177c213f 39 *left= 0.4f;
beacon 4:67d7177c213f 40 *right= 0.6f;
beacon 4:67d7177c213f 41 }
beacon 4:67d7177c213f 42
beacon 1:388c915756f5 43 void Robot::turnLeft()
beacon 1:388c915756f5 44 {
beacon 1:388c915756f5 45
beacon 1:388c915756f5 46 *powerSignal = 1;
beacon 1:388c915756f5 47 *left= 0.4f;
beacon 1:388c915756f5 48 *right= 0.4f;
beacon 1:388c915756f5 49
beacon 0:d267b248eff4 50 }
beacon 0:d267b248eff4 51
beacon 4:67d7177c213f 52 void Robot::turnLeftS()
beacon 4:67d7177c213f 53 {
beacon 4:67d7177c213f 54
beacon 4:67d7177c213f 55 *powerSignal = 1;
beacon 4:67d7177c213f 56 *left= 0.45f;
beacon 4:67d7177c213f 57 *right= 0.45f;
beacon 4:67d7177c213f 58
beacon 4:67d7177c213f 59 }
beacon 4:67d7177c213f 60
beacon 1:388c915756f5 61 void Robot::turnRight()
beacon 1:388c915756f5 62 {
beacon 1:388c915756f5 63 *powerSignal = 1;
beacon 1:388c915756f5 64 *left= 0.6f;
beacon 1:388c915756f5 65 *right= 0.6f;
beacon 1:388c915756f5 66 }
beacon 1:388c915756f5 67
beacon 4:67d7177c213f 68 void Robot::turnRightS()
beacon 4:67d7177c213f 69 {
beacon 4:67d7177c213f 70
beacon 4:67d7177c213f 71 *powerSignal = 1;
beacon 4:67d7177c213f 72 *left= 0.55f;
beacon 4:67d7177c213f 73 *right= 0.55f;
beacon 4:67d7177c213f 74
beacon 4:67d7177c213f 75 }
beacon 4:67d7177c213f 76
beacon 1:388c915756f5 77 void Robot::turnAround(int left)
beacon 1:388c915756f5 78 {
beacon 0:d267b248eff4 79 *powerSignal = 1;
beacon 1:388c915756f5 80
beacon 1:388c915756f5 81 if (left) {
beacon 1:388c915756f5 82 turnLeft();
beacon 1:388c915756f5 83 }
beacon 1:388c915756f5 84
beacon 1:388c915756f5 85 else {
beacon 1:388c915756f5 86 turnRight();
beacon 1:388c915756f5 87 }
beacon 1:388c915756f5 88 }
beacon 1:388c915756f5 89
beacon 1:388c915756f5 90 void Robot::stop()
beacon 1:388c915756f5 91 {
beacon 1:388c915756f5 92 *powerSignal = 1;
beacon 1:388c915756f5 93 *left= 0.5f;
beacon 1:388c915756f5 94 *right= 0.5f;
beacon 0:d267b248eff4 95 }
beacon 0:d267b248eff4 96
beacon 4:67d7177c213f 97
beacon 4:67d7177c213f 98 //Functions that use the drive functions
beacon 4:67d7177c213f 99 void Robot::wallRight(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 100 *counter += 1;
beacon 4:67d7177c213f 101
beacon 4:67d7177c213f 102 if (*lastAct != 1){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 103 *timer = 0;
beacon 4:67d7177c213f 104 *lastAct = 1;
beacon 4:67d7177c213f 105 }
beacon 4:67d7177c213f 106
beacon 4:67d7177c213f 107 this->turnLeft();
beacon 4:67d7177c213f 108 }
beacon 4:67d7177c213f 109
beacon 4:67d7177c213f 110 void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){
beacon 4:67d7177c213f 111 if (*lastAct != 0){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 112 *timer = 0;
beacon 4:67d7177c213f 113 *lastAct = 0;
beacon 4:67d7177c213f 114 }
beacon 4:67d7177c213f 115
beacon 4:67d7177c213f 116 if (*rando == -1){ //If rando was unused, set a new number.
beacon 4:67d7177c213f 117 *rando = rand() % 2;
beacon 4:67d7177c213f 118 }
beacon 4:67d7177c213f 119
beacon 4:67d7177c213f 120 if (this->sensors[FWD] < NEAR){ //While something is seen turn around.
beacon 4:67d7177c213f 121 this->turnAround(*rando);
beacon 4:67d7177c213f 122 }
beacon 4:67d7177c213f 123
beacon 4:67d7177c213f 124 else{
beacon 4:67d7177c213f 125 *rando = -1;
beacon 4:67d7177c213f 126 *counter = 0;
beacon 4:67d7177c213f 127 }
beacon 4:67d7177c213f 128 }
beacon 4:67d7177c213f 129
beacon 4:67d7177c213f 130 void Robot::wallLeft(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 131 *counter += 1;
beacon 4:67d7177c213f 132
beacon 4:67d7177c213f 133 if (*lastAct != 2){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 134 *timer = 0;
beacon 4:67d7177c213f 135 *lastAct = 2;
beacon 4:67d7177c213f 136 }
beacon 4:67d7177c213f 137
beacon 4:67d7177c213f 138 this->turnRight();
beacon 4:67d7177c213f 139 }
beacon 4:67d7177c213f 140
beacon 4:67d7177c213f 141 void Robot::wallFront(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 142 if (*lastAct != 3){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 143 *timer = 0;
beacon 4:67d7177c213f 144 *lastAct = 3;
beacon 4:67d7177c213f 145 }
beacon 4:67d7177c213f 146
beacon 4:67d7177c213f 147 *counter = MAX; //By setting the counter to MAX, next time it will go into the first if-statement (action 0).
beacon 4:67d7177c213f 148 }
beacon 4:67d7177c213f 149
beacon 4:67d7177c213f 150
beacon 5:1aaf5de776ff 151 void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound, int* found){
beacon 4:67d7177c213f 152 //*counter += 1;
beacon 4:67d7177c213f 153 *legoFound = 0;
beacon 4:67d7177c213f 154
beacon 5:1aaf5de776ff 155 if (*lastAct != 4){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 156 *timer = 0;
beacon 4:67d7177c213f 157 *lastAct = 4;
beacon 4:67d7177c213f 158 }
beacon 4:67d7177c213f 159
beacon 5:1aaf5de776ff 160 if (this->sensors[FWD] < NEAR){ //If Sam sees a wall turn around
beacon 4:67d7177c213f 161 *legoFound = -1;
beacon 5:1aaf5de776ff 162 *counter = MAX; //setting counter to MAX will couse sam to turnAround
beacon 4:67d7177c213f 163 }
beacon 4:67d7177c213f 164
beacon 5:1aaf5de776ff 165 if (this->sensors[FWD_L] > 0.16f){
beacon 4:67d7177c213f 166 this->drive();
beacon 4:67d7177c213f 167 }
beacon 4:67d7177c213f 168 else{
beacon 5:1aaf5de776ff 169 *found = 1;
beacon 4:67d7177c213f 170 }
beacon 4:67d7177c213f 171 }
beacon 4:67d7177c213f 172
beacon 4:67d7177c213f 173 void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){
beacon 4:67d7177c213f 174 //*counter += 1;
beacon 4:67d7177c213f 175 *legoFound = 1;
beacon 4:67d7177c213f 176
beacon 4:67d7177c213f 177 if (*lastAct != 5){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 178 *timer = 0;
beacon 4:67d7177c213f 179 *lastAct = 5;
beacon 4:67d7177c213f 180 }
beacon 4:67d7177c213f 181
beacon 5:1aaf5de776ff 182 if (this->sensors[FWD_L] > 0.22f){
beacon 4:67d7177c213f 183 this->turnRight();
beacon 4:67d7177c213f 184 }
beacon 4:67d7177c213f 185 else{
beacon 4:67d7177c213f 186 this->stop();
beacon 5:1aaf5de776ff 187 *legoFound = -1;
beacon 4:67d7177c213f 188 }
beacon 4:67d7177c213f 189 }
beacon 4:67d7177c213f 190
beacon 4:67d7177c213f 191 void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
beacon 4:67d7177c213f 192 //*counter += 1;
beacon 4:67d7177c213f 193 *legoFound = 2;
beacon 0:d267b248eff4 194
beacon 4:67d7177c213f 195 if (*lastAct != 6){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 196 *timer = 0;
beacon 4:67d7177c213f 197 *lastAct = 6;
beacon 4:67d7177c213f 198 }
beacon 4:67d7177c213f 199
beacon 5:1aaf5de776ff 200 if (this->sensors[FWD_L] > 0.22f){
beacon 4:67d7177c213f 201 this->turnLeft();
beacon 4:67d7177c213f 202 }
beacon 4:67d7177c213f 203 else{
beacon 4:67d7177c213f 204 this->stop();
beacon 5:1aaf5de776ff 205 *legoFound = -1;
beacon 4:67d7177c213f 206 }
beacon 4:67d7177c213f 207 }
beacon 4:67d7177c213f 208
beacon 4:67d7177c213f 209
beacon 4:67d7177c213f 210 void Robot::nothingFound(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 211 *counter = 0;
beacon 4:67d7177c213f 212 if (*lastAct != 7){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 213 *timer = 0;
beacon 4:67d7177c213f 214 *lastAct = 7;
beacon 4:67d7177c213f 215 }
beacon 4:67d7177c213f 216
beacon 4:67d7177c213f 217 this->drive();
beacon 4:67d7177c213f 218 }
beacon 4:67d7177c213f 219
beacon 4:67d7177c213f 220
beacon 4:67d7177c213f 221 void Robot::search(int* counter, int* timer, int* found){
beacon 4:67d7177c213f 222
beacon 4:67d7177c213f 223 static int rando = -1; //Rando will be used, to randomize turnAround()
beacon 4:67d7177c213f 224 //-1 := unused => set; 0 := turnRight(); 1 := turnLeft();
beacon 4:67d7177c213f 225
beacon 4:67d7177c213f 226 static int lastAct = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 4:67d7177c213f 227 //-1 := unused; x != -1 := action x was called last.
beacon 4:67d7177c213f 228
beacon 4:67d7177c213f 229 static int legoFound = -1; //Is used to determine, on what side the lego was found.
beacon 4:67d7177c213f 230 //-1 := unused; 0:= front; 1:= right, 2:= left.
beacon 4:67d7177c213f 231
beacon 4:67d7177c213f 232 *timer += 1; //timer holds the time in 0.1s
beacon 4:67d7177c213f 233
beacon 4:67d7177c213f 234
beacon 5:1aaf5de776ff 235 if (*counter >= MAX) { //Robot is stuck turning left & right
beacon 4:67d7177c213f 236 counterMax(counter, timer, &lastAct, &rando);
beacon 4:67d7177c213f 237 }
beacon 4:67d7177c213f 238
beacon 5:1aaf5de776ff 239 /*//Wall actions:
beacon 4:67d7177c213f 240 else if (this->sensors[RIGHT] < NEAR && legoFound == -1){ //Robot has spotted an obstacle on the right.
beacon 4:67d7177c213f 241 wallRight(counter, timer, &lastAct);
beacon 4:67d7177c213f 242 }
beacon 5:1aaf5de776ff 243 */
beacon 4:67d7177c213f 244 else if (this->sensors[LEFT] < NEAR && legoFound == -1) { //Robot has spotted an obstacle on the left.
beacon 4:67d7177c213f 245 wallLeft(counter, timer, &lastAct);
beacon 4:67d7177c213f 246 }
beacon 4:67d7177c213f 247
beacon 4:67d7177c213f 248 else if (this->sensors[FWD] < NEAR && legoFound == -1) { //Robot has spotted an obstacle infront
beacon 4:67d7177c213f 249 wallFront(counter, timer, &lastAct);
beacon 4:67d7177c213f 250 }
beacon 4:67d7177c213f 251
beacon 4:67d7177c213f 252 //Lego actions:
beacon 4:67d7177c213f 253 else if (this->sensors[FWD_L] < NEAR_LEGO && legoFound == -1 || legoFound == 0){ //There's a Lego in front.
beacon 5:1aaf5de776ff 254 legoFront(counter, timer, &lastAct, &legoFound, found);
beacon 4:67d7177c213f 255 }
beacon 4:67d7177c213f 256
beacon 4:67d7177c213f 257 else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){ //There's a Lego on the right.
beacon 4:67d7177c213f 258 legoRight(counter, timer, &lastAct, &legoFound);
beacon 4:67d7177c213f 259 }
beacon 4:67d7177c213f 260
beacon 4:67d7177c213f 261 else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){ //There's a Lego on the left.
beacon 4:67d7177c213f 262 legoLeft(counter, timer, &lastAct, &legoFound);
beacon 4:67d7177c213f 263 }
beacon 4:67d7177c213f 264
beacon 4:67d7177c213f 265 //Lego is ready to be grabbed.
beacon 4:67d7177c213f 266 else if(legoFound == 3){
beacon 4:67d7177c213f 267 *found = 1; //When found is set to 1, main will call arms functions
beacon 4:67d7177c213f 268 *counter = 0;
beacon 4:67d7177c213f 269 *timer = 0;
beacon 4:67d7177c213f 270 legoFound = -1;
beacon 2:01e9de508316 271
beacon 4:67d7177c213f 272 //Old! Check if still useful!{
beacon 4:67d7177c213f 273 //check if this was the last action
beacon 4:67d7177c213f 274 //reset timer
beacon 4:67d7177c213f 275
beacon 4:67d7177c213f 276 //dont add up counter
beacon 4:67d7177c213f 277 //use arm
beacon 4:67d7177c213f 278 //set legoFound to -1
beacon 4:67d7177c213f 279 //}
beacon 4:67d7177c213f 280 }
beacon 4:67d7177c213f 281
beacon 4:67d7177c213f 282
beacon 4:67d7177c213f 283 //Nothing found
beacon 4:67d7177c213f 284 else {
beacon 4:67d7177c213f 285 nothingFound(counter, timer, &lastAct);
beacon 4:67d7177c213f 286 }
beacon 4:67d7177c213f 287 }
beacon 4:67d7177c213f 288
beacon 4:67d7177c213f 289 void Robot::lego(int* counter, Timer* t){
beacon 4:67d7177c213f 290
beacon 4:67d7177c213f 291 if (this->sensors[RIGHT_L] < NEAR_LEGO){
beacon 4:67d7177c213f 292 t->reset();
beacon 4:67d7177c213f 293
beacon 4:67d7177c213f 294 *counter += 1;
beacon 4:67d7177c213f 295 while (this->sensors[FWD] > NEAR_LEGO){
beacon 2:01e9de508316 296 if ( t->read() > TIMEOUT ){
beacon 2:01e9de508316 297 break;
beacon 2:01e9de508316 298 }
beacon 2:01e9de508316 299
beacon 4:67d7177c213f 300 this->turnRight();
beacon 1:388c915756f5 301 }
beacon 4:67d7177c213f 302 this->stop();
beacon 1:388c915756f5 303 }
beacon 1:388c915756f5 304
beacon 4:67d7177c213f 305 else if (this->sensors[LEFT] < NEAR_LEGO){
beacon 2:01e9de508316 306 t-> reset();
beacon 2:01e9de508316 307
beacon 1:388c915756f5 308 *counter += 1;
beacon 4:67d7177c213f 309 while (this->sensors[FWD] > NEAR_LEGO){
beacon 2:01e9de508316 310 if ( t->read() > TIMEOUT ){
beacon 2:01e9de508316 311 break;
beacon 2:01e9de508316 312 }
beacon 2:01e9de508316 313
beacon 1:388c915756f5 314 this->turnLeft();
beacon 1:388c915756f5 315 }
beacon 4:67d7177c213f 316 this->stop();
beacon 0:d267b248eff4 317 }
beacon 0:d267b248eff4 318
beacon 4:67d7177c213f 319 else if (this->sensors[FWD] < 0.12f){
beacon 4:67d7177c213f 320 while (this->sensors[FWD] < 0.12f){
beacon 4:67d7177c213f 321 this->driveB();
beacon 1:388c915756f5 322 }
beacon 4:67d7177c213f 323 while (this->sensors[FWD] < NEAR_LEGO){
beacon 4:67d7177c213f 324 this->turnRight();
beacon 1:388c915756f5 325 }
beacon 4:67d7177c213f 326 while(1){
beacon 4:67d7177c213f 327 this->stop();
beacon 4:67d7177c213f 328 }
beacon 1:388c915756f5 329 }
beacon 1:388c915756f5 330
beacon 1:388c915756f5 331 else {
beacon 1:388c915756f5 332 *counter = 0;
beacon 1:388c915756f5 333 this->drive();
beacon 0:d267b248eff4 334 }
beacon 0:d267b248eff4 335 }
beacon 0:d267b248eff4 336
beacon 0:d267b248eff4 337 //void Robot::init(){
beacon 0:d267b248eff4 338 // Robot.DistanceSensors.init(&sensorVoltage, &bit0, &bit1, &bit2, ii);
beacon 0:d267b248eff4 339 //}
beacon 1:388c915756f5 340
beacon 0:d267b248eff4 341 //Remember to set
beacon 0:d267b248eff4 342 //DigitalOut powerMotor(PB_2) = 1;
beacon 0:d267b248eff4 343 //DigitalIn errorMotor(PB_14);
beacon 0:d267b248eff4 344 //if (errorMotor){
beacon 0:d267b248eff4 345 //reset
beacon 0:d267b248eff4 346 //}
beacon 0:d267b248eff4 347
beacon 0:d267b248eff4 348 //Add management for Overpower!! Pin PB_15