Arnoud Domhof / Mbed 2 deprecated project_demomode

Laatste versie van Arnoud, Data wordt geplot

Dependencies:   FastPWM MODSERIAL QEI mbed

main_demo.cpp@8:088904ebb85b, 2018-11-01 (annotated)

Committer:
arnouddomhof
Date:
Thu Nov 01 17:02:30 2018 +0000
Revision:
8:088904ebb85b
Parent:
7:2e0cdf836495 
Werkende Demomode met PID-controller. Setpoint gestuurd (dus geen feedback van de motoren).

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