Arnoud Domhof / Mbed 2 deprecated project_demomode

Laatste versie van Arnoud, Data wordt geplot

Dependencies:   FastPWM MODSERIAL QEI mbed

main_demo.cpp@3:fd1380ffb200, 2018-10-31 (annotated)

Committer:
arnouddomhof
Date:
Wed Oct 31 17:06:04 2018 +0000
Revision:
3:fd1380ffb200
Parent:
2:7e86ced5841f
Child:
4:98b3dfdd9ae1
Nu print de code de setpoints uit en naar welk punt de robot gaat. Hieruit blijkt dat de setpoints wel worden aangepast, maar dat er verder niets mee wordt gedaan.

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 2:7e86ced5841f 16 DigitalOut ledr(LED_RED);
AppelSab 2:7e86ced5841f 17 DigitalOut ledg(LED_GREEN);
AppelSab 2:7e86ced5841f 18 DigitalOut ledb(LED_BLUE);
AppelSab 0:755bc7c0f555 19
AppelSab 0:755bc7c0f555 20 // Tickers
AppelSab 0:755bc7c0f555 21 Ticker Demo;
AppelSab 0:755bc7c0f555 22 Ticker Write;
AppelSab 0:755bc7c0f555 23
AppelSab 0:755bc7c0f555 24 // ---------------------------------------------------
AppelSab 0:755bc7c0f555 25 // ----------------- GLOBAL VARIABLES ----------------
AppelSab 0:755bc7c0f555 26 volatile int counts1;
AppelSab 0:755bc7c0f555 27 volatile int counts2;
AppelSab 0:755bc7c0f555 28 volatile double theta1;
AppelSab 0:755bc7c0f555 29 volatile double theta2;
AppelSab 0:755bc7c0f555 30 const double pi = 3.14159265359;
AppelSab 0:755bc7c0f555 31 volatile double error1;
AppelSab 0:755bc7c0f555 32 volatile double error2;
AppelSab 0:755bc7c0f555 33 double point1x = 200.0;
AppelSab 0:755bc7c0f555 34 double point1y = 200.0;
AppelSab 2:7e86ced5841f 35 double point2x = 200.0;
AppelSab 2:7e86ced5841f 36 double point2y = 100.0;
arnouddomhof 3:fd1380ffb200 37 //double point3x = 350.0;
arnouddomhof 3:fd1380ffb200 38 //double point3y = 0.0;
arnouddomhof 3:fd1380ffb200 39 //double point4x = 200.0;
arnouddomhof 3:fd1380ffb200 40 //double point4y = 0.0;
arnouddomhof 3:fd1380ffb200 41 volatile int track = 1;
AppelSab 0:755bc7c0f555 42 const double x0 = 80.0; //zero x position after homing
AppelSab 0:755bc7c0f555 43 const double y0 = 141.0; //zero y position after homing
AppelSab 0:755bc7c0f555 44 volatile double setpointx = x0;
AppelSab 0:755bc7c0f555 45 volatile double setpointy = y0;
AppelSab 0:755bc7c0f555 46 volatile double U1;
AppelSab 0:755bc7c0f555 47 volatile double U2;
AppelSab 0:755bc7c0f555 48
AppelSab 0:755bc7c0f555 49 // Inverse Kinematics
AppelSab 0:755bc7c0f555 50 volatile double q1_diff;
AppelSab 0:755bc7c0f555 51 volatile double q2_diff;
AppelSab 0:755bc7c0f555 52 double sq = 2.0; //to square numbers
AppelSab 0:755bc7c0f555 53 const double L1 = 250.0; //length of the first link
AppelSab 0:755bc7c0f555 54 const double L3 = 350.0; //length of the second link
AppelSab 0:755bc7c0f555 55
AppelSab 0:755bc7c0f555 56 // Reference angles of the starting position
AppelSab 0:755bc7c0f555 57 double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
AppelSab 0:755bc7c0f555 58 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 59 double q2_0_enc = q2_0 + q1_0;
AppelSab 0:755bc7c0f555 60
AppelSab 0:755bc7c0f555 61 // --------------------------------------------------------------------
AppelSab 0:755bc7c0f555 62 // ---------------Read out encoders------------------------------------
AppelSab 0:755bc7c0f555 63 // --------------------------------------------------------------------
AppelSab 0:755bc7c0f555 64 double counts2angle1()
AppelSab 0:755bc7c0f555 65 {
AppelSab 0:755bc7c0f555 66 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
AppelSab 0:755bc7c0f555 67 theta1 = -(double(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
AppelSab 0:755bc7c0f555 68 return theta1;
AppelSab 0:755bc7c0f555 69 }
AppelSab 0:755bc7c0f555 70
AppelSab 0:755bc7c0f555 71 double counts2angle2()
AppelSab 0:755bc7c0f555 72 {
AppelSab 0:755bc7c0f555 73 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
AppelSab 0:755bc7c0f555 74 theta2 = (double(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
AppelSab 0:755bc7c0f555 75 return theta2;
AppelSab 0:755bc7c0f555 76 }
AppelSab 0:755bc7c0f555 77
AppelSab 0:755bc7c0f555 78 // -------------------------------------------------------------------------
AppelSab 0:755bc7c0f555 79 // -------------- Determine Setpoints --------------------------------------
AppelSab 0:755bc7c0f555 80 // -------------------------------------------------------------------------
AppelSab 2:7e86ced5841f 81 void determinedemoset()
AppelSab 0:755bc7c0f555 82 {
arnouddomhof 3:fd1380ffb200 83 if ((setpointx < point1x) && (track == 1)){
AppelSab 2:7e86ced5841f 84 setpointx = setpointx + 0.2;
AppelSab 1:7afdfab34bf7 85 }
arnouddomhof 3:fd1380ffb200 86 if ((setpointy < point1y) && (track == 1)){
AppelSab 2:7e86ced5841f 87 setpointy = setpointy + 0.1;
AppelSab 2:7e86ced5841f 88 }
AppelSab 0:755bc7c0f555 89
AppelSab 1:7afdfab34bf7 90 // Van punt 1 naar punt 2.
arnouddomhof 3:fd1380ffb200 91 if ((setpointy >= point1y - 0.3) && (setpointx >= point1x - 0.3) && (setpointy <= point1y + 0.3) && (setpointx <= point1x + 0.3 && track == 1)){
arnouddomhof 3:fd1380ffb200 92 //setpointx = point2x;
arnouddomhof 3:fd1380ffb200 93 //setpointy = setpointy + (-0.2); // Van punt 1 naar punt 2 op dezelfde y blijven.
arnouddomhof 3:fd1380ffb200 94 ledr = !ledr; // Aangeven met een ROOD lampje dat hij op de plaats van bestemming is
AppelSab 1:7afdfab34bf7 95 track = 12;
AppelSab 1:7afdfab34bf7 96 }
arnouddomhof 3:fd1380ffb200 97
arnouddomhof 3:fd1380ffb200 98 if ((setpointy > point2y) && (track == 12)) {
arnouddomhof 3:fd1380ffb200 99 setpointx = point2x;
arnouddomhof 3:fd1380ffb200 100 //setpointx = setpointx + 0.2;
arnouddomhof 3:fd1380ffb200 101 setpointy = setpointy + (0.2);
arnouddomhof 3:fd1380ffb200 102 }
arnouddomhof 3:fd1380ffb200 103
arnouddomhof 3:fd1380ffb200 104 if ((fabs(setpointx - point2x) <= 0.3) && (fabs(setpointy - point2y) <= 0.3) && (track == 12)) {
arnouddomhof 3:fd1380ffb200 105 ledr = !ledr;
arnouddomhof 3:fd1380ffb200 106 ledg = !ledg;
arnouddomhof 3:fd1380ffb200 107 track = 23;
arnouddomhof 3:fd1380ffb200 108 }
arnouddomhof 3:fd1380ffb200 109
arnouddomhof 3:fd1380ffb200 110
arnouddomhof 3:fd1380ffb200 111 /**
AppelSab 2:7e86ced5841f 112 if (setpointy > point2y && track == 12){
AppelSab 2:7e86ced5841f 113 setpointx = point2x;
AppelSab 2:7e86ced5841f 114 setpointy = setpointy + (-0.2);
AppelSab 1:7afdfab34bf7 115 }
AppelSab 0:755bc7c0f555 116
AppelSab 2:7e86ced5841f 117
arnouddomhof 3:fd1380ffb200 118 // Van punt 2 naar punt 3.
AppelSab 1:7afdfab34bf7 119 if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3)
AppelSab 1:7afdfab34bf7 120 {
AppelSab 2:7e86ced5841f 121 //setpointx = setpointx - 0.2;
AppelSab 2:7e86ced5841f 122 //setpointy = setpointy;
AppelSab 2:7e86ced5841f 123 ledr = 1;
AppelSab 2:7e86ced5841f 124 ledg = 0;
AppelSab 1:7afdfab34bf7 125 track = 23;
AppelSab 1:7afdfab34bf7 126 }
AppelSab 1:7afdfab34bf7 127 if (setpointy > point3y && track == 23)
AppelSab 1:7afdfab34bf7 128 {
AppelSab 2:7e86ced5841f 129 //setpointx = setpointx - 0.2; // Van punt 1 naar punt 2 op dezelfde y blijven.
AppelSab 2:7e86ced5841f 130 //setpointy = setpointy;
AppelSab 1:7afdfab34bf7 131 track = 23;
AppelSab 1:7afdfab34bf7 132 }
arnouddomhof 3:fd1380ffb200 133
arnouddomhof 3:fd1380ffb200 134
AppelSab 0:755bc7c0f555 135 // Van punt 3 naar punt 4.
AppelSab 0:755bc7c0f555 136 if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
AppelSab 0:755bc7c0f555 137 {
AppelSab 0:755bc7c0f555 138 setpointx = setpointx - 0.1; // Van punt 1 naar punt 2 op dezelfde y blijven.
AppelSab 2:7e86ced5841f 139 setpointy = setpointy;
AppelSab 0:755bc7c0f555 140 track = 34;
AppelSab 0:755bc7c0f555 141 }
AppelSab 0:755bc7c0f555 142 if (setpointy > point3y && track == 34)
AppelSab 0:755bc7c0f555 143 {
AppelSab 0:755bc7c0f555 144 setpointx = setpointx - 0.1;
AppelSab 2:7e86ced5841f 145 setpointy = setpointy;
AppelSab 2:7e86ced5841f 146
AppelSab 0:755bc7c0f555 147 }
AppelSab 0:755bc7c0f555 148 */
arnouddomhof 3:fd1380ffb200 149
AppelSab 0:755bc7c0f555 150 }
AppelSab 0:755bc7c0f555 151
AppelSab 0:755bc7c0f555 152
AppelSab 0:755bc7c0f555 153 // -----------------------------------------------------------------
AppelSab 0:755bc7c0f555 154 // --------------------------- PI controllers ----------------------
AppelSab 0:755bc7c0f555 155 // -----------------------------------------------------------------
AppelSab 0:755bc7c0f555 156 double PI_controller1(double error1)
AppelSab 0:755bc7c0f555 157 {
AppelSab 0:755bc7c0f555 158 static double error_integral1 = 0;
AppelSab 0:755bc7c0f555 159
AppelSab 0:755bc7c0f555 160 // Proportional part
AppelSab 0:755bc7c0f555 161 double Kp1 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 162 double u_p1 = Kp1*error1; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
AppelSab 0:755bc7c0f555 163
AppelSab 0:755bc7c0f555 164 // Integral part
AppelSab 0:755bc7c0f555 165 double Ki1 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 166 double Ts1 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
AppelSab 0:755bc7c0f555 167 error_integral1 = error_integral1 + error1 * Ts1;
AppelSab 0:755bc7c0f555 168 double u_i1 = Ki1 * error_integral1;
AppelSab 0:755bc7c0f555 169
AppelSab 0:755bc7c0f555 170 // Sum
AppelSab 0:755bc7c0f555 171 U1 = u_p1 + u_i1;
AppelSab 0:755bc7c0f555 172
AppelSab 0:755bc7c0f555 173 // Return
AppelSab 0:755bc7c0f555 174 return U1;
AppelSab 0:755bc7c0f555 175 }
AppelSab 0:755bc7c0f555 176 double PI_controller2(double error2)
AppelSab 0:755bc7c0f555 177 {
AppelSab 0:755bc7c0f555 178 static double error_integral2 = 0;
AppelSab 0:755bc7c0f555 179
AppelSab 0:755bc7c0f555 180 // Proportional part
AppelSab 0:755bc7c0f555 181 double Kp2 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 182 double u_p2 = Kp2*error2; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
AppelSab 0:755bc7c0f555 183
AppelSab 0:755bc7c0f555 184 // Integral part
AppelSab 0:755bc7c0f555 185 double Ki2 = 6.0; // Ki (Integrale deel vd controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 186 double Ts2 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
AppelSab 0:755bc7c0f555 187 error_integral2 = error_integral2 + error2 * Ts2;
AppelSab 0:755bc7c0f555 188 double u_i2 = Ki2 * error_integral2;
AppelSab 0:755bc7c0f555 189
AppelSab 0:755bc7c0f555 190 // Sum +
AppelSab 0:755bc7c0f555 191 U2 = u_p2 + u_i2;
AppelSab 0:755bc7c0f555 192
AppelSab 0:755bc7c0f555 193 // Return
AppelSab 0:755bc7c0f555 194 return U2;
AppelSab 0:755bc7c0f555 195 }
AppelSab 0:755bc7c0f555 196 // ------------------------------------------------------------
AppelSab 0:755bc7c0f555 197 // ------------ Inverse Kinematics ----------------------------
AppelSab 0:755bc7c0f555 198 // ------------------------------------------------------------
AppelSab 0:755bc7c0f555 199 double makeAngleq1(double x, double y){
AppelSab 0:755bc7c0f555 200 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 201 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 202 return q1_diff;
AppelSab 0:755bc7c0f555 203 }
AppelSab 0:755bc7c0f555 204
AppelSab 0:755bc7c0f555 205 double makeAngleq2(double x, double y){
AppelSab 0:755bc7c0f555 206 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 207 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 208 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 209 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 210 return -q2_diff;
AppelSab 0:755bc7c0f555 211 }
AppelSab 0:755bc7c0f555 212
AppelSab 0:755bc7c0f555 213
AppelSab 0:755bc7c0f555 214 // -----------------------------------------------
AppelSab 0:755bc7c0f555 215 // ------------ RUN MOTORS -----------------------
AppelSab 0:755bc7c0f555 216 // -----------------------------------------------
AppelSab 0:755bc7c0f555 217 void motoraansturing()
AppelSab 0:755bc7c0f555 218 {
AppelSab 2:7e86ced5841f 219 determinedemoset();
AppelSab 0:755bc7c0f555 220 q1_diff = makeAngleq1(setpointx, setpointy);
AppelSab 0:755bc7c0f555 221 q2_diff = makeAngleq2(setpointx, setpointy);
AppelSab 0:755bc7c0f555 222
AppelSab 0:755bc7c0f555 223 theta2 = counts2angle2();
AppelSab 0:755bc7c0f555 224 error2 = q2_diff - theta2;
AppelSab 0:755bc7c0f555 225 theta1 = counts2angle1();
AppelSab 0:755bc7c0f555 226 error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as.
AppelSab 0:755bc7c0f555 227
AppelSab 0:755bc7c0f555 228 U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
AppelSab 0:755bc7c0f555 229 U2 = PI_controller2(error2);
AppelSab 0:755bc7c0f555 230
AppelSab 0:755bc7c0f555 231 motor1_pwm.write(fabs(U1)); // Motor aansturen
AppelSab 0:755bc7c0f555 232 directionM1 = U1 > 0.0f; // Richting van de motor bepalen
AppelSab 0:755bc7c0f555 233 motor2_pwm.write(fabs(U2));
AppelSab 0:755bc7c0f555 234 directionM2 = U2 > 0.0f;
AppelSab 0:755bc7c0f555 235 }
AppelSab 0:755bc7c0f555 236
AppelSab 0:755bc7c0f555 237
AppelSab 0:755bc7c0f555 238 void rundemo()
AppelSab 0:755bc7c0f555 239 {
AppelSab 0:755bc7c0f555 240 motoraansturing();
AppelSab 0:755bc7c0f555 241 }
AppelSab 0:755bc7c0f555 242
AppelSab 0:755bc7c0f555 243
AppelSab 0:755bc7c0f555 244 int main()
AppelSab 0:755bc7c0f555 245 {
AppelSab 2:7e86ced5841f 246 ledr = 1;
AppelSab 2:7e86ced5841f 247 ledg = 1;
AppelSab 2:7e86ced5841f 248 ledb = 1;
AppelSab 2:7e86ced5841f 249
AppelSab 0:755bc7c0f555 250 pc.baud(115200);
AppelSab 0:755bc7c0f555 251 motor1_pwm.period_us(60); // Period is 60 microseconde
AppelSab 0:755bc7c0f555 252 motor2_pwm.period_us(60);
AppelSab 0:755bc7c0f555 253 Demo.attach(&rundemo, 0.005f);
AppelSab 0:755bc7c0f555 254
arnouddomhof 3:fd1380ffb200 255 pc.printf("\r\n\r\nDOE HET AUB!!! \r\n\r\n");
arnouddomhof 3:fd1380ffb200 256
AppelSab 0:755bc7c0f555 257 while (true) {
arnouddomhof 3:fd1380ffb200 258 pc.printf("Setpointx: %0.2f en Setpointy: %02f \r\n", setpointx,setpointy);
arnouddomhof 3:fd1380ffb200 259 if (track == 1) {
arnouddomhof 3:fd1380ffb200 260 pc.printf("Gaat naar positie 1\r\n");
arnouddomhof 3:fd1380ffb200 261 }
arnouddomhof 3:fd1380ffb200 262 else if (track == 12) {
arnouddomhof 3:fd1380ffb200 263 pc.printf("Gaat naar positie 2\r\n");
arnouddomhof 3:fd1380ffb200 264 }
arnouddomhof 3:fd1380ffb200 265 wait(0.5f);
AppelSab 0:755bc7c0f555 266 }
AppelSab 0:755bc7c0f555 267 }