Diff: main.cpp

Revision:

15:caf29b6f5261

Parent:

14:65780d480874

Child:

16:3c9a3ff09765

--- a/main.cpp	Wed Nov 02 18:56:54 2016 +0000
+++ b/main.cpp	Thu Nov 03 12:12:03 2016 +0000
@@ -24,6 +24,7 @@
 // servo
 Servo servo(D9);
 
+
 HIDScope    scope(2); //scope has two ports for the two EMG signals
 
 //======== Miscellaneous =======================================================
@@ -49,9 +50,11 @@
 
 // ticker
 Ticker tick_part;                       // ticker to switch parts 
-Ticker tick_part_arm;    
+Ticker tick_part_arm;  
+Ticker tick_part_claw;  
 Ticker sampleTicker; 
-Ticker measureTicker;  
+Ticker measureTicker; 
+Ticker sampleTicker_claw; 
 
 //======== Variables ===========================================================
 // counters
@@ -84,6 +87,8 @@
 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
 
+int waiting_claw = 1000;
+
 //======== Variables Filter ====================================================
 /*coefficients of each filter
     lno = left tricep notch filter
@@ -170,6 +175,8 @@
 // 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_claw= 0.08;
+
 
 
 /* declaration of each biquad
@@ -280,9 +287,9 @@
     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();
     }
 
 
@@ -312,7 +319,7 @@
     switch (part_id) {
         //Cart
         case 2: {
-            led_r = LedOn;          
+            led_r = LedOn;         
             if(led_r == LedOn){
                 num_turned_on_0++;
             
@@ -392,13 +399,13 @@
     switch (part_id) {
         //Cart
         case 3: {
-            led_g = LedOn;          
+            led_g = LedOn;         
             if(led_g == LedOn){
                 num_turned_on_1++;
                 Arm.stop(1)==1;
 
             
-                if (btn&&!btn2) {
+                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;
                     
@@ -416,7 +423,7 @@
                             Cart.stop(1)==1;
                         }
          
-                }else if (!btn&&btn2) {
+                }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;
                     
@@ -461,6 +468,113 @@
       }
 }
 
+void SwitchClaw(){
+switch (part_id) {
+    case 4: {
+    led_b = LedOn;
+
+           if(rlf_y > threshold_value_claw){
+           servo_id ++;
+                 
+                switch (servo_id) {
+                    case 0: {
+                        led_r = LedOn;
+                        if (led_r == LedOn) {
+                                num_claw_turned_on_0++;
+                            }
+                            
+                            led_b = not LedOn;
+                            led_g = not LedOn;
+                            servo.position(-18);
+                            //pc.printf("Servo position is: left \r\n");
+                            
+                            break;
+                        }
+                        case 1: {
+                            led_b = LedOn;
+                            if (led_b == LedOn) {
+                                num_claw_turned_on_1++;
+                            }
+                            
+                            led_r = not LedOn;
+                            led_g = not LedOn;
+                            
+                            servo.position(3);
+                            //pc.printf("Servo position is: center \r\n");
+                            
+                            wait_ms(waiting_claw);
+                            
+                            break;
+                        }
+                        case 2: {
+                            led_g = LedOn;
+                            if (led_g == LedOn) {
+                                num_claw_turned_on_2++;
+                            }
+                            
+                            led_r = not LedOn;
+                            led_b = not LedOn;
+                            
+                            servo.position(27);
+                            //pc.printf("Servo position is: %i\r\n", num_claw_turned_on);
+                            
+                            break;
+                        }
+                    }
+                 
+                 
+                }else if(llf_y > threshold_value_claw){
+                servo_id --;
+                
+                switch (servo_id) {
+                    case 0: {
+                        led_r = LedOn;                       
+                        led_b = not LedOn;
+                        led_g = not LedOn;
+                        
+                        servo.position(-18);
+                        //pc.printf("Servo position is: left \r\n");
+
+                        
+                        break;
+                    }
+                    case 1: {
+                        led_b = LedOn;                        
+                        led_r = not LedOn;
+                        led_g = not LedOn;
+                        
+                        servo.position(3);
+                        //pc.printf("Servo position is: center \r\n");
+                        
+                        wait_ms(waiting_claw);
+                        
+                        break;
+                    }
+                    case 2: {
+                        led_g = LedOn;                        
+                        led_r = not LedOn;
+                        led_b = not LedOn;
+                        
+                        servo.position(27);
+                        //pc.printf("Servo position is: right \r\n");
+
+                        
+                        break;
+                    }
+                }
+                
+                }else{}       
+            }
+            led_r = not LedOn;
+            led_g = not LedOn;                
+            
+            position_claw = servo.read();
+            pc.baud(115200);
+            pc.printf("Servo position is: %i, %i, %i \r\n", num_claw_turned_on_0, num_claw_turned_on_1, num_claw_turned_on_2);
+            
+            break;
+        }    
+}
 
 
 // Switch the part
@@ -486,9 +600,12 @@
     led_b = not LedOn;
     
     tick_part.attach(&SwitchCart,kTimeToggle);
-    tick_part_arm.attach(&SwitchArm,kTimeToggle);  
-    sampleTicker.attach(scopeSend,0.01);
+    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); 
+   
    
     btn_cart.fall(&SetValue2);
     btn_arm.fall(&SetValue3);