Diff: main.cpp

Revision:

13:dd7b41766f5f

Parent:

12:b31df384b170

Child:

14:65780d480874

--- a/main.cpp	Wed Nov 02 18:24:10 2016 +0000
+++ b/main.cpp	Wed Nov 02 18:46:03 2016 +0000
@@ -293,6 +293,14 @@
      position_cart = (Encoder_Cart.getPulses()*factor_cart) ;    
      ain_cart = pot_cart.read();
      
+     // encoder
+     position_arm = (Encoder_Arm.getPulses()*factor_arm) ;    
+     ain_arm = pot_arm.read();
+   
+     //if (ain_arm == 0){
+     //   Encoder_Arm.reset();   
+     //}else {}
+     
      
     
 }    
@@ -387,14 +395,11 @@
             led_g = LedOn;          
             if(led_g == LedOn){
                 num_turned_on_1++;
+                Arm.stop(1)==1;
+
             
-                if(btn && btn2) {            
-                    Arm.speed(0) == 0; 
-                    Cart.speed(0) == 0;                           
-                }
-            
-                else if (btn && !btn2) {
-                    if(position_cart > -105 && position_arm >= 45){             //If the cart is not at the end, the arm can't move any further than 45 degrees
+                if (rlf_y > threshold_value) {
+                    if(position_cart > -105 && position_arm >= 95){             //If the cart is not at the end, the arm can't move any further than 45 degrees
                     Arm.stop(1)==1;
                     
                     }else if(position_cart > -105 && position_arm >= 25 && position_claw == 27){
@@ -406,9 +411,13 @@
                     }else{
                     Arm.speed(arm_speed)==arm_speed;
                     } 
+                    
+                    if (llf_y > threshold_value){
+                            Cart.stop(1)==1;
+                        }
          
-                }else if (!btn && btn2) {
-                    if(position_cart < 105 && position_arm <= -45){             //If the cart is not at the end, the arm can't move any further than 45 degrees 
+                }else if (llf_y > threshold_value) {
+                    if(position_cart < 105 && position_arm <= -95){             //If the cart is not at the end, the arm can't move any further than 45 degrees 
                     Arm.stop(1)==1;
                     
                     }else if(position_cart < 105 && position_arm <= -25 && position_claw == -18){
@@ -420,9 +429,17 @@
                     }else{
                     Arm.speed(-arm_speed)==-arm_speed;
                     } 
+                    
+                        if (rlf_y > threshold_value){
+                        Cart.stop(1)==1;
+                        }
                         
                 }else {
-                Cart.speed(0) == 0;
+                    Arm.stop(1)==1;
+                }
+                
+                if(!btn&&!btn2){
+                    Cart.speed(0) == 0;
                 if(position_arm>0){
                 Arm.speed(-0.1)== -0.1;
                 }else if(position_arm<0){
@@ -430,21 +447,15 @@
                 }else{
                 Arm.stop(0)==0;
                     }    
-                }
+                    }
             } 
             // controle LED   
             led_r = not LedOn;
             led_b = not LedOn;           
             
-            // encoder
-            position_arm = (Encoder_Arm.getPulses()*factor_arm) ;    
-            ain_arm = pot_arm.read();
-   
-                if (ain_arm == 0){
-                Encoder_Arm.reset();   
-                }else {}
-        
-            wait(0.1);
+            pc.baud(115200);
+            pc.printf("Degrees: %i\n", position_arm);
+            
             break;
         }
       }