zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official

Do NOT modify!

Dependencies:   mbed Servo ServoArm

Fork of PES_Yanick by zhaw_st16b_pes2_10

Sources/Robot.cpp@12:c0bcb95885dd, 2017-05-10 (annotated)

Committer:
beacon
Date:
Wed May 10 13:30:13 2017 +0000
Revision:
12:c0bcb95885dd
Parent:
11:292bdbd85a9c 
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 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 11:292bdbd85a9c 171 this->stop();
beacon 5:1aaf5de776ff 172 *found = 1;
beacon 4:67d7177c213f 173 }
beacon 4:67d7177c213f 174 }
beacon 4:67d7177c213f 175
beacon 4:67d7177c213f 176 void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){
beacon 4:67d7177c213f 177 //*counter += 1;
beacon 4:67d7177c213f 178 *legoFound = 1;
beacon 4:67d7177c213f 179
beacon 4:67d7177c213f 180 if (*lastAct != 5){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 181 *timer = 0;
beacon 4:67d7177c213f 182 *lastAct = 5;
beacon 4:67d7177c213f 183 }
beacon 4:67d7177c213f 184
beacon 5:1aaf5de776ff 185 if (this->sensors[FWD_L] > 0.22f){
beacon 4:67d7177c213f 186 this->turnRight();
beacon 4:67d7177c213f 187 }
beacon 4:67d7177c213f 188 else{
beacon 4:67d7177c213f 189 this->stop();
beacon 5:1aaf5de776ff 190 *legoFound = -1;
beacon 4:67d7177c213f 191 }
beacon 4:67d7177c213f 192 }
beacon 4:67d7177c213f 193
beacon 4:67d7177c213f 194 void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
beacon 4:67d7177c213f 195 //*counter += 1;
beacon 4:67d7177c213f 196 *legoFound = 2;
beacon 0:d267b248eff4 197
beacon 4:67d7177c213f 198 if (*lastAct != 6){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 199 *timer = 0;
beacon 4:67d7177c213f 200 *lastAct = 6;
beacon 4:67d7177c213f 201 }
beacon 4:67d7177c213f 202
beacon 5:1aaf5de776ff 203 if (this->sensors[FWD_L] > 0.22f){
beacon 4:67d7177c213f 204 this->turnLeft();
beacon 4:67d7177c213f 205 }
beacon 4:67d7177c213f 206 else{
beacon 4:67d7177c213f 207 this->stop();
beacon 5:1aaf5de776ff 208 *legoFound = -1;
beacon 4:67d7177c213f 209 }
beacon 4:67d7177c213f 210 }
beacon 4:67d7177c213f 211
beacon 4:67d7177c213f 212
beacon 4:67d7177c213f 213 void Robot::nothingFound(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 214 *counter = 0;
beacon 4:67d7177c213f 215 if (*lastAct != 7){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 216 *timer = 0;
beacon 4:67d7177c213f 217 *lastAct = 7;
beacon 4:67d7177c213f 218 }
beacon 4:67d7177c213f 219
beacon 4:67d7177c213f 220 this->drive();
beacon 4:67d7177c213f 221 }
beacon 4:67d7177c213f 222
beacon 4:67d7177c213f 223
beacon 11:292bdbd85a9c 224 int Robot::search(int* timer){
beacon 11:292bdbd85a9c 225 enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
beacon 4:67d7177c213f 226
beacon 11:292bdbd85a9c 227 static int state = neutral;
beacon 4:67d7177c213f 228
beacon 11:292bdbd85a9c 229 static int counter = 0; //counter is used blabla
beacon 4:67d7177c213f 230
beacon 11:292bdbd85a9c 231 static int rando = -1;
beacon 10:f76476943a6c 232
beacon 11:292bdbd85a9c 233 //static int lastAct = 0;
beacon 4:67d7177c213f 234
beacon 11:292bdbd85a9c 235 switch( state ){
beacon 11:292bdbd85a9c 236 case neutral:
beacon 11:292bdbd85a9c 237 if( counter > MAX ){
beacon 11:292bdbd85a9c 238 state = max;
beacon 11:292bdbd85a9c 239 }
beacon 11:292bdbd85a9c 240 /*else if( this->sensors[FWD] < NEAR ){
beacon 11:292bdbd85a9c 241 state = wallF;
beacon 11:292bdbd85a9c 242 }*/
beacon 11:292bdbd85a9c 243 else if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 244 state = wallL;
beacon 11:292bdbd85a9c 245 }
beacon 11:292bdbd85a9c 246 else if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 247 state = wallR;
beacon 11:292bdbd85a9c 248 }
beacon 11:292bdbd85a9c 249 else if( this->sensors[FWD_L] < NEAR ){
beacon 11:292bdbd85a9c 250 state = legoF;
beacon 11:292bdbd85a9c 251 }
beacon 11:292bdbd85a9c 252 else if( this->sensors[LEFT_L] < NEAR ){
beacon 11:292bdbd85a9c 253 state = legoL;
beacon 11:292bdbd85a9c 254 }
beacon 11:292bdbd85a9c 255 else if( this->sensors[RIGHT_L] < NEAR ){
beacon 11:292bdbd85a9c 256 state = legoR;
beacon 11:292bdbd85a9c 257 }
beacon 11:292bdbd85a9c 258 else{
beacon 11:292bdbd85a9c 259 this->drive();
beacon 11:292bdbd85a9c 260 counter = 0;
beacon 11:292bdbd85a9c 261 }
beacon 11:292bdbd85a9c 262 break;
beacon 11:292bdbd85a9c 263
beacon 11:292bdbd85a9c 264 case max: {
beacon 11:292bdbd85a9c 265 int time = 0;
beacon 11:292bdbd85a9c 266 if( time < 15 && this->sensors[FWD] > NEAR ){
beacon 11:292bdbd85a9c 267 rando == -1 ? rando = rand() % 2 : rando = rando;
beacon 11:292bdbd85a9c 268 this->turnAround(rando);
beacon 11:292bdbd85a9c 269 }
beacon 11:292bdbd85a9c 270 else{
beacon 11:292bdbd85a9c 271 state = neutral;
beacon 11:292bdbd85a9c 272 counter = 0;
beacon 11:292bdbd85a9c 273 rando = -1;
beacon 11:292bdbd85a9c 274 }
beacon 11:292bdbd85a9c 275 break;
beacon 11:292bdbd85a9c 276 }
beacon 11:292bdbd85a9c 277
beacon 11:292bdbd85a9c 278 case wallF:
beacon 11:292bdbd85a9c 279 counter++;
beacon 11:292bdbd85a9c 280 if( this->sensors[FWD] < NEAR ){
beacon 11:292bdbd85a9c 281 rando == -1 ? rando = rand() % 2 : rando = rando;
beacon 11:292bdbd85a9c 282 this->turnAround(rando);
beacon 11:292bdbd85a9c 283 }
beacon 11:292bdbd85a9c 284 else{
beacon 11:292bdbd85a9c 285 state = neutral;
beacon 11:292bdbd85a9c 286 rando = -1;
beacon 11:292bdbd85a9c 287 }
beacon 11:292bdbd85a9c 288 break;
beacon 11:292bdbd85a9c 289
beacon 11:292bdbd85a9c 290 case wallL:
beacon 11:292bdbd85a9c 291 counter++;
beacon 11:292bdbd85a9c 292 if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 293 this->turnRight();
beacon 11:292bdbd85a9c 294 }
beacon 11:292bdbd85a9c 295 else{
beacon 11:292bdbd85a9c 296 state = neutral;
beacon 11:292bdbd85a9c 297 }
beacon 11:292bdbd85a9c 298 break;
beacon 11:292bdbd85a9c 299
beacon 11:292bdbd85a9c 300 case wallR:
beacon 11:292bdbd85a9c 301 counter++;
beacon 11:292bdbd85a9c 302 if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 303 this->turnLeft();
beacon 11:292bdbd85a9c 304 }
beacon 11:292bdbd85a9c 305 else{
beacon 11:292bdbd85a9c 306 state = neutral;
beacon 11:292bdbd85a9c 307 }
beacon 11:292bdbd85a9c 308 break;
beacon 11:292bdbd85a9c 309
beacon 11:292bdbd85a9c 310 case legoF:
beacon 11:292bdbd85a9c 311 //counter++;
beacon 11:292bdbd85a9c 312 if( this->sensors[FWD_L] > 0.15f ){
beacon 11:292bdbd85a9c 313 this->drive();
beacon 11:292bdbd85a9c 314 }
beacon 11:292bdbd85a9c 315 else{
beacon 11:292bdbd85a9c 316 state = neutral;
beacon 11:292bdbd85a9c 317 counter = 0;
beacon 11:292bdbd85a9c 318 this->stop();
beacon 11:292bdbd85a9c 319 return 1;
beacon 11:292bdbd85a9c 320 }
beacon 11:292bdbd85a9c 321 break;
beacon 11:292bdbd85a9c 322
beacon 11:292bdbd85a9c 323 case legoL:
beacon 11:292bdbd85a9c 324 counter++;
beacon 11:292bdbd85a9c 325 if( this->sensors[FWD_L] < NEAR_LEGO+0.05f ){
beacon 11:292bdbd85a9c 326 this->turnLeft();
beacon 11:292bdbd85a9c 327 }
beacon 11:292bdbd85a9c 328 else{
beacon 11:292bdbd85a9c 329 state = legoF;
beacon 11:292bdbd85a9c 330 }
beacon 11:292bdbd85a9c 331 break;
beacon 11:292bdbd85a9c 332
beacon 11:292bdbd85a9c 333 case legoR:
beacon 11:292bdbd85a9c 334 counter++;
beacon 11:292bdbd85a9c 335 if( this->sensors[FWD_L] < NEAR_LEGO+0.05f ){
beacon 11:292bdbd85a9c 336 this->turnRight();
beacon 11:292bdbd85a9c 337 }
beacon 11:292bdbd85a9c 338 else{
beacon 11:292bdbd85a9c 339 state = legoF;
beacon 11:292bdbd85a9c 340 }
beacon 11:292bdbd85a9c 341 break;
beacon 11:292bdbd85a9c 342
beacon 4:67d7177c213f 343 }
beacon 11:292bdbd85a9c 344 return 0;
beacon 4:67d7177c213f 345 }
beacon 4:67d7177c213f 346
beacon 10:f76476943a6c 347 int Robot::getErrorMotor(){
beacon 10:f76476943a6c 348 return 0; //errorMotor;
beacon 0:d267b248eff4 349 }
beacon 0:d267b248eff4 350
beacon 11:292bdbd85a9c 351 /*int Robot::searchh(int* timer){
beacon 11:292bdbd85a9c 352 enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
beacon 11:292bdbd85a9c 353
beacon 11:292bdbd85a9c 354 static int state = neutral;
beacon 11:292bdbd85a9c 355
beacon 11:292bdbd85a9c 356 static int counter = 0; //counter is used blabla
beacon 11:292bdbd85a9c 357
beacon 11:292bdbd85a9c 358 //static int lastAct = 0;
beacon 11:292bdbd85a9c 359
beacon 11:292bdbd85a9c 360 switch( state ){
beacon 11:292bdbd85a9c 361 case neutral:
beacon 11:292bdbd85a9c 362 if( counter > MAX ){
beacon 11:292bdbd85a9c 363 state = max;
beacon 11:292bdbd85a9c 364 }
beacon 11:292bdbd85a9c 365 else if( this->sensora[FWD] < NEAR ){
beacon 11:292bdbd85a9c 366 state = wallF;
beacon 11:292bdbd85a9c 367 counter = 0;
beacon 11:292bdbd85a9c 368 }
beacon 11:292bdbd85a9c 369 else if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 370 state = wallL;
beacon 11:292bdbd85a9c 371 counter = 0;
beacon 11:292bdbd85a9c 372 }
beacon 11:292bdbd85a9c 373 else if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 374 state = wallR;
beacon 11:292bdbd85a9c 375 counter = 0;
beacon 11:292bdbd85a9c 376 }
beacon 11:292bdbd85a9c 377 else if( this->sensors[FWD_L] < NEAR ){
beacon 11:292bdbd85a9c 378 state = legoF;
beacon 11:292bdbd85a9c 379 counter = 0;
beacon 11:292bdbd85a9c 380 }
beacon 11:292bdbd85a9c 381 else if( this->sensors[LEFT_L] < NEAR ){
beacon 11:292bdbd85a9c 382 state = legoL;
beacon 11:292bdbd85a9c 383 counter = 0;
beacon 11:292bdbd85a9c 384 }
beacon 11:292bdbd85a9c 385 else if( this->sensors[RIGHT_L] < NEAR ){
beacon 11:292bdbd85a9c 386 state = legoR;
beacon 11:292bdbd85a9c 387 counter = 0;
beacon 11:292bdbd85a9c 388 }
beacon 11:292bdbd85a9c 389 break;
beacon 11:292bdbd85a9c 390
beacon 11:292bdbd85a9c 391 case max:
beacon 11:292bdbd85a9c 392
beacon 11:292bdbd85a9c 393 break;
beacon 11:292bdbd85a9c 394 */
beacon 11:292bdbd85a9c 395
beacon 0:d267b248eff4 396 //reset
beacon 0:d267b248eff4 397 //}
beacon 0:d267b248eff4 398
beacon 0:d267b248eff4 399 //Add management for Overpower!! Pin PB_15