merged EMG and PID codes
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Revision 2:625837aa7a56, committed 2016-10-27
- Comitter:
- mefix
- Date:
- Thu Oct 27 07:54:43 2016 +0000
- Parent:
- 1:ffa6f4d78c8e
- Child:
- 3:511a14a12629
- Commit message:
- controller is now ready to bo merged with emg reference generator. For testing purposes a sign wave is added for the refference of motor 1
Changed in this revision
--- a/FastPWM.lib Wed Oct 26 10:46:50 2016 +0000 +++ b/FastPWM.lib Thu Oct 27 07:54:43 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/Sissors/code/FastPWM/#10e2e171f430 +http://mbed.org/users/Sissors/code/FastPWM/#87e38b846651
--- a/MODSERIAL.lib Wed Oct 26 10:46:50 2016 +0000 +++ b/MODSERIAL.lib Thu Oct 27 07:54:43 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/AjK/code/MODSERIAL/#ae0408ebdd68 +http://mbed.org/users/Sissors/code/MODSERIAL/#4737f8a5b018
--- a/main.cpp Wed Oct 26 10:46:50 2016 +0000 +++ b/main.cpp Thu Oct 27 07:54:43 2016 +0000 @@ -13,7 +13,7 @@ DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw Ticker control; //Ticker for processing encoder input -volatile bool control_go=false; +volatile bool controller_go=false; HIDScope scope(3); // aantal scopes in hidscope opzetten @@ -32,22 +32,22 @@ 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.0; // minimum radius of arm -const double maxRadius=0.3; // maximum radius of arm +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 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 MODSERIAL pc(USBTX,USBRX); -void activate_controller(){controller_go=true}; //activate go flag +void activate_controller(){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 ) { +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), @@ -55,24 +55,28 @@ 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 + double u=b0*v+b1*v1+b2*v2; v2=v1; v1=v; return u; } void controller(){ //function for executing controller action - //converting radius and theta to gearbox angle - ref_angle1=16*theta; - ref_angle2=-radius/pi/pulleyDiameter; + + 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; - angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive) - angle2 = Encoder2.getPulses()/res; //get number of pulses + 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); @@ -93,18 +97,15 @@ //hidsopce to check what the code does exactly scope.set(0,angle1); - scope.set(1,refangle1-angle1); - scope.set(2,m1_pwm + scope.set(1,ref_angle1-angle1); //error + scope.set(2,m1_pwm); scope.send(); - + timer=timer+0.001; } - } - -int main() -{ +int main(){ 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 @@ -113,8 +114,9 @@ control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input pc.printf("RESET\n\r"); while (true) { - if(controller_go){ + if(controller_go){ // go flag controller_go=false; controller(); + } } } \ No newline at end of file
--- a/mbed.bld Wed Oct 26 10:46:50 2016 +0000 +++ b/mbed.bld Thu Oct 27 07:54:43 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3 \ No newline at end of file +http://mbed.org/users/mbed_official/code/mbed/builds/aae6fcc7d9bb \ No newline at end of file