Rick Poppe
merged EMG and PID codes
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 3:511a14a12629
- Parent:
- 2:625837aa7a56
--- a/main.cpp Thu Oct 27 07:54:43 2016 +0000
+++ b/main.cpp Thu Oct 27 08:43:37 2016 +0000
@@ -2,121 +2,304 @@
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"
-#include "HIDScope.h" //make sure hidscope cable is also attached
-
-
-// in gebruik: D(0,1,4,5,6,7,10,11,12,13)
+#include "HIDScope.h" //make sure hidscope cable is also attached
+#include "BiQuad.h"
-DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
-FastPWM motor1(D6); // speed of motor 1
-FastPWM motor2(D5); //speed of motor 2
-DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw
+MODSERIAL pc(USBTX,USBRX);
+HIDScope scope(3); // the amount of scopes to send to the computer
-Ticker control; //Ticker for processing encoder input
-volatile bool controller_go=false;
-
-HIDScope scope(3); // aantal scopes in hidscope opzetten
+// in gebruik: D(0,1,4,5,6,7,8,10,11,12,13) A(0,1,2)
-
-double m1_pwm=0; //variable for PWM control motor 1
-double m2_pwm=0; //variable for PWM control motor 2
-
-const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 1.0, m1_N = 100; // controller constants motor 1
-double m1_v1 = 0, m1_v2 = 0; // Memory variables
-const double m1_Ts = 0.01; // Controller sample time
+//Define the button interrupt for the calibration
+InterruptIn button_calibrate(PTA4);
-const double m2_Kp = 2.5, m2_Ki = 1.0, m2_Kd = 1.0, m2_N = 100; // controller constants motor 2
-double m2_v1 = 0, m2_v2 = 0; // Memory variables
-const double m2_Ts = 0.01; // Controller sample time
+AnalogIn emg1( A0 ); // first emg input
+AnalogIn emg2( A1 ); // second emg input
+AnalogIn emg3( A2 ); // third emg input
-const double pi=3.14159265359;
-const double res = 64/(1/131.25*2*pi); // resolution on gearbox shaft per pulse
-const double V_max=9.0; // maximum voltage supplied by trafo
-const double minRadius=0.3; // minimum radius of arm
-const double maxRadius=0.6; // maximum radius of arm
-const double pulleyDiameter=0.0398; // pulley diameter
-const double minAngle=-1.25; // minimum angle for limiting controller
+DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
+FastPWM motor1(D6); // speed of motor 1
+FastPWM motor2(D5); //speed of motor 2
+DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw
QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
-double timer=0.001; // needed for testing
+//Define the Tickers
+Ticker pos_timer; // the timer which is used to print the position every second
+Ticker sample_timer; // the timer which is used to decide when a sample needs to be taken
+Ticker control; //Ticker for processing encoder input
+
+//Initialize all variables
+volatile bool sampletimer = false; // a variable which is changed when a sample needs to be taken
+volatile bool controller_go=false;
+
+double threshold = 0.04; // the threshold which the emg signals need to surpass to do something
+double samplefreq=0.002; // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
+double emg02; // the first emg signal
+double emg12; // the second emg signal
+double emg22; // the third emg signal
+double ref_x=0.0000; // the x reference position
+double ref_y=0.0000; // the y reference position
+double old_ref_x; // the old x reference
+double old_ref_y; // the old y reference
+double speed=0.00002; // the variable with which a speed is reached of 1cm/s
+double theta=0.0; // angle of the arm
+double radius=0.0; // radius of the arm
+
+const double minRadius=0.3; // minimum radius of arm
+const double maxRadius=0.6; // maximum radius of arm
+const double minAngle=-1.25; // minimum angle for limiting controller
+const double min_y=-0.28; // minimum height which the spatula can reach
+char key; // variable to place the keyboard input
+
+double m1_pwm=0; //variable for PWM control motor 1
+double m2_pwm=0; //variable for PWM control motor 2
+
+const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 1.0, m1_N = 100; // controller constants motor 1
+double m1_v1 = 0, m1_v2 = 0; // Memory variables
+const double m1_Ts = 0.01; // Controller sample time
+
+const double m2_Kp = 2.5, m2_Ki = 1.0, m2_Kd = 1.0, m2_N = 100; // controller constants motor 2
+double m2_v1 = 0, m2_v2 = 0; // Memory variables
+const double m2_Ts = 0.01; // Controller sample time
+
+const double pi=3.14159265359;
+const double res = 64/(1/131.25*2*pi); // resolution on gearbox shaft per pulse
+const double V_max=9.0; // maximum voltage supplied by trafo
+const double pulleyDiameter=0.0398; // pulley diameter
+
+double timer=0.001; // needed for testing
+
+//Define the needed Biquad chains
+BiQuadChain bqc11;
+BiQuadChain bqc13;
+BiQuadChain bqc21;
+BiQuadChain bqc23;
+BiQuadChain bqc31;
+BiQuadChain bqc33;
-MODSERIAL pc(USBTX,USBRX);
+//Define the BiQuads for the filter of the first emg signal
+//Notch filter
+BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
+BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
+BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
+//High pass filter
+//BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
+BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941);
+BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450);
+BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403);
+//Low pass filter
+BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
+
+//Define the BiQuads for the filter of the second emg signal
+//Notch filter
+BiQuad bq211 = bq111;
+BiQuad bq212 = bq112;
+BiQuad bq213 = bq113;
+//High pass filter
+BiQuad bq221 = bq121;
+BiQuad bq222 = bq122;
+BiQuad bq223 = bq123;
+//Low pass filter
+BiQuad bq231 = bq131;
-void activate_controller(){controller_go=true;} //activate go flag
+//Define the BiQuads for the filter of the third emg signal
+//notch filter
+BiQuad bq311 = bq111;
+BiQuad bq312 = bq112;
+BiQuad bq313 = bq113;
+//High pass filter
+BiQuad bq321 = bq121;
+BiQuad bq323 = bq122;
+BiQuad bq322 = bq123;
+//low pass filter
+BiQuad bq331 = bq131;
+
+void sampleflag()
+{
+ if (sampletimer==true) {
+ // this if statement is used to see if the code takes too long before it is called again
+ pc.printf("rate too high error in sampleflag\n\r");
+ }
+ //This sets the go flag for when the function sample needs to be called
+ sampletimer=true;
+}
+
+void activate_controller()
+{
+ if (sampletimer==true) {
+ // this if statement is used to see if the code takes too long before it is called again
+ pc.printf("rate too high error in activate_controller\n\r");
+ }
+ controller_go=true; //activate go flag
+}
double PID( double err, const double Kp, const double Ki, const double Kd,
-const double Ts, const double N, double &v1, double &v2 ) { //discrete PIDF filter
+ const double Ts, const double N, double &v1, double &v2 ) //discrete PIDF filter
+{
const double a1 =-4/(N*Ts+2),
- a2=-(N*Ts-2)/(N*Ts+2),
- b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
- b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
- b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
+ a2=-(N*Ts-2)/(N*Ts+2),
+ b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
+ b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
+ b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
double v=err-a1*v1-a2*v2;
double u=b0*v+b1*v1+b2*v2;
- v2=v1; v1=v;
+ v2=v1;
+ v1=v;
return u;
}
-
- void controller(){ //function for executing controller action
-
- double theta = sin(0.5*pi*timer); // just for testing
- double radius = 0.3; // just for testing
- //converting radius and theta to gearbox angle
- double ref_angle1=16*theta;
- double ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
-
-
- double angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
- double angle2 = Encoder2.getPulses()/res; //get number of pulses
- m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
- //divide by voltage to get pwm duty cycle percentage)
- m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
-
- //limit pwm value and change motor direction when pwm becomes either negative or positive
- if (m1_pwm >=0.0f && m1_pwm <=1.0f){
- motor1dir=0;
- motor1.write(m1_pwm);
- }
- else if (m1_pwm < 0.0f && m1_pwm >= -1.0f){
- motor1dir=1;
- motor1.write(-m1_pwm);
- }
-
- if (m2_pwm >=0.0f && m2_pwm <=1.0f){
- motor1dir=0;
- motor1.write(m2_pwm);
- }
- else if (m2_pwm < 0.0f && m2_pwm >= -1.0f){
- motor1dir=1;
- motor1.write(-m2_pwm);
- }
-
- //hidsopce to check what the code does exactly
- scope.set(0,angle1);
- scope.set(1,ref_angle1-angle1); //error
- scope.set(2,m1_pwm);
- scope.send();
-
- timer=timer+0.001;
- }
+void calibrate()
+{
+ //This resets the reference signals so that the robot can be calibrated
+ ref_x=0.0000;
+ ref_y=0.0000;
+}
+
+void sample()
+{
+ //This checks if a key is pressed and changes the variable key in the pressed key
+ if (pc.readable()==1) {
+ key=pc.getc();
+ }
+ //Read the emg signals and filter it
+
+ emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 0
+ emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 1
+ emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 2
+
+ //remember what the reference was
+ old_ref_x=ref_x;
+ old_ref_y=ref_y;
+ //look if the emg signals go over the threshold and change the reference accordingly
+ if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
+ ref_x=ref_x-speed;
+ ref_y=ref_y-speed;
+
+ } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
+ ref_x=ref_x-speed;
+
+ } else if (emg02>threshold&&emg22>threshold || key == 's') {
+ ref_y=ref_y-speed;
+
+ } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
+ ref_x=ref_x+speed;
+ ref_y=ref_y+speed;
+
+ } else if (emg12>threshold || key == 'q' ) {
+ ref_x=ref_x+speed;
+
+ } else if (emg22>threshold || key == 'w') {
+ ref_y=ref_y+speed;
+ }
+
+ // convert the x and y reference to the theta and radius reference
+ theta=atan(ref_y/(ref_x+minRadius));
+ radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
+ //look if the new reference is outside the possible range and revert back to the old reference if it is outside the range
+ if (theta < minAngle) {
+ ref_x=old_ref_x;
+ ref_y=old_ref_y;
+ } else if (radius < minRadius) {
+ ref_x=old_ref_x;
+ ref_y=old_ref_y;
+ } else if ( radius > maxRadius) {
+ ref_x=old_ref_x;
+ ref_y=old_ref_y;
+ } else if (ref_y<min_y) {
+ ref_x=old_ref_x;
+ ref_y=old_ref_y;
+ }
+}
-int main(){
+void controller() //function for executing controller action
+{
+
+ double theta = sin(0.5*pi*timer); // just for testing
+ double radius = 0.3; // just for testing
+ //converting radius and theta to gearbox angle
+ double ref_angle1=16*theta;
+ double ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
+
+ double angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
+ double angle2 = Encoder2.getPulses()/res; //get number of pulses
+
+ //divide by voltage to get pwm duty cycle percentage)
+ m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
+ m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
+
+ //limit pwm value and change motor direction when pwm becomes either negative or positive
+ if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
+ motor1dir=0;
+ motor1.write(m1_pwm);
+ } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
+ motor1dir=1;
+ motor1.write(-m1_pwm);
+ }
+
+ if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
+ motor1dir=0;
+ motor1.write(m2_pwm);
+ } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
+ motor1dir=1;
+ motor1.write(-m2_pwm);
+ }
+
+ //hidsopce to check what the code does exactly
+ scope.set(0,angle1);
+ scope.set(1,ref_angle1-angle1); //error
+ scope.set(2,m1_pwm);
+ scope.send();
+
+ timer=timer+0.001;
+}
+
+void my_pos()
+{
+ //This function is attached to a ticker so that the reference position is printed every second.
+ pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
+
+}
+
+int main()
+{
+ pc.printf("RESET\n\r");
pc.baud(115200);
motor1.period(0.02f); //period of pwm signal for motor 1
motor2.period(0.02f); // period of pwm signal for motor 2
- motor1dir=0; // setting direction to ccw
+ motor1dir=0; // setting direction to ccw
motor2dir=0; // setting direction to ccw
control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
- pc.printf("RESET\n\r");
+
+ //Attach the Biquads to the Biquad chains
+ bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
+ bqc13.add( &bq131);
+ bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
+ bqc23.add( &bq231);
+ bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
+ bqc33.add( &bq331);
+
+ //Attach the 'sample' function to the timer 'sample_timer'.
+ //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
+ sample_timer.attach(&sampleflag, samplefreq);
+ //Attach the function calibrate to the button interrupt
+ button_calibrate.fall(&calibrate);
+ //Attach the function my_pos to the timer pos_timer.
+ //This ensures that the position is printed every second.
+ pos_timer.attach(&my_pos, 1);
while (true) {
- if(controller_go){ // go flag
- controller_go=false;
+ //Only take a sample when the go flag is true.
+ if (sampletimer==true) {
+ sample();
+ sampletimer = false; //change sampletimer to false if sample() has finished
+ }
+
+ if(controller_go) { // go flag
controller();
+ controller_go=false; //change controller_go to false if controller() has finished
}
}
}
\ No newline at end of file