group14 - k9
testcode pid
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
testPID
by 
Martijn Kern
Diff: main.cpp
- Revision:
- 25:49ccdc98639a
- Parent:
- 24:56db31267f10
- Child:
- 26:fe3a5469dd6b
--- a/main.cpp Wed Oct 14 13:04:54 2015 +0000
+++ b/main.cpp Fri Oct 16 13:46:39 2015 +0000
@@ -24,6 +24,11 @@
DigitalIn button(PTA1); //buttons
DigitalIn button1(PTB9); //
+//Debug legs
+DigitalOut red(LED_RED);
+DigitalOut green(LED_GREEN);
+DigitalOut blue(LED_BLUE);
+
//EMG shields
AnalogIn emg1(A0); //Analog input - Biceps EMG
AnalogIn emg2(A1); //Analog input - Triceps EMG
@@ -48,8 +53,8 @@
DigitalOut dir_motor2(D4); //Direction motor 2
//Limit Switches
-DigitalIn shoulder_limit(PTA4); //using FRDM buttons for now
-DigitalIn elbow_limit(PTC6); //using FRDM buttons for now
+DigitalIn shoulder_limit(PTA4); //using FRDM buttons
+DigitalIn elbow_limit(PTC6); //using FRDM buttons
/*--------------------------------------------------------------------------------------------------------------------
@@ -62,6 +67,9 @@
double emg_flexor; double flexor_power; double flexorMVC = 0;
double emg_extens; double extens_power; double extensMVC = 0;
+int muscle;
+double calibrate_time;
+
//PID variables
double u1; double u2; //Output of PID controller (PWM value for motor 1 and 2)
double m1_error=0; double m1_e_int=0; double m1_e_prev=0; //Error, integrated error, previous error
@@ -130,6 +138,22 @@
biquadFilter notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
biquadFilter notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
biquadFilter lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
+
+biquadFilter highpass2( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
+biquadFilter notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
+biquadFilter notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
+biquadFilter lowpass2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
+
+biquadFilter highpass3( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
+biquadFilter notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
+biquadFilter notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
+biquadFilter lowpass3( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
+
+biquadFilter highpass4( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 ); // removes DC and movement artefacts
+biquadFilter notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 ); // removes 49-51 Hz power interference
+biquadFilter notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 ); //
+biquadFilter lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 ); // EMG envelope
+
biquadFilter derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 ); // derivative filter
/*--------------------------------------------------------------------------------------------------------------------
@@ -138,7 +162,8 @@
void keep_in_range(double * in, double min, double max);
void sample_filter(void);
void control();
-void calibrate_emg(int muscle);
+void calibrate_emg();
+void emg_mvc();
void calibrate_arm(void);
void start_sampling(void);
void stop_sampling(void);
@@ -156,24 +181,22 @@
int main()
{
pc.baud(115200);
+ red=1; green=1; blue=1;
// make a menu, user has to initiate next step
- mainMenu();
- caliMenu(); // Menu function
- calibrate_arm(); //Start Calibration
- start_sampling(); //500 Hz EMG
- calibrate_emg(1); //calibrate muscle 1
- start_control(); //100 Hz control
+ //mainMenu();
+ //caliMenu(); // Menu function
+ //calibrate_arm(); //Start Calibration
+
+ calibrate_emg();
+
+ //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
}
@@ -193,11 +216,12 @@
//Filter: high-pass -> notch -> rectify -> lowpass or moving average
// Can we use same biquadFilter (eg. highpass) for other muscles or does each muscle need its own biquad?
- 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 = highpass.step(emg_biceps); triceps_power = highpass2.step(emg_triceps); flexor_power = highpass3.step(emg_flexor); extens_power = highpass4.step(emg_extens);
+ biceps_power = notch1.step(biceps_power); triceps_power = notch1_2.step(triceps_power); flexor_power = notch1_3.step(flexor_power); extens_power = notch1_4.step(extens_power);
+ biceps_power = notch2.step(biceps_power); triceps_power = notch2_2.step(triceps_power); flexor_power = notch2_3.step(flexor_power); extens_power = notch2_4.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);
+ biceps_power = lowpass.step(biceps_power); triceps_power = lowpass2.step(triceps_power); flexor_power = lowpass3.step(flexor_power); extens_power = lowpass4.step(extens_power);
+
/* alternative for lowpass filter: moving average
window=30; //30 samples
@@ -220,10 +244,22 @@
//Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
void calibrate_arm(void)
{
- bool hit = true;
+ //const double hit = 0.5;
dir_motor1=1; //ccw
dir_motor2=1; //ccw
- while(shoulder_limit != hit){
+ pwm_motor1.write(0.2f); //move upper arm slowly ccw
+ pwm_motor2.write(0.2f); //move forearm slowly ccw
+
+ if(shoulder_limit.read() < 0.5){ //when limit switches are hit, stop motor and reset encoder
+ pwm_motor1.write(0);
+ Encoder1.reset();
+ }
+ if(elbow_limit.read() < 0.5){
+ pwm_motor2.write(0);
+ Encoder2.reset();
+ }
+
+ /* while(shoulder_limit != hit){
pwm_motor1.write(0.4);
}
Encoder1.reset();
@@ -232,24 +268,27 @@
pwm_motor2.write(0.4);
}
Encoder2.reset();
+ */
}
//EMG Maximum Voluntary Contraction measurement
-void calibrate_emg(int muscle)
-{
- start_sampling();
-
+void emg_mvc()
+{
//double sampletime=0;
//sampletime=+SAMPLE_RATE;
//
// if(sampletime<5)
-
- for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
+ //int muscle=1;
+ //for(int index=0; index<2500;index++){ //measure 5 seconds@500hz = 2500 samples
+
if(muscle==1){
if(biceps_power>bicepsMVC){
+ printf("+ ");
bicepsMVC=biceps_power;
}
+ else
+ printf("- ");
}
if(muscle==2){
@@ -272,8 +311,69 @@
extensMVC=extens_power;
}
}
- }
- stop_sampling();
+
+ //}
+ calibrate_time = calibrate_time + 0.002;
+
+
+
+}
+
+//EMG calibration
+void calibrate_emg()
+{
+ Ticker timer;
+
+ pc.printf("|-- Robot Started --| \r\n");
+ pc.printf("Testcode calibration \r\n");
+ wait(1);
+ pc.printf("+ means current sample is higher than stored MVC\r\n");
+ pc.printf("- means current sample is lower than stored MVC\r\n");
+ wait(3);
+ pc.printf(" Biceps is first. "); wait(1);
+ pc.printf(" Press any key to begin... "); wait(1);
+ char input;
+ input=pc.getc();
+ pc.putc(input);
+ pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
+ pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
+ pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
+
+ start_sampling();
+ muscle=1;
+ timer.attach(&emg_mvc,0.002);
+ wait(5);
+ timer.detach();
+ pc.printf("\r\n MVC = %f \r\n",bicepsMVC);
+
+ pc.printf("Calibrate_emg() exited \r\n");
+ pc.printf("Measured time: %f seconds \r\n",calibrate_time);
+ calibrate_time=0;
+
+ // Triceps:
+ pc.printf(" Triceps is next "); wait(1);
+ pc.printf(" Press any key to begin... "); wait(1);
+ input=pc.getc();
+ pc.putc(input);
+ pc.printf(" \r\n Starting in 3... \r\n"); wait(1);
+ pc.printf(" \r\n Starting in 2... \r\n"); wait(1);
+ pc.printf(" \r\n Starting in 1... \r\n"); wait(1);
+ start_sampling();
+ muscle=1;
+ timer.attach(&emg_mvc,0.002);
+ wait(5);
+ timer.detach();
+ pc.printf("\r\n Triceps MVC = %f \r\n",tricepsMVC);
+
+ pc.printf("Calibrate_emg() exited \r\n");
+ pc.printf("Measured time: %f seconds \r\n",calibrate_time);
+ calibrate_time=0;
+
+ //Flexor:
+
+ //Extensor:
+
+ stop_sampling();
}
@@ -316,8 +416,8 @@
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));
+ dir_motor2 = u2>0 ? 1 : 0; //conditional statement, same as if else below
+ pwm_motor2.write(abs(u2));
/*if(u1 > 0)
{
@@ -347,18 +447,22 @@
void start_sampling(void)
{
sample_timer.attach(&sample_filter,SAMPLE_RATE); //500 Hz EMG
+ blue=0; green=0;
+ pc.printf("||- started sampling -|| \r\n");
}
//stop sampling
void stop_sampling(void)
{
sample_timer.detach();
+ blue=1; green=1;
+ pc.printf("||- stopped sampling -|| \r\n");
}
//Start control
void start_control(void)
{
- control_timer.attach(&control,CONTROL_RATE); //100 Hz control
+ control_timer.attach(&control,SAMPLE_RATE); //100 Hz control
}
//stop control