Diff: main.cpp

Revision:

32:46b18f465600

Parent:

31:372ff8d49430

Child:

33:3c9f8c1e9adf

--- a/main.cpp	Thu May 21 15:38:12 2015 +0000
+++ b/main.cpp	Fri May 22 08:35:28 2015 +0000
@@ -4,6 +4,8 @@
 #include "HIDScope.h"
 
 #define P_Gain 0.995
+#define K_Gain 65      //Gain of the filtered EMG signal
+#define Damp 2          //Deceleration of the motr
 #define tres_bi 0.05    //Biceps emg treshold
 #define Mass 1          // Mass value
 #define dt 0.002        //Sample frequency
@@ -33,7 +35,7 @@
 C12832_LCD lcd;
 
 //Variables for motor control
-float setpoint = 6500; //Frequentie
+float setpoint = 9000; //Frequentie
 float step_freq = 1;
 
 
@@ -45,8 +47,6 @@
 float lowpass_const[] = {0.0201, 0.0402 , 0.0201, 1.5610, -0.6414};
 //Lowpass filter potmeter: Fc = 0.5 Hz, Fs = 500 Hz,
 //float lowpass_const[] = {0.000009825916403675327, 0.000019651832807350654, 0.000009825916403675327, 1.991114207740345, -0.9911535114059596};
-//lowpass for step_freq: Fc = 2 Hz, Fs = 100, Gain = 6 dB
-//float lowpass1_const[] = {0.007820199259120319, 0.015640398518240638, 0.007820199259120319, 1.7347238224240125, -0.7660046194604936};
 
 //EMG filter
 arm_biquad_casd_df1_inst_f32 lowpass_biceps;
@@ -61,24 +61,20 @@
 float lowpass_biceps_states[4];
 float highnotch_biceps_states[8];
 float lowpass_pot_states[4];
-float lowpass1_step_states[4];
 
 //global variabels
 float filtered_biceps;
 float filtered_average_bi;
+float filt_avg_bi_old;
 float filtered_pot;
-float filtered_average_pot;
-float filtered_step;
 float pot_value1_f32;
-float filt_avg_bi_old;
 float speed_old;
 float acc;
 float force;
-float spd;
-float spd_old;
-float D = 0;
-float Damp;
-float K_Gain;
+float speed;
+float speed_old;
+float D;
+
 
 void average_biceps(float filtered_biceps,float *average)
 {
@@ -167,16 +163,14 @@
 void looper_motor()
 {
     Dir = 0;
-    K_Gain = 20*Pot2.read();
     force = K_Gain*filtered_biceps;
     force = force - D;
     acc = force/Mass;
-    spd = spd_old + (acc * dt);
-    Damp = 2*Pot1.read();
-    D = spd * Damp;
-    step_freq = (setpoint*spd);
+    speed = speed_old + (acc * dt);
+    D = speed * Damp;
+    step_freq = (setpoint*speed);
     Step.period(1.0/step_freq);
-    spd_old = spd;
+    speed_old = speed;
 
     if (step_freq < 800) {
         Enable = 1;
@@ -215,7 +209,7 @@
         lcd.printf("Freq %.0f Hz \n", step_freq); //snelheid meting op lcd
         //pc.printf(" %.4f \n", Pot1.read());
         //lcd.printf("filt %.3f raw %.3f \n", filtered_biceps, emg0.read());
-        //pc.printf("Spd %.0f Hz p1 %.4f \n", step_freq, pot_value1_f32); //snelheid meting op lcd
+        //pc.printf("speed %.0f Hz p1 %.4f \n", step_freq, pot_value1_f32); //snelheid meting op lcd
         wait(0.01);