Diff: main.cpp

Revision:

16:3c9a3ff09765

Parent:

15:caf29b6f5261

--- a/main.cpp	Thu Nov 03 12:12:03 2016 +0000
+++ b/main.cpp	Thu Nov 03 16:50:49 2016 +0000
@@ -24,7 +24,6 @@
 // servo
 Servo servo(D9);
 
-
 HIDScope    scope(2); //scope has two ports for the two EMG signals
 
 //======== Miscellaneous =======================================================
@@ -67,7 +66,7 @@
 int num_claw_turned_on_2 = 0;           // count number of times blue LED turned on
 
 // speed
-double cart_speed = 0.3;
+double cart_speed = 0.2;
 double arm_speed = 0.1;
 
 // position
@@ -172,10 +171,12 @@
 double rlf_y;
 double rrect_y;
 
+
 // set the threshold value for the filtered signal
 //if the signal exceeds this value the motors will start to rotate 
-const double threshold_value= 0.08;
+const double threshold_value= 0.1;
 const double threshold_value_claw= 0.08;
+const double threshold_value_cart=0.08;
 
 
 
@@ -287,12 +288,11 @@
     rhf_y = biquad_rhf(rno2_y, rhf_v1, rhf_v2, rhf_a1, rhf_a2, rhf_b0, rhf_b1, rhf_b2);
     rrect_y = fabs(rhf_y);
     rlf_y = biquad_rlf(rrect_y, rlf_v1, rlf_v2, rlf_a1, rlf_a2, rlf_b0, rlf_b1, rlf_b2)/0.2;
-    //scope.set(1, llf_y);
-    //scope.set(0, rlf_y);
-    //scope.send();
+    scope.set(1, llf_y);
+    scope.set(0, rlf_y);
+    scope.send();
     }
 
-
 //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
 //======== Functions and main ==============================================================
 void Measure(){
@@ -304,9 +304,9 @@
      position_arm = (Encoder_Arm.getPulses()*factor_arm) ;    
      ain_arm = pot_arm.read();
    
-     //if (ain_arm == 0){
-     //   Encoder_Arm.reset();   
-     //}else {}
+     if (ain_arm == 0){
+        Encoder_Arm.reset();   
+     }else {}
      
      
     
@@ -323,45 +323,39 @@
             if(led_r == LedOn){
                 num_turned_on_0++;
             
-                if (rlf_y > threshold_value) {
+                if (rlf_y > threshold_value_cart) {
                     if(position_cart <= -170){                                  //If the cart is at the right side, it stops
                     Cart.stop(1)==1;
-                    
-                    }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.
+                                    
+                    }else if(position_cart >= 170 && position_arm <=-100){       //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 >= 150 && position_arm <=-35 && position_claw == -18){
+                    }else if(position_cart >= 170 && position_arm <=-160 && position_claw == -18){
                     Cart.stop(1) == 1;    
     
                     }else{
                     Cart.speed(cart_speed)==cart_speed;
                     }
                     
-                        if (llf_y > threshold_value){
+                        if (llf_y > threshold_value_cart){
                             Cart.stop(1)==1;
                         }
          
-                }else if (llf_y > threshold_value) {
-                    if(position_cart >= 150){                                   //If the cart is at the left side, it stops
+                }else if (llf_y > threshold_value_cart) {
+                    if(position_cart >= 170){                                   //If the cart is at the left side, it stops
                     Cart.stop(1)==1;
-                    
-                    }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.
+
+                    }else if(position_cart <= -170 && position_arm >=100){       //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){
+                    }else if(position_cart <= -170 && position_arm >=160 && position_claw == 27){
                     Cart.stop(1)==1;
                     
                     }else{
                     Cart.speed(-cart_speed)==-cart_speed;
                     }
                     
-                        if (rlf_y > threshold_value){
+                        if (rlf_y > threshold_value_cart){
                             Cart.stop(1)==1;
                         }
                                             
@@ -405,15 +399,18 @@
                 Arm.stop(1)==1;
 
             
-                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
+                if (rlf_y > threshold_value_claw) {
+                    if(position_cart > -170 && 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){
+                    }else if(position_cart > -200 && position_arm >= 60 && position_claw == 27){
                     Arm.stop(1)==1;
                         
-                    }else if(position_cart<= -105 && position_arm>=160){         //If the cart is at the right end, the arm can't move any further than 70 degrees
+                    }else if(position_cart<= -170 && position_arm>=160){         //If the cart is at the right end, the arm can't move any further than 70 degrees
                     Arm.stop(1)==1;
+                    
+                    }else if(position_cart <= -170 && position_arm >= 95){
+                    Arm.speed(0.075)==0.075;
     
                     }else{
                     Arm.speed(arm_speed)==arm_speed;
@@ -423,16 +420,16 @@
                             Cart.stop(1)==1;
                         }
          
-                }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 
+                }else if (llf_y > threshold_value_claw) {
+                    if(position_cart < 170 && 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){
+                    }else if(position_cart < 170 && position_arm <= -60 && position_claw == -18){
                     Arm.stop(1)==1;
                     
-                    }else if(position_cart>=105 && position_arm<=-160){          //If the cart is at the left end, the arm can't move any further than 70 degrees
+                    }else if(position_cart>=170 && position_arm<=-160){          //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;
                     } 
@@ -603,7 +600,6 @@
     tick_part_arm.attach(&SwitchArm,kTimeToggle);
     tick_part_claw.attach(&SwitchClaw,0.1f);   
     measureTicker.attach(Measure, 0.005);
-    sampleTicker_claw.attach(scopeSend,1);
     sampleTicker.attach(scopeSend,0.01);