Diff: main.cpp

Revision:

1:fd3cef0f116d

Parent:

0:15a8480061e8

Child:

2:c8c965d48f8d

--- a/main.cpp	Mon May 22 11:24:46 2017 +0000
+++ b/main.cpp	Tue May 23 16:24:49 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);
@@ -77,12 +77,13 @@
     
     PID_Control pid;
     pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000);
-    pc.baud( 115200 );
+   // pc.baud( 115200 );
     
-    int state = search;
+    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());
@@ -93,52 +94,63 @@
     while( 1 ){
         switch( state ){
             case test:
-            int x = pixy.getX(), y = pixy.getY();
-                if( !mybutton ) printf("\n\rX:%d\tY:%d", x, y);
                 break;
                 
             case search:
-                if( sam.search(&time) ) state = found;
+            if(pixy.getDetects())sam.leds[5]=1;
+            else sam.leds[5]=0;
+                if( sam.search(&time) ) state = setX;
                 break;
                 
-            case setX:{
+            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 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;
             }
             
             case take:{
-                //sam.leds[1] = 1;
+                sam.leds[1] = 1;
                 sam.stop();
-                while (1) wait(0.1f);
-                static int down = 0, closed = 0, up = 0;
-                if( down || sam.Arm.backToDown() )         down = 1;
-                else if( closed || sam.Greifer.take() )    closed = 1;
-                else if( up || sam.Arm.downToBack() )      up = 1;
-                else if( sam.Greifer.leave() ){
-                    state = search;
-                    down = 0;
-                    closed = 0;
-                    up = 0;
-                }
+                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;
             }
         }
         time++;
-        wait(0.001f);
+        wait(0.05f);
     }
         
     return 0;