Revision 21:d6a46315d5f5, committed 2015-10-13

Comitter:

Vigilance88

Date:

Tue Oct 13 10:02:48 2015 +0000

Parent:

20:0ede3818e08e

Child:

22:1ba637601dc3

Commit message:

made: sample_filter and calibrate_emg; started with menu functions;

Changed in this revision

--- a/main.cpp	Mon Oct 12 14:02:20 2015 +0000
+++ b/main.cpp	Tue Oct 13 10:02:48 2015 +0000
@@ -3,18 +3,28 @@
 #include "MODSERIAL.h"
 #include "biquadFilter.h"
 #include "QEI.h"
+#include "math.h"
+
+/*--------------------------------------------------------------------------------------------------------------------
+-------------------------------- BIOROBOTICS GROUP 14 ----------------------------------------------------------------
+--------------------------------------------------------------------------------------------------------------------*/
 
 //Define important constants in memory
+#define     PI              3.14159265
 #define     SAMPLE_RATE     0.002   //500 Hz EMG sample rate
 #define     CONTROL_RATE    0.01    //100 Hz Control rate
-#define     ENCODER1_CPR    4200    //encoders have 4200 counts per revolution of the output shaft (X2)
-#define     ENCODER2_CPR    4200    //64 X4, 32 X4 counts per revolution of motor shaft, gearbox 1:131.25
+#define     ENCODER1_CPR    4200    //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
+#define     ENCODER2_CPR    4200    //gearbox 1:131.25 ->  4200 counts per revolution of the output shaft (X2), 
 
-//Objects
+/*--------------------------------------------------------------------------------------------------------------------
+---- OBJECTS ---------------------------------------------------------------------------------------------------------
+--------------------------------------------------------------------------------------------------------------------*/
+
 MODSERIAL pc(USBTX,USBRX);      //serial communication
 DigitalIn button(PTA1);         //buttons
 DigitalIn button1(PTB9);        //
 
+//EMG shields
 AnalogIn    emg1(A0);           //Analog input - Biceps EMG
 AnalogIn    emg2(A1);           //Analog input - Triceps EMG
 AnalogIn    emg3(A2);           //Analog input - Flexor EMG
@@ -27,27 +37,90 @@
 // AnalogIn potmeter(A4);       //potmeters
 // AnalogIn potmeter2(A5);      //
 
+//Encoders 
 QEI Encoder1(D13,D12,NC,32);    //channel A and B from encoder, counts = Encoder.getPulses();
 QEI Encoder2(D10,D9,NC,32);     //channel A and B from encoder, 
-PwmOut pwm_motor1(D6);          //D4 and D5 = motor 2, D7 and D6 = motor 1
-PwmOut pwm_motor2(D5);          //D4 and D5 = motor 2, D7 and D6 = motor 1
+
+//Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1
+PwmOut pwm_motor1(D6);          //PWM motor 1
+PwmOut pwm_motor2(D5);          //PWM motor 2
 DigitalOut dir_motor1(D7);      //Direction motor 1
 DigitalOut dir_motor2(D4);      //Direction motor 2
 
 //Filters
-//biquadFilter     filter(a1, a2, b0, b1, b2); coefficients
-biquadFilter     filter1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     //eg. notch / lowpass / highpass
-biquadFilter     filter2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     //eg. notch / lowpass / highpass
-biquadFilter     filter3( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     //eg. notch / lowpass / highpass
-biquadFilter     filter4( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     //eg. notch / lowpass / highpass
+//Syntax: biquadFilter     filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
+biquadFilter     highpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );    // removes DC and movement artefacts
+biquadFilter     notch1( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );      // removes 49-51 Hz power interference
+biquadFilter     notch2( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );      //
+biquadFilter     lowpass( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );     // EMG envelope    
+biquadFilter     derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );   // derivative filter 
+
+/*--------------------------------------------------------------------------------------------------------------------
+---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
+--------------------------------------------------------------------------------------------------------------------*/
+
+//EMG variables
+double biceps_power; double bicepsMVC = 0;
+double triceps_power; double tricepsMVC = 0;
+double flexor_power; double flexorMVC = 0;
+double extens_power; double extensorMVC = 0;
+
+
+
+/*--------------------------------------------------------------------------------------------------------------------
+---- DECLARE FUNCTION NAMES-------------------------------------------------------------------------------------------
+--------------------------------------------------------------------------------------------------------------------*/
+void keep_in_range(float * in, float min, float max);
+void sample_filter(void);
+void control();
+void calibrate_emg(int muscle);
+void calibrate_arm(void);
+void start_sampling(void);
+void stop_sampling(void);
+void start_control(void);
+void stop_control(void);
+double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
 
