zhaw_st16b_pes2_10
x
Dependencies: Servo ServoArm mbed
Fork of
PES_PIXY_Officiall
by 
zhaw_st16b_pes2_10
Diff: main.cpp
- Revision:
- 2:c8c965d48f8d
- Parent:
- 1:fd3cef0f116d
- Child:
- 3:63da1d1fae15
--- a/main.cpp Tue May 23 16:24:49 2017 +0000
+++ b/main.cpp Thu May 25 18:12:28 2017 +0000
@@ -14,7 +14,7 @@
Pixy pixy(cam);
//DistanceSensors related bottom:
-//Serial pc(SERIAL_TX, SERIAL_RX);
+Serial pc(SERIAL_TX, SERIAL_RX);
AnalogIn sensorVoltage(PB_1);
DigitalOut enable(PC_1);
@@ -70,90 +70,113 @@
}
}
/* */
-int main(){
+int main()
+{
initializeDistanceSensors(); //Initialises IR-Sensors.
- enum states {search = 0, setX, setY, fine, take, test};
+ enum states {search = 0, setPos, take};
int time = 0; //Time keeps track of time. [time] = ms
-
+
PID_Control pid;
- pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
- // pc.baud( 115200 );
-
- int state = take;
-
- sam.stop();
- while(!sam.Arm.collectToBack()) wait(0.1);
- wait(0.25);
- /*
- while( 1 ){
- printf("\n\rX: %d,\t Y: %d", pixy.getX(), pixy.getY());
- wait(1.0f);
- }
- */
+ pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000);
+ // pc.baud( 115200 );
+
+ int state = search;
+ while(!sam.Arm.collectToBack())wait(0.075);
+ while(!sam.Greifer.leave())wait(0.001);
+
+
+ while( 1 ) {
+
+ switch(state){
- while( 1 ){
- switch( state ){
- case test:
- break;
-
- case search:
- if(pixy.getDetects())sam.leds[5]=1;
- else sam.leds[5]=0;
- if( sam.search(&time) ) state = setX;
- break;
-
- case setX:{
- static int messungen[20], i = 0;
- float e = 132.5f - pixy.getX();
- float diff = pid.calc( e, 0.001f );
-
- sam.setLeft(0.5f - diff);
- sam.setRight(0.5f - diff);
- if(pixy.getX()>130 && pixy.getX()<135){
- state = setY;
- }
- break;
+ case search: {
+ if(!((pixy.getX()>50 && pixy.getX()<300)&&(pixy.getY()>50 && pixy.getY()<300))){
+ sam.search(&time);
}
- case setY:{
-
- static int messungen[20], i = 0;
- float e = 121.5f - pixy.getY();
- float diff = pid.calc( e, 0.001f );
-
- sam.setLeft(0.5f + diff);
- sam.setRight(0.5f - diff);
- if(pixy.getY()>119 && pixy.getY()<124){
- state = take;
- }
- break;
+ else {
+ state = setPos;
+ }
+
+ break;
+ }
+
+ case setPos: {
+ sam.leds[5] = 1;
+ static int i = 0;
+ float eX = 133.0f - pixy.getX();
+ float diffX = pid.calc( eX, 0.006f );
+
+ float aX = 0.03; //minimum diff
+ float aY = 0.05;
+ if(diffX>0) {
+ if(diffX<aX)diffX=aX;
+ }
+ else if(diffX>-aX)diffX=-aX;
+
+ if(!(pixy.getX()>128 && pixy.getX()<138)){
+ sam.setLeft(0.5f - diffX);
+ sam.setRight(0.5f - diffX);
+ }
+
+ wait(0.001f);
+
+ float eY = 121.0f - pixy.getY();
+ float diffY = pid.calc( eY, 0.006f );
+ if(diffY>0) {
+ if(diffY<aY)diffY=aY;
+ }
+ else if(diffY>-aY) diffY=-aY;
+ if(!(pixy.getY()>115 && pixy.getY()<125)){
+ sam.setLeft(0.5f + diffY);
+ sam.setRight(0.5f - diffY);
+ }
+ i++;
+ if((pixy.getX()>132 && pixy.getX()<134)&&(pixy.getY()>119 && pixy.getY()<122)||!(i%1000)) {
+ state = take;
+ i = 0;
+ sam.stop();
+ sam.leds[5] = 0;
+ }
+ break;
+ }
+
+ case take: {
+ enum takeStates {down=0, take, up, leave};
+ static int tState=down;
+
+ sam.leds[1] = 1;
+ sam.stop();
+ //tState=leave;
+
+ switch(tState) {
+ case down:
+ if(sam.Arm.backToDown()) tState=take;
+ else tState=down;
+ break;
+ case take:
+ if(sam.Greifer.take()) tState=up;
+ else tState=take;
+ break;
+ case up:
+ if(sam.Arm.downToBack()) {wait(0.05);tState=leave;}
+ else tState=up;
+ break;
+ case leave:
+ if(sam.Greifer.leave()) {state=search; tState=down;}
+ else tState=leave;
+ break;
}
-
- case take:{
- sam.leds[1] = 1;
- sam.stop();
- enum takeStates{down=0, take, up, leave};
- static int tState=down;
- switch(tState){
- case down: if(sam.Arm.backToDown()) tState=take;
- else tState=down; break;
- case take: if(sam.Greifer.take()) tState=up;
- else tState=take; break;
- case up: if(sam.Arm.downToBack()) tState=leave;
- else tState=up; break;
- case leave: if(sam.Greifer.leave()) state=search;
- else tState=leave; break;
- }
-
-
-
- break;
- }
+
+
+ sam.leds[1] = 0;
+ break;
}
- time++;
- wait(0.05f);
}
-
- return 0;
+ time++;
+ wait(0.005f);
+}
+
+return 0;
}
/* * /
@@ -166,32 +189,32 @@
//int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
//int done = 0;
int color;
-
+
enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
int state = search;
-
+
static float messung = 0;
-
+
while( 1 ){
printf("\n\r%f", sam.see(FWD_L));
-
+
wait(1.0f);
-
+
}
-
+
while( 1 ) {
-
+
if ( timer > TIMEOUT ) {
NVIC_SystemReset(); //Resets Sam.
}
-
+
//if( timer == 0 )
//printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
-
-
+
+
//printf("\n\rcurrent main state: %d", state);
-
+
sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
switch( state ) {
case search:
@@ -201,7 +224,7 @@
timer = 0;
}
break;
-
+
case LeisteDown:
int count = 0;
if( sam.Leiste.upToDown() ){
@@ -210,7 +233,7 @@
timer = 0;
}
break;
-
+
case turn:
static int i = 0;
if( i > 7 ){
@@ -223,7 +246,7 @@
sam.turnRight();
}
break;
-
+
case push:{
static int i = 0;
if( i > 5 ){
@@ -238,7 +261,7 @@
}
break;
}
-
+
case backOff:{
static int i = 0;
if( i > 1 ){
@@ -253,8 +276,8 @@
}
break;
}
-
-
+
+
case forward:
if( sam.Arm.backToCollect() ){
state = downward;
@@ -275,40 +298,40 @@
timer = 0;
}
break;
-
+
case upward:
if( sam.Arm.downToCollect() ){
state = colorS;
timer = 0;
}
break;
-
+
case colorS:
led = 1;
color = sam.FarbVoltage.read();
-
+
if( color == -1 ){
//Do nothing
}
-
-
+
+
else if( color == 0 || color == GREEN ){
state = backward;
led = 0;
timer = 0;
}
-
+
else if( color == RED ){
state = readyDrop;
led = 0;
timer = 0;
}
-
+
else{
//Shit...
}
break;
-
+
case readyDrop:
if( sam.Greifer.leave() ){
if( color == GREEN || color == 0 ){
@@ -319,9 +342,9 @@
}
timer = 0;
}
-
+
break;
-
+
case backward:
if( sam.Arm.collectToBack() ){
//sam.Greifer.nullPos();
@@ -336,18 +359,18 @@
}
}
break;
-
+
case LeisteUp:
if( sam.Leiste.downToUp() ){
state = search;
timer = 0;
}
break;
- }
+ }
timer++;
wait(0.1f);
}
-
+
return 0;
}
/* */
\ No newline at end of file