zhaw_st16b_pes2_10 / Mbed 2 deprecated PES-Yanick2

ahah

Dependencies:   Servo mbed

Fork of PES_Official by zhaw_st16b_pes2_10

Sources/Robot.cpp@6:9627ef490cd5, 2017-05-02 (annotated)

Committer:
EpicG10
Date:
Tue May 02 10:35:15 2017 +0000
Revision:
6:9627ef490cd5
Parent:
5:1aaf5de776ff 
ok;

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