Bouke Scheltinga
/
Werk_college_23sept
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Encoder HIDScope MODSERIAL mbed QEI biquadFilter
Revision 42:b5186a54d839, committed 2015-10-21
- Comitter:
- sigert
- Date:
- Wed Oct 21 12:43:01 2015 +0000
- Parent:
- 39:9fa091753592
- Child:
- 43:9dc385093c8e
- Commit message:
- Pwm aansturen met EMG spierkracht
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 20 11:15:38 2015 +0000
+++ b/main.cpp Wed Oct 21 12:43:01 2015 +0000
@@ -2,15 +2,22 @@
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "biquadFilter.h" //Filter direct form II
+#include "QEI.h"
// [DEFINE INPUTS] //
-DigitalOut debug_led_red(LED_RED); // Debug LED
+DigitalOut debug_led_red (LED_RED); // Debug LED
DigitalOut debug_led_green(LED_GREEN); // Debug LED
-DigitalOut debug_led_blue(LED_BLUE); // Debug LED
+DigitalOut debug_led_blue (LED_BLUE); // Debug LED
const double off=1; // Led off
const double on=0; // Led on
+// MOTORS
+QEI motor_turn(D12,D13,NC,32); QEI motor_strike(D9,D10,NC,32); // TURN - STRIKE : Encoder for motor
+PwmOut pwm_motor_turn(D5); PwmOut pwm_motor_strike(D6); // TURN - STRIKE : Pwm for motor
+DigitalOut motordirection_turn(D4); DigitalOut motordirection_strike(D7); // TURN - STRIKE : Direction of the motor
+
+
// [DEFINE CONSTANTS] //
HIDScope scope(3); // Aantal HIDScope kanalen
MODSERIAL pc(USBTX,USBRX);
@@ -69,8 +76,8 @@
volatile bool sample_error = false;
volatile bool sample_error_strike = false;
-double Sample_EMG_L_1, Sample_EMG_L_2, Sample_EMG_L_3, Sample_EMG_L_4, Sample_EMG_L_5, Sample_EMG_L_6, Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left;
-double Sample_EMG_R_1, Sample_EMG_R_2, Sample_EMG_R_3, Sample_EMG_R_4, Sample_EMG_R_5, Sample_EMG_R_6, Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right;
+double Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
+
double minimum_L; double maximum_L;
double EMG_L_min; double EMG_L_max;
@@ -93,6 +100,19 @@
void calibration();
void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black();
+double RS_0,RS_1,RS_2,RS_3,RS_4,RS_5,RS_6,RS_7,RS_8,RS_9,RS_10,
+ RS_11,RS_12,RS_13,RS_14,RS_15,RS_16,RS_17,RS_18,RS_19,RS_20,
+ RS_21,RS_22,RS_23,RS_24,RS_25,RS_26,RS_27,RS_28,RS_29,RS_30,RS_31;
+
+double LS_0,LS_1,LS_2,LS_3,LS_4,LS_5,LS_6,LS_7,LS_8,LS_9,LS_10,
+ LS_11,LS_12,LS_13,LS_14,LS_15,LS_16,LS_17,LS_18,LS_19,LS_20,
+ LS_21,LS_22,LS_23,LS_24,LS_25,LS_26,LS_27,LS_28,LS_29,LS_30,LS_31;
+
+double moving_average_right, moving_average_left;
+
+void keep_in_range (double * in, double min, double max);
+
+double pwm_strike;
//==========================//
// [MAIN FUNCTION] //
@@ -101,22 +121,33 @@
{
control_tick.attach(&ControlGo, (float)1/Fs);
pc.baud(115200);
- double n=0;
- // calibration(); // calibreer min en max positie
+ //double n=0;
+ calibration(); // calibreer min en max positie
while(1)
{
+ // if(control_go)
+// {
+// //n++;
+// //pc.printf("n %f",n);
+// sample_filter();
+// scope.set(0,EMG_left_Bicep); //left bicep unfiltered
+// scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
+// scope.set(2,moving_average_left); //
+// scope.send();
+// control_go = 0;
+// }
+ yellow();
+
if(control_go)
- {
- n++;
- pc.printf("n %f",n);
- sample_filter();
- scope.set(0,n); //left bicep unfiltered
- scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
- scope.set(2,moving_average_left); //
- scope.send();
- control_go = 0;
- }
- yellow();
+ {control_go=0;
+ sample_filter();
+ double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
+ double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
+ pwm_strike=signal_above_threshold/max_signal;
+ pc.printf("Pwm=%f \n\r", pwm_strike);
+ keep_in_range(&pwm_strike, -1,1);
+
+ pwm_motor_strike=fabs(pwm_strike);}
} // while end
} // main end
@@ -134,26 +165,25 @@
if(sample)
{
sample=false;
- Sample_EMG_L_1 = EMG_Left_Bicep_filtered; Sample_EMG_R_1 = EMG_Right_Bicep_filtered;
+ LS_0 = EMG_Left_Bicep_filtered; RS_0 = EMG_Right_Bicep_filtered;
+
+ LS_30=LS_29; LS_29=LS_28;LS_28=LS_27; LS_27=LS_26; LS_26=LS_25; LS_25=LS_24; LS_24=LS_23; LS_23=LS_22; LS_22=LS_21; LS_20=LS_19;
+ LS_19=LS_18; LS_18=LS_17;LS_17=LS_16; LS_16=LS_15; LS_15=LS_14; LS_14=LS_13; LS_13=LS_12; LS_12=LS_11; LS_11=LS_10; LS_10=LS_9;
+ LS_9=LS_8; LS_8=LS_7;LS_7=LS_6; LS_6=LS_5; LS_5=LS_4; LS_4=LS_3; LS_3=LS_2; LS_2=LS_1; LS_1=LS_0;
- Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9;
- Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8;
- Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7;
- Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6;
- Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5;
- Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4;
- Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3;
- Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2;
- Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1;
+ RS_30=RS_29; RS_29=RS_28;RS_28=RS_27; RS_27=RS_26; RS_26=RS_25; RS_25=RS_24; RS_24=RS_23; RS_23=RS_22; RS_22=RS_21; RS_20=RS_19;
+ RS_19=RS_18; RS_18=RS_17;RS_17=RS_16; RS_16=RS_15; RS_15=RS_14; RS_14=RS_13; RS_13=RS_12; RS_12=RS_11; RS_11=RS_10; RS_10=RS_9;
+ RS_9=RS_8; RS_8=RS_7;RS_7=RS_6; RS_6=RS_5; RS_5=RS_4; RS_4=RS_3; RS_3=RS_2; RS_2=RS_1; RS_1=RS_0;
+
}
- moving_average_left=Sample_EMG_L_1*0.1+Sample_EMG_L_2*0.1+Sample_EMG_L_3*0.1+Sample_EMG_L_4*0.1+Sample_EMG_L_5*0.1+Sample_EMG_L_6*0.1+Sample_EMG_L_7*0.1+Sample_EMG_L_8*0.1+Sample_EMG_L_9*0.1+Sample_EMG_L_10*0.1;
- moving_average_right=Sample_EMG_R_1*0.1+Sample_EMG_R_2*0.1+Sample_EMG_R_3*0.1+Sample_EMG_R_4*0.1+Sample_EMG_R_5*0.1+Sample_EMG_R_6*0.1+Sample_EMG_R_7*0.1+Sample_EMG_R_8*0.1+Sample_EMG_R_9*0.1+Sample_EMG_R_10*0.1;
+ moving_average_left= (LS_30+LS_29+LS_28+LS_27+LS_26+LS_25+LS_24+LS_23+LS_22+LS_21+LS_20+LS_19+LS_18+LS_17+LS_16+LS_15+LS_14+LS_13+LS_12+LS_11+LS_10+LS_9+LS_8+LS_7+LS_6+LS_5+LS_4+LS_3+LS_2+LS_1+LS_0)/31;
+ moving_average_right= (RS_30+RS_29+RS_28+RS_27+RS_26+RS_25+RS_24+RS_23+RS_22+RS_21+RS_20+RS_19+RS_18+RS_17+RS_16+RS_15+RS_14+RS_13+RS_12+RS_11+RS_10+RS_9+RS_8+RS_7+RS_6+RS_5+RS_4+RS_3+RS_2+RS_1+RS_0)/31;
n++;
}
void take_sample() // Take a sample every 25th sample
{
- if(n==25)
+ if(n==8)
{
sample = true; n=0;
}
@@ -176,10 +206,12 @@
EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
- EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
- EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1);
+ //EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
+ //EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1);
- EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2);
+ //EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2);
+
+ EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
}
void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
@@ -191,7 +223,6 @@
void calibration()
{
-
// [MINIMUM VALUE BICEPS CALIBRATION] //
pc.printf("Start minimum calibration in 5 seconds \n\r");
@@ -253,10 +284,10 @@
// [MAXIMUM VALUE BICEPS CALIBRATION] //
// Calculate threshold percentages //
- const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
- const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
- const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
- const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
+ Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2); //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
+ Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6); //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
+ Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2); //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
+ Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6); //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
pc.printf("left 1: %f left 2: %f right 1: %f right 2: %f \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2);
@@ -271,3 +302,7 @@
void purple() { debug_led_red=on; debug_led_blue=on; debug_led_green=off; }
void black() { debug_led_red=off; debug_led_blue=off; debug_led_green=off; }
+void keep_in_range(double * in, double min, double max) // Put in certain min and max values that the input needs to stay within
+{
+ *in > min ? *in < max? : *in = max: *in = min;
+}
\ No newline at end of file
