zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official

Do NOT modify!

Dependencies:   mbed Servo ServoArm

Fork of PES_Yanick by zhaw_st16b_pes2_10

Sources/Robot.cpp@1:388c915756f5, 2017-03-19 (annotated)

Committer:
beacon
Date:
Sun Mar 19 12:20:26 2017 +0000
Revision:
1:388c915756f5
Parent:
0:d267b248eff4
Child:
2:01e9de508316
[404 message not found]

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:d267b248eff4 1 #include "Robot.h"
beacon 0:d267b248eff4 2
beacon 0:d267b248eff4 3 /* Work in progress -------------------------------------------- */
beacon 0:d267b248eff4 4
beacon 1:388c915756f5 5 Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage )
beacon 0:d267b248eff4 6 {
beacon 0:d267b248eff4 7 this->powerSignal = enableSignal;
beacon 1:388c915756f5 8 this->left = left;
beacon 1:388c915756f5 9 this->right = right;
beacon 0:d267b248eff4 10
beacon 0:d267b248eff4 11 this->left->period(0.00005f);
beacon 0:d267b248eff4 12 this->right->period(0.00005f);
beacon 1:388c915756f5 13
beacon 1:388c915756f5 14 this->leds = leds;
beacon 1:388c915756f5 15
beacon 1:388c915756f5 16 this->FarbVoltage = FarbVoltage;
beacon 1:388c915756f5 17
beacon 0:d267b248eff4 18 }
beacon 0:d267b248eff4 19
beacon 0:d267b248eff4 20 /* work in progress ------------------------------------------- */
beacon 0:d267b248eff4 21 void Robot::drive()
beacon 0:d267b248eff4 22 {
beacon 0:d267b248eff4 23 //pwm determine what direction it goes.
beacon 0:d267b248eff4 24 *powerSignal = 1;
beacon 1:388c915756f5 25 *left= 0.6f;
beacon 1:388c915756f5 26 *right= 0.4f;
beacon 1:388c915756f5 27 }
beacon 1:388c915756f5 28
beacon 1:388c915756f5 29 void Robot::turnLeft()
beacon 1:388c915756f5 30 {
beacon 1:388c915756f5 31
beacon 1:388c915756f5 32 *powerSignal = 1;
beacon 1:388c915756f5 33 *left= 0.4f;
beacon 1:388c915756f5 34 *right= 0.4f;
beacon 1:388c915756f5 35
beacon 0:d267b248eff4 36 }
beacon 0:d267b248eff4 37
beacon 1:388c915756f5 38 void Robot::turnRight()
beacon 1:388c915756f5 39 {
beacon 1:388c915756f5 40 *powerSignal = 1;
beacon 1:388c915756f5 41 *left= 0.6f;
beacon 1:388c915756f5 42 *right= 0.6f;
beacon 1:388c915756f5 43 }
beacon 1:388c915756f5 44
beacon 1:388c915756f5 45 void Robot::turnAround(int left)
beacon 1:388c915756f5 46 {
beacon 0:d267b248eff4 47 *powerSignal = 1;
beacon 1:388c915756f5 48
beacon 1:388c915756f5 49 if (left) {
beacon 1:388c915756f5 50 turnLeft();
beacon 1:388c915756f5 51 }
beacon 1:388c915756f5 52
beacon 1:388c915756f5 53 else {
beacon 1:388c915756f5 54 turnRight();
beacon 1:388c915756f5 55 }
beacon 1:388c915756f5 56 }
beacon 1:388c915756f5 57
beacon 1:388c915756f5 58 void Robot::stop()
beacon 1:388c915756f5 59 {
beacon 1:388c915756f5 60 *powerSignal = 1;
beacon 1:388c915756f5 61 *left= 0.5f;
beacon 1:388c915756f5 62 *right= 0.5f;
beacon 0:d267b248eff4 63 }
beacon 0:d267b248eff4 64
beacon 1:388c915756f5 65 void Robot::search(int* counter){
beacon 1:388c915756f5 66 int rando;
beacon 0:d267b248eff4 67
beacon 1:388c915756f5 68 if (*counter >= 5) {
beacon 1:388c915756f5 69 rando = rand() % 2;
beacon 1:388c915756f5 70 while (this->sensors[FWD] < NEAR){
beacon 1:388c915756f5 71 this->leds[FWD] = 1;
beacon 1:388c915756f5 72 this->turnAround(rando);
beacon 1:388c915756f5 73 }
beacon 1:388c915756f5 74 this->leds[FWD] = 0;
beacon 1:388c915756f5 75 }
beacon 1:388c915756f5 76
beacon 1:388c915756f5 77 else if (this->sensors[RIGHT] < NEAR){
beacon 1:388c915756f5 78 *counter += 1;
beacon 1:388c915756f5 79 while (this->sensors[RIGHT] < NEAR){
beacon 1:388c915756f5 80 this->turnLeft();
beacon 1:388c915756f5 81 this->leds[RIGHT] = 1;
beacon 1:388c915756f5 82 }
beacon 1:388c915756f5 83 this->leds[RIGHT] = 0;
beacon 0:d267b248eff4 84 }
beacon 0:d267b248eff4 85
beacon 1:388c915756f5 86 else if (this->sensors[LEFT] < NEAR){
beacon 1:388c915756f5 87 *counter += 1;
beacon 1:388c915756f5 88 while (this->sensors[LEFT] < NEAR){
beacon 1:388c915756f5 89 this->turnRight();
beacon 1:388c915756f5 90 this->leds[LEFT] = 1;
beacon 1:388c915756f5 91 }
beacon 1:388c915756f5 92 this->leds[LEFT] = 0;
beacon 1:388c915756f5 93 }
beacon 1:388c915756f5 94
beacon 1:388c915756f5 95 else if (this->sensors[FWD] < NEAR) {
beacon 1:388c915756f5 96 rando = rand() % 2;
beacon 1:388c915756f5 97 while (this->sensors[FWD] < NEAR){
beacon 1:388c915756f5 98 this->leds[FWD] = 1;
beacon 1:388c915756f5 99 this->turnAround(rando);
beacon 1:388c915756f5 100 }
beacon 1:388c915756f5 101 this->leds[FWD] = 0;
beacon 1:388c915756f5 102 }
beacon 1:388c915756f5 103
beacon 1:388c915756f5 104 else {
beacon 1:388c915756f5 105 *counter = 0;
beacon 1:388c915756f5 106 this->drive();
beacon 0:d267b248eff4 107 }
beacon 0:d267b248eff4 108 }
beacon 0:d267b248eff4 109
beacon 0:d267b248eff4 110 //void Robot::init(){
beacon 0:d267b248eff4 111 // Robot.DistanceSensors.init(&sensorVoltage, &bit0, &bit1, &bit2, ii);
beacon 0:d267b248eff4 112 //}
beacon 1:388c915756f5 113
beacon 0:d267b248eff4 114 //Remember to set
beacon 0:d267b248eff4 115 //DigitalOut powerMotor(PB_2) = 1;
beacon 0:d267b248eff4 116 //DigitalIn errorMotor(PB_14);
beacon 0:d267b248eff4 117 //if (errorMotor){
beacon 0:d267b248eff4 118 //reset
beacon 0:d267b248eff4 119 //}
beacon 0:d267b248eff4 120
beacon 0:d267b248eff4 121 //Add management for Overpower!! Pin PB_15