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@9:ac362674c480, 2017-05-02 (annotated)

Committer:
beacon
Date:
Tue May 02 08:39:35 2017 +0000
Revision:
9:ac362674c480
kl

Who changed what in which revision?

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