Revision 33:3c9f8c1e9adf, committed 2015-05-22

Comitter:

jessekaiser

Date:

Fri May 22 08:43:27 2015 +0000

Parent:

32:46b18f465600

Child:

34:025b324d15d6

Commit message:

Proportional control werkt. Overbodige code verwijderd.

Changed in this revision

--- a/main.cpp	Fri May 22 08:35:28 2015 +0000
+++ b/main.cpp	Fri May 22 08:43:27 2015 +0000
@@ -3,12 +3,12 @@
 #include "arm_math.h"
 #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
+#define K_Gain      65      //Gain of the filtered EMG signal
+#define Damp        2         //Deceleration of the motr
+#define Mass        1         // Mass value
+#define dt          0.002       //Sample frequency
+#define MAX_emg 
+#define MIN_freq    900 
 
 //Motor control
 DigitalOut Dir(p21);
@@ -56,7 +56,6 @@
 //highpass filter settings: Fc = 20 Hz, Fs = 500 Hz, Gain = -3 dB, notch Fc = 50, Fs =500Hz, Gain = -3 dB
 float highnotch_const[] = {0.8370879899975344, -1.6741759799950688, 0.8370879899975344, 1.6474576182593796, -0.7008943417307579, 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, 1.1429772843080923, -0.41279762014290533};
 
-
 //state values
 float lowpass_biceps_states[4];
 float highnotch_biceps_states[8];
@@ -75,20 +74,6 @@
 float speed_old;
 float D;
 
-
-void average_biceps(float filtered_biceps,float *average)
-{
-    static float total=0;
-    static float number=0;
-    total = total + filtered_biceps;
-    number = number + 1;
-    if ( number == 50) {
-        *average = total/50;
-        total = 0;
-        number = 0;
-    }
-}
-
 void looper_emg()
 {
     /*variable to store value in*/
@@ -96,9 +81,8 @@
 
     float emg_value1_f32;
 
-
     /*put raw emg value both in red and in emg_value*/
-    emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V)
+    emg_value1 = emg0.read_u16(); // read direct ADC result, converted to 16 bit integer (0..2^16 = 0..65536 = 0..3.3V) (KAN weg?)
     emg_value1_f32 = emg0.read();
 
     //process emg biceps
@@ -108,57 +92,8 @@
     average_biceps(filtered_biceps,&filtered_average_bi);
 
     /*send value to PC. */
-    scope.set(0,filtered_average_bi);  //Raw EMG signal biceps
-    scope.set(1,filtered_biceps); //Filtered signal
-
+    scope.set(0,filtered_biceps); //Filtered EMG signal
 }
-/*void looper_pot()
-{
-
-    pot_value1_f32 = Pot1.read() - 0.500;
-
-    //process input
-    arm_biquad_cascade_df1_f32(&lowpass_pot, &pot_value1_f32, &filtered_pot, 1 );
-
-}*/
-
-
-/*void looper_motor()
-{
-    float new_step_freq;
-    float speed;
-
-    speed = 0.02*filtered_average_bi + 0.02*filt_avg_bi_old + 0.96*speed_old; //Value between 0 and 1
-    new_step_freq = (setpoint*speed);
-    step_freq = abs(new_step_freq); //Gives the PWM frequenty to the motor.
-    speed_old = speed;
-    filt_avg_bi_old = filtered_average_bi;
-
-    if (step_freq < 500 || step_freq > 8000) {
-        Enable = 1;
-    } else {
-        Enable = 0;
-    }
-    Step.period(1.0/(100 + step_freq)); //Step_freq is het aantal Hz.
-
-}*/
-//Motor accelereren met EMG treshold
-/*void looper_motor()
-{
-    float new_step_freq;
-    Dir = 0;
-
-    if (filtered_average_bi > tres_bi) {
-        Enable = 0;
-        new_step_freq = ((1-P)*setpoint) + (P*step_freq);
-        step_freq = new_step_freq;
-        Step.period(1.0/step_freq);
-    } else {
-        Enable = 1;
-        step_freq = 1;
-    }
-
-}*/
 
 void looper_motor()
 {
@@ -172,46 +107,35 @@
     Step.period(1.0/step_freq);
     speed_old = speed;
 
-    if (step_freq < 800) {
+    if (step_freq < MIN_freq) {
         Enable = 1;
     } else {
         Enable = 0;
     }
 }
 
-
 int main()
 {
     // Attach the HIDScope::send method from the scope object to the timer at 50Hz. Hier wordt de sample freq aangegeven.
     scopeTimer.attach_us(&scope, &HIDScope::send, 2e3);
 
-    /* Ticker log_timer;
-     //set up filters. Use external array for constants
-     arm_biquad_cascade_df1_init_f32(&lowpass_pot, 1 , lowpass_const, lowpass_pot_states);
-     log_timer.attach(looper_pot, 0.01);*/
-
     Ticker emgtimer;
     arm_biquad_cascade_df1_init_f32(&lowpass_biceps, 1 , lowpass_const, lowpass_biceps_states);
     arm_biquad_cascade_df1_init_f32(&highnotch_biceps, 2 , highnotch_const, highnotch_biceps_states);
     emgtimer.attach(looper_emg, 0.002);
 
     Ticker looptimer;
-    looptimer.attach(looper_motor, 0.01);
-
+    looptimer.attach(looper_motor, 0.01); //Uitzoeken waarom deze frequentie!
+    
+    //Microstepping control
     MS1 = 1;
     MS2 = 0;
     MS3 = 0;
-    //Step.period(1/step_freq);
     Step.write(0.5); // Duty cycle van 50%
 
     while (1) {
 
         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("speed %.0f Hz p1 %.4f \n", step_freq, pot_value1_f32); //snelheid meting op lcd
         wait(0.01);
-
-
     }
 }