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@15:915f8839fe48, 2017-05-11 (annotated)

Committer:
EpicG10
Date:
Thu May 11 18:57:45 2017 +0000
Revision:
15:915f8839fe48
Parent:
13:6c1f8e35b242
Child:
16:f157e5ccd7d3
Child:
17:4e1be70bdedb
c

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
EpicG10 15:915f8839fe48 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 )
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;
EpicG10 15:915f8839fe48 25 this->Leiste = Leiste;
beacon 1:388c915756f5 26
beacon 0:d267b248eff4 27 }
beacon 0:d267b248eff4 28
beacon 4:67d7177c213f 29 //Drive functions
beacon 0:d267b248eff4 30 void Robot::drive()
beacon 0:d267b248eff4 31 {
beacon 0:d267b248eff4 32 //pwm determine what direction it goes.
beacon 0:d267b248eff4 33 *powerSignal = 1;
beacon 1:388c915756f5 34 *left= 0.6f;
beacon 1:388c915756f5 35 *right= 0.4f;
beacon 1:388c915756f5 36 }
beacon 1:388c915756f5 37
beacon 4:67d7177c213f 38 void Robot::driveB()
beacon 4:67d7177c213f 39 {
beacon 4:67d7177c213f 40 //pwm determine what direction it goes.
beacon 4:67d7177c213f 41 *powerSignal = 1;
beacon 4:67d7177c213f 42 *left= 0.4f;
beacon 4:67d7177c213f 43 *right= 0.6f;
beacon 4:67d7177c213f 44 }
beacon 4:67d7177c213f 45
beacon 1:388c915756f5 46 void Robot::turnLeft()
beacon 1:388c915756f5 47 {
beacon 1:388c915756f5 48
beacon 1:388c915756f5 49 *powerSignal = 1;
beacon 1:388c915756f5 50 *left= 0.4f;
beacon 1:388c915756f5 51 *right= 0.4f;
beacon 1:388c915756f5 52
beacon 0:d267b248eff4 53 }
beacon 0:d267b248eff4 54
beacon 4:67d7177c213f 55 void Robot::turnLeftS()
beacon 4:67d7177c213f 56 {
beacon 4:67d7177c213f 57
beacon 4:67d7177c213f 58 *powerSignal = 1;
beacon 4:67d7177c213f 59 *left= 0.45f;
beacon 4:67d7177c213f 60 *right= 0.45f;
beacon 4:67d7177c213f 61
beacon 4:67d7177c213f 62 }
beacon 4:67d7177c213f 63
beacon 1:388c915756f5 64 void Robot::turnRight()
beacon 1:388c915756f5 65 {
beacon 1:388c915756f5 66 *powerSignal = 1;
beacon 1:388c915756f5 67 *left= 0.6f;
beacon 1:388c915756f5 68 *right= 0.6f;
beacon 1:388c915756f5 69 }
beacon 1:388c915756f5 70
beacon 4:67d7177c213f 71 void Robot::turnRightS()
beacon 4:67d7177c213f 72 {
beacon 4:67d7177c213f 73
beacon 4:67d7177c213f 74 *powerSignal = 1;
beacon 4:67d7177c213f 75 *left= 0.55f;
beacon 4:67d7177c213f 76 *right= 0.55f;
beacon 4:67d7177c213f 77
beacon 4:67d7177c213f 78 }
beacon 4:67d7177c213f 79
beacon 1:388c915756f5 80 void Robot::turnAround(int left)
beacon 1:388c915756f5 81 {
beacon 0:d267b248eff4 82 *powerSignal = 1;
beacon 1:388c915756f5 83
beacon 1:388c915756f5 84 if (left) {
beacon 1:388c915756f5 85 turnLeft();
beacon 1:388c915756f5 86 }
beacon 1:388c915756f5 87
beacon 1:388c915756f5 88 else {
beacon 1:388c915756f5 89 turnRight();
beacon 1:388c915756f5 90 }
beacon 1:388c915756f5 91 }
beacon 1:388c915756f5 92
beacon 1:388c915756f5 93 void Robot::stop()
beacon 1:388c915756f5 94 {
beacon 1:388c915756f5 95 *powerSignal = 1;
beacon 1:388c915756f5 96 *left= 0.5f;
beacon 1:388c915756f5 97 *right= 0.5f;
beacon 0:d267b248eff4 98 }
beacon 0:d267b248eff4 99
beacon 4:67d7177c213f 100
beacon 4:67d7177c213f 101 //Functions that use the drive functions
beacon 4:67d7177c213f 102 void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){
beacon 4:67d7177c213f 103 if (*lastAct != 0){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 104 *timer = 0;
beacon 4:67d7177c213f 105 *lastAct = 0;
beacon 4:67d7177c213f 106 }
beacon 4:67d7177c213f 107
beacon 4:67d7177c213f 108 if (*rando == -1){ //If rando was unused, set a new number.
beacon 4:67d7177c213f 109 *rando = rand() % 2;
beacon 4:67d7177c213f 110 }
beacon 4:67d7177c213f 111
beacon 4:67d7177c213f 112 if (this->sensors[FWD] < NEAR){ //While something is seen turn around.
beacon 4:67d7177c213f 113 this->turnAround(*rando);
beacon 4:67d7177c213f 114 }
beacon 4:67d7177c213f 115
beacon 4:67d7177c213f 116 else{
beacon 4:67d7177c213f 117 *rando = -1;
beacon 4:67d7177c213f 118 *counter = 0;
beacon 4:67d7177c213f 119 }
beacon 4:67d7177c213f 120 }
beacon 4:67d7177c213f 121
beacon 10:f76476943a6c 122 void Robot::wallRight(int* counter, int* timer, int* lastAct){
beacon 10:f76476943a6c 123 *counter += 1;
beacon 10:f76476943a6c 124
beacon 10:f76476943a6c 125 if (*lastAct != 1){ //If this wasn't the last called action, reset the timer.
beacon 10:f76476943a6c 126 *timer = 0;
beacon 10:f76476943a6c 127 *lastAct = 1;
beacon 10:f76476943a6c 128 }
beacon 10:f76476943a6c 129
beacon 10:f76476943a6c 130 this->turnLeft();
beacon 10:f76476943a6c 131 }
beacon 10:f76476943a6c 132
beacon 4:67d7177c213f 133 void Robot::wallLeft(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 134 *counter += 1;
beacon 4:67d7177c213f 135
beacon 4:67d7177c213f 136 if (*lastAct != 2){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 137 *timer = 0;
beacon 4:67d7177c213f 138 *lastAct = 2;
beacon 4:67d7177c213f 139 }
beacon 4:67d7177c213f 140
beacon 4:67d7177c213f 141 this->turnRight();
beacon 4:67d7177c213f 142 }
beacon 4:67d7177c213f 143
beacon 4:67d7177c213f 144 void Robot::wallFront(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 145 if (*lastAct != 3){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 146 *timer = 0;
beacon 4:67d7177c213f 147 *lastAct = 3;
beacon 4:67d7177c213f 148 }
beacon 4:67d7177c213f 149
beacon 4:67d7177c213f 150 *counter = MAX; //By setting the counter to MAX, next time it will go into the first if-statement (action 0).
beacon 4:67d7177c213f 151 }
beacon 4:67d7177c213f 152
beacon 4:67d7177c213f 153
beacon 5:1aaf5de776ff 154 void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound, int* found){
beacon 4:67d7177c213f 155 //*counter += 1;
beacon 4:67d7177c213f 156 *legoFound = 0;
beacon 4:67d7177c213f 157
beacon 5:1aaf5de776ff 158 if (*lastAct != 4){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 159 *timer = 0;
beacon 4:67d7177c213f 160 *lastAct = 4;
beacon 4:67d7177c213f 161 }
beacon 4:67d7177c213f 162
beacon 5:1aaf5de776ff 163 if (this->sensors[FWD] < NEAR){ //If Sam sees a wall turn around
beacon 4:67d7177c213f 164 *legoFound = -1;
beacon 5:1aaf5de776ff 165 *counter = MAX; //setting counter to MAX will couse sam to turnAround
beacon 4:67d7177c213f 166 }
beacon 4:67d7177c213f 167
beacon 5:1aaf5de776ff 168 if (this->sensors[FWD_L] > 0.16f){
beacon 4:67d7177c213f 169 this->drive();
beacon 4:67d7177c213f 170 }
beacon 4:67d7177c213f 171 else{
beacon 11:292bdbd85a9c 172 this->stop();
beacon 5:1aaf5de776ff 173 *found = 1;
beacon 4:67d7177c213f 174 }
beacon 4:67d7177c213f 175 }
beacon 4:67d7177c213f 176
beacon 4:67d7177c213f 177 void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){
beacon 4:67d7177c213f 178 //*counter += 1;
beacon 4:67d7177c213f 179 *legoFound = 1;
beacon 4:67d7177c213f 180
beacon 4:67d7177c213f 181 if (*lastAct != 5){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 182 *timer = 0;
beacon 4:67d7177c213f 183 *lastAct = 5;
beacon 4:67d7177c213f 184 }
beacon 4:67d7177c213f 185
beacon 5:1aaf5de776ff 186 if (this->sensors[FWD_L] > 0.22f){
beacon 4:67d7177c213f 187 this->turnRight();
beacon 4:67d7177c213f 188 }
beacon 4:67d7177c213f 189 else{
beacon 4:67d7177c213f 190 this->stop();
beacon 5:1aaf5de776ff 191 *legoFound = -1;
beacon 4:67d7177c213f 192 }
beacon 4:67d7177c213f 193 }
beacon 4:67d7177c213f 194
beacon 4:67d7177c213f 195 void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
beacon 4:67d7177c213f 196 //*counter += 1;
beacon 4:67d7177c213f 197 *legoFound = 2;
beacon 0:d267b248eff4 198
beacon 4:67d7177c213f 199 if (*lastAct != 6){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 200 *timer = 0;
beacon 4:67d7177c213f 201 *lastAct = 6;
beacon 4:67d7177c213f 202 }
beacon 4:67d7177c213f 203
beacon 5:1aaf5de776ff 204 if (this->sensors[FWD_L] > 0.22f){
beacon 4:67d7177c213f 205 this->turnLeft();
beacon 4:67d7177c213f 206 }
beacon 4:67d7177c213f 207 else{
beacon 4:67d7177c213f 208 this->stop();
beacon 5:1aaf5de776ff 209 *legoFound = -1;
beacon 4:67d7177c213f 210 }
beacon 4:67d7177c213f 211 }
beacon 4:67d7177c213f 212
beacon 4:67d7177c213f 213
beacon 4:67d7177c213f 214 void Robot::nothingFound(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 215 *counter = 0;
beacon 4:67d7177c213f 216 if (*lastAct != 7){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 217 *timer = 0;
beacon 4:67d7177c213f 218 *lastAct = 7;
beacon 4:67d7177c213f 219 }
beacon 4:67d7177c213f 220
beacon 4:67d7177c213f 221 this->drive();
beacon 4:67d7177c213f 222 }
beacon 4:67d7177c213f 223
beacon 4:67d7177c213f 224
beacon 11:292bdbd85a9c 225 int Robot::search(int* timer){
EpicG10 13:6c1f8e35b242 226 //return 1;
beacon 11:292bdbd85a9c 227 enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
beacon 4:67d7177c213f 228
beacon 11:292bdbd85a9c 229 static int state = neutral;
beacon 4:67d7177c213f 230
beacon 11:292bdbd85a9c 231 static int counter = 0; //counter is used blabla
beacon 4:67d7177c213f 232
beacon 11:292bdbd85a9c 233 static int rando = -1;
beacon 10:f76476943a6c 234
beacon 11:292bdbd85a9c 235 //static int lastAct = 0;
beacon 4:67d7177c213f 236
beacon 11:292bdbd85a9c 237 switch( state ){
beacon 11:292bdbd85a9c 238 case neutral:
beacon 11:292bdbd85a9c 239 if( counter > MAX ){
beacon 11:292bdbd85a9c 240 state = max;
beacon 11:292bdbd85a9c 241 }
EpicG10 13:6c1f8e35b242 242 else if( this->sensors[FWD] < NEAR ){
beacon 11:292bdbd85a9c 243 state = wallF;
EpicG10 13:6c1f8e35b242 244 }
beacon 11:292bdbd85a9c 245 else if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 246 state = wallL;
beacon 11:292bdbd85a9c 247 }
beacon 11:292bdbd85a9c 248 else if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 249 state = wallR;
beacon 11:292bdbd85a9c 250 }
beacon 11:292bdbd85a9c 251 else if( this->sensors[FWD_L] < NEAR ){
beacon 11:292bdbd85a9c 252 state = legoF;
beacon 11:292bdbd85a9c 253 }
beacon 11:292bdbd85a9c 254 else if( this->sensors[LEFT_L] < NEAR ){
beacon 11:292bdbd85a9c 255 state = legoL;
beacon 11:292bdbd85a9c 256 }
beacon 11:292bdbd85a9c 257 else if( this->sensors[RIGHT_L] < NEAR ){
beacon 11:292bdbd85a9c 258 state = legoR;
beacon 11:292bdbd85a9c 259 }
beacon 11:292bdbd85a9c 260 else{
beacon 11:292bdbd85a9c 261 this->drive();
beacon 11:292bdbd85a9c 262 counter = 0;
beacon 11:292bdbd85a9c 263 }
beacon 11:292bdbd85a9c 264 break;
beacon 11:292bdbd85a9c 265
beacon 11:292bdbd85a9c 266 case max: {
beacon 11:292bdbd85a9c 267 int time = 0;
beacon 11:292bdbd85a9c 268 if( time < 15 && this->sensors[FWD] > NEAR ){
beacon 11:292bdbd85a9c 269 rando == -1 ? rando = rand() % 2 : rando = rando;
beacon 11:292bdbd85a9c 270 this->turnAround(rando);
beacon 11:292bdbd85a9c 271 }
beacon 11:292bdbd85a9c 272 else{
beacon 11:292bdbd85a9c 273 state = neutral;
beacon 11:292bdbd85a9c 274 counter = 0;
beacon 11:292bdbd85a9c 275 rando = -1;
beacon 11:292bdbd85a9c 276 }
beacon 11:292bdbd85a9c 277 break;
beacon 11:292bdbd85a9c 278 }
beacon 11:292bdbd85a9c 279
beacon 11:292bdbd85a9c 280 case wallF:
beacon 11:292bdbd85a9c 281 counter++;
beacon 11:292bdbd85a9c 282 if( this->sensors[FWD] < NEAR ){
beacon 11:292bdbd85a9c 283 rando == -1 ? rando = rand() % 2 : rando = rando;
beacon 11:292bdbd85a9c 284 this->turnAround(rando);
beacon 11:292bdbd85a9c 285 }
beacon 11:292bdbd85a9c 286 else{
beacon 11:292bdbd85a9c 287 state = neutral;
beacon 11:292bdbd85a9c 288 rando = -1;
beacon 11:292bdbd85a9c 289 }
beacon 11:292bdbd85a9c 290 break;
beacon 11:292bdbd85a9c 291
beacon 11:292bdbd85a9c 292 case wallL:
beacon 11:292bdbd85a9c 293 counter++;
beacon 11:292bdbd85a9c 294 if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 295 this->turnRight();
beacon 11:292bdbd85a9c 296 }
beacon 11:292bdbd85a9c 297 else{
beacon 11:292bdbd85a9c 298 state = neutral;
beacon 11:292bdbd85a9c 299 }
beacon 11:292bdbd85a9c 300 break;
beacon 11:292bdbd85a9c 301
beacon 11:292bdbd85a9c 302 case wallR:
beacon 11:292bdbd85a9c 303 counter++;
beacon 11:292bdbd85a9c 304 if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 305 this->turnLeft();
beacon 11:292bdbd85a9c 306 }
beacon 11:292bdbd85a9c 307 else{
beacon 11:292bdbd85a9c 308 state = neutral;
beacon 11:292bdbd85a9c 309 }
beacon 11:292bdbd85a9c 310 break;
beacon 11:292bdbd85a9c 311
beacon 11:292bdbd85a9c 312 case legoF:
beacon 11:292bdbd85a9c 313 //counter++;
EpicG10 13:6c1f8e35b242 314 if( this->sensors[FWD_L] > 0.17f ){
beacon 11:292bdbd85a9c 315 this->drive();
beacon 11:292bdbd85a9c 316 }
beacon 11:292bdbd85a9c 317 else{
beacon 11:292bdbd85a9c 318 state = neutral;
beacon 11:292bdbd85a9c 319 counter = 0;
beacon 11:292bdbd85a9c 320 this->stop();
beacon 11:292bdbd85a9c 321 return 1;
beacon 11:292bdbd85a9c 322 }
beacon 11:292bdbd85a9c 323 break;
beacon 11:292bdbd85a9c 324
beacon 11:292bdbd85a9c 325 case legoL:
beacon 11:292bdbd85a9c 326 counter++;
EpicG10 13:6c1f8e35b242 327 if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){
beacon 11:292bdbd85a9c 328 this->turnLeft();
beacon 11:292bdbd85a9c 329 }
beacon 11:292bdbd85a9c 330 else{
beacon 11:292bdbd85a9c 331 state = legoF;
beacon 11:292bdbd85a9c 332 }
beacon 11:292bdbd85a9c 333 break;
beacon 11:292bdbd85a9c 334
beacon 11:292bdbd85a9c 335 case legoR:
beacon 11:292bdbd85a9c 336 counter++;
EpicG10 13:6c1f8e35b242 337 if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){
beacon 11:292bdbd85a9c 338 this->turnRight();
beacon 11:292bdbd85a9c 339 }
beacon 11:292bdbd85a9c 340 else{
beacon 11:292bdbd85a9c 341 state = legoF;
beacon 11:292bdbd85a9c 342 }
beacon 11:292bdbd85a9c 343 break;
beacon 11:292bdbd85a9c 344
beacon 4:67d7177c213f 345 }
beacon 11:292bdbd85a9c 346 return 0;
beacon 4:67d7177c213f 347 }
beacon 4:67d7177c213f 348
beacon 10:f76476943a6c 349 int Robot::getErrorMotor(){
beacon 10:f76476943a6c 350 return 0; //errorMotor;
beacon 0:d267b248eff4 351 }
beacon 0:d267b248eff4 352
beacon 11:292bdbd85a9c 353 /*int Robot::searchh(int* timer){
beacon 11:292bdbd85a9c 354 enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
beacon 11:292bdbd85a9c 355
beacon 11:292bdbd85a9c 356 static int state = neutral;
beacon 11:292bdbd85a9c 357
beacon 11:292bdbd85a9c 358 static int counter = 0; //counter is used blabla
beacon 11:292bdbd85a9c 359
beacon 11:292bdbd85a9c 360 //static int lastAct = 0;
beacon 11:292bdbd85a9c 361
beacon 11:292bdbd85a9c 362 switch( state ){
beacon 11:292bdbd85a9c 363 case neutral:
beacon 11:292bdbd85a9c 364 if( counter > MAX ){
beacon 11:292bdbd85a9c 365 state = max;
beacon 11:292bdbd85a9c 366 }
beacon 11:292bdbd85a9c 367 else if( this->sensora[FWD] < NEAR ){
beacon 11:292bdbd85a9c 368 state = wallF;
beacon 11:292bdbd85a9c 369 counter = 0;
beacon 11:292bdbd85a9c 370 }
beacon 11:292bdbd85a9c 371 else if( this->sensors[LEFT] < NEAR ){
beacon 11:292bdbd85a9c 372 state = wallL;
beacon 11:292bdbd85a9c 373 counter = 0;
beacon 11:292bdbd85a9c 374 }
beacon 11:292bdbd85a9c 375 else if( this->sensors[RIGHT] < NEAR ){
beacon 11:292bdbd85a9c 376 state = wallR;
beacon 11:292bdbd85a9c 377 counter = 0;
beacon 11:292bdbd85a9c 378 }
beacon 11:292bdbd85a9c 379 else if( this->sensors[FWD_L] < NEAR ){
beacon 11:292bdbd85a9c 380 state = legoF;
beacon 11:292bdbd85a9c 381 counter = 0;
beacon 11:292bdbd85a9c 382 }
beacon 11:292bdbd85a9c 383 else if( this->sensors[LEFT_L] < NEAR ){
beacon 11:292bdbd85a9c 384 state = legoL;
beacon 11:292bdbd85a9c 385 counter = 0;
beacon 11:292bdbd85a9c 386 }
beacon 11:292bdbd85a9c 387 else if( this->sensors[RIGHT_L] < NEAR ){
beacon 11:292bdbd85a9c 388 state = legoR;
beacon 11:292bdbd85a9c 389 counter = 0;
beacon 11:292bdbd85a9c 390 }
beacon 11:292bdbd85a9c 391 break;
beacon 11:292bdbd85a9c 392
beacon 11:292bdbd85a9c 393 case max:
beacon 11:292bdbd85a9c 394
beacon 11:292bdbd85a9c 395 break;
beacon 11:292bdbd85a9c 396 */
beacon 11:292bdbd85a9c 397
beacon 0:d267b248eff4 398 //reset
beacon 0:d267b248eff4 399 //}
beacon 0:d267b248eff4 400
beacon 0:d267b248eff4 401 //Add management for Overpower!! Pin PB_15