zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Official_Richtig

x

Dependencies:   Servo ServoArm mbed

Fork of PES_PIXY_Officiall by zhaw_st16b_pes2_10

Sources/Robot.cpp@5:acb938f45b9c, 2017-05-26 (annotated)

Committer:
EpicG10
Date:
Fri May 26 13:43:19 2017 +0000
Revision:
5:acb938f45b9c
Parent:
4:d611df1ed42b 
c

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:15a8480061e8 1 #include "Robot.h"
beacon 0:15a8480061e8 2 #include "Declarations.h"
beacon 0:15a8480061e8 3
beacon 0:15a8480061e8 4
EpicG10 3:63da1d1fae15 5 Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Pixy* pixy){
beacon 0:15a8480061e8 6 this->left = left;
beacon 0:15a8480061e8 7 this->right = right;
beacon 0:15a8480061e8 8 this->powerSignal = powerSignal;
beacon 0:15a8480061e8 9 //this->errorMotor = errorMotor;
beacon 0:15a8480061e8 10
beacon 0:15a8480061e8 11 this->left->period(0.00005f);
beacon 0:15a8480061e8 12 this->right->period(0.00005f);
beacon 0:15a8480061e8 13
beacon 0:15a8480061e8 14 this->leds = leds;
beacon 0:15a8480061e8 15
EpicG10 3:63da1d1fae15 16 //this->FarbVoltage = FarbVoltage;
beacon 0:15a8480061e8 17 this->frontS = frontS;
beacon 0:15a8480061e8 18 this->leftS = leftS;
beacon 0:15a8480061e8 19 this->rightS = rightS;
beacon 0:15a8480061e8 20
beacon 0:15a8480061e8 21 this->Arm = Arm;
beacon 0:15a8480061e8 22 this->Greifer = Greifer;
beacon 0:15a8480061e8 23
beacon 0:15a8480061e8 24 this->pixy = pixy;
beacon 0:15a8480061e8 25 }
beacon 0:15a8480061e8 26
beacon 0:15a8480061e8 27 //Drive functions
EpicG10 3:63da1d1fae15 28 void Robot::drive(){
beacon 0:15a8480061e8 29 //pwm determine what direction it goes.
beacon 0:15a8480061e8 30 *powerSignal = 1;
beacon 0:15a8480061e8 31 *left= 0.6f;
beacon 0:15a8480061e8 32 *right= 0.4f;
beacon 0:15a8480061e8 33 }
beacon 0:15a8480061e8 34
EpicG10 3:63da1d1fae15 35 void Robot::driveB(){
beacon 0:15a8480061e8 36 //pwm determine what direction it goes.
beacon 0:15a8480061e8 37 *powerSignal = 1;
beacon 0:15a8480061e8 38 *left= 0.4f;
beacon 0:15a8480061e8 39 *right= 0.6f;
beacon 0:15a8480061e8 40 }
beacon 0:15a8480061e8 41
beacon 0:15a8480061e8 42 void Robot::setLeft(float pwm){
beacon 0:15a8480061e8 43 *powerSignal = 1;
beacon 0:15a8480061e8 44 *left = pwm;
beacon 0:15a8480061e8 45 }
beacon 0:15a8480061e8 46
beacon 0:15a8480061e8 47 void Robot::setRight(float pwm){
beacon 0:15a8480061e8 48 *powerSignal = 1;
beacon 0:15a8480061e8 49 *right = pwm;
beacon 0:15a8480061e8 50 }
beacon 0:15a8480061e8 51
EpicG10 3:63da1d1fae15 52 void Robot::turnLeft(){
beacon 0:15a8480061e8 53 *powerSignal = 1;
beacon 0:15a8480061e8 54 *left= 0.4f;
beacon 0:15a8480061e8 55 *right= 0.4f;
beacon 0:15a8480061e8 56
beacon 0:15a8480061e8 57 }
beacon 0:15a8480061e8 58
EpicG10 3:63da1d1fae15 59 void Robot::turnLeftS(){
beacon 0:15a8480061e8 60 *powerSignal = 1;
beacon 0:15a8480061e8 61 *left= 0.45f;
beacon 0:15a8480061e8 62 *right= 0.45f;
beacon 0:15a8480061e8 63 }
beacon 0:15a8480061e8 64
EpicG10 3:63da1d1fae15 65 void Robot::turnRight(){
beacon 0:15a8480061e8 66 *powerSignal = 1;
beacon 0:15a8480061e8 67 *left= 0.6f;
beacon 0:15a8480061e8 68 *right= 0.6f;
beacon 0:15a8480061e8 69 }
beacon 0:15a8480061e8 70
EpicG10 3:63da1d1fae15 71 void Robot::turnRightS(){
beacon 0:15a8480061e8 72 *powerSignal = 1;
beacon 0:15a8480061e8 73 *left= 0.55f;
beacon 0:15a8480061e8 74 *right= 0.55f;
beacon 0:15a8480061e8 75 }
beacon 0:15a8480061e8 76
EpicG10 3:63da1d1fae15 77 void Robot::turnAround(int left){
beacon 0:15a8480061e8 78 *powerSignal = 1;
beacon 0:15a8480061e8 79
EpicG10 3:63da1d1fae15 80 if (left){
beacon 0:15a8480061e8 81 turnLeft();
beacon 0:15a8480061e8 82 }
EpicG10 3:63da1d1fae15 83 else{
beacon 0:15a8480061e8 84 turnRight();
beacon 0:15a8480061e8 85 }
beacon 0:15a8480061e8 86 }
beacon 0:15a8480061e8 87
EpicG10 3:63da1d1fae15 88 void Robot::stop(){
beacon 0:15a8480061e8 89 *powerSignal = 1;
beacon 0:15a8480061e8 90 *left= 0.5f;
beacon 0:15a8480061e8 91 *right= 0.5f;
beacon 0:15a8480061e8 92 }
beacon 0:15a8480061e8 93
beacon 0:15a8480061e8 94 void Robot::driveSlowly(){
beacon 0:15a8480061e8 95 static int i = 0;
beacon 0:15a8480061e8 96 i++;
beacon 0:15a8480061e8 97 if( i % 2 ){
beacon 0:15a8480061e8 98 this->drive();
beacon 0:15a8480061e8 99 }
beacon 0:15a8480061e8 100 else{
beacon 0:15a8480061e8 101 this->stop();
beacon 0:15a8480061e8 102 }
beacon 0:15a8480061e8 103 }
beacon 0:15a8480061e8 104
beacon 0:15a8480061e8 105 void Robot::driveBackSlowly(){
beacon 0:15a8480061e8 106 static int i = 0;
beacon 0:15a8480061e8 107 i++;
beacon 0:15a8480061e8 108 if( i % 2 ){
beacon 0:15a8480061e8 109 this->driveB();
beacon 0:15a8480061e8 110 }
beacon 0:15a8480061e8 111 else{
beacon 0:15a8480061e8 112 this->stop();
beacon 0:15a8480061e8 113 }
beacon 0:15a8480061e8 114 }
beacon 0:15a8480061e8 115
EpicG10 4:d611df1ed42b 116 void Robot::search(int* time){
EpicG10 3:63da1d1fae15 117 enum states {neutral = 0, counterMax, wallF, wallL, wallR};
beacon 0:15a8480061e8 118 static int state = neutral;
EpicG10 3:63da1d1fae15 119 static int rando = -1;
beacon 0:15a8480061e8 120 int oldx = this->pixy->getX();
beacon 0:15a8480061e8 121 static int counter = 0;
EpicG10 3:63da1d1fae15 122
EpicG10 4:d611df1ed42b 123 //if( this->pixy->getDetects() ) return 1;
beacon 0:15a8480061e8 124 switch( state ){
beacon 0:15a8480061e8 125 case neutral:
beacon 0:15a8480061e8 126 if( counter > MAX ){
beacon 0:15a8480061e8 127 state = counterMax;
beacon 0:15a8480061e8 128 }
beacon 0:15a8480061e8 129 else if( this->sensors[FWD].read() < NEAR ){
beacon 0:15a8480061e8 130 state = wallF;
EpicG10 4:d611df1ed42b 131 time = 0;
beacon 0:15a8480061e8 132 }
beacon 0:15a8480061e8 133 else if( this->sensors[LEFT].read() < NEAR ){
beacon 0:15a8480061e8 134 state = wallL;
EpicG10 4:d611df1ed42b 135 time = 0;
beacon 0:15a8480061e8 136 }
beacon 0:15a8480061e8 137 else if( this->sensors[RIGHT].read() < NEAR ){
beacon 0:15a8480061e8 138 state = wallR;
EpicG10 4:d611df1ed42b 139 time = 0;
beacon 0:15a8480061e8 140 }
beacon 0:15a8480061e8 141 else{
beacon 0:15a8480061e8 142 this->drive();
EpicG10 4:d611df1ed42b 143 time = 0;
beacon 0:15a8480061e8 144 }
beacon 0:15a8480061e8 145 break;
beacon 0:15a8480061e8 146
beacon 0:15a8480061e8 147 case counterMax:{
beacon 0:15a8480061e8 148 int i = 0;
beacon 0:15a8480061e8 149 if( i < 1000 ){
beacon 0:15a8480061e8 150 rando == -1 ? rando = rand() % 2 : rando = rando;
beacon 0:15a8480061e8 151 this->turnAround(rando);
beacon 0:15a8480061e8 152 i++;
beacon 0:15a8480061e8 153 }
beacon 0:15a8480061e8 154 else{
beacon 0:15a8480061e8 155 state = neutral;
beacon 0:15a8480061e8 156 rando = -1;
beacon 0:15a8480061e8 157 counter = 0;
beacon 0:15a8480061e8 158 i = 0;
beacon 0:15a8480061e8 159 }
beacon 0:15a8480061e8 160 break;
beacon 0:15a8480061e8 161 }
beacon 0:15a8480061e8 162
beacon 0:15a8480061e8 163 case wallF:
beacon 0:15a8480061e8 164 if( this->sensors[FWD].read() < NEAR ){
beacon 0:15a8480061e8 165 rando == -1 ? rando = rand() % 2 : rando = rando;
beacon 0:15a8480061e8 166 this->turnAround(rando);
beacon 0:15a8480061e8 167 counter++;
beacon 0:15a8480061e8 168 }
beacon 0:15a8480061e8 169 else{
beacon 0:15a8480061e8 170 state = neutral;
beacon 0:15a8480061e8 171 rando = -1;
beacon 0:15a8480061e8 172 }
beacon 0:15a8480061e8 173 break;
beacon 0:15a8480061e8 174
beacon 0:15a8480061e8 175 case wallL:
beacon 0:15a8480061e8 176 if( this->sensors[LEFT].read() < NEAR ){
beacon 0:15a8480061e8 177 this->turnRight();
beacon 0:15a8480061e8 178 counter++;
beacon 0:15a8480061e8 179 }
beacon 0:15a8480061e8 180 else{
beacon 0:15a8480061e8 181 state = neutral;
beacon 0:15a8480061e8 182 }
beacon 0:15a8480061e8 183 break;
beacon 0:15a8480061e8 184
beacon 0:15a8480061e8 185 case wallR:
beacon 0:15a8480061e8 186 if( this->sensors[RIGHT].read() < NEAR ){
beacon 0:15a8480061e8 187 this->turnLeft();
beacon 0:15a8480061e8 188 counter++;
beacon 0:15a8480061e8 189 }
beacon 0:15a8480061e8 190 else{
beacon 0:15a8480061e8 191 state = neutral;
beacon 0:15a8480061e8 192 }
beacon 0:15a8480061e8 193 break;
beacon 0:15a8480061e8 194
beacon 0:15a8480061e8 195 }
beacon 0:15a8480061e8 196 }
beacon 0:15a8480061e8 197
beacon 0:15a8480061e8 198
beacon 0:15a8480061e8 199 int Robot::getErrorMotor(){
beacon 0:15a8480061e8 200 return 0; //errorMotor;
beacon 0:15a8480061e8 201 }