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@10:f76476943a6c, 2017-05-03 (annotated)

Committer:
beacon
Date:
Wed May 03 13:54:51 2017 +0000
Revision:
10:f76476943a6c
Parent:
6:ba26dd3251b3
Child:
11:292bdbd85a9c
oi

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 5:1aaf5de776ff 171 *found = 1;
beacon 4:67d7177c213f 172 }
beacon 4:67d7177c213f 173 }
beacon 4:67d7177c213f 174
beacon 4:67d7177c213f 175 void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){
beacon 4:67d7177c213f 176 //*counter += 1;
beacon 4:67d7177c213f 177 *legoFound = 1;
beacon 4:67d7177c213f 178
beacon 4:67d7177c213f 179 if (*lastAct != 5){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 180 *timer = 0;
beacon 4:67d7177c213f 181 *lastAct = 5;
beacon 4:67d7177c213f 182 }
beacon 4:67d7177c213f 183
beacon 5:1aaf5de776ff 184 if (this->sensors[FWD_L] > 0.22f){
beacon 4:67d7177c213f 185 this->turnRight();
beacon 4:67d7177c213f 186 }
beacon 4:67d7177c213f 187 else{
beacon 4:67d7177c213f 188 this->stop();
beacon 5:1aaf5de776ff 189 *legoFound = -1;
beacon 4:67d7177c213f 190 }
beacon 4:67d7177c213f 191 }
beacon 4:67d7177c213f 192
beacon 4:67d7177c213f 193 void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){
beacon 4:67d7177c213f 194 //*counter += 1;
beacon 4:67d7177c213f 195 *legoFound = 2;
beacon 0:d267b248eff4 196
beacon 4:67d7177c213f 197 if (*lastAct != 6){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 198 *timer = 0;
beacon 4:67d7177c213f 199 *lastAct = 6;
beacon 4:67d7177c213f 200 }
beacon 4:67d7177c213f 201
beacon 5:1aaf5de776ff 202 if (this->sensors[FWD_L] > 0.22f){
beacon 4:67d7177c213f 203 this->turnLeft();
beacon 4:67d7177c213f 204 }
beacon 4:67d7177c213f 205 else{
beacon 4:67d7177c213f 206 this->stop();
beacon 5:1aaf5de776ff 207 *legoFound = -1;
beacon 4:67d7177c213f 208 }
beacon 4:67d7177c213f 209 }
beacon 4:67d7177c213f 210
beacon 4:67d7177c213f 211
beacon 4:67d7177c213f 212 void Robot::nothingFound(int* counter, int* timer, int* lastAct){
beacon 4:67d7177c213f 213 *counter = 0;
beacon 4:67d7177c213f 214 if (*lastAct != 7){ //If this wasn't the last called action, reset the timer.
beacon 4:67d7177c213f 215 *timer = 0;
beacon 4:67d7177c213f 216 *lastAct = 7;
beacon 4:67d7177c213f 217 }
beacon 4:67d7177c213f 218
beacon 4:67d7177c213f 219 this->drive();
beacon 4:67d7177c213f 220 }
beacon 4:67d7177c213f 221
beacon 4:67d7177c213f 222
beacon 4:67d7177c213f 223 void Robot::search(int* counter, int* timer, int* found){
beacon 4:67d7177c213f 224
beacon 4:67d7177c213f 225 static int rando = -1; //Rando will be used, to randomize turnAround()
beacon 4:67d7177c213f 226 //-1 := unused => set; 0 := turnRight(); 1 := turnLeft();
beacon 4:67d7177c213f 227
beacon 4:67d7177c213f 228 static int lastAct = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 4:67d7177c213f 229 //-1 := unused; x != -1 := action x was called last.
beacon 4:67d7177c213f 230
beacon 4:67d7177c213f 231 static int legoFound = -1; //Is used to determine, on what side the lego was found.
beacon 4:67d7177c213f 232 //-1 := unused; 0:= front; 1:= right, 2:= left.
beacon 4:67d7177c213f 233
beacon 4:67d7177c213f 234 *timer += 1; //timer holds the time in 0.1s
beacon 4:67d7177c213f 235
beacon 4:67d7177c213f 236
beacon 5:1aaf5de776ff 237 if (*counter >= MAX) { //Robot is stuck turning left & right
beacon 4:67d7177c213f 238 counterMax(counter, timer, &lastAct, &rando);
beacon 4:67d7177c213f 239 }
beacon 4:67d7177c213f 240
beacon 10:f76476943a6c 241 //Wall actions:
beacon 4:67d7177c213f 242 else if (this->sensors[RIGHT] < NEAR && legoFound == -1){ //Robot has spotted an obstacle on the right.
beacon 4:67d7177c213f 243 wallRight(counter, timer, &lastAct);
beacon 4:67d7177c213f 244 }
beacon 10:f76476943a6c 245
beacon 4:67d7177c213f 246 else if (this->sensors[LEFT] < NEAR && legoFound == -1) { //Robot has spotted an obstacle on the left.
beacon 4:67d7177c213f 247 wallLeft(counter, timer, &lastAct);
beacon 4:67d7177c213f 248 }
beacon 4:67d7177c213f 249
beacon 4:67d7177c213f 250 else if (this->sensors[FWD] < NEAR && legoFound == -1) { //Robot has spotted an obstacle infront
beacon 4:67d7177c213f 251 wallFront(counter, timer, &lastAct);
beacon 4:67d7177c213f 252 }
beacon 4:67d7177c213f 253
beacon 4:67d7177c213f 254 //Lego actions:
beacon 10:f76476943a6c 255 else if (this->sensors[FWD_L] < NEAR_LEGO + 0.03f && legoFound == -1 || legoFound == 0){ //There's a Lego in front.
beacon 5:1aaf5de776ff 256 legoFront(counter, timer, &lastAct, &legoFound, found);
beacon 4:67d7177c213f 257 }
beacon 4:67d7177c213f 258
beacon 10:f76476943a6c 259 else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){ //There's a Lego on the right.
beacon 4:67d7177c213f 260 legoRight(counter, timer, &lastAct, &legoFound);
beacon 4:67d7177c213f 261 }
beacon 4:67d7177c213f 262
beacon 10:f76476943a6c 263 else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){ //There's a Lego on the left.
beacon 4:67d7177c213f 264 legoLeft(counter, timer, &lastAct, &legoFound);
beacon 4:67d7177c213f 265 }
beacon 4:67d7177c213f 266
beacon 4:67d7177c213f 267 //Lego is ready to be grabbed.
beacon 4:67d7177c213f 268 else if(legoFound == 3){
beacon 4:67d7177c213f 269 *found = 1; //When found is set to 1, main will call arms functions
beacon 4:67d7177c213f 270 *counter = 0;
beacon 4:67d7177c213f 271 *timer = 0;
beacon 4:67d7177c213f 272 legoFound = -1;
beacon 4:67d7177c213f 273 }
beacon 4:67d7177c213f 274
beacon 4:67d7177c213f 275
beacon 4:67d7177c213f 276 //Nothing found
beacon 4:67d7177c213f 277 else {
beacon 4:67d7177c213f 278 nothingFound(counter, timer, &lastAct);
beacon 4:67d7177c213f 279 }
beacon 4:67d7177c213f 280 }
beacon 4:67d7177c213f 281
beacon 10:f76476943a6c 282 int Robot::getErrorMotor(){
beacon 10:f76476943a6c 283 return 0; //errorMotor;
beacon 0:d267b248eff4 284 }
beacon 0:d267b248eff4 285
beacon 0:d267b248eff4 286 //reset
beacon 0:d267b248eff4 287 //}
beacon 0:d267b248eff4 288
beacon 0:d267b248eff4 289 //Add management for Overpower!! Pin PB_15