Diff: Sources/Arm.cpp

Revision:

3:63da1d1fae15

Parent:

2:c8c965d48f8d

Child:

5:acb938f45b9c

--- a/Sources/Arm.cpp	Thu May 25 18:12:28 2017 +0000
+++ b/Sources/Arm.cpp	Fri May 26 08:01:48 2017 +0000
@@ -11,66 +11,27 @@
 
 void Arm::init(ServoArm* arm){
     this->arm = arm;
-    
     this->arm->calibrate(0.0015f, 180.0f);
     this->arm->position(RELEASE_POS);
 }
 
-int Arm::collectToDown(){
-    static float pos=COLLECT_POS;
-    if(pos>TAKE_POS) {
-        pos-=3;
-        this->arm->position(pos);
-        return 0;
-    }
-    else{
-        pos = COLLECT_POS;
-        return 1;
-    }
-}
-
-int Arm::downToCollect(){
-    static float pos=TAKE_POS;
-    if(pos<COLLECT_POS) {
-        pos+=2;
-        this->arm->position(pos);
-        return 0;
-    }
-    else{
-        pos = TAKE_POS;
-        return 1;
-    }
-}
 
 int Arm::collectToBack(){
-    static float i=0;;
-    if(i<5) {
+    static float i=0;
+    if(i<5){
         this->arm->position(85.0);
         i++;
         return 0;
-        }
+    }
     else{
         i=0;        
         return 1;
     }
 }
 
-int Arm::backToCollect(){
-    static float pos=RELEASE_POS;
-    if(pos>COLLECT_POS) {
-        pos-=3;
-        this->arm->position(pos);
-        return 0;
-        }
-    else{
-        pos = RELEASE_POS;
-        return 1;
-    }
-}
 
 int Arm::backToDown(){
     static float pos = RELEASE_POS;
-    static int i=0;
     if( pos > TAKE_POS ){
         pos -= 0.3f;
         this->arm->position(pos);
@@ -78,7 +39,6 @@
     }
     else{
         pos = RELEASE_POS;
-       
         return 1;
     }
 }
@@ -95,10 +55,12 @@
     else{
     i++;
     arm->position(RELEASE_POS+10);
-    if(i>30){ pos = TAKE_POS; return 1;}
-    
-       
-    else  return 0;
+        if(i>30){ 
+            pos = TAKE_POS;
+            i=0; 
+            return 1;
+        }      
+        else  return 0;
     }
     
 }
\ No newline at end of file