Rick Poppe / Mbed 2 deprecated Biorobotics

merged EMG and PID codes

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@2:625837aa7a56, 2016-10-27 (annotated)

Committer:
mefix
Date:
Thu Oct 27 07:54:43 2016 +0000
Revision:
2:625837aa7a56
Parent:
1:ffa6f4d78c8e
Child:
3:511a14a12629
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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mefix 0:d02f4c7e8906 1 #include "mbed.h"
mefix 0:d02f4c7e8906 2 #include "FastPWM.h"
mefix 0:d02f4c7e8906 3 #include "MODSERIAL.h"
mefix 0:d02f4c7e8906 4 #include "QEI.h"
mefix 0:d02f4c7e8906 5 #include "HIDScope.h" //make sure hidscope cable is also attached
mefix 1:ffa6f4d78c8e 6
mefix 0:d02f4c7e8906 7
mefix 0:d02f4c7e8906 8 // in gebruik: D(0,1,4,5,6,7,10,11,12,13)
mefix 0:d02f4c7e8906 9
mefix 0:d02f4c7e8906 10 DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
mefix 0:d02f4c7e8906 11 FastPWM motor1(D6); // speed of motor 1
mefix 0:d02f4c7e8906 12 FastPWM motor2(D5); //speed of motor 2
mefix 0:d02f4c7e8906 13 DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw
mefix 0:d02f4c7e8906 14
mefix 0:d02f4c7e8906 15 Ticker control; //Ticker for processing encoder input
mefix 2:625837aa7a56 16 volatile bool controller_go=false;
mefix 0:d02f4c7e8906 17
mefix 0:d02f4c7e8906 18 HIDScope scope(3); // aantal scopes in hidscope opzetten
mefix 0:d02f4c7e8906 19
mefix 0:d02f4c7e8906 20
mefix 0:d02f4c7e8906 21 double m1_pwm=0; //variable for PWM control motor 1
mefix 0:d02f4c7e8906 22 double m2_pwm=0; //variable for PWM control motor 2
mefix 0:d02f4c7e8906 23
mefix 0:d02f4c7e8906 24 const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 1.0, m1_N = 100; // controller constants motor 1
mefix 0:d02f4c7e8906 25 double m1_v1 = 0, m1_v2 = 0; // Memory variables
mefix 0:d02f4c7e8906 26 const double m1_Ts = 0.01; // Controller sample time
mefix 0:d02f4c7e8906 27
mefix 0:d02f4c7e8906 28 const double m2_Kp = 2.5, m2_Ki = 1.0, m2_Kd = 1.0, m2_N = 100; // controller constants motor 2
mefix 0:d02f4c7e8906 29 double m2_v1 = 0, m2_v2 = 0; // Memory variables
mefix 0:d02f4c7e8906 30 const double m2_Ts = 0.01; // Controller sample time
mefix 0:d02f4c7e8906 31
mefix 0:d02f4c7e8906 32 const double pi=3.14159265359;
mefix 0:d02f4c7e8906 33 const double res = 64/(1/131.25*2*pi); // resolution on gearbox shaft per pulse
mefix 0:d02f4c7e8906 34 const double V_max=9.0; // maximum voltage supplied by trafo
mefix 2:625837aa7a56 35 const double minRadius=0.3; // minimum radius of arm
mefix 2:625837aa7a56 36 const double maxRadius=0.6; // maximum radius of arm
mefix 0:d02f4c7e8906 37 const double pulleyDiameter=0.0398; // pulley diameter
mefix 0:d02f4c7e8906 38 const double minAngle=-1.25; // minimum angle for limiting controller
mefix 0:d02f4c7e8906 39
mefix 0:d02f4c7e8906 40 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
mefix 0:d02f4c7e8906 41 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
mefix 0:d02f4c7e8906 42
mefix 2:625837aa7a56 43 double timer=0.001; // needed for testing
mefix 0:d02f4c7e8906 44
mefix 0:d02f4c7e8906 45 MODSERIAL pc(USBTX,USBRX);
mefix 0:d02f4c7e8906 46
mefix 2:625837aa7a56 47 void activate_controller(){controller_go=true;} //activate go flag
mefix 0:d02f4c7e8906 48
mefix 0:d02f4c7e8906 49 double PID( double err, const double Kp, const double Ki, const double Kd,
mefix 2:625837aa7a56 50 const double Ts, const double N, double &v1, double &v2 ) { //discrete PIDF filter
mefix 0:d02f4c7e8906 51 const double a1 =-4/(N*Ts+2),
mefix 0:d02f4c7e8906 52 a2=-(N*Ts-2)/(N*Ts+2),
mefix 0:d02f4c7e8906 53 b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
mefix 0:d02f4c7e8906 54 b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
mefix 0:d02f4c7e8906 55 b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
mefix 0:d02f4c7e8906 56
mefix 0:d02f4c7e8906 57 double v=err-a1*v1-a2*v2;
mefix 2:625837aa7a56 58 double u=b0*v+b1*v1+b2*v2;
mefix 0:d02f4c7e8906 59 v2=v1; v1=v;
mefix 0:d02f4c7e8906 60 return u;
mefix 0:d02f4c7e8906 61 }
mefix 0:d02f4c7e8906 62
mefix 0:d02f4c7e8906 63
mefix 1:ffa6f4d78c8e 64 void controller(){ //function for executing controller action
mefix 2:625837aa7a56 65
mefix 2:625837aa7a56 66 double theta = sin(0.5*pi*timer); // just for testing
mefix 2:625837aa7a56 67 double radius = 0.3; // just for testing
mefix 2:625837aa7a56 68 //converting radius and theta to gearbox angle
mefix 2:625837aa7a56 69 double ref_angle1=16*theta;
mefix 2:625837aa7a56 70 double ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
mefix 0:d02f4c7e8906 71
mefix 0:d02f4c7e8906 72
mefix 2:625837aa7a56 73 double angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
mefix 2:625837aa7a56 74 double angle2 = Encoder2.getPulses()/res; //get number of pulses
mefix 0:d02f4c7e8906 75 m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
mefix 0:d02f4c7e8906 76 //divide by voltage to get pwm duty cycle percentage)
mefix 0:d02f4c7e8906 77 m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
mefix 0:d02f4c7e8906 78
mefix 2:625837aa7a56 79 //limit pwm value and change motor direction when pwm becomes either negative or positive
mefix 0:d02f4c7e8906 80 if (m1_pwm >=0.0f && m1_pwm <=1.0f){
mefix 0:d02f4c7e8906 81 motor1dir=0;
mefix 0:d02f4c7e8906 82 motor1.write(m1_pwm);
mefix 0:d02f4c7e8906 83 }
mefix 0:d02f4c7e8906 84 else if (m1_pwm < 0.0f && m1_pwm >= -1.0f){
mefix 0:d02f4c7e8906 85 motor1dir=1;
mefix 0:d02f4c7e8906 86 motor1.write(-m1_pwm);
mefix 0:d02f4c7e8906 87 }
mefix 0:d02f4c7e8906 88
mefix 0:d02f4c7e8906 89 if (m2_pwm >=0.0f && m2_pwm <=1.0f){
mefix 0:d02f4c7e8906 90 motor1dir=0;
mefix 0:d02f4c7e8906 91 motor1.write(m2_pwm);
mefix 0:d02f4c7e8906 92 }
mefix 0:d02f4c7e8906 93 else if (m2_pwm < 0.0f && m2_pwm >= -1.0f){
mefix 0:d02f4c7e8906 94 motor1dir=1;
mefix 0:d02f4c7e8906 95 motor1.write(-m2_pwm);
mefix 0:d02f4c7e8906 96 }
mefix 0:d02f4c7e8906 97
mefix 0:d02f4c7e8906 98 //hidsopce to check what the code does exactly
mefix 0:d02f4c7e8906 99 scope.set(0,angle1);
mefix 2:625837aa7a56 100 scope.set(1,ref_angle1-angle1); //error
mefix 2:625837aa7a56 101 scope.set(2,m1_pwm);
mefix 0:d02f4c7e8906 102 scope.send();
mefix 0:d02f4c7e8906 103
mefix 2:625837aa7a56 104 timer=timer+0.001;
mefix 0:d02f4c7e8906 105 }
mefix 0:d02f4c7e8906 106
mefix 0:d02f4c7e8906 107
mefix 2:625837aa7a56 108 int main(){
mefix 0:d02f4c7e8906 109 pc.baud(115200);
mefix 0:d02f4c7e8906 110 motor1.period(0.02f); //period of pwm signal for motor 1
mefix 0:d02f4c7e8906 111 motor2.period(0.02f); // period of pwm signal for motor 2
mefix 0:d02f4c7e8906 112 motor1dir=0; // setting direction to ccw
mefix 0:d02f4c7e8906 113 motor2dir=0; // setting direction to ccw
mefix 1:ffa6f4d78c8e 114 control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
mefix 0:d02f4c7e8906 115 pc.printf("RESET\n\r");
mefix 0:d02f4c7e8906 116 while (true) {
mefix 2:625837aa7a56 117 if(controller_go){ // go flag
mefix 0:d02f4c7e8906 118 controller_go=false;
mefix 0:d02f4c7e8906 119 controller();
mefix 2:625837aa7a56 120 }
mefix 0:d02f4c7e8906 121 }
mefix 0:d02f4c7e8906 122 }