zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Officiall

c

Dependencies:   Servo ServoArm mbed

Fork of PES_PIXY_Official by zhaw_st16b_pes2_10

main.cpp@1:fd3cef0f116d, 2017-05-23 (annotated)

Committer:
EpicG10
Date:
Tue May 23 16:24:49 2017 +0000
Revision:
1:fd3cef0f116d
Parent:
0:15a8480061e8 
a

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 1:fd3cef0f116d 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 /* */
beacon 0:15a8480061e8 73 int main(){
beacon 0:15a8480061e8 74 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 0:15a8480061e8 75 enum states {search = 0, setX, setY, fine, take, test};
beacon 0:15a8480061e8 76 int time = 0; //Time keeps track of time. [time] = ms
beacon 0:15a8480061e8 77
beacon 0:15a8480061e8 78 PID_Control pid;
beacon 0:15a8480061e8 79 pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
EpicG10 1:fd3cef0f116d 80 // pc.baud( 115200 );
beacon 0:15a8480061e8 81
EpicG10 1:fd3cef0f116d 82 int state = take;
beacon 0:15a8480061e8 83
beacon 0:15a8480061e8 84 sam.stop();
EpicG10 1:fd3cef0f116d 85 while(!sam.Arm.collectToBack()) wait(0.1);
EpicG10 1:fd3cef0f116d 86 wait(0.25);
beacon 0:15a8480061e8 87 /*
beacon 0:15a8480061e8 88 while( 1 ){
beacon 0:15a8480061e8 89 printf("\n\rX: %d,\t Y: %d", pixy.getX(), pixy.getY());
beacon 0:15a8480061e8 90 wait(1.0f);
beacon 0:15a8480061e8 91 }
beacon 0:15a8480061e8 92 */
beacon 0:15a8480061e8 93
beacon 0:15a8480061e8 94 while( 1 ){
beacon 0:15a8480061e8 95 switch( state ){
beacon 0:15a8480061e8 96 case test:
beacon 0:15a8480061e8 97 break;
beacon 0:15a8480061e8 98
beacon 0:15a8480061e8 99 case search:
EpicG10 1:fd3cef0f116d 100 if(pixy.getDetects())sam.leds[5]=1;
EpicG10 1:fd3cef0f116d 101 else sam.leds[5]=0;
EpicG10 1:fd3cef0f116d 102 if( sam.search(&time) ) state = setX;
beacon 0:15a8480061e8 103 break;
beacon 0:15a8480061e8 104
EpicG10 1:fd3cef0f116d 105 case setX:{
beacon 0:15a8480061e8 106 static int messungen[20], i = 0;
beacon 0:15a8480061e8 107 float e = 132.5f - pixy.getX();
beacon 0:15a8480061e8 108 float diff = pid.calc( e, 0.001f );
beacon 0:15a8480061e8 109
beacon 0:15a8480061e8 110 sam.setLeft(0.5f - diff);
beacon 0:15a8480061e8 111 sam.setRight(0.5f - diff);
EpicG10 1:fd3cef0f116d 112 if(pixy.getX()>130 && pixy.getX()<135){
EpicG10 1:fd3cef0f116d 113 state = setY;
EpicG10 1:fd3cef0f116d 114 }
beacon 0:15a8480061e8 115 break;
beacon 0:15a8480061e8 116 }
beacon 0:15a8480061e8 117 case setY:{
EpicG10 1:fd3cef0f116d 118
beacon 0:15a8480061e8 119 static int messungen[20], i = 0;
beacon 0:15a8480061e8 120 float e = 121.5f - pixy.getY();
beacon 0:15a8480061e8 121 float diff = pid.calc( e, 0.001f );
beacon 0:15a8480061e8 122
beacon 0:15a8480061e8 123 sam.setLeft(0.5f + diff);
beacon 0:15a8480061e8 124 sam.setRight(0.5f - diff);
EpicG10 1:fd3cef0f116d 125 if(pixy.getY()>119 && pixy.getY()<124){
EpicG10 1:fd3cef0f116d 126 state = take;
EpicG10 1:fd3cef0f116d 127 }
beacon 0:15a8480061e8 128 break;
beacon 0:15a8480061e8 129 }
beacon 0:15a8480061e8 130
beacon 0:15a8480061e8 131 case take:{
EpicG10 1:fd3cef0f116d 132 sam.leds[1] = 1;
beacon 0:15a8480061e8 133 sam.stop();
EpicG10 1:fd3cef0f116d 134 enum takeStates{down=0, take, up, leave};
EpicG10 1:fd3cef0f116d 135 static int tState=down;
EpicG10 1:fd3cef0f116d 136 switch(tState){
EpicG10 1:fd3cef0f116d 137 case down: if(sam.Arm.backToDown()) tState=take;
EpicG10 1:fd3cef0f116d 138 else tState=down; break;
EpicG10 1:fd3cef0f116d 139 case take: if(sam.Greifer.take()) tState=up;
EpicG10 1:fd3cef0f116d 140 else tState=take; break;
EpicG10 1:fd3cef0f116d 141 case up: if(sam.Arm.downToBack()) tState=leave;
EpicG10 1:fd3cef0f116d 142 else tState=up; break;
EpicG10 1:fd3cef0f116d 143 case leave: if(sam.Greifer.leave()) state=search;
EpicG10 1:fd3cef0f116d 144 else tState=leave; break;
EpicG10 1:fd3cef0f116d 145 }
EpicG10 1:fd3cef0f116d 146
EpicG10 1:fd3cef0f116d 147
EpicG10 1:fd3cef0f116d 148
beacon 0:15a8480061e8 149 break;
beacon 0:15a8480061e8 150 }
beacon 0:15a8480061e8 151 }
beacon 0:15a8480061e8 152 time++;
EpicG10 1:fd3cef0f116d 153 wait(0.05f);
beacon 0:15a8480061e8 154 }
beacon 0:15a8480061e8 155
beacon 0:15a8480061e8 156 return 0;
beacon 0:15a8480061e8 157 }
beacon 0:15a8480061e8 158
beacon 0:15a8480061e8 159 /* * /
beacon 0:15a8480061e8 160 int main()
beacon 0:15a8480061e8 161 {
beacon 0:15a8480061e8 162 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 0:15a8480061e8 163 //int counter = 0; //Counts how many times the robot has turned, before driving
beacon 0:15a8480061e8 164 int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
beacon 0:15a8480061e8 165 //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 0:15a8480061e8 166 //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
beacon 0:15a8480061e8 167 //int done = 0;
beacon 0:15a8480061e8 168 int color;
beacon 0:15a8480061e8 169
beacon 0:15a8480061e8 170 enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
beacon 0:15a8480061e8 171
beacon 0:15a8480061e8 172 int state = search;
beacon 0:15a8480061e8 173
beacon 0:15a8480061e8 174 static float messung = 0;
beacon 0:15a8480061e8 175
beacon 0:15a8480061e8 176 while( 1 ){
beacon 0:15a8480061e8 177 printf("\n\r%f", sam.see(FWD_L));
beacon 0:15a8480061e8 178
beacon 0:15a8480061e8 179 wait(1.0f);
beacon 0:15a8480061e8 180
beacon 0:15a8480061e8 181 }
beacon 0:15a8480061e8 182
beacon 0:15a8480061e8 183 while( 1 ) {
beacon 0:15a8480061e8 184
beacon 0:15a8480061e8 185 if ( timer > TIMEOUT ) {
beacon 0:15a8480061e8 186 NVIC_SystemReset(); //Resets Sam.
beacon 0:15a8480061e8 187 }
beacon 0:15a8480061e8 188
beacon 0:15a8480061e8 189 //if( timer == 0 )
beacon 0:15a8480061e8 190 //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
beacon 0:15a8480061e8 191
beacon 0:15a8480061e8 192
beacon 0:15a8480061e8 193 //printf("\n\rcurrent main state: %d", state);
beacon 0:15a8480061e8 194
beacon 0:15a8480061e8 195 sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
beacon 0:15a8480061e8 196 switch( state ) {
beacon 0:15a8480061e8 197 case search:
beacon 0:15a8480061e8 198 if( sam.search(&timer) ){
beacon 0:15a8480061e8 199 //sam.Greifer.nullPos();
beacon 0:15a8480061e8 200 state = LeisteDown;
beacon 0:15a8480061e8 201 timer = 0;
beacon 0:15a8480061e8 202 }
beacon 0:15a8480061e8 203 break;
beacon 0:15a8480061e8 204
beacon 0:15a8480061e8 205 case LeisteDown:
beacon 0:15a8480061e8 206 int count = 0;
beacon 0:15a8480061e8 207 if( sam.Leiste.upToDown() ){
beacon 0:15a8480061e8 208 //sam.Greifer.leave();
beacon 0:15a8480061e8 209 state = turn;
beacon 0:15a8480061e8 210 timer = 0;
beacon 0:15a8480061e8 211 }
beacon 0:15a8480061e8 212 break;
beacon 0:15a8480061e8 213
beacon 0:15a8480061e8 214 case turn:
beacon 0:15a8480061e8 215 static int i = 0;
beacon 0:15a8480061e8 216 if( i > 7 ){
beacon 0:15a8480061e8 217 sam.stop();
beacon 0:15a8480061e8 218 state = push;
beacon 0:15a8480061e8 219 i = 0;
beacon 0:15a8480061e8 220 }
beacon 0:15a8480061e8 221 else{
beacon 0:15a8480061e8 222 i++;
beacon 0:15a8480061e8 223 sam.turnRight();
beacon 0:15a8480061e8 224 }
beacon 0:15a8480061e8 225 break;
beacon 0:15a8480061e8 226
beacon 0:15a8480061e8 227 case push:{
beacon 0:15a8480061e8 228 static int i = 0;
beacon 0:15a8480061e8 229 if( i > 5 ){
beacon 0:15a8480061e8 230 sam.stop();
beacon 0:15a8480061e8 231 i = 0;
beacon 0:15a8480061e8 232 state = backOff;
beacon 0:15a8480061e8 233 timer = 0;
beacon 0:15a8480061e8 234 }
beacon 0:15a8480061e8 235 else{
beacon 0:15a8480061e8 236 sam.driveSlowly();
beacon 0:15a8480061e8 237 i++;
beacon 0:15a8480061e8 238 }
beacon 0:15a8480061e8 239 break;
beacon 0:15a8480061e8 240 }
beacon 0:15a8480061e8 241
beacon 0:15a8480061e8 242 case backOff:{
beacon 0:15a8480061e8 243 static int i = 0;
beacon 0:15a8480061e8 244 if( i > 1 ){
beacon 0:15a8480061e8 245 sam.stop();
beacon 0:15a8480061e8 246 i = 0;
beacon 0:15a8480061e8 247 state = forward;
beacon 0:15a8480061e8 248 timer = 0;
beacon 0:15a8480061e8 249 }
beacon 0:15a8480061e8 250 else{
beacon 0:15a8480061e8 251 sam.driveBackSlowly();
beacon 0:15a8480061e8 252 i++;
beacon 0:15a8480061e8 253 }
beacon 0:15a8480061e8 254 break;
beacon 0:15a8480061e8 255 }
beacon 0:15a8480061e8 256
beacon 0:15a8480061e8 257
beacon 0:15a8480061e8 258 case forward:
beacon 0:15a8480061e8 259 if( sam.Arm.backToCollect() ){
beacon 0:15a8480061e8 260 state = downward;
beacon 0:15a8480061e8 261 timer = 0;
beacon 0:15a8480061e8 262 }
beacon 0:15a8480061e8 263 break;
beacon 0:15a8480061e8 264
beacon 0:15a8480061e8 265 case downward:
beacon 0:15a8480061e8 266 if( sam.Arm.collectToDown() ){
beacon 0:15a8480061e8 267 state = down;
beacon 0:15a8480061e8 268 timer = 0;
beacon 0:15a8480061e8 269 }
beacon 0:15a8480061e8 270 break;
beacon 0:15a8480061e8 271
beacon 0:15a8480061e8 272 case down:
beacon 0:15a8480061e8 273 if( sam.Greifer.take() ) {
beacon 0:15a8480061e8 274 state = upward;
beacon 0:15a8480061e8 275 timer = 0;
beacon 0:15a8480061e8 276 }
beacon 0:15a8480061e8 277 break;
beacon 0:15a8480061e8 278
beacon 0:15a8480061e8 279 case upward:
beacon 0:15a8480061e8 280 if( sam.Arm.downToCollect() ){
beacon 0:15a8480061e8 281 state = colorS;
beacon 0:15a8480061e8 282 timer = 0;
beacon 0:15a8480061e8 283 }
beacon 0:15a8480061e8 284 break;
beacon 0:15a8480061e8 285
beacon 0:15a8480061e8 286 case colorS:
beacon 0:15a8480061e8 287 led = 1;
beacon 0:15a8480061e8 288 color = sam.FarbVoltage.read();
beacon 0:15a8480061e8 289
beacon 0:15a8480061e8 290 if( color == -1 ){
beacon 0:15a8480061e8 291 //Do nothing
beacon 0:15a8480061e8 292 }
beacon 0:15a8480061e8 293
beacon 0:15a8480061e8 294
beacon 0:15a8480061e8 295 else if( color == 0 || color == GREEN ){
beacon 0:15a8480061e8 296 state = backward;
beacon 0:15a8480061e8 297 led = 0;
beacon 0:15a8480061e8 298 timer = 0;
beacon 0:15a8480061e8 299 }
beacon 0:15a8480061e8 300
beacon 0:15a8480061e8 301 else if( color == RED ){
beacon 0:15a8480061e8 302 state = readyDrop;
beacon 0:15a8480061e8 303 led = 0;
beacon 0:15a8480061e8 304 timer = 0;
beacon 0:15a8480061e8 305 }
beacon 0:15a8480061e8 306
beacon 0:15a8480061e8 307 else{
beacon 0:15a8480061e8 308 //Shit...
beacon 0:15a8480061e8 309 }
beacon 0:15a8480061e8 310 break;
beacon 0:15a8480061e8 311
beacon 0:15a8480061e8 312 case readyDrop:
beacon 0:15a8480061e8 313 if( sam.Greifer.leave() ){
beacon 0:15a8480061e8 314 if( color == GREEN || color == 0 ){
beacon 0:15a8480061e8 315 state = LeisteUp;
beacon 0:15a8480061e8 316 }
beacon 0:15a8480061e8 317 else{
beacon 0:15a8480061e8 318 state = backward;
beacon 0:15a8480061e8 319 }
beacon 0:15a8480061e8 320 timer = 0;
beacon 0:15a8480061e8 321 }
beacon 0:15a8480061e8 322
beacon 0:15a8480061e8 323 break;
beacon 0:15a8480061e8 324
beacon 0:15a8480061e8 325 case backward:
beacon 0:15a8480061e8 326 if( sam.Arm.collectToBack() ){
beacon 0:15a8480061e8 327 //sam.Greifer.nullPos();
beacon 0:15a8480061e8 328 state = LeisteUp;
beacon 0:15a8480061e8 329 timer = 0;
beacon 0:15a8480061e8 330 if( color == GREEN || color == 0 ){
beacon 0:15a8480061e8 331 state = readyDrop;
beacon 0:15a8480061e8 332 sam.Greifer.leave();
beacon 0:15a8480061e8 333 }
beacon 0:15a8480061e8 334 else{
beacon 0:15a8480061e8 335 state = LeisteUp;
beacon 0:15a8480061e8 336 }
beacon 0:15a8480061e8 337 }
beacon 0:15a8480061e8 338 break;
beacon 0:15a8480061e8 339
beacon 0:15a8480061e8 340 case LeisteUp:
beacon 0:15a8480061e8 341 if( sam.Leiste.downToUp() ){
beacon 0:15a8480061e8 342 state = search;
beacon 0:15a8480061e8 343 timer = 0;
beacon 0:15a8480061e8 344 }
beacon 0:15a8480061e8 345 break;
beacon 0:15a8480061e8 346 }
beacon 0:15a8480061e8 347 timer++;
beacon 0:15a8480061e8 348 wait(0.1f);
beacon 0:15a8480061e8 349 }
beacon 0:15a8480061e8 350
beacon 0:15a8480061e8 351 return 0;
beacon 0:15a8480061e8 352 }
beacon 0:15a8480061e8 353 /* */