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

main.cpp@2:c8c965d48f8d, 2017-05-25 (annotated)

Committer:
EpicG10
Date:
Thu May 25 18:12:28 2017 +0000
Revision:
2:c8c965d48f8d
Parent:
1:fd3cef0f116d
Child:
3:63da1d1fae15
c

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:15a8480061e8 1 #include "mbed.h"
beacon 0:15a8480061e8 2 #include "Robot.h"
beacon 0:15a8480061e8 3 #include "Declarations.h"
beacon 0:15a8480061e8 4 #include "Ultraschall.h"
beacon 0:15a8480061e8 5 #include "Pixy.h"
beacon 0:15a8480061e8 6 #include "PID_Control.h"
beacon 0:15a8480061e8 7 #include "LowpassFilter.h"
beacon 0:15a8480061e8 8 #include "EncoderCounter.h"
beacon 0:15a8480061e8 9
beacon 0:15a8480061e8 10 #include <cstdlib>
beacon 0:15a8480061e8 11
beacon 0:15a8480061e8 12 //Cam:
beacon 0:15a8480061e8 13 Serial cam(PA_9, PA_10);
beacon 0:15a8480061e8 14 Pixy pixy(cam);
beacon 0:15a8480061e8 15
beacon 0:15a8480061e8 16 //DistanceSensors related bottom:
EpicG10 2:c8c965d48f8d 17 Serial pc(SERIAL_TX, SERIAL_RX);
beacon 0:15a8480061e8 18
beacon 0:15a8480061e8 19 AnalogIn sensorVoltage(PB_1);
beacon 0:15a8480061e8 20 DigitalOut enable(PC_1);
beacon 0:15a8480061e8 21 DigitalOut bit0(PH_1);
beacon 0:15a8480061e8 22 DigitalOut bit1(PC_2);
beacon 0:15a8480061e8 23 DigitalOut bit2(PC_3);
beacon 0:15a8480061e8 24
beacon 0:15a8480061e8 25 //DistanceSensors top:
beacon 0:15a8480061e8 26 AnalogIn frontS(A1);
beacon 0:15a8480061e8 27 AnalogIn leftS(A2);
beacon 0:15a8480061e8 28 AnalogIn rightS(A3);
beacon 0:15a8480061e8 29
beacon 0:15a8480061e8 30
beacon 0:15a8480061e8 31 //Ultraschall
beacon 0:15a8480061e8 32 Ultraschall usensor(D6,D5);
beacon 0:15a8480061e8 33
beacon 0:15a8480061e8 34 //Leds related:
beacon 0:15a8480061e8 35 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
beacon 0:15a8480061e8 36
beacon 0:15a8480061e8 37 //motor related:
beacon 0:15a8480061e8 38 PwmOut left(PA_8);
beacon 0:15a8480061e8 39 PwmOut right(PA_9);
beacon 0:15a8480061e8 40
beacon 0:15a8480061e8 41 DigitalOut powerSignal(PB_2);
beacon 0:15a8480061e8 42 DigitalIn errorMotor(PB_14);
beacon 0:15a8480061e8 43 DigitalIn overtemperatur(PB_15);
beacon 0:15a8480061e8 44
beacon 0:15a8480061e8 45 //Arm:
beacon 0:15a8480061e8 46 ServoArm servoArm(PA_6);
beacon 0:15a8480061e8 47
beacon 0:15a8480061e8 48 //Greifer:
beacon 0:15a8480061e8 49 Servo servoGreifer(PC_7);
beacon 0:15a8480061e8 50
beacon 0:15a8480061e8 51 //Leiste:
beacon 0:15a8480061e8 52 Servo servoLeiste(PB_6);
beacon 0:15a8480061e8 53
beacon 0:15a8480061e8 54 //Button:
beacon 0:15a8480061e8 55 DigitalIn mybutton(USER_BUTTON);
beacon 0:15a8480061e8 56
beacon 0:15a8480061e8 57 //Farbsensor:
beacon 0:15a8480061e8 58 AnalogIn FarbVoltage(A0);
beacon 0:15a8480061e8 59 //DigitalOut led(D2);
beacon 0:15a8480061e8 60
beacon 0:15a8480061e8 61
beacon 0:15a8480061e8 62 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor, &pixy ); //Implement the Farbsensor into the Robot init function!!
beacon 0:15a8480061e8 63
beacon 0:15a8480061e8 64 void initializeDistanceSensors()
beacon 0:15a8480061e8 65 {
beacon 0:15a8480061e8 66 for( int ii = 0; ii<9; ++ii) {
beacon 0:15a8480061e8 67 sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
beacon 0:15a8480061e8 68
beacon 0:15a8480061e8 69 enable = 1;
beacon 0:15a8480061e8 70 }
beacon 0:15a8480061e8 71 }
beacon 0:15a8480061e8 72 /* */
EpicG10 2:c8c965d48f8d 73 int main()
EpicG10 2:c8c965d48f8d 74 {
beacon 0:15a8480061e8 75 initializeDistanceSensors(); //Initialises IR-Sensors.
EpicG10 2:c8c965d48f8d 76 enum states {search = 0, setPos, take};
beacon 0:15a8480061e8 77 int time = 0; //Time keeps track of time. [time] = ms
EpicG10 2:c8c965d48f8d 78
beacon 0:15a8480061e8 79 PID_Control pid;
EpicG10 2:c8c965d48f8d 80 pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000);
EpicG10 2:c8c965d48f8d 81 // pc.baud( 115200 );
EpicG10 2:c8c965d48f8d 82
EpicG10 2:c8c965d48f8d 83 int state = search;
EpicG10 2:c8c965d48f8d 84 while(!sam.Arm.collectToBack())wait(0.075);
EpicG10 2:c8c965d48f8d 85 while(!sam.Greifer.leave())wait(0.001);
EpicG10 2:c8c965d48f8d 86
EpicG10 2:c8c965d48f8d 87
EpicG10 2:c8c965d48f8d 88 while( 1 ) {
EpicG10 2:c8c965d48f8d 89
EpicG10 2:c8c965d48f8d 90 switch(state){
beacon 0:15a8480061e8 91
EpicG10 2:c8c965d48f8d 92 case search: {
EpicG10 2:c8c965d48f8d 93 if(!((pixy.getX()>50 && pixy.getX()<300)&&(pixy.getY()>50 && pixy.getY()<300))){
EpicG10 2:c8c965d48f8d 94 sam.search(&time);
beacon 0:15a8480061e8 95 }
EpicG10 2:c8c965d48f8d 96 else {
EpicG10 2:c8c965d48f8d 97 state = setPos;
EpicG10 2:c8c965d48f8d 98 }
EpicG10 2:c8c965d48f8d 99
EpicG10 2:c8c965d48f8d 100 break;
EpicG10 2:c8c965d48f8d 101 }
EpicG10 2:c8c965d48f8d 102
EpicG10 2:c8c965d48f8d 103 case setPos: {
EpicG10 2:c8c965d48f8d 104 sam.leds[5] = 1;
EpicG10 2:c8c965d48f8d 105 static int i = 0;
EpicG10 2:c8c965d48f8d 106 float eX = 133.0f - pixy.getX();
EpicG10 2:c8c965d48f8d 107 float diffX = pid.calc( eX, 0.006f );
EpicG10 2:c8c965d48f8d 108
EpicG10 2:c8c965d48f8d 109 float aX = 0.03; //minimum diff
EpicG10 2:c8c965d48f8d 110 float aY = 0.05;
EpicG10 2:c8c965d48f8d 111 if(diffX>0) {
EpicG10 2:c8c965d48f8d 112 if(diffX<aX)diffX=aX;
EpicG10 2:c8c965d48f8d 113 }
EpicG10 2:c8c965d48f8d 114 else if(diffX>-aX)diffX=-aX;
EpicG10 2:c8c965d48f8d 115
EpicG10 2:c8c965d48f8d 116 if(!(pixy.getX()>128 && pixy.getX()<138)){
EpicG10 2:c8c965d48f8d 117 sam.setLeft(0.5f - diffX);
EpicG10 2:c8c965d48f8d 118 sam.setRight(0.5f - diffX);
EpicG10 2:c8c965d48f8d 119 }
EpicG10 2:c8c965d48f8d 120
EpicG10 2:c8c965d48f8d 121 wait(0.001f);
EpicG10 2:c8c965d48f8d 122
EpicG10 2:c8c965d48f8d 123 float eY = 121.0f - pixy.getY();
EpicG10 2:c8c965d48f8d 124 float diffY = pid.calc( eY, 0.006f );
EpicG10 2:c8c965d48f8d 125 if(diffY>0) {
EpicG10 2:c8c965d48f8d 126 if(diffY<aY)diffY=aY;
EpicG10 2:c8c965d48f8d 127 }
EpicG10 2:c8c965d48f8d 128 else if(diffY>-aY) diffY=-aY;
EpicG10 2:c8c965d48f8d 129 if(!(pixy.getY()>115 && pixy.getY()<125)){
EpicG10 2:c8c965d48f8d 130 sam.setLeft(0.5f + diffY);
EpicG10 2:c8c965d48f8d 131 sam.setRight(0.5f - diffY);
EpicG10 2:c8c965d48f8d 132 }
EpicG10 2:c8c965d48f8d 133 i++;
EpicG10 2:c8c965d48f8d 134 if((pixy.getX()>132 && pixy.getX()<134)&&(pixy.getY()>119 && pixy.getY()<122)||!(i%1000)) {
EpicG10 2:c8c965d48f8d 135 state = take;
EpicG10 2:c8c965d48f8d 136 i = 0;
EpicG10 2:c8c965d48f8d 137 sam.stop();
EpicG10 2:c8c965d48f8d 138 sam.leds[5] = 0;
EpicG10 2:c8c965d48f8d 139 }
EpicG10 2:c8c965d48f8d 140 break;
EpicG10 2:c8c965d48f8d 141 }
EpicG10 2:c8c965d48f8d 142
EpicG10 2:c8c965d48f8d 143 case take: {
EpicG10 2:c8c965d48f8d 144 enum takeStates {down=0, take, up, leave};
EpicG10 2:c8c965d48f8d 145 static int tState=down;
EpicG10 2:c8c965d48f8d 146
EpicG10 2:c8c965d48f8d 147 sam.leds[1] = 1;
EpicG10 2:c8c965d48f8d 148 sam.stop();
EpicG10 2:c8c965d48f8d 149 //tState=leave;
EpicG10 2:c8c965d48f8d 150
EpicG10 2:c8c965d48f8d 151 switch(tState) {
EpicG10 2:c8c965d48f8d 152 case down:
EpicG10 2:c8c965d48f8d 153 if(sam.Arm.backToDown()) tState=take;
EpicG10 2:c8c965d48f8d 154 else tState=down;
EpicG10 2:c8c965d48f8d 155 break;
EpicG10 2:c8c965d48f8d 156 case take:
EpicG10 2:c8c965d48f8d 157 if(sam.Greifer.take()) tState=up;
EpicG10 2:c8c965d48f8d 158 else tState=take;
EpicG10 2:c8c965d48f8d 159 break;
EpicG10 2:c8c965d48f8d 160 case up:
EpicG10 2:c8c965d48f8d 161 if(sam.Arm.downToBack()) {wait(0.05);tState=leave;}
EpicG10 2:c8c965d48f8d 162 else tState=up;
EpicG10 2:c8c965d48f8d 163 break;
EpicG10 2:c8c965d48f8d 164 case leave:
EpicG10 2:c8c965d48f8d 165 if(sam.Greifer.leave()) {state=search; tState=down;}
EpicG10 2:c8c965d48f8d 166 else tState=leave;
EpicG10 2:c8c965d48f8d 167 break;
beacon 0:15a8480061e8 168 }
EpicG10 2:c8c965d48f8d 169
EpicG10 2:c8c965d48f8d 170
EpicG10 2:c8c965d48f8d 171 sam.leds[1] = 0;
EpicG10 2:c8c965d48f8d 172 break;
beacon 0:15a8480061e8 173 }
beacon 0:15a8480061e8 174 }
EpicG10 2:c8c965d48f8d 175 time++;
EpicG10 2:c8c965d48f8d 176 wait(0.005f);
EpicG10 2:c8c965d48f8d 177 }
EpicG10 2:c8c965d48f8d 178
EpicG10 2:c8c965d48f8d 179 return 0;
beacon 0:15a8480061e8 180 }
beacon 0:15a8480061e8 181
beacon 0:15a8480061e8 182 /* * /
beacon 0:15a8480061e8 183 int main()
beacon 0:15a8480061e8 184 {
beacon 0:15a8480061e8 185 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 0:15a8480061e8 186 //int counter = 0; //Counts how many times the robot has turned, before driving
beacon 0:15a8480061e8 187 int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
beacon 0:15a8480061e8 188 //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 0:15a8480061e8 189 //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
beacon 0:15a8480061e8 190 //int done = 0;
beacon 0:15a8480061e8 191 int color;
EpicG10 2:c8c965d48f8d 192
beacon 0:15a8480061e8 193 enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
beacon 0:15a8480061e8 194
beacon 0:15a8480061e8 195 int state = search;
EpicG10 2:c8c965d48f8d 196
beacon 0:15a8480061e8 197 static float messung = 0;
EpicG10 2:c8c965d48f8d 198
beacon 0:15a8480061e8 199 while( 1 ){
beacon 0:15a8480061e8 200 printf("\n\r%f", sam.see(FWD_L));
EpicG10 2:c8c965d48f8d 201
beacon 0:15a8480061e8 202 wait(1.0f);
EpicG10 2:c8c965d48f8d 203
beacon 0:15a8480061e8 204 }
EpicG10 2:c8c965d48f8d 205
beacon 0:15a8480061e8 206 while( 1 ) {
EpicG10 2:c8c965d48f8d 207
beacon 0:15a8480061e8 208 if ( timer > TIMEOUT ) {
beacon 0:15a8480061e8 209 NVIC_SystemReset(); //Resets Sam.
beacon 0:15a8480061e8 210 }
EpicG10 2:c8c965d48f8d 211
beacon 0:15a8480061e8 212 //if( timer == 0 )
beacon 0:15a8480061e8 213 //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
EpicG10 2:c8c965d48f8d 214
EpicG10 2:c8c965d48f8d 215
beacon 0:15a8480061e8 216 //printf("\n\rcurrent main state: %d", state);
EpicG10 2:c8c965d48f8d 217
beacon 0:15a8480061e8 218 sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
beacon 0:15a8480061e8 219 switch( state ) {
beacon 0:15a8480061e8 220 case search:
beacon 0:15a8480061e8 221 if( sam.search(&timer) ){
beacon 0:15a8480061e8 222 //sam.Greifer.nullPos();
beacon 0:15a8480061e8 223 state = LeisteDown;
beacon 0:15a8480061e8 224 timer = 0;
beacon 0:15a8480061e8 225 }
beacon 0:15a8480061e8 226 break;
EpicG10 2:c8c965d48f8d 227
beacon 0:15a8480061e8 228 case LeisteDown:
beacon 0:15a8480061e8 229 int count = 0;
beacon 0:15a8480061e8 230 if( sam.Leiste.upToDown() ){
beacon 0:15a8480061e8 231 //sam.Greifer.leave();
beacon 0:15a8480061e8 232 state = turn;
beacon 0:15a8480061e8 233 timer = 0;
beacon 0:15a8480061e8 234 }
beacon 0:15a8480061e8 235 break;
EpicG10 2:c8c965d48f8d 236
beacon 0:15a8480061e8 237 case turn:
beacon 0:15a8480061e8 238 static int i = 0;
beacon 0:15a8480061e8 239 if( i > 7 ){
beacon 0:15a8480061e8 240 sam.stop();
beacon 0:15a8480061e8 241 state = push;
beacon 0:15a8480061e8 242 i = 0;
beacon 0:15a8480061e8 243 }
beacon 0:15a8480061e8 244 else{
beacon 0:15a8480061e8 245 i++;
beacon 0:15a8480061e8 246 sam.turnRight();
beacon 0:15a8480061e8 247 }
beacon 0:15a8480061e8 248 break;
EpicG10 2:c8c965d48f8d 249
beacon 0:15a8480061e8 250 case push:{
beacon 0:15a8480061e8 251 static int i = 0;
beacon 0:15a8480061e8 252 if( i > 5 ){
beacon 0:15a8480061e8 253 sam.stop();
beacon 0:15a8480061e8 254 i = 0;
beacon 0:15a8480061e8 255 state = backOff;
beacon 0:15a8480061e8 256 timer = 0;
beacon 0:15a8480061e8 257 }
beacon 0:15a8480061e8 258 else{
beacon 0:15a8480061e8 259 sam.driveSlowly();
beacon 0:15a8480061e8 260 i++;
beacon 0:15a8480061e8 261 }
beacon 0:15a8480061e8 262 break;
beacon 0:15a8480061e8 263 }
EpicG10 2:c8c965d48f8d 264
beacon 0:15a8480061e8 265 case backOff:{
beacon 0:15a8480061e8 266 static int i = 0;
beacon 0:15a8480061e8 267 if( i > 1 ){
beacon 0:15a8480061e8 268 sam.stop();
beacon 0:15a8480061e8 269 i = 0;
beacon 0:15a8480061e8 270 state = forward;
beacon 0:15a8480061e8 271 timer = 0;
beacon 0:15a8480061e8 272 }
beacon 0:15a8480061e8 273 else{
beacon 0:15a8480061e8 274 sam.driveBackSlowly();
beacon 0:15a8480061e8 275 i++;
beacon 0:15a8480061e8 276 }
beacon 0:15a8480061e8 277 break;
beacon 0:15a8480061e8 278 }
EpicG10 2:c8c965d48f8d 279
EpicG10 2:c8c965d48f8d 280
beacon 0:15a8480061e8 281 case forward:
beacon 0:15a8480061e8 282 if( sam.Arm.backToCollect() ){
beacon 0:15a8480061e8 283 state = downward;
beacon 0:15a8480061e8 284 timer = 0;
beacon 0:15a8480061e8 285 }
beacon 0:15a8480061e8 286 break;
beacon 0:15a8480061e8 287
beacon 0:15a8480061e8 288 case downward:
beacon 0:15a8480061e8 289 if( sam.Arm.collectToDown() ){
beacon 0:15a8480061e8 290 state = down;
beacon 0:15a8480061e8 291 timer = 0;
beacon 0:15a8480061e8 292 }
beacon 0:15a8480061e8 293 break;
beacon 0:15a8480061e8 294
beacon 0:15a8480061e8 295 case down:
beacon 0:15a8480061e8 296 if( sam.Greifer.take() ) {
beacon 0:15a8480061e8 297 state = upward;
beacon 0:15a8480061e8 298 timer = 0;
beacon 0:15a8480061e8 299 }
beacon 0:15a8480061e8 300 break;
EpicG10 2:c8c965d48f8d 301
beacon 0:15a8480061e8 302 case upward:
beacon 0:15a8480061e8 303 if( sam.Arm.downToCollect() ){
beacon 0:15a8480061e8 304 state = colorS;
beacon 0:15a8480061e8 305 timer = 0;
beacon 0:15a8480061e8 306 }
beacon 0:15a8480061e8 307 break;
EpicG10 2:c8c965d48f8d 308
beacon 0:15a8480061e8 309 case colorS:
beacon 0:15a8480061e8 310 led = 1;
beacon 0:15a8480061e8 311 color = sam.FarbVoltage.read();
EpicG10 2:c8c965d48f8d 312
beacon 0:15a8480061e8 313 if( color == -1 ){
beacon 0:15a8480061e8 314 //Do nothing
beacon 0:15a8480061e8 315 }
EpicG10 2:c8c965d48f8d 316
EpicG10 2:c8c965d48f8d 317
beacon 0:15a8480061e8 318 else if( color == 0 || color == GREEN ){
beacon 0:15a8480061e8 319 state = backward;
beacon 0:15a8480061e8 320 led = 0;
beacon 0:15a8480061e8 321 timer = 0;
beacon 0:15a8480061e8 322 }
EpicG10 2:c8c965d48f8d 323
beacon 0:15a8480061e8 324 else if( color == RED ){
beacon 0:15a8480061e8 325 state = readyDrop;
beacon 0:15a8480061e8 326 led = 0;
beacon 0:15a8480061e8 327 timer = 0;
beacon 0:15a8480061e8 328 }
EpicG10 2:c8c965d48f8d 329
beacon 0:15a8480061e8 330 else{
beacon 0:15a8480061e8 331 //Shit...
beacon 0:15a8480061e8 332 }
beacon 0:15a8480061e8 333 break;
EpicG10 2:c8c965d48f8d 334
beacon 0:15a8480061e8 335 case readyDrop:
beacon 0:15a8480061e8 336 if( sam.Greifer.leave() ){
beacon 0:15a8480061e8 337 if( color == GREEN || color == 0 ){
beacon 0:15a8480061e8 338 state = LeisteUp;
beacon 0:15a8480061e8 339 }
beacon 0:15a8480061e8 340 else{
beacon 0:15a8480061e8 341 state = backward;
beacon 0:15a8480061e8 342 }
beacon 0:15a8480061e8 343 timer = 0;
beacon 0:15a8480061e8 344 }
EpicG10 2:c8c965d48f8d 345
beacon 0:15a8480061e8 346 break;
EpicG10 2:c8c965d48f8d 347
beacon 0:15a8480061e8 348 case backward:
beacon 0:15a8480061e8 349 if( sam.Arm.collectToBack() ){
beacon 0:15a8480061e8 350 //sam.Greifer.nullPos();
beacon 0:15a8480061e8 351 state = LeisteUp;
beacon 0:15a8480061e8 352 timer = 0;
beacon 0:15a8480061e8 353 if( color == GREEN || color == 0 ){
beacon 0:15a8480061e8 354 state = readyDrop;
beacon 0:15a8480061e8 355 sam.Greifer.leave();
beacon 0:15a8480061e8 356 }
beacon 0:15a8480061e8 357 else{
beacon 0:15a8480061e8 358 state = LeisteUp;
beacon 0:15a8480061e8 359 }
beacon 0:15a8480061e8 360 }
beacon 0:15a8480061e8 361 break;
EpicG10 2:c8c965d48f8d 362
beacon 0:15a8480061e8 363 case LeisteUp:
beacon 0:15a8480061e8 364 if( sam.Leiste.downToUp() ){
beacon 0:15a8480061e8 365 state = search;
beacon 0:15a8480061e8 366 timer = 0;
beacon 0:15a8480061e8 367 }
beacon 0:15a8480061e8 368 break;
EpicG10 2:c8c965d48f8d 369 }
beacon 0:15a8480061e8 370 timer++;
beacon 0:15a8480061e8 371 wait(0.1f);
beacon 0:15a8480061e8 372 }
EpicG10 2:c8c965d48f8d 373
beacon 0:15a8480061e8 374 return 0;
beacon 0:15a8480061e8 375 }
beacon 0:15a8480061e8 376 /* */