Rick Poppe / Mbed 2 deprecated Biorobotics

merged EMG and PID codes

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@1:ffa6f4d78c8e, 2016-10-26 (annotated)

Committer:
mefix
Date:
Wed Oct 26 10:46:50 2016 +0000
Revision:
1:ffa6f4d78c8e
Parent:
0:d02f4c7e8906
Child:
2:625837aa7a56
limiting part is removed and placed in the emg part

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 0:d02f4c7e8906 16 volatile bool control_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 1:ffa6f4d78c8e 35 const double minRadius=0.0; // minimum radius of arm
mefix 1:ffa6f4d78c8e 36 const double maxRadius=0.3; // 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 0:d02f4c7e8906 43
mefix 0:d02f4c7e8906 44
mefix 0:d02f4c7e8906 45 MODSERIAL pc(USBTX,USBRX);
mefix 0:d02f4c7e8906 46
mefix 0:d02f4c7e8906 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 0:d02f4c7e8906 50 const double Ts, const double N, double &v1, double &v2 ) {
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 0:d02f4c7e8906 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 0:d02f4c7e8906 65 //converting radius and theta to gearbox angle
mefix 0:d02f4c7e8906 66 ref_angle1=16*theta;
mefix 1:ffa6f4d78c8e 67 ref_angle2=-radius/pi/pulleyDiameter;
mefix 0:d02f4c7e8906 68
mefix 0:d02f4c7e8906 69
mefix 0:d02f4c7e8906 70 angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
mefix 0:d02f4c7e8906 71 angle2 = Encoder2.getPulses()/res; //get number of pulses
mefix 0:d02f4c7e8906 72 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 73 //divide by voltage to get pwm duty cycle percentage)
mefix 0:d02f4c7e8906 74 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 75
mefix 0:d02f4c7e8906 76 if (m1_pwm >=0.0f && m1_pwm <=1.0f){
mefix 0:d02f4c7e8906 77 motor1dir=0;
mefix 0:d02f4c7e8906 78 motor1.write(m1_pwm);
mefix 0:d02f4c7e8906 79 }
mefix 0:d02f4c7e8906 80 else if (m1_pwm < 0.0f && m1_pwm >= -1.0f){
mefix 0:d02f4c7e8906 81 motor1dir=1;
mefix 0:d02f4c7e8906 82 motor1.write(-m1_pwm);
mefix 0:d02f4c7e8906 83 }
mefix 0:d02f4c7e8906 84
mefix 0:d02f4c7e8906 85 if (m2_pwm >=0.0f && m2_pwm <=1.0f){
mefix 0:d02f4c7e8906 86 motor1dir=0;
mefix 0:d02f4c7e8906 87 motor1.write(m2_pwm);
mefix 0:d02f4c7e8906 88 }
mefix 0:d02f4c7e8906 89 else if (m2_pwm < 0.0f && m2_pwm >= -1.0f){
mefix 0:d02f4c7e8906 90 motor1dir=1;
mefix 0:d02f4c7e8906 91 motor1.write(-m2_pwm);
mefix 0:d02f4c7e8906 92 }
mefix 0:d02f4c7e8906 93
mefix 0:d02f4c7e8906 94 //hidsopce to check what the code does exactly
mefix 0:d02f4c7e8906 95 scope.set(0,angle1);
mefix 0:d02f4c7e8906 96 scope.set(1,refangle1-angle1);
mefix 0:d02f4c7e8906 97 scope.set(2,m1_pwm
mefix 0:d02f4c7e8906 98 scope.send();
mefix 0:d02f4c7e8906 99
mefix 0:d02f4c7e8906 100
mefix 0:d02f4c7e8906 101 }
mefix 0:d02f4c7e8906 102
mefix 0:d02f4c7e8906 103
mefix 0:d02f4c7e8906 104 }
mefix 0:d02f4c7e8906 105
mefix 0:d02f4c7e8906 106 int main()
mefix 0:d02f4c7e8906 107 {
mefix 0:d02f4c7e8906 108 pc.baud(115200);
mefix 0:d02f4c7e8906 109 motor1.period(0.02f); //period of pwm signal for motor 1
mefix 0:d02f4c7e8906 110 motor2.period(0.02f); // period of pwm signal for motor 2
mefix 0:d02f4c7e8906 111 motor1dir=0; // setting direction to ccw
mefix 0:d02f4c7e8906 112 motor2dir=0; // setting direction to ccw
mefix 1:ffa6f4d78c8e 113 control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
mefix 0:d02f4c7e8906 114 pc.printf("RESET\n\r");
mefix 0:d02f4c7e8906 115 while (true) {
mefix 0:d02f4c7e8906 116 if(controller_go){
mefix 0:d02f4c7e8906 117 controller_go=false;
mefix 0:d02f4c7e8906 118 controller();
mefix 0:d02f4c7e8906 119 }
mefix 0:d02f4c7e8906 120 }