Diff: main.cpp

Revision:

3:98cdee5d9b63

Parent:

2:f630aff31d27

Child:

4:f19e7669d1b5

--- a/main.cpp	Tue Aug 07 12:01:03 2018 +0000
+++ b/main.cpp	Tue Aug 07 14:20:09 2018 +0000
@@ -19,6 +19,7 @@
 DigitalOut LED(A4);            // check if the code is running
 DigitalOut led2(A5);            // check if the code is running interrupt
 uint8_t led2f;
+AnalogIn EMG(PC_4);
 
 // Timer
 Ticker timer1;
@@ -26,6 +27,13 @@
 float Ts = ITR_time1/1000000;
 uint8_t flag;
 
+// EMG
+float lpf(float input, float output_old, float frequency);
+float emg_value;
+float emg_value_f;
+float emg_value_f_old;
+float Tf = ITR_time1*0.000001;
+
 // uart_tx
 union splitter {
     short j;
@@ -116,7 +124,11 @@
                 row_cmd = servo_cmd;
             }
 
-            dynamixelClass.torque(SERVO_ID, servo_cmd);
+            // EMG
+            emg_value = EMG.read();
+            emg_value_f = lpf(emg_value,emg_value_f_old,15);
+            emg_value_f_old = emg_value_f;
+//            dynamixelClass.torque(SERVO_ID, servo_cmd);
             uart_tx();
             flag = 0;
         }
@@ -167,7 +179,7 @@
 //    s4.j = 1;
 //    s5.j = 3;
     s4.j = (Angle*3-D_Angle)*360/4096;
-    s5.j = torque_measured*1000;
+    s5.j = emg_value_f*100;
     s6.j = row_cmd;
     s7.j = 1;
 
@@ -222,3 +234,12 @@
 
     torque_measured = angle_difference*ks;
 }
+
+float lpf(float input, float output_old, float frequency)
+{
+    float output = 0;
+
+    output = (output_old + frequency*Tf*input) / (1 + frequency*Tf);
+
+    return output;
+}
\ No newline at end of file