Diff: main.cpp

Revision:

8:ac4e5afbdbcd

Parent:

7:9715323b20ac

Child:

9:90227be52903

--- a/main.cpp	Wed Nov 02 15:22:16 2016 +0000
+++ b/main.cpp	Wed Nov 02 15:27:34 2016 +0000
@@ -45,7 +45,8 @@
 AnalogIn emgr(A0);
 
 // ticker
-Ticker tick_part;                       // ticker to switch parts     
+Ticker tick_part;                       // ticker to switch parts 
+Ticker tick_part_arm;    
 Ticker sampleTicker;   
 
 //======== Variables ===========================================================
@@ -284,7 +285,7 @@
 
 // Switch between Cart, Arm and Claw
 
-void SwitchPart(){
+void SwitchCart(){
     switch (part_id) {
         //Cart
         case 2: {
@@ -356,8 +357,82 @@
             
             break;
         }
+      }
+}
+
+void SwitchArm(){
+    switch (part_id) {
+        //Cart
+        case 3: {
+            led_g = LedOn;          
+            if(led_g == LedOn){
+                num_turned_on_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
+                    Arm.stop(1)==1;
+                    
+                    }else if(position_cart > -105 && position_arm >= 25 && position_claw == 27){
+                    Arm.stop(1)==1;
+                        
+                    }else if(position_cart<= -105 && position_arm>=80){         //If the cart is at the right end, the arm can't move any further than 70 degrees
+                    Arm.stop(1)==1;
+    
+                    }else{
+                    Arm.speed(arm_speed)==arm_speed;
+                    } 
+         
+                }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 
+                    Arm.stop(1)==1;
+                    
+                    }else if(position_cart < 105 && position_arm <= -25 && position_claw == -18){
+                    Arm.stop(1)==1;
+                    
+                    }else if(position_cart>=105 && position_arm<=-80){          //If the cart is at the left end, the arm can't move any further than 70 degrees
+                    Arm.stop(1)==1;
+    
+                    }else{
+                    Arm.speed(-arm_speed)==-arm_speed;
+                    } 
+                        
+                }else {
+                Cart.speed(0) == 0;
+                if(position_arm>0){
+                Arm.speed(-0.1)== -0.1;
+                }else if(position_arm<0){
+                Arm.speed(0.1)==0.1;
+                }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;
         }
-        }
+      }
+}
+
 
 
 // Switch the part
@@ -382,7 +457,8 @@
     led_g = not LedOn;
     led_b = not LedOn;
     
-    tick_part.attach(&SwitchPart,kTimeToggle); 
+    tick_part.attach(&SwitchCart,kTimeToggle);
+    tick_part_arm.attach(&SwitchArm,kTimeToggle);  
     sampleTicker.attach(scopeSend,0.01);
    
     btn_cart.fall(&SetValue2);