Revision 9:888ed3c72795, committed 2015-10-23

Comitter:

riannebulthuis

Date:

Fri Oct 23 09:32:09 2015 +0000

Parent:

8:b219ca30967f

Commit message:

clockwise bewegen met rechter arm, homepos werkt.

Changed in this revision

diff -r b219ca30967f -r 888ed3c72795 main.cpp
--- a/main.cpp	Thu Oct 22 15:07:20 2015 +0000
+++ b/main.cpp	Fri Oct 23 09:32:09 2015 +0000
@@ -10,13 +10,13 @@
 DigitalIn   button_1(PTC6); //counterclockwise
 DigitalIn   button_2(PTA4); //clockwise
 AnalogIn    PotMeter1(A4);
-AnalogIn    EMG(A0);
+AnalogIn    EMG_bicepsright (A0);
 Ticker      controller;
 Ticker      ticker_regelaar;
 Ticker      EMG_Filter;
 Ticker      Scope;
 Encoder     motor1(D12,D13);
-HIDScope    scope(3);
+HIDScope    scope(2);
 MODSERIAL   pc(USBTX, USBRX);
 
 
@@ -83,8 +83,9 @@
 const double flp1_a1=-0.97922725527, flp1_a2=0.00000000000, flp1_b0= 1, flp1_b1=1, flp1_b2=0;
 const double flp2_a1=-1.97879353121, flp2_a2=0.97922951943, flp2_b0= 1, flp2_b1=2, flp2_b2=1;
 
-double y1, y2, y3, y4, y5, y6, y7, y8, y9, u1, u2, u3, u4, u5, u6, u7, u8, u9;
-double final_filter1;
+double y1, y2, y3, y4, y5, y6, y7, y8, y9;
+double u1, u2, u3, u4, u5, u6, u7, u8, u9;
+double filter_br, filter_bl;
 
 // Standaard formule voor het biquad filter
 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2){
@@ -97,7 +98,7 @@
 
 // script voor filters
 void Filters(){
-    u1 = EMG.read();
+    u1 = EMG_bicepsright.read();
     //Highpass
     y1 = biquad (u1, fh1_v1, fh1_v2, fh1_a1, fh1_a2, fh1_b0*0.924547, fh1_b1*0.924547, fh1_b2*0.924547);
     u2 = y1;
@@ -119,8 +120,8 @@
     u8 = y8;
     y9 = biquad (u8, flp1_v1, flp1_v2, flp1_a1, flp1_a2, flp1_b0* 0.010386, flp1_b1* 0.010386, flp1_b2* 0.010386);
     u9 = y9;
-    final_filter1 = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
-} 
+    filter_br = biquad(u9, flp2_v1, flp2_v2, flp2_a1, flp2_a2, flp2_b0*0.000109, flp2_b1*0.000109, flp2_b2*0.000109);
+}
  
 
 void motor1_controlPI(){
@@ -134,20 +135,18 @@
 // constantes voor berekening homepositie
 double H;
 double P;
-double D;
-
 
 void move_motor1()
 {
-        if (final_filter1 > 0.03){
+        if (filter_br > 0.03 || button_1 == pressed){
             pc.printf("Moving clockwise \n\r");
-            motor1_direction = 1; //clockwise
-            motor1_speed = 0.4;
+            motor1_direction = 0; //counterclockwise
+            motor1_speed = 0.2;
             }
         else if (button_2 == pressed){
             pc.printf("Moving counterclockwise \n\r");
-            motor1_direction = 0; //counterclockwise
-            motor1_speed = 0.4;
+            motor1_direction = 1; //clockwise
+            motor1_speed = 0.2;
             }
         else {
             pc.printf("Not moving \n\r"); 
@@ -172,7 +171,8 @@
 }
 
 void HIDScope (){
-    scope.set (0, final_filter1);
+    scope.set (0, filter_br);
+    //scope.set (1, filter_bl);
     scope.set(1, motor1.getPosition());
     scope.send ();
     }