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@3:fa61be4ecaa6, 2017-03-22 (annotated)

Committer:
beacon
Date:
Wed Mar 22 13:33:07 2017 +0000
Revision:
3:fa61be4ecaa6
Parent:
2:01e9de508316
Child:
4:67d7177c213f
Official

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 2:01e9de508316 65 void Robot::search(int* counter, Timer* t){
beacon 1:388c915756f5 66 int rando;
beacon 0:d267b248eff4 67
beacon 1:388c915756f5 68 if (*counter >= 5) {
beacon 3:fa61be4ecaa6 69 //t->reset();
beacon 2:01e9de508316 70
beacon 1:388c915756f5 71 rando = rand() % 2;
beacon 1:388c915756f5 72 while (this->sensors[FWD] < NEAR){
beacon 2:01e9de508316 73 if ( t->read() > TIMEOUT ){
beacon 2:01e9de508316 74 break;
beacon 2:01e9de508316 75 }
beacon 2:01e9de508316 76
beacon 1:388c915756f5 77 this->leds[FWD] = 1;
beacon 1:388c915756f5 78 this->turnAround(rando);
beacon 1:388c915756f5 79 }
beacon 2:01e9de508316 80 this->leds[FWD] = 0;
beacon 1:388c915756f5 81 }
beacon 1:388c915756f5 82
beacon 1:388c915756f5 83 else if (this->sensors[RIGHT] < NEAR){
beacon 2:01e9de508316 84 t-> reset();
beacon 2:01e9de508316 85
beacon 1:388c915756f5 86 *counter += 1;
beacon 1:388c915756f5 87 while (this->sensors[RIGHT] < NEAR){
beacon 2:01e9de508316 88 if ( t->read() > TIMEOUT ){
beacon 2:01e9de508316 89 break;
beacon 2:01e9de508316 90 }
beacon 2:01e9de508316 91
beacon 1:388c915756f5 92 this->turnLeft();
beacon 1:388c915756f5 93 this->leds[RIGHT] = 1;
beacon 1:388c915756f5 94 }
beacon 1:388c915756f5 95 this->leds[RIGHT] = 0;
beacon 0:d267b248eff4 96 }
beacon 0:d267b248eff4 97
beacon 1:388c915756f5 98 else if (this->sensors[LEFT] < NEAR){
beacon 2:01e9de508316 99 t-> reset();
beacon 2:01e9de508316 100
beacon 1:388c915756f5 101 *counter += 1;
beacon 1:388c915756f5 102 while (this->sensors[LEFT] < NEAR){
beacon 2:01e9de508316 103 if ( t->read() > TIMEOUT ){
beacon 2:01e9de508316 104 break;
beacon 2:01e9de508316 105 }
beacon 2:01e9de508316 106
beacon 1:388c915756f5 107 this->turnRight();
beacon 1:388c915756f5 108 this->leds[LEFT] = 1;
beacon 1:388c915756f5 109 }
beacon 1:388c915756f5 110 this->leds[LEFT] = 0;
beacon 1:388c915756f5 111 }
beacon 1:388c915756f5 112
beacon 1:388c915756f5 113 else if (this->sensors[FWD] < NEAR) {
beacon 2:01e9de508316 114 t-> reset();
beacon 2:01e9de508316 115
beacon 1:388c915756f5 116 rando = rand() % 2;
beacon 1:388c915756f5 117 while (this->sensors[FWD] < NEAR){
beacon 2:01e9de508316 118 if ( t->read() > TIMEOUT ){
beacon 2:01e9de508316 119 break;
beacon 2:01e9de508316 120 }
beacon 2:01e9de508316 121
beacon 1:388c915756f5 122 this->leds[FWD] = 1;
beacon 1:388c915756f5 123 this->turnAround(rando);
beacon 1:388c915756f5 124 }
beacon 1:388c915756f5 125 this->leds[FWD] = 0;
beacon 1:388c915756f5 126 }
beacon 1:388c915756f5 127
beacon 1:388c915756f5 128 else {
beacon 1:388c915756f5 129 *counter = 0;
beacon 1:388c915756f5 130 this->drive();
beacon 0:d267b248eff4 131 }
beacon 0:d267b248eff4 132 }
beacon 0:d267b248eff4 133
beacon 0:d267b248eff4 134 //void Robot::init(){
beacon 0:d267b248eff4 135 // Robot.DistanceSensors.init(&sensorVoltage, &bit0, &bit1, &bit2, ii);
beacon 0:d267b248eff4 136 //}
beacon 1:388c915756f5 137
beacon 0:d267b248eff4 138 //Remember to set
beacon 0:d267b248eff4 139 //DigitalOut powerMotor(PB_2) = 1;
beacon 0:d267b248eff4 140 //DigitalIn errorMotor(PB_14);
beacon 0:d267b248eff4 141 //if (errorMotor){
beacon 0:d267b248eff4 142 //reset
beacon 0:d267b248eff4 143 //}
beacon 0:d267b248eff4 144
beacon 0:d267b248eff4 145 //Add management for Overpower!! Pin PB_15