Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@7:757e95b4dc46, 2017-10-23 (annotated)

Committer:
jordiluong
Date:
Mon Oct 23 09:42:21 2017 +0000
Revision:
7:757e95b4dc46
Parent:
5:0d3e8694726e
Child:
8:78d8ccf84a38
Added BiQuad Filter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jordiluong 0:80ac024b84cb 1 #include "BiQuad.h"
jordiluong 0:80ac024b84cb 2 #include "FastPWM.h"
jordiluong 0:80ac024b84cb 3 #include "HIDScope.h"
jordiluong 5:0d3e8694726e 4 #include <math.h>
jordiluong 0:80ac024b84cb 5 #include "mbed.h"
jordiluong 5:0d3e8694726e 6 #include "MODSERIAL.h"
jordiluong 0:80ac024b84cb 7 #include "QEI.h"
jordiluong 0:80ac024b84cb 8
jordiluong 5:0d3e8694726e 9 const double pi = 3.1415926535897; // Definition of pi
jordiluong 5:0d3e8694726e 10
jordiluong 7:757e95b4dc46 11 // SERIAL COMMUNICATION WITH PC ////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 12 MODSERIAL pc(USBTX, USBRX);
jordiluong 0:80ac024b84cb 13
jordiluong 7:757e95b4dc46 14 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 3:5c3edcd29448 15 enum states{MOTORS_OFF, MOVING, HITTING};
jordiluong 0:80ac024b84cb 16 states currentState = MOTORS_OFF; // Start with motors off
jordiluong 0:80ac024b84cb 17 bool stateChanged = true; // Make sure the initialization of first state is executed
jordiluong 0:80ac024b84cb 18
jordiluong 7:757e95b4dc46 19 // ENCODER /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 20 QEI Encoder1(D10,D11,NC,32); // CONNECT ENC1A TO D10, ENC1B TO D11
jordiluong 5:0d3e8694726e 21 QEI Encoder2(D12,D13,NC,32); // CONNECT ENC2A TO D12, ENC2B TO D13
jordiluong 4:ea7689bf97e1 22
jordiluong 7:757e95b4dc46 23 // PINS ////////////////////////////////////////////////////////////////////////
jordiluong 4:ea7689bf97e1 24 DigitalOut motor1DirectionPin(D4); // Value 0: CCW; 1: CW
jordiluong 3:5c3edcd29448 25 PwmOut motor1MagnitudePin(D5);
jordiluong 3:5c3edcd29448 26 DigitalOut motor2DirectionPin(D7); // Value 0: CW or CCW?; 1: CW or CCW?
jordiluong 3:5c3edcd29448 27 PwmOut motor2MagnitudePin(D6);
jordiluong 4:ea7689bf97e1 28 InterruptIn button1(D2); // CONNECT BUT1 TO D2
jordiluong 4:ea7689bf97e1 29 InterruptIn button2(D3); // CONNECT BUT2 TO D3
jordiluong 7:757e95b4dc46 30 AnalogIn emg(A0);
jordiluong 3:5c3edcd29448 31
jordiluong 7:757e95b4dc46 32 // MOTOR CONTROL ///////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 33 Ticker controllerTicker;
jordiluong 5:0d3e8694726e 34 const double controller_Ts = 1/5000; // Time step for controllerTicker; Should be between 5kHz and 10kHz [s]
jordiluong 5:0d3e8694726e 35 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 7:757e95b4dc46 36 const double radPerPulse = 2*pi/(32*motorRatio); // Amount of radians the motor rotates per encoder pulse [rad/pulse]
jordiluong 7:757e95b4dc46 37
jordiluong 7:757e95b4dc46 38 // MOTOR 1
jordiluong 7:757e95b4dc46 39 double reference1 = 0.0; // Desired rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 40 // Controller gains
jordiluong 5:0d3e8694726e 41 const double motor1_KP = 2.5; // Position gain []
jordiluong 5:0d3e8694726e 42 const double motor1_KI = 1.0; // Integral gain []
jordiluong 5:0d3e8694726e 43 const double motor1_KD = 0.5; // Derivative gain []
jordiluong 5:0d3e8694726e 44 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 45 // Derivative filter coefficients
jordiluong 7:757e95b4dc46 46 const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0; // Derivative filter coefficients []
jordiluong 7:757e95b4dc46 47 const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0; // Derivative filter coefficients []
jordiluong 5:0d3e8694726e 48 // Filter variables
jordiluong 5:0d3e8694726e 49 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 7:757e95b4dc46 50
jordiluong 7:757e95b4dc46 51 // MOTOR 2
jordiluong 5:0d3e8694726e 52 // Controller gains
jordiluong 5:0d3e8694726e 53 // Derivative filter coefficients
jordiluong 5:0d3e8694726e 54 // Filter variables
jordiluong 4:ea7689bf97e1 55
jordiluong 0:80ac024b84cb 56
jordiluong 7:757e95b4dc46 57 // EMG FILTER //////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 58 BiQuadChain bqc;
jordiluong 7:757e95b4dc46 59 BiQuad bq1(0, 0, 0, 0, 0); // EMG filter coefficients []
jordiluong 7:757e95b4dc46 60 BiQuad bq2(0, 0, 0, 0, 0); // EMG filter coefficients []
jordiluong 7:757e95b4dc46 61 Ticker emgSampleTicker;
jordiluong 7:757e95b4dc46 62 double emg_Ts = 0.01; // Time step for EMG sampling
jordiluong 7:757e95b4dc46 63
jordiluong 7:757e95b4dc46 64
jordiluong 7:757e95b4dc46 65 // FUNCTIONS ///////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 66 // EMG /////////////////////////////////////////////////////////////////////////
jordiluong 7:757e95b4dc46 67 void EmgSample()
jordiluong 7:757e95b4dc46 68 {
jordiluong 7:757e95b4dc46 69 double emgFiltered = bqc.step(emg.read()); // Filters the EMG signal
jordiluong 7:757e95b4dc46 70 }
jordiluong 7:757e95b4dc46 71
jordiluong 7:757e95b4dc46 72 // BIQUAD FILTER FOR DERIVATIVE OF ERROR ///////////////////////////////////////
jordiluong 5:0d3e8694726e 73 double biquad(double u, double &v1, double &v2, const double a1,
jordiluong 5:0d3e8694726e 74 const double a2, const double b0, const double b1, const double b2)
jordiluong 0:80ac024b84cb 75 {
jordiluong 5:0d3e8694726e 76 double v = u - a1*v1 - a2*v2;
jordiluong 5:0d3e8694726e 77 double y = b0*v + b1*v1 + b2*v2;
jordiluong 5:0d3e8694726e 78 v2 = v1; v1 = v;
jordiluong 5:0d3e8694726e 79 return y;
jordiluong 0:80ac024b84cb 80 }
jordiluong 0:80ac024b84cb 81
jordiluong 7:757e95b4dc46 82 // P-CONTROLLER ////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 83 double P_Controller(double error, const double Kp)
jordiluong 0:80ac024b84cb 84 {
jordiluong 5:0d3e8694726e 85 return Kp * error;
jordiluong 0:80ac024b84cb 86 }
jordiluong 0:80ac024b84cb 87
jordiluong 7:757e95b4dc46 88 // PID-CONTROLLER WITH FILTER //////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 89 double PID_Controller(double e, const double Kp, const double Ki,
jordiluong 5:0d3e8694726e 90 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
jordiluong 5:0d3e8694726e 91 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
jordiluong 5:0d3e8694726e 92 const double f_b1, const double f_b2)
jordiluong 0:80ac024b84cb 93 {
jordiluong 5:0d3e8694726e 94 // Derivative
jordiluong 7:757e95b4dc46 95 double e_der = (e - e_prev)/Ts; // Calculates the derivative of error
jordiluong 7:757e95b4dc46 96 e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); // Filter the derivative of error
jordiluong 5:0d3e8694726e 97 e_prev = e;
jordiluong 5:0d3e8694726e 98 // Integral
jordiluong 7:757e95b4dc46 99 e_int = e_int + Ts*e; // Calculates the integral of error
jordiluong 5:0d3e8694726e 100 // PID
jordiluong 5:0d3e8694726e 101 return Kp*e + Ki*e_int + Kd*e_der;
jordiluong 3:5c3edcd29448 102 }
jordiluong 3:5c3edcd29448 103
jordiluong 7:757e95b4dc46 104 // MOTOR 1 /////////////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 105 void RotateMotor1(double motor1Value)
jordiluong 3:5c3edcd29448 106 {
jordiluong 5:0d3e8694726e 107 if(currentState == MOVING) // Checks if state is MOVING
jordiluong 5:0d3e8694726e 108 {
jordiluong 5:0d3e8694726e 109 if(motor1Value >= 0) motor1DirectionPin = 1; // Rotate motor 1 CW
jordiluong 5:0d3e8694726e 110 else motor1DirectionPin = 0; // Rotate motor 1 CCW
jordiluong 5:0d3e8694726e 111
jordiluong 5:0d3e8694726e 112 if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
jordiluong 5:0d3e8694726e 113 else motor1MagnitudePin = fabs(motor1Value);
jordiluong 5:0d3e8694726e 114 }
jordiluong 5:0d3e8694726e 115 else motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 116 }
jordiluong 0:80ac024b84cb 117
jordiluong 7:757e95b4dc46 118 // MOTOR 1 P-CONTROLLER ////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 119 void Motor1PController()
jordiluong 5:0d3e8694726e 120 {
jordiluong 5:0d3e8694726e 121 double position1 = radPerPulse * Encoder1.getPulses(); // Calculates the rotation of motor 1
jordiluong 5:0d3e8694726e 122 double motor1Value = P_Controller(reference1 - position1, motor1_KP);
jordiluong 5:0d3e8694726e 123 RotateMotor1(motor1Value);
jordiluong 5:0d3e8694726e 124 }
jordiluong 5:0d3e8694726e 125
jordiluong 7:757e95b4dc46 126 // MOTOR 1 PID-CONTROLLER //////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 127 void Motor1Controller()
jordiluong 5:0d3e8694726e 128 {
jordiluong 5:0d3e8694726e 129 double position1 = radPerPulse * Encoder1.getPulses();
jordiluong 5:0d3e8694726e 130 double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
jordiluong 5:0d3e8694726e 131 motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
jordiluong 5:0d3e8694726e 132 motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
jordiluong 5:0d3e8694726e 133 motor1_f_b1, motor1_f_b2);
jordiluong 5:0d3e8694726e 134 RotateMotor1(motor1Value);
jordiluong 5:0d3e8694726e 135 }
jordiluong 5:0d3e8694726e 136
jordiluong 7:757e95b4dc46 137 // TURN OFF MOTORS /////////////////////////////////////////////////////////////
jordiluong 5:0d3e8694726e 138 void TurnMotorsOff()
jordiluong 5:0d3e8694726e 139 {
jordiluong 5:0d3e8694726e 140 motor1MagnitudePin = 0;
jordiluong 5:0d3e8694726e 141 motor2MagnitudePin = 0;
jordiluong 5:0d3e8694726e 142 }
jordiluong 5:0d3e8694726e 143
jordiluong 7:757e95b4dc46 144 // STATES //////////////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 145 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 146 {
jordiluong 0:80ac024b84cb 147 switch(currentState)
jordiluong 0:80ac024b84cb 148 {
jordiluong 0:80ac024b84cb 149 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 150 {
jordiluong 0:80ac024b84cb 151 // State initialization
jordiluong 0:80ac024b84cb 152 if(stateChanged)
jordiluong 0:80ac024b84cb 153 {
jordiluong 7:757e95b4dc46 154 pc.printf("Entering MOTORS_OFF \r\n Press button 1 to enter MOVING \r\n");
jordiluong 5:0d3e8694726e 155 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 156 stateChanged = false;
jordiluong 0:80ac024b84cb 157 }
jordiluong 0:80ac024b84cb 158
jordiluong 0:80ac024b84cb 159 // Home command
jordiluong 4:ea7689bf97e1 160 if(!button1)
jordiluong 0:80ac024b84cb 161 {
jordiluong 0:80ac024b84cb 162 currentState = MOVING;
jordiluong 0:80ac024b84cb 163 stateChanged = true;
jordiluong 0:80ac024b84cb 164 break;
jordiluong 0:80ac024b84cb 165 }
jordiluong 4:ea7689bf97e1 166 break;
jordiluong 0:80ac024b84cb 167 }
jordiluong 0:80ac024b84cb 168
jordiluong 0:80ac024b84cb 169 case MOVING:
jordiluong 0:80ac024b84cb 170 {
jordiluong 0:80ac024b84cb 171 // State initialization
jordiluong 0:80ac024b84cb 172 if(stateChanged)
jordiluong 0:80ac024b84cb 173 {
jordiluong 7:757e95b4dc46 174 pc.printf("Entering MOVING \r\n Press button 2 to enter HITTING \r\n");
jordiluong 0:80ac024b84cb 175 stateChanged = false;
jordiluong 0:80ac024b84cb 176 }
jordiluong 0:80ac024b84cb 177
jordiluong 0:80ac024b84cb 178 // Hit command
jordiluong 7:757e95b4dc46 179 if(!button2)
jordiluong 0:80ac024b84cb 180 {
jordiluong 0:80ac024b84cb 181 currentState = HITTING;
jordiluong 0:80ac024b84cb 182 stateChanged = true;
jordiluong 0:80ac024b84cb 183 break;
jordiluong 0:80ac024b84cb 184 }
jordiluong 5:0d3e8694726e 185
jordiluong 4:ea7689bf97e1 186 break;
jordiluong 0:80ac024b84cb 187 }
jordiluong 0:80ac024b84cb 188
jordiluong 0:80ac024b84cb 189 case HITTING:
jordiluong 0:80ac024b84cb 190 {
jordiluong 0:80ac024b84cb 191 // State initialization
jordiluong 0:80ac024b84cb 192 if(stateChanged)
jordiluong 0:80ac024b84cb 193 {
jordiluong 0:80ac024b84cb 194 pc.printf("Entering HITTING \r\n");
jordiluong 5:0d3e8694726e 195 stateChanged = false;
jordiluong 4:ea7689bf97e1 196 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 197 currentState = MOVING;
jordiluong 0:80ac024b84cb 198 stateChanged = true;
jordiluong 0:80ac024b84cb 199 break;
jordiluong 0:80ac024b84cb 200 }
jordiluong 4:ea7689bf97e1 201 break;
jordiluong 0:80ac024b84cb 202 }
jordiluong 0:80ac024b84cb 203
jordiluong 0:80ac024b84cb 204 default:
jordiluong 0:80ac024b84cb 205 {
jordiluong 5:0d3e8694726e 206 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 207 break;
jordiluong 0:80ac024b84cb 208 }
jordiluong 0:80ac024b84cb 209 }
jordiluong 0:80ac024b84cb 210 }
jordiluong 0:80ac024b84cb 211
jordiluong 7:757e95b4dc46 212 // MAIN FUNCTION ///////////////////////////////////////////////////////////////
jordiluong 0:80ac024b84cb 213 int main()
jordiluong 0:80ac024b84cb 214 {
jordiluong 0:80ac024b84cb 215 // Serial communication
jordiluong 0:80ac024b84cb 216 pc.baud(115200);
jordiluong 0:80ac024b84cb 217
jordiluong 4:ea7689bf97e1 218 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 219
jordiluong 7:757e95b4dc46 220 bqc.add(&bq1).add(&bq2);
jordiluong 7:757e95b4dc46 221
jordiluong 5:0d3e8694726e 222 controllerTicker.attach(&Motor1PController, controller_Ts); // Ticker to control motor 1
jordiluong 5:0d3e8694726e 223 //controllerTicker.attach(&Motor1Controller, controller_Ts);
jordiluong 7:757e95b4dc46 224 emgSampleTicker.attach(&EmgSample, emg_Ts);
jordiluong 4:ea7689bf97e1 225
jordiluong 0:80ac024b84cb 226 while(true)
jordiluong 0:80ac024b84cb 227 {
jordiluong 0:80ac024b84cb 228 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 229 }
jordiluong 0:80ac024b84cb 230 }