Diff: main.cpp

Revision:

5:acb938f45b9c

Parent:

4:d611df1ed42b

diff -r d611df1ed42b -r acb938f45b9c main.cpp
--- a/main.cpp	Fri May 26 08:24:59 2017 +0000
+++ b/main.cpp	Fri May 26 13:43:19 2017 +0000
@@ -60,27 +60,26 @@
 void competition(){
     initializeDistanceSensors();            //Initialises IR-Sensors
     enum states {search = 0, setPos, take}; //3-states machine
-    int time = 0;                           //Time keeps track of time. [time] = ms
+    int time = 0;                           //Time keeps track of time
 
     PID_Control pid;
     pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000);
     
-    sam.Arm.init(&servoArm);                //Arm initialisation
-    sam.Greifer.init(&servoGreifer);        //Greifer initialisation
+    
     
     int state = search;
+    sam.Arm.init(&servoArm);
     while(!sam.Arm.collectToBack())wait(0.075f); 
     while(!sam.Greifer.leave())wait(0.001f);
-    
-    //while(mybutton) wait(0.01);
 
 
     while(1){
+        if(time>TIMEOUT)return;
         
         switch(state){
     
         case search:{
-                if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){
+                if(!((pixy.getX()>50 && pixy.getX()<250)&&(pixy.getY()>50 && pixy.getY()<300))){
                    sam.search(&time);     
                 }
                 else{
@@ -91,25 +90,26 @@
             }
             
         case setPos: {
-            
+            time++;
             sam.leds[5] = 1;
             static int i = 0;
-            if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){
+            if(!((pixy.getX()>50 && pixy.getX()<250)&&(pixy.getY()>50 && pixy.getY()<300))){
+                time=0;
                 state=search;
                 i=0;
             }
-            float eX = 133.0f - pixy.getX();
+            float eX = 131.0f - pixy.getX();
             float diffX = pid.calc( eX, 0.006f );
             
-            float aX = 0.03;             //minimum diff X
-            float aY = 0.05;             //minimum diff Y
+            float aX = 0.04;             //minimum diff X
+            float aY = 0.04;             //minimum diff Y
             if(diffX>0) {          
                 if(diffX<aX)diffX=aX;
             }
             else if(diffX>-aX)diffX=-aX;
             
             //Set the X position
-            if(!(pixy.getX()>128 && pixy.getX()<138)){
+            if(!(pixy.getX()>130 && pixy.getX()<133)){
             sam.setLeft(0.5f - diffX);
             sam.setRight(0.5f - diffX);
             }
@@ -124,12 +124,12 @@
             else if(diffY>-aY)diffY=-aY;
             
             //Set the Y position
-            if(!(pixy.getY()>115 && pixy.getY()<125)){
+            if(!(pixy.getY()>119 && pixy.getY()<121)){
             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)) {
+            if((pixy.getX()>130 && pixy.getX()<132)&&(pixy.getY()>120 && pixy.getY()<122)||(!(i%1000))) {
                 state = take;
                 i = 0;
                 sam.stop();
@@ -139,15 +139,19 @@
         }
     
         case take:{
+                if(time>TIMEOUT)return;
                 enum takeStates {down=0, take, up, leave}; //4-states machine
                 static int tState=down;
     
                 sam.leds[1] = 1;
                 sam.stop();
-    
+                
                 switch(tState){
                     case down:
-                        if(sam.Arm.backToDown()) tState=take;
+                        if(sam.Arm.backToDown()){
+                             sam.Greifer.leave();
+                             tState=take;
+                        }
                         else tState=down;
                         break;
                     case take:
@@ -155,7 +159,7 @@
                         else tState=take;
                         break;
                     case up:
-                        if(sam.Arm.downToBack()) {wait(0.1);tState=leave;}
+                        if(sam.Arm.downToBack()) tState=leave;
                         else tState=up;
                         break;
                     case leave:
@@ -177,8 +181,15 @@
 int main(){
     int start = 0;
     while( 1 ){
-        if( !mybutton ) start++;
-        if( start ) competition();
-        wait(0.1f);
+        if( !mybutton ){
+             start++;
+             sam.Arm.init(&servoArm);                //Arm initialisation
+             sam.Greifer.init(&servoGreifer);        //Greifer initialisation
+        }      
+        if( start ){
+             competition();
+             sam.stop();
+        }
+         wait(0.1f);
     }
 }
\ No newline at end of file