Arnoud Domhof / Mbed 2 deprecated project_demomode

Laatste versie van Arnoud, Data wordt geplot

Dependencies:   FastPWM MODSERIAL QEI mbed

main_demo.cpp@7:2e0cdf836495, 2018-11-01 (annotated)

Committer:
arnouddomhof
Date:
Thu Nov 01 15:54:44 2018 +0000
Revision:
7:2e0cdf836495
Parent:
6:5431ddb9d881
Child:
8:088904ebb85b
Versie met PID-controller. Deze werkt nog zonder Feedback van de motor (dus alleen op setpoint).

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"
arnouddomhof 5:cd329205f037 6 //toch forken
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);
arnouddomhof 5:cd329205f037 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);
arnouddomhof 6:5431ddb9d881 19 AnalogIn potmeter1(PTC10);
arnouddomhof 6:5431ddb9d881 20 AnalogIn potmeter2(PTC11);
arnouddomhof 6:5431ddb9d881 21
AppelSab 0:755bc7c0f555 22
AppelSab 0:755bc7c0f555 23 // Tickers
AppelSab 0:755bc7c0f555 24 Ticker Demo;
AppelSab 0:755bc7c0f555 25 Ticker Write;
AppelSab 0:755bc7c0f555 26
AppelSab 0:755bc7c0f555 27 // ---------------------------------------------------
AppelSab 0:755bc7c0f555 28 // ----------------- GLOBAL VARIABLES ----------------
AppelSab 0:755bc7c0f555 29 volatile int counts1;
AppelSab 0:755bc7c0f555 30 volatile int counts2;
AppelSab 0:755bc7c0f555 31 volatile double theta1;
AppelSab 0:755bc7c0f555 32 volatile double theta2;
AppelSab 0:755bc7c0f555 33 const double pi = 3.14159265359;
AppelSab 0:755bc7c0f555 34 volatile double error1;
AppelSab 0:755bc7c0f555 35 volatile double error2;
AppelSab 0:755bc7c0f555 36 double point1x = 200.0;
AppelSab 0:755bc7c0f555 37 double point1y = 200.0;
arnouddomhof 5:cd329205f037 38 double point2x = 350.0;
arnouddomhof 5:cd329205f037 39 double point2y = 200.0;
arnouddomhof 5:cd329205f037 40 double point3x = 350.0;
arnouddomhof 6:5431ddb9d881 41 double point3y = 0;
arnouddomhof 5:cd329205f037 42 double point4x = 200.0;
arnouddomhof 6:5431ddb9d881 43 double point4y = 0;
arnouddomhof 3:fd1380ffb200 44 volatile int track = 1;
AppelSab 0:755bc7c0f555 45 const double x0 = 80.0; //zero x position after homing
AppelSab 0:755bc7c0f555 46 const double y0 = 141.0; //zero y position after homing
AppelSab 0:755bc7c0f555 47 volatile double setpointx = x0;
AppelSab 0:755bc7c0f555 48 volatile double setpointy = y0;
AppelSab 0:755bc7c0f555 49 volatile double U1;
AppelSab 0:755bc7c0f555 50 volatile double U2;
AppelSab 0:755bc7c0f555 51
arnouddomhof 7:2e0cdf836495 52 // Determine demo setpoints
arnouddomhof 7:2e0cdf836495 53 const double stepsize1 = 0.15;
arnouddomhof 7:2e0cdf836495 54 const double stepsize2 = 0.25;
arnouddomhof 7:2e0cdf836495 55 const double setpoint_error = 0.3;
arnouddomhof 7:2e0cdf836495 56
AppelSab 0:755bc7c0f555 57 // Inverse Kinematics
AppelSab 0:755bc7c0f555 58 volatile double q1_diff;
AppelSab 0:755bc7c0f555 59 volatile double q2_diff;
AppelSab 0:755bc7c0f555 60 double sq = 2.0; //to square numbers
AppelSab 0:755bc7c0f555 61 const double L1 = 250.0; //length of the first link
AppelSab 0:755bc7c0f555 62 const double L3 = 350.0; //length of the second link
AppelSab 0:755bc7c0f555 63
AppelSab 0:755bc7c0f555 64 // Reference angles of the starting position
AppelSab 0:755bc7c0f555 65 double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
AppelSab 0:755bc7c0f555 66 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))));
arnouddomhof 6:5431ddb9d881 67 double q2_0_enc = q2_0 - q1_0;
arnouddomhof 6:5431ddb9d881 68
arnouddomhof 6:5431ddb9d881 69 // Controller
arnouddomhof 6:5431ddb9d881 70 double potwaarde1;
arnouddomhof 6:5431ddb9d881 71 double pot1;
arnouddomhof 6:5431ddb9d881 72 double potwaarde2;
arnouddomhof 6:5431ddb9d881 73 double pot2;
arnouddomhof 7:2e0cdf836495 74 volatile double Kp1 = 20.0; // Kp (proportionele controller, nu nog een random waarde)
arnouddomhof 7:2e0cdf836495 75 volatile double Kp2 = 20.0; // Kp (proportionele controller, nu nog een random waarde)
arnouddomhof 7:2e0cdf836495 76 volatile double Ki1 = 6.0; // Ki (I-actie van de controller)
arnouddomhof 7:2e0cdf836495 77 volatile double Ki2 = 6.0; // Ki (I-actie van de controller)
arnouddomhof 7:2e0cdf836495 78 volatile double Kd1 = 2.0; // Kd (demping van de controller)
arnouddomhof 7:2e0cdf836495 79 volatile double Kd2 = 2.0; // Kd (demping van de controller)
arnouddomhof 6:5431ddb9d881 80
AppelSab 0:755bc7c0f555 81
AppelSab 0:755bc7c0f555 82 // --------------------------------------------------------------------
AppelSab 0:755bc7c0f555 83 // ---------------Read out encoders------------------------------------
AppelSab 0:755bc7c0f555 84 // --------------------------------------------------------------------
AppelSab 0:755bc7c0f555 85 double counts2angle1()
AppelSab 0:755bc7c0f555 86 {
AppelSab 0:755bc7c0f555 87 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
AppelSab 0:755bc7c0f555 88 theta1 = -(double(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
AppelSab 0:755bc7c0f555 89 return theta1;
AppelSab 0:755bc7c0f555 90 }
AppelSab 0:755bc7c0f555 91
AppelSab 0:755bc7c0f555 92 double counts2angle2()
AppelSab 0:755bc7c0f555 93 {
AppelSab 0:755bc7c0f555 94 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
AppelSab 0:755bc7c0f555 95 theta2 = (double(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
AppelSab 0:755bc7c0f555 96 return theta2;
AppelSab 0:755bc7c0f555 97 }
AppelSab 0:755bc7c0f555 98
AppelSab 0:755bc7c0f555 99 // -------------------------------------------------------------------------
AppelSab 0:755bc7c0f555 100 // -------------- Determine Setpoints --------------------------------------
AppelSab 0:755bc7c0f555 101 // -------------------------------------------------------------------------
arnouddomhof 5:cd329205f037 102 double determinedemosetx(double setpointx, double setpointy)
AppelSab 0:755bc7c0f555 103 {
arnouddomhof 5:cd329205f037 104
arnouddomhof 5:cd329205f037 105 if (setpointx < point1x && track == 1){
arnouddomhof 7:2e0cdf836495 106 setpointx = setpointx + stepsize1;
AppelSab 1:7afdfab34bf7 107 }
arnouddomhof 3:fd1380ffb200 108
arnouddomhof 5:cd329205f037 109 // Van punt 1 naar punt 2.
arnouddomhof 7:2e0cdf836495 110 if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)) {
arnouddomhof 5:cd329205f037 111 track = 12;
arnouddomhof 5:cd329205f037 112 }
arnouddomhof 7:2e0cdf836495 113
arnouddomhof 5:cd329205f037 114 if (setpointx < point2x && track == 12){
arnouddomhof 7:2e0cdf836495 115 setpointx = setpointx + stepsize2;
arnouddomhof 3:fd1380ffb200 116 }
arnouddomhof 3:fd1380ffb200 117
arnouddomhof 6:5431ddb9d881 118 // Van punt 2 naar punt 3.
arnouddomhof 7:2e0cdf836495 119 if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && track == 12)
AppelSab 1:7afdfab34bf7 120 {
arnouddomhof 5:cd329205f037 121 setpointx = point3x;
AppelSab 1:7afdfab34bf7 122 track = 23;
AppelSab 1:7afdfab34bf7 123 }
arnouddomhof 6:5431ddb9d881 124
AppelSab 1:7afdfab34bf7 125 if (setpointy > point3y && track == 23)
AppelSab 1:7afdfab34bf7 126 {
arnouddomhof 6:5431ddb9d881 127 setpointx = point3x; // Van punt 1 naar punt 2 op dezelfde x blijven.
AppelSab 1:7afdfab34bf7 128 }
arnouddomhof 5:cd329205f037 129
arnouddomhof 5:cd329205f037 130
AppelSab 0:755bc7c0f555 131 // Van punt 3 naar punt 4.
arnouddomhof 7:2e0cdf836495 132 if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23))
AppelSab 0:755bc7c0f555 133 {
arnouddomhof 7:2e0cdf836495 134 setpointy = point4y;
arnouddomhof 5:cd329205f037 135 track = 34;
arnouddomhof 5:cd329205f037 136 }
arnouddomhof 5:cd329205f037 137
arnouddomhof 6:5431ddb9d881 138 if (setpointx > point4x && track == 34)
arnouddomhof 5:cd329205f037 139 {
arnouddomhof 7:2e0cdf836495 140 setpointx = setpointx - stepsize2;
arnouddomhof 5:cd329205f037 141 }
arnouddomhof 6:5431ddb9d881 142
arnouddomhof 6:5431ddb9d881 143 // Van punt 4 naar punt 1.
arnouddomhof 7:2e0cdf836495 144 if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34))
arnouddomhof 5:cd329205f037 145 {
arnouddomhof 6:5431ddb9d881 146 setpointx = point4x;
arnouddomhof 6:5431ddb9d881 147 track = 41;
arnouddomhof 5:cd329205f037 148 }
arnouddomhof 6:5431ddb9d881 149
arnouddomhof 6:5431ddb9d881 150 if (setpointy < point1y && track == 41)
arnouddomhof 6:5431ddb9d881 151 {
arnouddomhof 6:5431ddb9d881 152 setpointx = point4x; // Van punt 4 naar punt 2 op dezelfde x blijven.
arnouddomhof 6:5431ddb9d881 153 }
arnouddomhof 6:5431ddb9d881 154
arnouddomhof 5:cd329205f037 155
arnouddomhof 5:cd329205f037 156 return setpointx;
arnouddomhof 5:cd329205f037 157 }
arnouddomhof 5:cd329205f037 158
arnouddomhof 5:cd329205f037 159 double determinedemosety(double setpointx, double setpointy)
arnouddomhof 5:cd329205f037 160 {
arnouddomhof 5:cd329205f037 161 // Van reference positie naar punt 1.
arnouddomhof 5:cd329205f037 162 if(setpointy < point1y && track == 1){
arnouddomhof 7:2e0cdf836495 163 setpointy = setpointy + (stepsize2);
arnouddomhof 5:cd329205f037 164 }
arnouddomhof 5:cd329205f037 165
arnouddomhof 5:cd329205f037 166 // Van punt 1 naar punt 2.
arnouddomhof 7:2e0cdf836495 167 if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 41)){
arnouddomhof 6:5431ddb9d881 168 ledg = 1; ledr = 0; ledb = 1; // Turns the Led RED
arnouddomhof 5:cd329205f037 169 setpointy = point2y; // Van punt 1 naar punt 2 op dezelfde y blijven.
arnouddomhof 5:cd329205f037 170 track = 12;
arnouddomhof 5:cd329205f037 171 }
arnouddomhof 5:cd329205f037 172 if (setpointx < point2x && track == 12){
arnouddomhof 5:cd329205f037 173 setpointy = point2y;
arnouddomhof 5:cd329205f037 174 }
arnouddomhof 5:cd329205f037 175
arnouddomhof 5:cd329205f037 176 // Van punt 2 naar punt 3.
arnouddomhof 7:2e0cdf836495 177 if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){
arnouddomhof 6:5431ddb9d881 178 ledr = 1; ledg = 0; ledb = 1; // Turns the Led GREEN
arnouddomhof 5:cd329205f037 179 setpointx = point3x;
arnouddomhof 5:cd329205f037 180 track = 23;
arnouddomhof 5:cd329205f037 181 }
arnouddomhof 5:cd329205f037 182 if ((setpointy > point3y) && (track == 23))
arnouddomhof 5:cd329205f037 183 {
arnouddomhof 7:2e0cdf836495 184 setpointy = setpointy + (-stepsize2);
arnouddomhof 5:cd329205f037 185 track = 23;
arnouddomhof 5:cd329205f037 186 }
arnouddomhof 5:cd329205f037 187
arnouddomhof 5:cd329205f037 188 // Van punt 3 naar punt 4.
arnouddomhof 7:2e0cdf836495 189 if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23))
arnouddomhof 5:cd329205f037 190 {
arnouddomhof 6:5431ddb9d881 191 ledg = 1; ledr = 1; ledb = 0; // Turns the Led BLUE
AppelSab 2:7e86ced5841f 192 setpointy = setpointy;
AppelSab 0:755bc7c0f555 193 track = 34;
AppelSab 0:755bc7c0f555 194 }
arnouddomhof 6:5431ddb9d881 195 if (setpointx > point4x && track == 34)
AppelSab 0:755bc7c0f555 196 {
AppelSab 2:7e86ced5841f 197 setpointy = setpointy;
arnouddomhof 5:cd329205f037 198 }
arnouddomhof 5:cd329205f037 199
arnouddomhof 6:5431ddb9d881 200 // Van punt 4 naar punt 1.
arnouddomhof 7:2e0cdf836495 201 if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34))
arnouddomhof 5:cd329205f037 202 {
arnouddomhof 6:5431ddb9d881 203 ledg = 0; ledr = 0; ledb = 0; // Turns the LED WHITE
arnouddomhof 6:5431ddb9d881 204 track = 41;
arnouddomhof 6:5431ddb9d881 205 }
arnouddomhof 6:5431ddb9d881 206
arnouddomhof 6:5431ddb9d881 207 if (setpointy < point1y && track == 41)
arnouddomhof 6:5431ddb9d881 208 {
arnouddomhof 7:2e0cdf836495 209 setpointy = setpointy + (stepsize2); // Van punt 4 naar punt 2 op dezelfde x blijven.
AppelSab 0:755bc7c0f555 210 }
arnouddomhof 3:fd1380ffb200 211
arnouddomhof 5:cd329205f037 212 return setpointy;
arnouddomhof 5:cd329205f037 213
arnouddomhof 5:cd329205f037 214 }
AppelSab 0:755bc7c0f555 215
AppelSab 0:755bc7c0f555 216 // -----------------------------------------------------------------
AppelSab 0:755bc7c0f555 217 // --------------------------- PI controllers ----------------------
AppelSab 0:755bc7c0f555 218 // -----------------------------------------------------------------
AppelSab 0:755bc7c0f555 219 double PI_controller1(double error1)
AppelSab 0:755bc7c0f555 220 {
AppelSab 0:755bc7c0f555 221 static double error_integral1 = 0;
arnouddomhof 6:5431ddb9d881 222 static double error_prev1 = error1; // initialization with this value only done once!
AppelSab 0:755bc7c0f555 223
AppelSab 0:755bc7c0f555 224 // Proportional part
arnouddomhof 6:5431ddb9d881 225 //double Kp1 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 226 double u_p1 = Kp1*error1; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
AppelSab 0:755bc7c0f555 227
AppelSab 0:755bc7c0f555 228 // Integral part
AppelSab 0:755bc7c0f555 229 double Ts1 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
AppelSab 0:755bc7c0f555 230 error_integral1 = error_integral1 + error1 * Ts1;
AppelSab 0:755bc7c0f555 231 double u_i1 = Ki1 * error_integral1;
AppelSab 0:755bc7c0f555 232
arnouddomhof 6:5431ddb9d881 233 // Derivative part
arnouddomhof 6:5431ddb9d881 234 double error_derivative1 = (error1 - error_prev1)/Ts1;
arnouddomhof 6:5431ddb9d881 235 double u_d1 = Kd1 * error_derivative1;
arnouddomhof 6:5431ddb9d881 236 error_prev1 = error1;
arnouddomhof 6:5431ddb9d881 237
AppelSab 0:755bc7c0f555 238 // Sum
arnouddomhof 6:5431ddb9d881 239 U1 = u_p1 + u_i1 + u_d1;
AppelSab 0:755bc7c0f555 240
AppelSab 0:755bc7c0f555 241 // Return
AppelSab 0:755bc7c0f555 242 return U1;
AppelSab 0:755bc7c0f555 243 }
AppelSab 0:755bc7c0f555 244 double PI_controller2(double error2)
AppelSab 0:755bc7c0f555 245 {
AppelSab 0:755bc7c0f555 246 static double error_integral2 = 0;
arnouddomhof 6:5431ddb9d881 247 static double error_prev2 = error2; // initialization with this value only done once!
AppelSab 0:755bc7c0f555 248
AppelSab 0:755bc7c0f555 249 // Proportional part
arnouddomhof 6:5431ddb9d881 250 //double Kp2 = 3.95; // Kp (proportionele controller, nu nog een random waarde)
AppelSab 0:755bc7c0f555 251 double u_p2 = Kp2*error2; // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
AppelSab 0:755bc7c0f555 252
AppelSab 0:755bc7c0f555 253 // Integral part
AppelSab 0:755bc7c0f555 254 double Ts2 = 0.005; // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)
AppelSab 0:755bc7c0f555 255 error_integral2 = error_integral2 + error2 * Ts2;
AppelSab 0:755bc7c0f555 256 double u_i2 = Ki2 * error_integral2;
AppelSab 0:755bc7c0f555 257
arnouddomhof 6:5431ddb9d881 258 // Derivative part
arnouddomhof 6:5431ddb9d881 259 double error_derivative2 = (error2 - error_prev2)/Ts2;
arnouddomhof 6:5431ddb9d881 260 double u_d2 = Kd2 * error_derivative2;
arnouddomhof 6:5431ddb9d881 261 error_prev2 = error2;
arnouddomhof 6:5431ddb9d881 262
AppelSab 0:755bc7c0f555 263 // Sum +
arnouddomhof 6:5431ddb9d881 264 U2 = u_p2 + u_i2 + u_d2;
AppelSab 0:755bc7c0f555 265
AppelSab 0:755bc7c0f555 266 // Return
AppelSab 0:755bc7c0f555 267 return U2;
AppelSab 0:755bc7c0f555 268 }
AppelSab 0:755bc7c0f555 269 // ------------------------------------------------------------
AppelSab 0:755bc7c0f555 270 // ------------ Inverse Kinematics ----------------------------
AppelSab 0:755bc7c0f555 271 // ------------------------------------------------------------
AppelSab 0:755bc7c0f555 272 double makeAngleq1(double x, double y){
AppelSab 0:755bc7c0f555 273 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 274 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 275 return q1_diff;
AppelSab 0:755bc7c0f555 276 }
AppelSab 0:755bc7c0f555 277
AppelSab 0:755bc7c0f555 278 double makeAngleq2(double x, double y){
AppelSab 0:755bc7c0f555 279 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 280 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
arnouddomhof 6:5431ddb9d881 281 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
arnouddomhof 6:5431ddb9d881 282 q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
arnouddomhof 6:5431ddb9d881 283 return q2_diff;
AppelSab 0:755bc7c0f555 284 }
AppelSab 0:755bc7c0f555 285
AppelSab 0:755bc7c0f555 286
AppelSab 0:755bc7c0f555 287 // -----------------------------------------------
AppelSab 0:755bc7c0f555 288 // ------------ RUN MOTORS -----------------------
AppelSab 0:755bc7c0f555 289 // -----------------------------------------------
AppelSab 0:755bc7c0f555 290 void motoraansturing()
AppelSab 0:755bc7c0f555 291 {
arnouddomhof 5:cd329205f037 292 setpointx = determinedemosetx(setpointx, setpointy);
arnouddomhof 5:cd329205f037 293 setpointy = determinedemosety(setpointx, setpointy);
AppelSab 0:755bc7c0f555 294 q1_diff = makeAngleq1(setpointx, setpointy);
AppelSab 0:755bc7c0f555 295 q2_diff = makeAngleq2(setpointx, setpointy);
AppelSab 0:755bc7c0f555 296
AppelSab 0:755bc7c0f555 297 theta2 = counts2angle2();
AppelSab 0:755bc7c0f555 298 error2 = q2_diff - theta2;
AppelSab 0:755bc7c0f555 299 theta1 = counts2angle1();
AppelSab 0:755bc7c0f555 300 error1 = q1_diff - theta1; // Setpoint error, te behalen setpoint minus de huidige positie van de as.
AppelSab 0:755bc7c0f555 301
AppelSab 0:755bc7c0f555 302 U1 = PI_controller1(error1); // Voltage dat naar de motor gestuurd wordt.
AppelSab 0:755bc7c0f555 303 U2 = PI_controller2(error2);
AppelSab 0:755bc7c0f555 304
AppelSab 0:755bc7c0f555 305 motor1_pwm.write(fabs(U1)); // Motor aansturen
AppelSab 0:755bc7c0f555 306 directionM1 = U1 > 0.0f; // Richting van de motor bepalen
AppelSab 0:755bc7c0f555 307 motor2_pwm.write(fabs(U2));
arnouddomhof 5:cd329205f037 308 directionM2 = U2 > 0.0f;
AppelSab 0:755bc7c0f555 309 }
AppelSab 0:755bc7c0f555 310
AppelSab 0:755bc7c0f555 311
AppelSab 0:755bc7c0f555 312 void rundemo()
AppelSab 0:755bc7c0f555 313 {
AppelSab 0:755bc7c0f555 314 motoraansturing();
AppelSab 0:755bc7c0f555 315 }
AppelSab 0:755bc7c0f555 316
AppelSab 0:755bc7c0f555 317
AppelSab 0:755bc7c0f555 318 int main()
AppelSab 0:755bc7c0f555 319 {
AppelSab 0:755bc7c0f555 320 pc.baud(115200);
AppelSab 0:755bc7c0f555 321 motor1_pwm.period_us(60); // Period is 60 microseconde
AppelSab 0:755bc7c0f555 322 motor2_pwm.period_us(60);
AppelSab 0:755bc7c0f555 323 Demo.attach(&rundemo, 0.005f);
AppelSab 0:755bc7c0f555 324
arnouddomhof 6:5431ddb9d881 325 pc.printf("\r\n_____ DEMO _____\r\n\r\n");
arnouddomhof 6:5431ddb9d881 326
arnouddomhof 5:cd329205f037 327 ledr = 1;
arnouddomhof 5:cd329205f037 328 ledg = 1;
arnouddomhof 5:cd329205f037 329 ledb = 1;
arnouddomhof 3:fd1380ffb200 330
AppelSab 0:755bc7c0f555 331 while (true) {
arnouddomhof 7:2e0cdf836495 332 pc.printf("Setpointx: %0.2f, Setpointy: %0.2f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
arnouddomhof 7:2e0cdf836495 333 /**
arnouddomhof 3:fd1380ffb200 334 if (track == 1) {
arnouddomhof 6:5431ddb9d881 335 pc.printf("Gaat van home naar positie 1\r\n");
arnouddomhof 3:fd1380ffb200 336 }
arnouddomhof 3:fd1380ffb200 337 else if (track == 12) {
arnouddomhof 3:fd1380ffb200 338 pc.printf("Gaat naar positie 2\r\n");
arnouddomhof 3:fd1380ffb200 339 }
arnouddomhof 5:cd329205f037 340 else if (track == 23) {
arnouddomhof 5:cd329205f037 341 pc.printf("Gaat naar positie 3\r\n");
arnouddomhof 5:cd329205f037 342 }
arnouddomhof 5:cd329205f037 343 else if (track == 34) {
arnouddomhof 5:cd329205f037 344 pc.printf("Gaat naar positie 4\r\n");
arnouddomhof 5:cd329205f037 345 }
arnouddomhof 6:5431ddb9d881 346 else if (track == 41) {
arnouddomhof 6:5431ddb9d881 347 pc.printf("Gaat naar positie 1\r\n");
arnouddomhof 6:5431ddb9d881 348 }
arnouddomhof 7:2e0cdf836495 349 */
arnouddomhof 6:5431ddb9d881 350
arnouddomhof 7:2e0cdf836495 351 wait(0.1f);
AppelSab 0:755bc7c0f555 352 }
AppelSab 0:755bc7c0f555 353 }