-void keep_in_range(float * in, float min, float max);
-void sample(void);
+void mainMenu();
+void caliMenu();
+
+/*--------------------------------------------------------------------------------------------------------------------
+---- MAIN LOOP -------------------------------------------------------------------------------------------------------
+--------------------------------------------------------------------------------------------------------------------*/
+
+int main()
+{
+    // make a menu, user has to initiated next step
+    mainMenu();
+    caliMenu();            // Menu function
+    calibrate_arm();        //Start Calibration
+    start_sampling();       //500 Hz EMG 
+    calibrate_emg(1);        
+    start_control();        //100 Hz control
+    
+    //maybe some stop commands when a button is pressed: detach from timers.
+    //stop_control();
+    //stop_sampling();
+    
+    while(1) {
+    scope.set(0,emg_biceps);
+    scope.set(1,emg_triceps);
+    scope.set(2,emg_flexor);
+    scope.set(3,emg_extens);
+    scope.send();        
+    }
+    //end of while loop
+}
+//end of main
+
+/*--------------------------------------------------------------------------------------------------------------------
+---- FUNCTIONS -------------------------------------------------------------------------------------------------------
+--------------------------------------------------------------------------------------------------------------------*/
 
 //Start sampling
 void start_sampling(void)
 {
-    sample_timer.attach(&sample,SAMPLE_RATE);
+    sample_timer.attach(&sample_filter,SAMPLE_RATE);   //500 Hz EMG 
 }
 
 //stop sampling
@@ -56,18 +129,35 @@
     sample_timer.detach();
 }
 
-//Sample function 
-void sample(void)
+//Sample and Filter  
+void sample_filter(void)
 {
-    double emg_value1 = emg1.read();
-    double emg_value2 = emg2.read();
-    double emg_value3 = emg3.read();
-    double emg_value4 = emg4.read();
-    scope.set(0,emg_value1);
-    scope.set(1,emg_value2);
-    scope.set(2,emg_value3);
-    scope.set(3,emg_value4);
-    scope.send();
+    double emg_biceps = emg1.read();    //Biceps
+    double emg_triceps = emg2.read();    //Triceps
+    double emg_flexor = emg3.read();    //Flexor
+    double emg_extens = emg4.read();    //Extensor
+    
+    //Filter: high-pass -> notch -> rectify -> lowpass or moving average
+    biceps_power = highpass.step(emg_biceps); triceps_power = highpass.step(emg_triceps); flexor_power = highpass.step(emg_flexor); extens_power = highpass.step(emg_extens);
+    biceps_power = notch1.step(biceps_power); triceps_power = notch1.step(triceps_power); flexor_power = notch1.step(flexor_power); extens_power = notch1.step(extens_power);
+    biceps_power = notch2.step(biceps_power); triceps_power = notch2.step(triceps_power); flexor_power = notch2.step(flexor_power); extens_power = notch2.step(extens_power);
+    biceps_power = abs(biceps_power); triceps_power = abs(triceps_power); flexor_power = abs(flexor_power); extens_power = abs(extens_power);
+    biceps_power = lowpass.step(biceps_power); triceps_power = lowpass.step(triceps_power); flexor_power = lowpass.step(flexor_power); extens_power = lowpass.step(extens_power);
+    
+    /* alternative for lowpass filter: moving average
+    window=30;                      //30 samples
+    int i=0;                        //buffer index
+    bicepsbuffer[i]=biceps_power    //fill array
+    
+    i++;                             
+    if(i==window){
+        i=0;
+    }
+    
+    */
+    
+
+    
 }
 
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
@@ -75,30 +165,44 @@
 {
 }
 
