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@13:6c1f8e35b242, 2017-05-10 (annotated)

Committer:
EpicG10
Date:
Wed May 10 16:53:01 2017 +0000
Revision:
13:6c1f8e35b242
Parent:
11:292bdbd85a9c
Child:
15:915f8839fe48
b

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){
EpicG10 13:6c1f8e35b242 225 //return 1;
beacon 11:292bdbd85a9c 226 enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
beacon 4:67d7177c213f 227
beacon 11:292bdbd85a9c 228 static int state = neutral;
beacon 4:67d7177c213f 229
beacon 11:292bdbd85a9c 230 static int counter = 0; //counter is used blabla
beacon 4:67d7177c213f 231
beacon 11:292bdbd85a9c 232 static int rando = -1;
beacon 10:f76476943a6c 233
beacon 11:292bdbd85a9c 234 //static int lastAct = 0;
beacon 4:67d7177c213f 235
beacon 11:292bdbd85a9c 236 switch( state ){
beacon 11:292bdbd85a9c 237 case neutral:
beacon 11:292bdbd85a9c 238 if( counter > MAX ){
beacon 11:292bdbd85a9c 239 state = max;
beacon 11:292bdbd85a9c 240 }
EpicG10 13:6c1f8e35b242 241 else if( this->sensors[FWD] < NEAR ){
beacon 11:292bdbd85a9c 242 state = wallF;
EpicG10 13:6c1f8e35b242 243 }
beacon 11:292bdbd85a9c 244 else if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 245 state = wallL;
beacon 11:292bdbd85a9c 246 }
beacon 11:292bdbd85a9c 247 else if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 248 state = wallR;
beacon 11:292bdbd85a9c 249 }
beacon 11:292bdbd85a9c 250 else if( this->sensors[FWD_L] < NEAR ){
beacon 11:292bdbd85a9c 251 state = legoF;
beacon 11:292bdbd85a9c 252 }
beacon 11:292bdbd85a9c 253 else if( this->sensors[LEFT_L] < NEAR ){
beacon 11:292bdbd85a9c 254 state = legoL;
beacon 11:292bdbd85a9c 255 }
beacon 11:292bdbd85a9c 256 else if( this->sensors[RIGHT_L] < NEAR ){
beacon 11:292bdbd85a9c 257 state = legoR;
beacon 11:292bdbd85a9c 258 }
beacon 11:292bdbd85a9c 259 else{
beacon 11:292bdbd85a9c 260 this->drive();
beacon 11:292bdbd85a9c 261 counter = 0;
beacon 11:292bdbd85a9c 262 }
beacon 11:292bdbd85a9c 263 break;
beacon 11:292bdbd85a9c 264
beacon 11:292bdbd85a9c 265 case max: {
beacon 11:292bdbd85a9c 266 int time = 0;
beacon 11:292bdbd85a9c 267 if( time < 15 && this->sensors[FWD] > NEAR ){
beacon 11:292bdbd85a9c 268 rando == -1 ? rando = rand() % 2 : rando = rando;
beacon 11:292bdbd85a9c 269 this->turnAround(rando);
beacon 11:292bdbd85a9c 270 }
beacon 11:292bdbd85a9c 271 else{
beacon 11:292bdbd85a9c 272 state = neutral;
beacon 11:292bdbd85a9c 273 counter = 0;
beacon 11:292bdbd85a9c 274 rando = -1;
beacon 11:292bdbd85a9c 275 }
beacon 11:292bdbd85a9c 276 break;
beacon 11:292bdbd85a9c 277 }
beacon 11:292bdbd85a9c 278
beacon 11:292bdbd85a9c 279 case wallF:
beacon 11:292bdbd85a9c 280 counter++;
beacon 11:292bdbd85a9c 281 if( this->sensors[FWD] < NEAR ){
beacon 11:292bdbd85a9c 282 rando == -1 ? rando = rand() % 2 : rando = rando;
beacon 11:292bdbd85a9c 283 this->turnAround(rando);
beacon 11:292bdbd85a9c 284 }
beacon 11:292bdbd85a9c 285 else{
beacon 11:292bdbd85a9c 286 state = neutral;
beacon 11:292bdbd85a9c 287 rando = -1;
beacon 11:292bdbd85a9c 288 }
beacon 11:292bdbd85a9c 289 break;
beacon 11:292bdbd85a9c 290
beacon 11:292bdbd85a9c 291 case wallL:
beacon 11:292bdbd85a9c 292 counter++;
beacon 11:292bdbd85a9c 293 if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 294 this->turnRight();
beacon 11:292bdbd85a9c 295 }
beacon 11:292bdbd85a9c 296 else{
beacon 11:292bdbd85a9c 297 state = neutral;
beacon 11:292bdbd85a9c 298 }
beacon 11:292bdbd85a9c 299 break;
beacon 11:292bdbd85a9c 300
beacon 11:292bdbd85a9c 301 case wallR:
beacon 11:292bdbd85a9c 302 counter++;
beacon 11:292bdbd85a9c 303 if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 304 this->turnLeft();
beacon 11:292bdbd85a9c 305 }
beacon 11:292bdbd85a9c 306 else{
beacon 11:292bdbd85a9c 307 state = neutral;
beacon 11:292bdbd85a9c 308 }
beacon 11:292bdbd85a9c 309 break;
beacon 11:292bdbd85a9c 310
beacon 11:292bdbd85a9c 311 case legoF:
beacon 11:292bdbd85a9c 312 //counter++;
EpicG10 13:6c1f8e35b242 313 if( this->sensors[FWD_L] > 0.17f ){
beacon 11:292bdbd85a9c 314 this->drive();
beacon 11:292bdbd85a9c 315 }
beacon 11:292bdbd85a9c 316 else{
beacon 11:292bdbd85a9c 317 state = neutral;
beacon 11:292bdbd85a9c 318 counter = 0;
beacon 11:292bdbd85a9c 319 this->stop();
beacon 11:292bdbd85a9c 320 return 1;
beacon 11:292bdbd85a9c 321 }
beacon 11:292bdbd85a9c 322 break;
beacon 11:292bdbd85a9c 323
beacon 11:292bdbd85a9c 324 case legoL:
beacon 11:292bdbd85a9c 325 counter++;
EpicG10 13:6c1f8e35b242 326 if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){
beacon 11:292bdbd85a9c 327 this->turnLeft();
beacon 11:292bdbd85a9c 328 }
beacon 11:292bdbd85a9c 329 else{
beacon 11:292bdbd85a9c 330 state = legoF;
beacon 11:292bdbd85a9c 331 }
beacon 11:292bdbd85a9c 332 break;
beacon 11:292bdbd85a9c 333
beacon 11:292bdbd85a9c 334 case legoR:
beacon 11:292bdbd85a9c 335 counter++;
EpicG10 13:6c1f8e35b242 336 if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){
beacon 11:292bdbd85a9c 337 this->turnRight();
beacon 11:292bdbd85a9c 338 }
beacon 11:292bdbd85a9c 339 else{
beacon 11:292bdbd85a9c 340 state = legoF;
beacon 11:292bdbd85a9c 341 }
beacon 11:292bdbd85a9c 342 break;
beacon 11:292bdbd85a9c 343
beacon 4:67d7177c213f 344 }
beacon 11:292bdbd85a9c 345 return 0;
beacon 4:67d7177c213f 346 }
beacon 4:67d7177c213f 347
beacon 10:f76476943a6c 348 int Robot::getErrorMotor(){
beacon 10:f76476943a6c 349 return 0; //errorMotor;
beacon 0:d267b248eff4 350 }
beacon 0:d267b248eff4 351
beacon 11:292bdbd85a9c 352 /*int Robot::searchh(int* timer){
beacon 11:292bdbd85a9c 353 enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
beacon 11:292bdbd85a9c 354
beacon 11:292bdbd85a9c 355 static int state = neutral;
beacon 11:292bdbd85a9c 356
beacon 11:292bdbd85a9c 357 static int counter = 0; //counter is used blabla
beacon 11:292bdbd85a9c 358
beacon 11:292bdbd85a9c 359 //static int lastAct = 0;
beacon 11:292bdbd85a9c 360
beacon 11:292bdbd85a9c 361 switch( state ){
beacon 11:292bdbd85a9c 362 case neutral:
beacon 11:292bdbd85a9c 363 if( counter > MAX ){
beacon 11:292bdbd85a9c 364 state = max;
beacon 11:292bdbd85a9c 365 }
beacon 11:292bdbd85a9c 366 else if( this->sensora[FWD] < NEAR ){
beacon 11:292bdbd85a9c 367 state = wallF;
beacon 11:292bdbd85a9c 368 counter = 0;
beacon 11:292bdbd85a9c 369 }
beacon 11:292bdbd85a9c 370 else if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 371 state = wallL;
beacon 11:292bdbd85a9c 372 counter = 0;
beacon 11:292bdbd85a9c 373 }
beacon 11:292bdbd85a9c 374 else if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 375 state = wallR;
beacon 11:292bdbd85a9c 376 counter = 0;
beacon 11:292bdbd85a9c 377 }
beacon 11:292bdbd85a9c 378 else if( this->sensors[FWD_L] < NEAR ){
beacon 11:292bdbd85a9c 379 state = legoF;
beacon 11:292bdbd85a9c 380 counter = 0;
beacon 11:292bdbd85a9c 381 }
beacon 11:292bdbd85a9c 382 else if( this->sensors[LEFT_L] < NEAR ){
beacon 11:292bdbd85a9c 383 state = legoL;
beacon 11:292bdbd85a9c 384 counter = 0;
beacon 11:292bdbd85a9c 385 }
beacon 11:292bdbd85a9c 386 else if( this->sensors[RIGHT_L] < NEAR ){
beacon 11:292bdbd85a9c 387 state = legoR;
beacon 11:292bdbd85a9c 388 counter = 0;
beacon 11:292bdbd85a9c 389 }
beacon 11:292bdbd85a9c 390 break;
beacon 11:292bdbd85a9c 391
beacon 11:292bdbd85a9c 392 case max:
beacon 11:292bdbd85a9c 393
beacon 11:292bdbd85a9c 394 break;
beacon 11:292bdbd85a9c 395 */
beacon 11:292bdbd85a9c 396
beacon 0:d267b248eff4 397 //reset
beacon 0:d267b248eff4 398 //}
beacon 0:d267b248eff4 399
beacon 0:d267b248eff4 400 //Add management for Overpower!! Pin PB_15