Diff: main.cpp

Revision:

12:b31df384b170

Parent:

11:97f824629da5

Child:

13:dd7b41766f5f

--- a/main.cpp	Wed Nov 02 16:13:32 2016 +0000
+++ b/main.cpp	Wed Nov 02 18:24:10 2016 +0000
@@ -68,7 +68,7 @@
 double arm_speed = 0.1;
 
 // position
-float factor_cart = 0.06559;
+float factor_cart = 0.05802;
 float factor_arm = 0.1539;
 int position_cart;
 int position_arm;
@@ -79,7 +79,7 @@
 
 
 // miscellaneous
-const float kTimeToggle = 0.25f;        // period with which to toggle the parts
+const float kTimeToggle = 0.05f;        // period with which to toggle the parts
 const int LedOn = 0;                    // LED on if 0
 volatile int part_id = 1;               // ID of what part should move, begins with the cart
 volatile int servo_id = 1;              // ID to the side the servo should move, begins in center position
@@ -292,6 +292,8 @@
      // encoder
      position_cart = (Encoder_Cart.getPulses()*factor_cart) ;    
      ain_cart = pot_cart.read();
+     
+     
     
 }    
 
@@ -307,40 +309,69 @@
                 num_turned_on_0++;
             
                 if (rlf_y > threshold_value) {
-                    if(position_cart <= -10){                                  //If the cart is at the right side, it stops
+                    if(position_cart <= -170){                                  //If the cart is at the right side, it stops
                     Cart.stop(1)==1;
                     
-                    }else if(position_cart >= 10 && position_arm <=-45){       //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
+                    }else if(position_cart <=100){
+                    Cart.speed(0.2)== 0.2;
+                                        
+                    }else if(position_cart >= 150 && position_arm <=-45){       //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
                     Cart.stop(1) == 1;
                     
-                    }else if(position_cart >= 10 && position_arm <=-35 && position_claw == -18){
+                    }else if(position_cart >= 150 && position_arm <=-35 && position_claw == -18){
                     Cart.stop(1) == 1;    
     
                     }else{
                     Cart.speed(cart_speed)==cart_speed;
                     }
+                    
+                        if (llf_y > threshold_value){
+                            Cart.stop(1)==1;
+                        }
          
                 }else if (llf_y > threshold_value) {
-                    if(position_cart >= 10){                                   //If the cart is at the left side, it stops
+                    if(position_cart >= 150){                                   //If the cart is at the left side, it stops
                     Cart.stop(1)==1;
                     
-                    }else if(position_cart <= -10 && position_arm >=35 && position_claw == 27){
+                    }else if(position_cart >=100){
+                    Cart.speed(-0.2)== -0.2;
+                    
+                    }else if(position_cart <= -170 && position_arm >=45){       //If the cart is at the left side and the arm is rotated 60 degrees to the left, the cart can't move to the right.
+                    Cart.stop(1) == 1;
+                    
+                    }else if(position_cart <= -170 && position_arm >=35 && position_claw == 27){
                     Cart.stop(1)==1;
                     
                     }else{
                     Cart.speed(-cart_speed)==-cart_speed;
                     }
+                    
+                        if (rlf_y > threshold_value){
+                            Cart.stop(1)==1;
+                        }
                                             
                 }else {
-
-                     
+                    Cart.stop(1)==1;    
                 }
             }
+            
+            if(!btn && !btn2) {
+                    Arm.speed(0) == 0;
+                    if(position_cart<0){
+                    Cart.speed(-0.1)== -0.1;
+                    }else if(position_cart>0){
+                    Cart.speed(0.1)==0.1;
+                    }else{
+                    Cart.stop(0)==0;
+                    }
+             }
+             
+             
+              
             // controle LED    
             led_g = not LedOn;
             led_b = not LedOn;     
-            
-            wait(0.1);
+
             pc.baud(115200);
             pc.printf("Distance in mm: %i\n", position_cart);
             
@@ -414,9 +445,6 @@
                 }else {}
         
             wait(0.1);
-            pc.baud(115200);
-            pc.printf("Degrees: %i\n", position_arm);
-            
             break;
         }
       }
@@ -455,7 +483,12 @@
     btn_arm.fall(&SetValue3);
     btn_claw.fall(&SetValue4);
 
+    if(part_id == 4){
+        Cart.stop(1)==1;
+        }
+
     while (true);
+    
 }