Rick Poppe / Mbed 2 deprecated Biorobotics

merged EMG and PID codes

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@0:d02f4c7e8906, 2016-10-26 (annotated)

Committer:
mefix
Date:
Wed Oct 26 08:45:24 2016 +0000
Revision:
0:d02f4c7e8906
Child:
1:ffa6f4d78c8e
PIDF controller

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 0:d02f4c7e8906 6 #include "BiQuad.h"
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 0:d02f4c7e8906 35 const double minRadius=0.3; // minimum radius of arm
mefix 0:d02f4c7e8906 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 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 0:d02f4c7e8906 64 void controller(){ //function for executing controller action
mefix 0:d02f4c7e8906 65 // convert ref to gearbox angle
mefix 0:d02f4c7e8906 66 double theta=atan(refy/refx);
mefix 0:d02f4c7e8906 67 double radius=sqrt(pow(refx,2)+pow(refy,2));
mefix 0:d02f4c7e8906 68
mefix 0:d02f4c7e8906 69 //limit theta
mefix 0:d02f4c7e8906 70 if (theta =< minAngle){
mefix 0:d02f4c7e8906 71 theta=minAngle
mefix 0:d02f4c7e8906 72 }
mefix 0:d02f4c7e8906 73 else if (theta => 0){
mefix 0:d02f4c7e8906 74 theta=0;
mefix 0:d02f4c7e8906 75 }
mefix 0:d02f4c7e8906 76
mefix 0:d02f4c7e8906 77 //limit radius
mefix 0:d02f4c7e8906 78 if (radius =< minRadius){
mefix 0:d02f4c7e8906 79 radius=minRadius
mefix 0:d02f4c7e8906 80 }
mefix 0:d02f4c7e8906 81 else if (radius > maxRadius){
mefix 0:d02f4c7e8906 82 radius=maxRadius;
mefix 0:d02f4c7e8906 83 }
mefix 0:d02f4c7e8906 84
mefix 0:d02f4c7e8906 85 //converting radius and theta to gearbox angle
mefix 0:d02f4c7e8906 86 ref_angle1=16*theta;
mefix 0:d02f4c7e8906 87 ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
mefix 0:d02f4c7e8906 88
mefix 0:d02f4c7e8906 89
mefix 0:d02f4c7e8906 90 angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
mefix 0:d02f4c7e8906 91 angle2 = Encoder2.getPulses()/res; //get number of pulses
mefix 0:d02f4c7e8906 92 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 93 //divide by voltage to get pwm duty cycle percentage)
mefix 0:d02f4c7e8906 94 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 95
mefix 0:d02f4c7e8906 96 if (m1_pwm >=0.0f && m1_pwm <=1.0f){
mefix 0:d02f4c7e8906 97 motor1dir=0;
mefix 0:d02f4c7e8906 98 motor1.write(m1_pwm);
mefix 0:d02f4c7e8906 99 }
mefix 0:d02f4c7e8906 100 else if (m1_pwm < 0.0f && m1_pwm >= -1.0f){
mefix 0:d02f4c7e8906 101 motor1dir=1;
mefix 0:d02f4c7e8906 102 motor1.write(-m1_pwm);
mefix 0:d02f4c7e8906 103 }
mefix 0:d02f4c7e8906 104
mefix 0:d02f4c7e8906 105 if (m2_pwm >=0.0f && m2_pwm <=1.0f){
mefix 0:d02f4c7e8906 106 motor1dir=0;
mefix 0:d02f4c7e8906 107 motor1.write(m2_pwm);
mefix 0:d02f4c7e8906 108 }
mefix 0:d02f4c7e8906 109 else if (m2_pwm < 0.0f && m2_pwm >= -1.0f){
mefix 0:d02f4c7e8906 110 motor1dir=1;
mefix 0:d02f4c7e8906 111 motor1.write(-m2_pwm);
mefix 0:d02f4c7e8906 112 }
mefix 0:d02f4c7e8906 113
mefix 0:d02f4c7e8906 114 //hidsopce to check what the code does exactly
mefix 0:d02f4c7e8906 115 scope.set(0,angle1);
mefix 0:d02f4c7e8906 116 scope.set(1,refangle1-angle1);
mefix 0:d02f4c7e8906 117 scope.set(2,m1_pwm
mefix 0:d02f4c7e8906 118 scope.send();
mefix 0:d02f4c7e8906 119
mefix 0:d02f4c7e8906 120
mefix 0:d02f4c7e8906 121 }
mefix 0:d02f4c7e8906 122
mefix 0:d02f4c7e8906 123
mefix 0:d02f4c7e8906 124 }
mefix 0:d02f4c7e8906 125
mefix 0:d02f4c7e8906 126 int main()
mefix 0:d02f4c7e8906 127 {
mefix 0:d02f4c7e8906 128 pc.baud(115200);
mefix 0:d02f4c7e8906 129 motor1.period(0.02f); //period of pwm signal for motor 1
mefix 0:d02f4c7e8906 130 motor2.period(0.02f); // period of pwm signal for motor 2
mefix 0:d02f4c7e8906 131 motor1dir=0; // setting direction to ccw
mefix 0:d02f4c7e8906 132 motor2dir=0; // setting direction to ccw
mefix 0:d02f4c7e8906 133 control.attach(&controller,m1_Ts); //Ticker for processing encoder input
mefix 0:d02f4c7e8906 134 pc.printf("RESET\n\r");
mefix 0:d02f4c7e8906 135 while (true) {
mefix 0:d02f4c7e8906 136 if(controller_go){
mefix 0:d02f4c7e8906 137 controller_go=false;
mefix 0:d02f4c7e8906 138 controller();
mefix 0:d02f4c7e8906 139 }
mefix 0:d02f4c7e8906 140 }