Tried using switch
Dependencies: FastPWM MODSERIAL QEI mbed
main_demo.cpp@2:785737b1cd38, 2018-10-31 (annotated)
- Committer:
- Mirjam
- Date:
- Wed Oct 31 12:32:16 2018 +0000
- Revision:
- 2:785737b1cd38
- Parent:
- 1:8cec72aa7728
changed volatile double setpoint x
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AppelSab | 0:755bc7c0f555 | 1 | #include "mbed.h" |
AppelSab | 0:755bc7c0f555 | 2 | #include "FastPWM.h" |
AppelSab | 0:755bc7c0f555 | 3 | #include "MODSERIAL.h" |
AppelSab | 0:755bc7c0f555 | 4 | #include "QEI.h" |
AppelSab | 0:755bc7c0f555 | 5 | #include "math.h" |
AppelSab | 0:755bc7c0f555 | 6 | |
AppelSab | 0:755bc7c0f555 | 7 | // -------------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 8 | // ----------------- SET UP ------------------------- |
AppelSab | 0:755bc7c0f555 | 9 | QEI Encoder1(D11, D10, NC, 4200) ; // Encoder motor 1, (pin 1A, pin 1B, counts/rev) |
AppelSab | 0:755bc7c0f555 | 10 | QEI Encoder2(D9, D8, NC, 4200) ; // Encoder motor 2, (pin 2A, pin 2B, counts/rev) |
AppelSab | 0:755bc7c0f555 | 11 | DigitalOut directionM1(D4); |
AppelSab | 0:755bc7c0f555 | 12 | DigitalOut directionM2(D7); |
AppelSab | 0:755bc7c0f555 | 13 | FastPWM motor1_pwm(D5); |
AppelSab | 0:755bc7c0f555 | 14 | FastPWM motor2_pwm(D6); |
AppelSab | 0:755bc7c0f555 | 15 | MODSERIAL pc(USBTX, USBRX); |
AppelSab | 0:755bc7c0f555 | 16 | |
AppelSab | 0:755bc7c0f555 | 17 | // Tickers |
AppelSab | 0:755bc7c0f555 | 18 | Ticker Demo; |
AppelSab | 0:755bc7c0f555 | 19 | Ticker Write; |
AppelSab | 0:755bc7c0f555 | 20 | |
AppelSab | 0:755bc7c0f555 | 21 | // --------------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 22 | // ----------------- GLOBAL VARIABLES ---------------- |
AppelSab | 0:755bc7c0f555 | 23 | volatile int counts1; |
AppelSab | 0:755bc7c0f555 | 24 | volatile int counts2; |
AppelSab | 0:755bc7c0f555 | 25 | volatile double theta1; |
AppelSab | 0:755bc7c0f555 | 26 | volatile double theta2; |
AppelSab | 0:755bc7c0f555 | 27 | const double pi = 3.14159265359; |
AppelSab | 0:755bc7c0f555 | 28 | volatile double error1; |
AppelSab | 0:755bc7c0f555 | 29 | volatile double error2; |
AppelSab | 0:755bc7c0f555 | 30 | double point1x = 200.0; |
AppelSab | 0:755bc7c0f555 | 31 | double point1y = 200.0; |
AppelSab | 0:755bc7c0f555 | 32 | double point2x = 350.0; |
AppelSab | 0:755bc7c0f555 | 33 | double point2y = 200.0; |
AppelSab | 0:755bc7c0f555 | 34 | double point3x = 350.0; |
AppelSab | 0:755bc7c0f555 | 35 | double point3y = 0.0; |
AppelSab | 0:755bc7c0f555 | 36 | double point4x = 200.0; |
AppelSab | 0:755bc7c0f555 | 37 | double point4y = 0.0; |
AppelSab | 0:755bc7c0f555 | 38 | const double x0 = 80.0; //zero x position after homing |
AppelSab | 0:755bc7c0f555 | 39 | const double y0 = 141.0; //zero y position after homing |
Mirjam | 2:785737b1cd38 | 40 | double setpointx = x0; |
Mirjam | 2:785737b1cd38 | 41 | double setpointy = y0; |
AppelSab | 0:755bc7c0f555 | 42 | volatile double U1; |
AppelSab | 0:755bc7c0f555 | 43 | volatile double U2; |
AppelSab | 0:755bc7c0f555 | 44 | |
AppelSab | 0:755bc7c0f555 | 45 | // Inverse Kinematics |
AppelSab | 0:755bc7c0f555 | 46 | volatile double q1_diff; |
AppelSab | 0:755bc7c0f555 | 47 | volatile double q2_diff; |
AppelSab | 0:755bc7c0f555 | 48 | double sq = 2.0; //to square numbers |
AppelSab | 0:755bc7c0f555 | 49 | const double L1 = 250.0; //length of the first link |
AppelSab | 0:755bc7c0f555 | 50 | const double L3 = 350.0; //length of the second link |
AppelSab | 0:755bc7c0f555 | 51 | |
AppelSab | 0:755bc7c0f555 | 52 | // Reference angles of the starting position |
AppelSab | 0:755bc7c0f555 | 53 | double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); |
AppelSab | 0:755bc7c0f555 | 54 | double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq)))); |
AppelSab | 0:755bc7c0f555 | 55 | double q2_0_enc = q2_0 + q1_0; |
AppelSab | 0:755bc7c0f555 | 56 | |
AppelSab | 0:755bc7c0f555 | 57 | // -------------------------------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 58 | // ---------------Read out encoders------------------------------------ |
AppelSab | 0:755bc7c0f555 | 59 | // -------------------------------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 60 | double counts2angle1() |
AppelSab | 0:755bc7c0f555 | 61 | { |
AppelSab | 0:755bc7c0f555 | 62 | counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1 |
AppelSab | 0:755bc7c0f555 | 63 | theta1 = -(double(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1 |
AppelSab | 0:755bc7c0f555 | 64 | return theta1; |
AppelSab | 0:755bc7c0f555 | 65 | } |
AppelSab | 0:755bc7c0f555 | 66 | |
AppelSab | 0:755bc7c0f555 | 67 | double counts2angle2() |
AppelSab | 0:755bc7c0f555 | 68 | { |
AppelSab | 0:755bc7c0f555 | 69 | counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2 |
AppelSab | 0:755bc7c0f555 | 70 | theta2 = (double(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2 |
AppelSab | 0:755bc7c0f555 | 71 | return theta2; |
AppelSab | 0:755bc7c0f555 | 72 | } |
AppelSab | 0:755bc7c0f555 | 73 | |
AppelSab | 0:755bc7c0f555 | 74 | // ------------------------------------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 75 | // -------------- Determine Setpoints -------------------------------------- |
AppelSab | 0:755bc7c0f555 | 76 | // ------------------------------------------------------------------------- |
Mirjam | 1:8cec72aa7728 | 77 | void determinedemosetpoints( double &setpointx, double &setpointy) |
AppelSab | 0:755bc7c0f555 | 78 | { |
Mirjam | 1:8cec72aa7728 | 79 | int track = 1; |
Mirjam | 1:8cec72aa7728 | 80 | if( fabs(setpointx - point1x) <= 0.2 && fabs(setpointy - point1y) <= 0.2){ |
Mirjam | 1:8cec72aa7728 | 81 | track = 2; |
Mirjam | 2:785737b1cd38 | 82 | pc.printf("We gaan nu naar setpoint %i",track); |
Mirjam | 1:8cec72aa7728 | 83 | } |
Mirjam | 1:8cec72aa7728 | 84 | if( fabs(setpointx - point2x) <= 0.2 && fabs(setpointy - point2y) <= 0.2){ |
Mirjam | 1:8cec72aa7728 | 85 | track = 3; |
Mirjam | 2:785737b1cd38 | 86 | pc.printf("We gaan nu naar setpoint %i", track); |
Mirjam | 1:8cec72aa7728 | 87 | } |
Mirjam | 1:8cec72aa7728 | 88 | if( fabs(setpointx - point3x) <= 0.2 && fabs(setpointy - point3y) <= 0.2){ |
Mirjam | 1:8cec72aa7728 | 89 | track = 4; |
Mirjam | 2:785737b1cd38 | 90 | pc.printf("We gaan nu naar setpoint %i",track); |
Mirjam | 1:8cec72aa7728 | 91 | } |
AppelSab | 0:755bc7c0f555 | 92 | |
Mirjam | 1:8cec72aa7728 | 93 | switch(track){ |
Mirjam | 1:8cec72aa7728 | 94 | case 1: |
Mirjam | 2:785737b1cd38 | 95 | setpointx = setpointx + (point1x - x0)/150; |
Mirjam | 2:785737b1cd38 | 96 | setpointy = setpointy + (point1y - y0)/150; |
Mirjam | 1:8cec72aa7728 | 97 | break; |
Mirjam | 1:8cec72aa7728 | 98 | |
Mirjam | 1:8cec72aa7728 | 99 | case 2: |
Mirjam | 1:8cec72aa7728 | 100 | setpointx = setpointx + 0.2; |
Mirjam | 1:8cec72aa7728 | 101 | setpointy = setpointy; |
Mirjam | 1:8cec72aa7728 | 102 | break; |
Mirjam | 1:8cec72aa7728 | 103 | |
Mirjam | 1:8cec72aa7728 | 104 | case 3: |
Mirjam | 1:8cec72aa7728 | 105 | setpointx = setpointx; |
AppelSab | 0:755bc7c0f555 | 106 | setpointy = setpointy - 0.2; |
Mirjam | 1:8cec72aa7728 | 107 | break; |
Mirjam | 1:8cec72aa7728 | 108 | |
Mirjam | 1:8cec72aa7728 | 109 | case 4: |
Mirjam | 1:8cec72aa7728 | 110 | setpointx = setpointx - 0.2; |
AppelSab | 0:755bc7c0f555 | 111 | setpointy = setpointy; |
Mirjam | 1:8cec72aa7728 | 112 | break; |
Mirjam | 1:8cec72aa7728 | 113 | } |
AppelSab | 0:755bc7c0f555 | 114 | } |
AppelSab | 0:755bc7c0f555 | 115 | // ----------------------------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 116 | // --------------------------- PI controllers ---------------------- |
AppelSab | 0:755bc7c0f555 | 117 | // ----------------------------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 118 | double PI_controller1(double error1) |
AppelSab | 0:755bc7c0f555 | 119 | { |
AppelSab | 0:755bc7c0f555 | 120 | static double error_integral1 = 0; |
AppelSab | 0:755bc7c0f555 | 121 | |
AppelSab | 0:755bc7c0f555 | 122 | // Proportional part |
AppelSab | 0:755bc7c0f555 | 123 | double Kp1 = 3.95; // Kp (proportionele controller, nu nog een random waarde) |
AppelSab | 0:755bc7c0f555 | 124 | double u_p1 = Kp1*error1; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp) |
AppelSab | 0:755bc7c0f555 | 125 | |
AppelSab | 0:755bc7c0f555 | 126 | // Integral part |
AppelSab | 0:755bc7c0f555 | 127 | double Ki1 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde) |
AppelSab | 0:755bc7c0f555 | 128 | double Ts1 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine) |
AppelSab | 0:755bc7c0f555 | 129 | error_integral1 = error_integral1 + error1 * Ts1; |
AppelSab | 0:755bc7c0f555 | 130 | double u_i1 = Ki1 * error_integral1; |
AppelSab | 0:755bc7c0f555 | 131 | |
AppelSab | 0:755bc7c0f555 | 132 | // Sum |
AppelSab | 0:755bc7c0f555 | 133 | U1 = u_p1 + u_i1; |
AppelSab | 0:755bc7c0f555 | 134 | |
AppelSab | 0:755bc7c0f555 | 135 | // Return |
AppelSab | 0:755bc7c0f555 | 136 | return U1; |
AppelSab | 0:755bc7c0f555 | 137 | } |
AppelSab | 0:755bc7c0f555 | 138 | double PI_controller2(double error2) |
AppelSab | 0:755bc7c0f555 | 139 | { |
AppelSab | 0:755bc7c0f555 | 140 | static double error_integral2 = 0; |
AppelSab | 0:755bc7c0f555 | 141 | |
AppelSab | 0:755bc7c0f555 | 142 | // Proportional part |
AppelSab | 0:755bc7c0f555 | 143 | double Kp2 = 3.95; // Kp (proportionele controller, nu nog een random waarde) |
AppelSab | 0:755bc7c0f555 | 144 | double u_p2 = Kp2*error2; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp) |
AppelSab | 0:755bc7c0f555 | 145 | |
AppelSab | 0:755bc7c0f555 | 146 | // Integral part |
AppelSab | 0:755bc7c0f555 | 147 | double Ki2 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde) |
AppelSab | 0:755bc7c0f555 | 148 | double Ts2 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine) |
AppelSab | 0:755bc7c0f555 | 149 | error_integral2 = error_integral2 + error2 * Ts2; |
AppelSab | 0:755bc7c0f555 | 150 | double u_i2 = Ki2 * error_integral2; |
AppelSab | 0:755bc7c0f555 | 151 | |
AppelSab | 0:755bc7c0f555 | 152 | // Sum + |
AppelSab | 0:755bc7c0f555 | 153 | U2 = u_p2 + u_i2; |
AppelSab | 0:755bc7c0f555 | 154 | |
AppelSab | 0:755bc7c0f555 | 155 | // Return |
AppelSab | 0:755bc7c0f555 | 156 | return U2; |
AppelSab | 0:755bc7c0f555 | 157 | } |
AppelSab | 0:755bc7c0f555 | 158 | // ------------------------------------------------------------ |
AppelSab | 0:755bc7c0f555 | 159 | // ------------ Inverse Kinematics ---------------------------- |
AppelSab | 0:755bc7c0f555 | 160 | // ------------------------------------------------------------ |
AppelSab | 0:755bc7c0f555 | 161 | double makeAngleq1(double x, double y){ |
AppelSab | 0:755bc7c0f555 | 162 | double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration |
AppelSab | 0:755bc7c0f555 | 163 | q1_diff = -2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint |
AppelSab | 0:755bc7c0f555 | 164 | return q1_diff; |
AppelSab | 0:755bc7c0f555 | 165 | } |
AppelSab | 0:755bc7c0f555 | 166 | |
AppelSab | 0:755bc7c0f555 | 167 | double makeAngleq2(double x, double y){ |
AppelSab | 0:755bc7c0f555 | 168 | double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration |
AppelSab | 0:755bc7c0f555 | 169 | double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration |
AppelSab | 0:755bc7c0f555 | 170 | double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done |
AppelSab | 0:755bc7c0f555 | 171 | q2_diff = (2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint |
AppelSab | 0:755bc7c0f555 | 172 | return -q2_diff; |
AppelSab | 0:755bc7c0f555 | 173 | } |
AppelSab | 0:755bc7c0f555 | 174 | |
AppelSab | 0:755bc7c0f555 | 175 | |
AppelSab | 0:755bc7c0f555 | 176 | // ----------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 177 | // ------------ RUN MOTORS ----------------------- |
AppelSab | 0:755bc7c0f555 | 178 | // ----------------------------------------------- |
AppelSab | 0:755bc7c0f555 | 179 | void motoraansturing() |
AppelSab | 0:755bc7c0f555 | 180 | { |
Mirjam | 2:785737b1cd38 | 181 | determinedemosetpoints(setpointx, setpointy); |
Mirjam | 2:785737b1cd38 | 182 | |
AppelSab | 0:755bc7c0f555 | 183 | q1_diff = makeAngleq1(setpointx, setpointy); |
AppelSab | 0:755bc7c0f555 | 184 | q2_diff = makeAngleq2(setpointx, setpointy); |
AppelSab | 0:755bc7c0f555 | 185 | |
AppelSab | 0:755bc7c0f555 | 186 | theta2 = counts2angle2(); |
AppelSab | 0:755bc7c0f555 | 187 | error2 = q2_diff - theta2; |
AppelSab | 0:755bc7c0f555 | 188 | theta1 = counts2angle1(); |
AppelSab | 0:755bc7c0f555 | 189 | error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as. |
AppelSab | 0:755bc7c0f555 | 190 | |
AppelSab | 0:755bc7c0f555 | 191 | U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt. |
AppelSab | 0:755bc7c0f555 | 192 | U2 = PI_controller2(error2); |
AppelSab | 0:755bc7c0f555 | 193 | |
AppelSab | 0:755bc7c0f555 | 194 | motor1_pwm.write(fabs(U1)); // Motor aansturen |
AppelSab | 0:755bc7c0f555 | 195 | directionM1 = U1 > 0.0f; // Richting van de motor bepalen |
AppelSab | 0:755bc7c0f555 | 196 | motor2_pwm.write(fabs(U2)); |
AppelSab | 0:755bc7c0f555 | 197 | directionM2 = U2 > 0.0f; |
AppelSab | 0:755bc7c0f555 | 198 | } |
AppelSab | 0:755bc7c0f555 | 199 | |
AppelSab | 0:755bc7c0f555 | 200 | |
AppelSab | 0:755bc7c0f555 | 201 | void rundemo() |
AppelSab | 0:755bc7c0f555 | 202 | { |
AppelSab | 0:755bc7c0f555 | 203 | motoraansturing(); |
AppelSab | 0:755bc7c0f555 | 204 | } |
AppelSab | 0:755bc7c0f555 | 205 | |
AppelSab | 0:755bc7c0f555 | 206 | |
AppelSab | 0:755bc7c0f555 | 207 | int main() |
AppelSab | 0:755bc7c0f555 | 208 | { |
AppelSab | 0:755bc7c0f555 | 209 | pc.baud(115200); |
AppelSab | 0:755bc7c0f555 | 210 | motor1_pwm.period_us(60); // Period is 60 microseconde |
AppelSab | 0:755bc7c0f555 | 211 | motor2_pwm.period_us(60); |
AppelSab | 0:755bc7c0f555 | 212 | Demo.attach(&rundemo, 0.005f); |
AppelSab | 0:755bc7c0f555 | 213 | |
AppelSab | 0:755bc7c0f555 | 214 | while (true) { |
AppelSab | 0:755bc7c0f555 | 215 | |
AppelSab | 0:755bc7c0f555 | 216 | } |
AppelSab | 0:755bc7c0f555 | 217 | } |