zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Officiall

c

Dependencies:   Servo ServoArm mbed

Fork of PES_PIXY_Official by zhaw_st16b_pes2_10

Sources/Robot.cpp@1:fd3cef0f116d, 2017-05-23 (annotated)

Committer:
EpicG10
Date:
Tue May 23 16:24:49 2017 +0000
Revision:
1:fd3cef0f116d
Parent:
0:15a8480061e8 
a

Who changed what in which revision?

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