-//EMG MVC measurement
-void calibrate_emg(void)
+//EMG Maximum Voluntary Contraction measurement
+void calibrate_emg(int muscle)
 {
+    if(muscle==1){
+    start_sampling();
+    
+    for(int index=0; index<2500;index++){   //measure 5 seconds@500hz = 2500 samples
+        if(biceps_power>bicepsMVC){
+            bicepsMVC=biceps_power;
+        }    
+    }
+    stop_sampling();
+    }
+    
+    if(muscle==2){
+    start_sampling();
+    
+    for(int index=0; index<2500;index++){   //measure 5 seconds@500hz = 2500 samples
+        if(triceps_power>tricepsMVC){
+            tricepsMVC=triceps_power;
+        }    
+    }
+    stop_sampling();
+    }
 }
 
-//use biquadFilter library, output filtered EMG (combine with sample?)
-double filter(double u)
-{
-   double test;
-   return test;
-}
 
 //Input error and Kp, Kd, Ki, output control signal
 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev)
 {
-
     // Derivative
-    double e_der = (error − e_prev)/CONTROL_RATE;
-    e_der = biquad( e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2 );
-    e_prev = e;
+    double e_der = (error-e_prev)/CONTROL_RATE;
+    e_der = derfilter.step(e_der);
+    e_prev = error;
     // Integral
-    e_int = e_int + CONTROL_RATE ∗ error;
+    e_int = e_int+CONTROL_RATE*error;
     // PID
-    return Kp ∗ e + Ki ∗ e_int + Kd ∗ e_der;
+    return kp*error + ki*e_int + kd * e_der;
  
 }
 
@@ -114,14 +218,46 @@
     //inverse kinematics (pos error to angle error)
     
     //PID controller
-    pid();
-    //send PWM and DIR to motors
+    double error=0;double kp=0;double ki=0;double kd=0;double e_int=0;double e_prev=0;
+    u1=pid(error,kp,ki,kd,e_int,e_prev);    //motor 1
+    u2=pid(error,kp,ki,kd,e_int,e_prev);    //motor 2
+    
+    keep_in_range(&u1,-1,1);    //keep u between -1 and 1, sign = direction, PWM = 0-1
+    keep_in_range(&u2,-1,1);
+    
+    //send PWM and DIR to motor 1
+    dir_motor1 = u1>0 ? 1 : 0;          //conditional statement dir_motor1 = [condition] ? [if met 1] : [else 0]], same as if else below. 
+    pwm_motor1.write(abs(u1));
+    
+    //send PWM and DIR to motor 2
+    dir_motor1 = u2>0 ? 1 : 0;          //conditional statement, same as if else below
+    pwm_motor1.write(abs(u2));
+    
+    /*if(u1 > 0)
+    {
+        dir_motor1 = 0;
+    else{
+        dir_motor1 = 1;
+        }
+    }    
+    pwm_motor1.write(abs(u1));
+    
+  
+    if(u2 > 0)
+    {
+        dir_motor1 = 1;
+    else{
+        dir_motor1 = 0;
+        }
+    }    
+    pwm_motor1.write(abs(u2));*/
+    
 }
 
 //Start control
 void start_control(void)
 {
-    control_timer.attach(&control,SAMPLE_RATE);
+    control_timer.attach(&control,SAMPLE_RATE);     //100 Hz control
 }
 
 //stop control
@@ -130,25 +266,14 @@
     control_timer.detach();
 }
 
-int main()
+
+void calibrate()
 {
-    // make a menu, user has to initiated next step
-    
-    calibrate_arm();        //Start Calibration
-    calibrate_emg();        
-    start_sampling();       //500 Hz EMG 
-    start_control();        //100 Hz control
-    
-    //maybe some stop commands when a button is pressed: detach from timers.
-    //stop_control();
-    //stop_sampling();
-    
-    while(1) {
-        
-        
-    }//end of while loop
-}//end of main
+
+}
 
+
+//keeps input limited between min max
 void keep_in_range(float * in, float min, float max)
 {
     *in > min ? *in < max? : *in = max: *in = min;