Revision 1:fd3cef0f116d, committed 2017-05-23

Comitter:

EpicG10

Date:

Tue May 23 16:24:49 2017 +0000

Parent:

0:15a8480061e8

Commit message:

a

Changed in this revision

--- a/Headers/Declarations.h	Mon May 22 11:24:46 2017 +0000
+++ b/Headers/Declarations.h	Tue May 23 16:24:49 2017 +0000
@@ -30,7 +30,7 @@
 
 //Arm
 #define COLLECT_POS 60.0f
-#define RELEASE_POS 85.0f
+#define RELEASE_POS 87.0f
 #define TAKE_POS -65.0f
 
 

--- a/Headers/Robot.h	Mon May 22 11:24:46 2017 +0000
+++ b/Headers/Robot.h	Tue May 23 16:24:49 2017 +0000
@@ -98,6 +98,7 @@
         
         
     private:
+    
         Servo*  leiste;
       
     
@@ -117,6 +118,7 @@
         void nullPos();
         
     private:
+    
         Servo* greifer;        
 
 };

--- a/Sources/Arm.cpp	Mon May 22 11:24:46 2017 +0000
+++ b/Sources/Arm.cpp	Tue May 23 16:24:49 2017 +0000
@@ -70,26 +70,30 @@
 
 int Arm::backToDown(){
     static float pos = RELEASE_POS;
+    static int i=0;
     if( pos > TAKE_POS ){
-        pos -= 0.0015f;
+        pos -= 3;
         this->arm->position(pos);
         return 0;
     }
     else{
-        pos = RELEASE_POS;
+        //pos = RELEASE_POS;
+       
         return 1;
     }
 }
 
 int Arm::downToBack(){
     static float pos = TAKE_POS;
+    static int i=0;
     if( pos < RELEASE_POS ){
-        pos += 0.0015f;
+        pos += 5;
         this->arm->position(pos);
         return 0;
     }
     else{
-        pos = TAKE_POS;
+        //pos = TAKE_POS;
+       
         return 1;
     }
 }
\ No newline at end of file

--- a/Sources/Greifer.cpp	Mon May 22 11:24:46 2017 +0000
+++ b/Sources/Greifer.cpp	Tue May 23 16:24:49 2017 +0000
@@ -26,9 +26,9 @@
 int Greifer::take()
 
 {
-    this->greifer->position(-70.0f);
+    this->greifer->position(-60.0f);
     static int time = 0;
-    if( time < 1000 ){
+    if( time < 30 ){
         time++;
         return 0;
     }
@@ -42,7 +42,7 @@
 {
     this->greifer->position(65.0f);
     static int time = 0;
-    if( time < 1000 ){
+    if( time < 10 ){
         time++;
         return 0;
     }

--- 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;