Jordi Luong / Mbed 2 deprecated Project_BioRobotics

Project BioRobotics Group 19

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@5:0d3e8694726e, 2017-10-20 (annotated)

Committer:
jordiluong
Date:
Fri Oct 20 11:04:26 2017 +0000
Revision:
5:0d3e8694726e
Parent:
4:ea7689bf97e1
Child:
7:757e95b4dc46
Added controller

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 0:80ac024b84cb 11 // SERIAL COMMUNICATION WITH PC
jordiluong 0:80ac024b84cb 12 MODSERIAL pc(USBTX, USBRX);
jordiluong 0:80ac024b84cb 13
jordiluong 0:80ac024b84cb 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 4:ea7689bf97e1 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 3:5c3edcd29448 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 3:5c3edcd29448 30
jordiluong 5:0d3e8694726e 31 // MOTOR CONTROL
jordiluong 5:0d3e8694726e 32 Ticker controllerTicker;
jordiluong 5:0d3e8694726e 33 const double controller_Ts = 1/5000; // Time step for controllerTicker; Should be between 5kHz and 10kHz [s]
jordiluong 5:0d3e8694726e 34 const double motorRatio = 131.25; // Ratio of the gearbox in the motors []
jordiluong 5:0d3e8694726e 35 const double radPerPulse = 2*pi/(32*motorRatio);
jordiluong 5:0d3e8694726e 36 // Motor 1
jordiluong 5:0d3e8694726e 37 // Controller gains
jordiluong 5:0d3e8694726e 38 const double motor1_KP = 2.5; // Position gain []
jordiluong 5:0d3e8694726e 39 const double motor1_KI = 1.0; // Integral gain []
jordiluong 5:0d3e8694726e 40 const double motor1_KD = 0.5; // Derivative gain []
jordiluong 5:0d3e8694726e 41 double motor1_err_int = 0, motor1_prev_err = 0;
jordiluong 5:0d3e8694726e 42 // Derivative filter coefficients
jordiluong 5:0d3e8694726e 43 const double motor1_f_a1 = 1.0, motor1_f_a2 = 2.0;
jordiluong 5:0d3e8694726e 44 const double motor1_f_b0 = 1.0, motor1_f_b1 = 3.0, motor1_f_b2 = 4.0;
jordiluong 5:0d3e8694726e 45 // Filter variables
jordiluong 5:0d3e8694726e 46 double motor1_f_v1 = 0, motor1_f_v2 = 0;
jordiluong 5:0d3e8694726e 47 double reference1 = 0.0; // Wanted rotation of motor 1 [rad]
jordiluong 5:0d3e8694726e 48 // Motor 2
jordiluong 5:0d3e8694726e 49 // Controller gains
jordiluong 5:0d3e8694726e 50 // Derivative filter coefficients
jordiluong 5:0d3e8694726e 51 // Filter variables
jordiluong 4:ea7689bf97e1 52
jordiluong 0:80ac024b84cb 53
jordiluong 0:80ac024b84cb 54 // FUNCTIONS
jordiluong 5:0d3e8694726e 55 // BIQUAD FILTER FOR DERIVATIVE OF ERROR
jordiluong 5:0d3e8694726e 56 double biquad(double u, double &v1, double &v2, const double a1,
jordiluong 5:0d3e8694726e 57 const double a2, const double b0, const double b1, const double b2)
jordiluong 0:80ac024b84cb 58 {
jordiluong 5:0d3e8694726e 59 double v = u - a1*v1 - a2*v2;
jordiluong 5:0d3e8694726e 60 double y = b0*v + b1*v1 + b2*v2;
jordiluong 5:0d3e8694726e 61 v2 = v1; v1 = v;
jordiluong 5:0d3e8694726e 62 return y;
jordiluong 0:80ac024b84cb 63 }
jordiluong 0:80ac024b84cb 64
jordiluong 5:0d3e8694726e 65 // P-CONTROLLER
jordiluong 5:0d3e8694726e 66 double P_Controller(double error, const double Kp)
jordiluong 0:80ac024b84cb 67 {
jordiluong 5:0d3e8694726e 68 return Kp * error;
jordiluong 0:80ac024b84cb 69 }
jordiluong 0:80ac024b84cb 70
jordiluong 5:0d3e8694726e 71 // PID-CONTROLLER WITH FILTER
jordiluong 5:0d3e8694726e 72 double PID_Controller(double e, const double Kp, const double Ki,
jordiluong 5:0d3e8694726e 73 const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1,
jordiluong 5:0d3e8694726e 74 double &f_v2, const double f_a1, const double f_a2, const double f_b0,
jordiluong 5:0d3e8694726e 75 const double f_b1, const double f_b2)
jordiluong 0:80ac024b84cb 76 {
jordiluong 5:0d3e8694726e 77 // Derivative
jordiluong 5:0d3e8694726e 78 double e_der = (e - e_prev)/Ts;
jordiluong 5:0d3e8694726e 79 e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
jordiluong 5:0d3e8694726e 80 e_prev = e;
jordiluong 5:0d3e8694726e 81 // Integral
jordiluong 5:0d3e8694726e 82 e_int = e_int + Ts*e;
jordiluong 5:0d3e8694726e 83 // PID
jordiluong 5:0d3e8694726e 84 return Kp*e + Ki*e_int + Kd*e_der;
jordiluong 3:5c3edcd29448 85 }
jordiluong 3:5c3edcd29448 86
jordiluong 5:0d3e8694726e 87 // MOTOR 1
jordiluong 5:0d3e8694726e 88 void RotateMotor1(double motor1Value)
jordiluong 3:5c3edcd29448 89 {
jordiluong 5:0d3e8694726e 90 if(currentState == MOVING) // Checks if state is MOVING
jordiluong 5:0d3e8694726e 91 {
jordiluong 5:0d3e8694726e 92 if(motor1Value >= 0) motor1DirectionPin = 1; // Rotate motor 1 CW
jordiluong 5:0d3e8694726e 93 else motor1DirectionPin = 0; // Rotate motor 1 CCW
jordiluong 5:0d3e8694726e 94
jordiluong 5:0d3e8694726e 95 if(fabs(motor1Value) > 1) motor1MagnitudePin = 1;
jordiluong 5:0d3e8694726e 96 else motor1MagnitudePin = fabs(motor1Value);
jordiluong 5:0d3e8694726e 97 }
jordiluong 5:0d3e8694726e 98 else motor1MagnitudePin = 0;
jordiluong 3:5c3edcd29448 99 }
jordiluong 0:80ac024b84cb 100
jordiluong 5:0d3e8694726e 101 // MOTOR 1 P-CONTROLLER
jordiluong 5:0d3e8694726e 102 void Motor1PController()
jordiluong 5:0d3e8694726e 103 {
jordiluong 5:0d3e8694726e 104 double position1 = radPerPulse * Encoder1.getPulses(); // Calculates the rotation of motor 1
jordiluong 5:0d3e8694726e 105 double motor1Value = P_Controller(reference1 - position1, motor1_KP);
jordiluong 5:0d3e8694726e 106 RotateMotor1(motor1Value);
jordiluong 5:0d3e8694726e 107 }
jordiluong 5:0d3e8694726e 108
jordiluong 5:0d3e8694726e 109 // MOTOR 1 PID-CONTROLLER
jordiluong 5:0d3e8694726e 110 void Motor1Controller()
jordiluong 5:0d3e8694726e 111 {
jordiluong 5:0d3e8694726e 112 double position1 = radPerPulse * Encoder1.getPulses();
jordiluong 5:0d3e8694726e 113 double motor1Value = PID_Controller(reference1 - position1, motor1_KP,
jordiluong 5:0d3e8694726e 114 motor1_KI, motor1_KD, controller_Ts, motor1_err_int, motor1_prev_err,
jordiluong 5:0d3e8694726e 115 motor1_f_v1, motor1_f_v2, motor1_f_a1, motor1_f_a2, motor1_f_b0,
jordiluong 5:0d3e8694726e 116 motor1_f_b1, motor1_f_b2);
jordiluong 5:0d3e8694726e 117 RotateMotor1(motor1Value);
jordiluong 5:0d3e8694726e 118 }
jordiluong 5:0d3e8694726e 119
jordiluong 5:0d3e8694726e 120 // TURN OFF MOTORS
jordiluong 5:0d3e8694726e 121 void TurnMotorsOff()
jordiluong 5:0d3e8694726e 122 {
jordiluong 5:0d3e8694726e 123 motor1MagnitudePin = 0;
jordiluong 5:0d3e8694726e 124 motor2MagnitudePin = 0;
jordiluong 5:0d3e8694726e 125 }
jordiluong 5:0d3e8694726e 126
jordiluong 5:0d3e8694726e 127 // STATES
jordiluong 0:80ac024b84cb 128 void ProcessStateMachine()
jordiluong 0:80ac024b84cb 129 {
jordiluong 0:80ac024b84cb 130 switch(currentState)
jordiluong 0:80ac024b84cb 131 {
jordiluong 0:80ac024b84cb 132 case MOTORS_OFF:
jordiluong 0:80ac024b84cb 133 {
jordiluong 0:80ac024b84cb 134 // State initialization
jordiluong 0:80ac024b84cb 135 if(stateChanged)
jordiluong 0:80ac024b84cb 136 {
jordiluong 0:80ac024b84cb 137 pc.printf("Entering MOTORS_OFF \r\n");
jordiluong 5:0d3e8694726e 138 TurnMotorsOff(); // Turn motors off
jordiluong 0:80ac024b84cb 139 stateChanged = false;
jordiluong 0:80ac024b84cb 140 }
jordiluong 0:80ac024b84cb 141
jordiluong 0:80ac024b84cb 142 // Home command
jordiluong 4:ea7689bf97e1 143 if(!button1)
jordiluong 0:80ac024b84cb 144 {
jordiluong 0:80ac024b84cb 145 currentState = MOVING;
jordiluong 0:80ac024b84cb 146 stateChanged = true;
jordiluong 0:80ac024b84cb 147 break;
jordiluong 0:80ac024b84cb 148 }
jordiluong 4:ea7689bf97e1 149 break;
jordiluong 0:80ac024b84cb 150 }
jordiluong 0:80ac024b84cb 151
jordiluong 0:80ac024b84cb 152 case MOVING:
jordiluong 0:80ac024b84cb 153 {
jordiluong 0:80ac024b84cb 154 // State initialization
jordiluong 0:80ac024b84cb 155 if(stateChanged)
jordiluong 0:80ac024b84cb 156 {
jordiluong 0:80ac024b84cb 157 pc.printf("Entering MOVING \r\n");
jordiluong 0:80ac024b84cb 158 stateChanged = false;
jordiluong 0:80ac024b84cb 159 }
jordiluong 0:80ac024b84cb 160
jordiluong 0:80ac024b84cb 161 // Hit command
jordiluong 5:0d3e8694726e 162 if(!button1)
jordiluong 0:80ac024b84cb 163 {
jordiluong 0:80ac024b84cb 164 currentState = HITTING;
jordiluong 0:80ac024b84cb 165 stateChanged = true;
jordiluong 0:80ac024b84cb 166 break;
jordiluong 0:80ac024b84cb 167 }
jordiluong 5:0d3e8694726e 168
jordiluong 4:ea7689bf97e1 169 break;
jordiluong 0:80ac024b84cb 170 }
jordiluong 0:80ac024b84cb 171
jordiluong 0:80ac024b84cb 172 case HITTING:
jordiluong 0:80ac024b84cb 173 {
jordiluong 0:80ac024b84cb 174 // State initialization
jordiluong 0:80ac024b84cb 175 if(stateChanged)
jordiluong 0:80ac024b84cb 176 {
jordiluong 0:80ac024b84cb 177 pc.printf("Entering HITTING \r\n");
jordiluong 5:0d3e8694726e 178 stateChanged = false;
jordiluong 4:ea7689bf97e1 179 //HitBall(); // Hit the ball
jordiluong 0:80ac024b84cb 180 currentState = MOVING;
jordiluong 0:80ac024b84cb 181 stateChanged = true;
jordiluong 0:80ac024b84cb 182 break;
jordiluong 0:80ac024b84cb 183 }
jordiluong 4:ea7689bf97e1 184 break;
jordiluong 0:80ac024b84cb 185 }
jordiluong 0:80ac024b84cb 186
jordiluong 0:80ac024b84cb 187 default:
jordiluong 0:80ac024b84cb 188 {
jordiluong 5:0d3e8694726e 189 TurnMotorsOff(); // Turn motors off for safety
jordiluong 4:ea7689bf97e1 190 break;
jordiluong 0:80ac024b84cb 191 }
jordiluong 0:80ac024b84cb 192 }
jordiluong 0:80ac024b84cb 193 }
jordiluong 0:80ac024b84cb 194
jordiluong 5:0d3e8694726e 195 // MAIN FUNCTION
jordiluong 0:80ac024b84cb 196 int main()
jordiluong 0:80ac024b84cb 197 {
jordiluong 0:80ac024b84cb 198 // Serial communication
jordiluong 0:80ac024b84cb 199 pc.baud(115200);
jordiluong 0:80ac024b84cb 200
jordiluong 4:ea7689bf97e1 201 pc.printf("START \r\n");
jordiluong 4:ea7689bf97e1 202
jordiluong 5:0d3e8694726e 203 controllerTicker.attach(&Motor1PController, controller_Ts); // Ticker to control motor 1
jordiluong 5:0d3e8694726e 204 //controllerTicker.attach(&Motor1Controller, controller_Ts);
jordiluong 4:ea7689bf97e1 205
jordiluong 0:80ac024b84cb 206 while(true)
jordiluong 0:80ac024b84cb 207 {
jordiluong 0:80ac024b84cb 208 ProcessStateMachine(); // Execute states function
jordiluong 0:80ac024b84cb 209 }
jordiluong 0:80ac024b84cb 210 }