Mirjam Bos / Mbed 2 deprecated project_demomode

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?

UserRevisionLine numberNew 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 }