Feike van Veen / Mbed 2 deprecated Minor_test_serial

Robot tryout

Dependencies:   mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@31:3c13f1c35ee5, 2019-10-31 (annotated)

Committer:
viviien
Date:
Thu Oct 31 12:42:24 2019 +0000
Revision:
31:3c13f1c35ee5
Parent:
30:390cab7cd6e6
Child:
32:60a71dcfdb7a
Steps versie zonder timer die werkt;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Feike 4:bd21569250c7 1 #include "mbed.h"
Feike 8:017b813c72bb 2 #include "MODSERIAL.h"
Feike 12:2382468d36a4 3 #include "QEI.h"
Feike 18:ab0fe311e7f3 4 #include "Math.h"
Feike 18:ab0fe311e7f3 5 #include "ttmath.h"
Feike 18:ab0fe311e7f3 6
Feike 17:6da57acb7bea 7 MODSERIAL pc(USBTX, USBRX);
Feike 18:ab0fe311e7f3 8 //Serial term (USBTX, USBRX);
Feike 17:6da57acb7bea 9 PwmOut motor1_pwm(PTC2);
Feike 17:6da57acb7bea 10 DigitalOut motor1_dir(PTC3);
Feike 18:ab0fe311e7f3 11 PwmOut motor2_pwm(PTA2);
Feike 18:ab0fe311e7f3 12 DigitalOut motor2_dir(PTB23);
Feike 18:ab0fe311e7f3 13 PwmOut motor3_pwm(PTC4);
Feike 18:ab0fe311e7f3 14 DigitalOut motor3_dir(PTC12);
Feike 4:bd21569250c7 15
viviien 30:390cab7cd6e6 16 AnalogIn potmeter1(A1);
viviien 30:390cab7cd6e6 17
viviien 30:390cab7cd6e6 18
viviien 30:390cab7cd6e6 19
viviien 23:18b0be02187f 20 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 21 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 22 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
Feike 7:e119b12e5e7f 23
viviien 25:eb3204e45d33 24 float steps1 = 0;
viviien 25:eb3204e45d33 25 float steps2 = 0;
viviien 25:eb3204e45d33 26 float steps3 = 0;
Feike 19:9c8ab7922191 27 int check;
Feike 13:18dd7a15603f 28 int quit;
Feike 19:9c8ab7922191 29 int limit_pos = 8400;
Feike 19:9c8ab7922191 30 float steps;
Feike 19:9c8ab7922191 31 int g = 0;
Feike 19:9c8ab7922191 32
Feike 19:9c8ab7922191 33 bool check1;
Feike 19:9c8ab7922191 34 bool check2;
Feike 19:9c8ab7922191 35 bool check3;
Feike 18:ab0fe311e7f3 36 int counts1 = 0;
Feike 18:ab0fe311e7f3 37 int counts2 = 0;
Feike 18:ab0fe311e7f3 38 int counts3 = 0;
viviien 27:3eb181cbe183 39
Feike 13:18dd7a15603f 40
Feike 18:ab0fe311e7f3 41 const float le = 15.0;
Feike 18:ab0fe311e7f3 42 const float f = 37.5;
Feike 18:ab0fe311e7f3 43 const float re = 174.0;
Feike 18:ab0fe311e7f3 44 const float rf = 50.0;
Feike 18:ab0fe311e7f3 45 const float pi = 3.14159265358979323846;
viviien 22:9f911405e096 46 const float cospi = -0.5;
viviien 22:9f911405e096 47 const float sinpi = 0.8660254;
Feike 18:ab0fe311e7f3 48 float y2;
Feike 18:ab0fe311e7f3 49 float y1;
Feike 18:ab0fe311e7f3 50 float z1;
Feike 18:ab0fe311e7f3 51 float z2;
Feike 18:ab0fe311e7f3 52 float rje2;
Feike 18:ab0fe311e7f3 53 float rje;
Feike 18:ab0fe311e7f3 54 float r2;
Feike 18:ab0fe311e7f3 55 float r;
Feike 19:9c8ab7922191 56
Feike 18:ab0fe311e7f3 57 float z0=-172;
Feike 18:ab0fe311e7f3 58 float y0=0;
Feike 18:ab0fe311e7f3 59 float x0=0;
viviien 21:c826abca79c3 60 float x00;
viviien 21:c826abca79c3 61 float y00;
viviien 21:c826abca79c3 62 float z00;
Feike 19:9c8ab7922191 63
Feike 18:ab0fe311e7f3 64 float theta1;
Feike 18:ab0fe311e7f3 65 float theta2;
Feike 18:ab0fe311e7f3 66 float theta3;
Feike 18:ab0fe311e7f3 67 float oldtheta1=0;
Feike 18:ab0fe311e7f3 68 float oldtheta2=0;
Feike 18:ab0fe311e7f3 69 float oldtheta3=0;
Feike 19:9c8ab7922191 70
viviien 28:43a1d67ff8ea 71 // Constant values for PI
viviien 30:390cab7cd6e6 72 float Kp;
viviien 30:390cab7cd6e6 73 float Kd;
viviien 30:390cab7cd6e6 74
viviien 30:390cab7cd6e6 75 float Ts = 0.0025;
viviien 28:43a1d67ff8ea 76 float error1 = 0;
viviien 30:390cab7cd6e6 77 float error2 = 0;
viviien 30:390cab7cd6e6 78 float error3 = 0;
viviien 28:43a1d67ff8ea 79
viviien 30:390cab7cd6e6 80 float newmotor1 = 1;
viviien 30:390cab7cd6e6 81 float newmotor2 = 1;
viviien 30:390cab7cd6e6 82 float newmotor3 = 1;
viviien 30:390cab7cd6e6 83
viviien 30:390cab7cd6e6 84 float u_k1 = 0;
viviien 30:390cab7cd6e6 85 float u_k2 = 0;
viviien 30:390cab7cd6e6 86 float u_k3 = 0;
viviien 30:390cab7cd6e6 87
viviien 30:390cab7cd6e6 88 float u_d1 = 0;
viviien 30:390cab7cd6e6 89 float u_d2 = 0;
viviien 30:390cab7cd6e6 90 float u_d3 = 0;
viviien 30:390cab7cd6e6 91
viviien 30:390cab7cd6e6 92 float lasterror1 = 0;
viviien 30:390cab7cd6e6 93 float lasterror2 = 0;
viviien 30:390cab7cd6e6 94 float lasterror3 = 0;
viviien 30:390cab7cd6e6 95
viviien 30:390cab7cd6e6 96 float derivative1 = 0;
viviien 30:390cab7cd6e6 97 float derivative2 = 0;
viviien 30:390cab7cd6e6 98 float derivative3 = 0;
viviien 30:390cab7cd6e6 99
viviien 31:3c13f1c35ee5 100 //bool timer = false;
viviien 30:390cab7cd6e6 101
viviien 30:390cab7cd6e6 102 Ticker motor_timer;
viviien 30:390cab7cd6e6 103 void motor()
viviien 30:390cab7cd6e6 104 {
viviien 30:390cab7cd6e6 105 counts1 = Encoder1.getPulses();
viviien 30:390cab7cd6e6 106 counts2 = Encoder2.getPulses();
viviien 30:390cab7cd6e6 107 counts3 = Encoder3.getPulses();
viviien 30:390cab7cd6e6 108
viviien 30:390cab7cd6e6 109 // angle1 = counts1/(8400.0)*2.0*pie;
viviien 30:390cab7cd6e6 110 // angle2 = counts2/(8400.0)*2.0*pie;
viviien 30:390cab7cd6e6 111 // angle3 = counts3/(8400.0)*2.0*pie;
viviien 30:390cab7cd6e6 112
viviien 30:390cab7cd6e6 113 // sinewave = sin(time_sin);
viviien 29:7eb028b359a1 114
viviien 30:390cab7cd6e6 115 lasterror1 = error1;
viviien 30:390cab7cd6e6 116 lasterror2 = error2;
viviien 30:390cab7cd6e6 117 lasterror3 = error3;
viviien 30:390cab7cd6e6 118
viviien 30:390cab7cd6e6 119 error1 = counts1 - steps1;
viviien 30:390cab7cd6e6 120 error2 = counts2 - steps2;
viviien 30:390cab7cd6e6 121 error3 = counts3 - steps3;
viviien 30:390cab7cd6e6 122
viviien 30:390cab7cd6e6 123 derivative1 = error1 - lasterror1;
viviien 30:390cab7cd6e6 124 derivative2 = error2 - lasterror2;
viviien 30:390cab7cd6e6 125 derivative3 = error3 - lasterror3;
viviien 30:390cab7cd6e6 126
viviien 30:390cab7cd6e6 127 Kp = 50;
viviien 31:3c13f1c35ee5 128 Kd = potmeter1 * 1000;
viviien 30:390cab7cd6e6 129
viviien 30:390cab7cd6e6 130
viviien 30:390cab7cd6e6 131 // Proportional part
viviien 30:390cab7cd6e6 132 u_k1 = Kp * error1;
viviien 30:390cab7cd6e6 133 u_k2 = Kp * error2;
viviien 30:390cab7cd6e6 134 u_k3 = Kp * error3;
viviien 30:390cab7cd6e6 135
viviien 30:390cab7cd6e6 136 u_d1 = Kd * derivative1;
viviien 30:390cab7cd6e6 137 u_d2 = Kd * derivative2;
viviien 30:390cab7cd6e6 138 u_d3 = Kd * derivative3;
viviien 30:390cab7cd6e6 139
viviien 30:390cab7cd6e6 140 newmotor1 = u_k1 + u_d1;
viviien 30:390cab7cd6e6 141 newmotor2 = u_k2 + u_d2;
viviien 30:390cab7cd6e6 142 newmotor3 = u_k3 + u_d3;
viviien 30:390cab7cd6e6 143
viviien 30:390cab7cd6e6 144 if (newmotor1>1.0f){
viviien 30:390cab7cd6e6 145 newmotor1 =1.0f;
viviien 23:18b0be02187f 146 }
viviien 30:390cab7cd6e6 147 if (newmotor1<-1.0f){
viviien 30:390cab7cd6e6 148 newmotor1 = -1.0f;
viviien 30:390cab7cd6e6 149 }
viviien 30:390cab7cd6e6 150
viviien 30:390cab7cd6e6 151 if (newmotor2>1.0f){
viviien 30:390cab7cd6e6 152 newmotor2 =1.0f;
viviien 30:390cab7cd6e6 153 }
viviien 30:390cab7cd6e6 154 if (newmotor2<-1.0f){
viviien 30:390cab7cd6e6 155 newmotor2 = -1.0f;
viviien 30:390cab7cd6e6 156 }
viviien 30:390cab7cd6e6 157
viviien 30:390cab7cd6e6 158 if (newmotor3>1.0f){
viviien 30:390cab7cd6e6 159 newmotor3 =1.0f;
viviien 30:390cab7cd6e6 160 }
viviien 30:390cab7cd6e6 161 if (newmotor3<-1.0f){
viviien 30:390cab7cd6e6 162 newmotor3 = -1.0f;
viviien 30:390cab7cd6e6 163 }
viviien 30:390cab7cd6e6 164
viviien 31:3c13f1c35ee5 165 if (timer==true) {
viviien 30:390cab7cd6e6 166 motor1_pwm.write(fabs(newmotor1));
viviien 30:390cab7cd6e6 167 motor1_dir.write(newmotor1>0);
viviien 30:390cab7cd6e6 168
viviien 30:390cab7cd6e6 169 motor2_pwm.write(fabs(newmotor2));
viviien 30:390cab7cd6e6 170 motor2_dir.write(newmotor2<0);
viviien 30:390cab7cd6e6 171
viviien 30:390cab7cd6e6 172 motor3_pwm.write(fabs(newmotor3));
viviien 30:390cab7cd6e6 173 motor3_dir.write(newmotor3>0);
viviien 30:390cab7cd6e6 174 }
viviien 30:390cab7cd6e6 175
viviien 31:3c13f1c35ee5 176 // else {
viviien 31:3c13f1c35ee5 177 // motor1_pwm.write(0);
viviien 31:3c13f1c35ee5 178 // motor2_pwm.write(0);
viviien 31:3c13f1c35ee5 179 // motor3_pwm.write(0);
viviien 31:3c13f1c35ee5 180 // }
viviien 30:390cab7cd6e6 181 }
viviien 25:eb3204e45d33 182
viviien 28:43a1d67ff8ea 183
viviien 21:c826abca79c3 184 float delta_calcangleyz(float x00, float y00, float z00) {
viviien 21:c826abca79c3 185 float y2 = y00 + le;
Feike 18:ab0fe311e7f3 186 float y1 = f;
Feike 18:ab0fe311e7f3 187 float z1 = 0.0;
viviien 21:c826abca79c3 188 float z2 = z00;
viviien 21:c826abca79c3 189 float rje2 = re*re - x00*x00;
Feike 18:ab0fe311e7f3 190 float rje = sqrt(rje2);
viviien 22:9f911405e096 191 float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00);
Feike 18:ab0fe311e7f3 192 float r = sqrt(r2);
Feike 18:ab0fe311e7f3 193
Feike 19:9c8ab7922191 194 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
Feike 19:9c8ab7922191 195 int check = 1;
Feike 19:9c8ab7922191 196 pc.printf("\n\rPunt bestaat niet");
Feike 19:9c8ab7922191 197 }
Feike 19:9c8ab7922191 198 else {
Feike 19:9c8ab7922191 199 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
Feike 19:9c8ab7922191 200 float beta = atan((z1-z2)/(y1-y2));
Feike 19:9c8ab7922191 201 if(beta<0) {
Feike 18:ab0fe311e7f3 202 beta = beta + pi;
Feike 18:ab0fe311e7f3 203 }
Feike 19:9c8ab7922191 204 float theta1 = (beta - alpha)*180.0/pi;
Feike 19:9c8ab7922191 205 return theta1;
viviien 28:43a1d67ff8ea 206 }
viviien 22:9f911405e096 207 }
viviien 28:43a1d67ff8ea 208
viviien 28:43a1d67ff8ea 209
Feike 18:ab0fe311e7f3 210
viviien 21:c826abca79c3 211 float delta_calcinverse(float x00, float y00, float z00) {
viviien 21:c826abca79c3 212 theta1 = theta2 = theta3 = 0;
viviien 22:9f911405e096 213 x00=x0;
viviien 22:9f911405e096 214 y00=y0;
viviien 22:9f911405e096 215 z00=z0;
viviien 21:c826abca79c3 216 theta1 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 217
viviien 21:c826abca79c3 218 if (check == 0) {
viviien 21:c826abca79c3 219 x00=x0*cospi+y0*sinpi;
viviien 21:c826abca79c3 220 y00=y0*cospi-x0*sinpi;
viviien 21:c826abca79c3 221 z00=z0;
viviien 22:9f911405e096 222 theta2 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 223 x00=x0*cospi-y0*sinpi;
viviien 21:c826abca79c3 224 y00=y0*cospi+y0*sinpi;
viviien 21:c826abca79c3 225 z00=z0;
viviien 22:9f911405e096 226 theta3 = delta_calcangleyz(x00, y00, z00);
viviien 21:c826abca79c3 227 }
viviien 21:c826abca79c3 228
viviien 21:c826abca79c3 229 return theta1, theta2, theta3;
Feike 18:ab0fe311e7f3 230 }
Feike 18:ab0fe311e7f3 231
viviien 28:43a1d67ff8ea 232 //double error;
Feike 12:2382468d36a4 233
viviien 28:43a1d67ff8ea 234 // Calculate the steps from angle
Feike 19:9c8ab7922191 235 float anglestep(float angle) {
Feike 18:ab0fe311e7f3 236 float steps;
Feike 18:ab0fe311e7f3 237 steps = angle / 360 * 8400;
Feike 18:ab0fe311e7f3 238 return steps;
Feike 17:6da57acb7bea 239 }
Feike 17:6da57acb7bea 240
viviien 28:43a1d67ff8ea 241 // Omschrijving
viviien 28:43a1d67ff8ea 242 float movefunctioninit () {
viviien 21:c826abca79c3 243
viviien 31:3c13f1c35ee5 244 theta1 = delta_calcinverse(x00,y00,z00);
viviien 21:c826abca79c3 245
viviien 31:3c13f1c35ee5 246 // theta1 = theta1 - oldtheta1;
viviien 31:3c13f1c35ee5 247 // theta2 = theta2 - oldtheta2;
viviien 31:3c13f1c35ee5 248 // theta3 = theta3 - oldtheta3;
viviien 31:3c13f1c35ee5 249 //
viviien 31:3c13f1c35ee5 250 // steps1 = steps1 + anglestep(theta1);
viviien 31:3c13f1c35ee5 251 // steps2 = steps2 + anglestep(theta2);
viviien 31:3c13f1c35ee5 252 // steps3 = steps3 + anglestep(theta3);
viviien 31:3c13f1c35ee5 253
viviien 31:3c13f1c35ee5 254 steps1 = anglestep(theta1);
viviien 31:3c13f1c35ee5 255 steps2 = anglestep(theta2);
viviien 31:3c13f1c35ee5 256 steps3 = anglestep(theta3);
viviien 31:3c13f1c35ee5 257
viviien 31:3c13f1c35ee5 258 pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);
viviien 31:3c13f1c35ee5 259 pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);
viviien 27:3eb181cbe183 260 pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
viviien 30:390cab7cd6e6 261 pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3);
viviien 30:390cab7cd6e6 262
viviien 28:43a1d67ff8ea 263
viviien 28:43a1d67ff8ea 264
viviien 30:390cab7cd6e6 265 // pc.printf("\n\r error1: %f", error1);
viviien 30:390cab7cd6e6 266
viviien 27:3eb181cbe183 267
viviien 21:c826abca79c3 268 // Set the direction of the motors.
viviien 30:390cab7cd6e6 269 // if (theta1 < 0) {
viviien 30:390cab7cd6e6 270 // motor1_dir.write(1);
viviien 30:390cab7cd6e6 271 // }
viviien 30:390cab7cd6e6 272 // else {
viviien 30:390cab7cd6e6 273 // motor1_dir.write(0);
viviien 30:390cab7cd6e6 274 // }
viviien 30:390cab7cd6e6 275 //
viviien 30:390cab7cd6e6 276 // if (theta2 < 0) {
viviien 30:390cab7cd6e6 277 // motor2_dir.write(0);
viviien 30:390cab7cd6e6 278 // }
viviien 30:390cab7cd6e6 279 // else {
viviien 30:390cab7cd6e6 280 // motor2_dir.write(1);
viviien 30:390cab7cd6e6 281 // }
viviien 30:390cab7cd6e6 282 //
viviien 30:390cab7cd6e6 283 // if (theta3 < 0) {
viviien 30:390cab7cd6e6 284 // motor3_dir.write(0);
viviien 30:390cab7cd6e6 285 // }
viviien 30:390cab7cd6e6 286 // else {
viviien 30:390cab7cd6e6 287 // motor3_dir.write(1);
viviien 30:390cab7cd6e6 288 // }
viviien 30:390cab7cd6e6 289 //
viviien 30:390cab7cd6e6 290 // int frequency_pwm = 10000; //10 kHz PWM
viviien 30:390cab7cd6e6 291 // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 292 // motor1_pwm.write(0.57); // write Duty Cycle
viviien 30:390cab7cd6e6 293 //
viviien 30:390cab7cd6e6 294 // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 295 // motor2_pwm.write(0.57); // write Duty Cycle
viviien 30:390cab7cd6e6 296 //
viviien 30:390cab7cd6e6 297 // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 298 // motor3_pwm.write(0.57); // write Duty Cycle
viviien 30:390cab7cd6e6 299 //
viviien 30:390cab7cd6e6 300 // check1 = true;
viviien 30:390cab7cd6e6 301 // check2 = true;
viviien 30:390cab7cd6e6 302 // check3 = true;
viviien 30:390cab7cd6e6 303 //
viviien 30:390cab7cd6e6 304 // Encoder1.reset();
viviien 30:390cab7cd6e6 305 // Encoder2.reset();
viviien 30:390cab7cd6e6 306 // Encoder3.reset();
viviien 30:390cab7cd6e6 307 //
viviien 28:43a1d67ff8ea 308 }
viviien 25:eb3204e45d33 309
viviien 28:43a1d67ff8ea 310 // Omschrijving
viviien 31:3c13f1c35ee5 311 //float movestop() {
viviien 31:3c13f1c35ee5 312 //wait(0.6);
viviien 31:3c13f1c35ee5 313 //timer = false;
viviien 31:3c13f1c35ee5 314 //}
viviien 30:390cab7cd6e6 315 //
viviien 30:390cab7cd6e6 316 // while (check1 || check2 || check3) {
viviien 30:390cab7cd6e6 317 //
viviien 30:390cab7cd6e6 318 // int frequency_pwm = 10000; //10 kHz PWM
viviien 30:390cab7cd6e6 319 // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 320 // motor1_pwm.write(newmotor1); // write Duty Cycle
viviien 30:390cab7cd6e6 321 //
viviien 30:390cab7cd6e6 322 // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 323 // motor2_pwm.write(newmotor2); // write Duty Cycle
viviien 30:390cab7cd6e6 324 //
viviien 30:390cab7cd6e6 325 // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 326 // motor3_pwm.write(newmotor3); // write Duty Cycle
viviien 30:390cab7cd6e6 327 //
viviien 30:390cab7cd6e6 328 // if (error1==0) {
viviien 30:390cab7cd6e6 329 // check1=false;
viviien 30:390cab7cd6e6 330 // }
viviien 30:390cab7cd6e6 331 // if (error2==0) {
viviien 30:390cab7cd6e6 332 // check2=false;
viviien 30:390cab7cd6e6 333 // }
viviien 30:390cab7cd6e6 334 // if (error3==0) {
viviien 30:390cab7cd6e6 335 // check3=false;
viviien 30:390cab7cd6e6 336 // }
viviien 30:390cab7cd6e6 337 //
viviien 30:390cab7cd6e6 338 //// if(abs(counts1)>=abs(steps1)) {
viviien 30:390cab7cd6e6 339 //// pc.printf("\n\r 1 is false");
viviien 30:390cab7cd6e6 340 //// motor1_pwm.write(0);
viviien 30:390cab7cd6e6 341 //// check1=false;
viviien 30:390cab7cd6e6 342 //// pc.printf("\n\rcounts1 %i", counts1);
viviien 30:390cab7cd6e6 343 //// }
viviien 30:390cab7cd6e6 344 ////
viviien 30:390cab7cd6e6 345 //// if (abs(counts2)>=abs(steps2)) {
viviien 30:390cab7cd6e6 346 //// pc.printf("\n\r 2 is false");
viviien 30:390cab7cd6e6 347 //// motor2_pwm.write(0);
viviien 30:390cab7cd6e6 348 //// check2=false;
viviien 30:390cab7cd6e6 349 //// pc.printf("\n\rcounts2 %i", counts2);
viviien 30:390cab7cd6e6 350 //// }
viviien 30:390cab7cd6e6 351 ////
viviien 30:390cab7cd6e6 352 //// if (abs(counts3)>=abs(steps3)) {
viviien 30:390cab7cd6e6 353 //// pc.printf("\n\r 3 is false");
viviien 30:390cab7cd6e6 354 //// motor3_pwm.write(0);
viviien 30:390cab7cd6e6 355 //// check3=false;
viviien 30:390cab7cd6e6 356 //// pc.printf("\n\rcounts3 %i", counts3);
viviien 30:390cab7cd6e6 357 //// }
viviien 30:390cab7cd6e6 358 // }
viviien 30:390cab7cd6e6 359 // wait(1.0);
viviien 30:390cab7cd6e6 360 // pc.printf("\n\rdone moving");
viviien 30:390cab7cd6e6 361 //}
viviien 22:9f911405e096 362
viviien 28:43a1d67ff8ea 363
viviien 28:43a1d67ff8ea 364 ///////////////////
viviien 28:43a1d67ff8ea 365 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 366 ///////////////////
viviien 28:43a1d67ff8ea 367
viviien 22:9f911405e096 368 int main(void) {
viviien 30:390cab7cd6e6 369 motor_timer.attach(&motor, 0.002);
viviien 26:432d3519ba86 370
viviien 28:43a1d67ff8ea 371 pc.baud(115200);
Feike 17:6da57acb7bea 372 char cc = pc.getc();
viviien 30:390cab7cd6e6 373
viviien 30:390cab7cd6e6 374 int frequency_pwm = 10000; //10 kHz PWM
viviien 30:390cab7cd6e6 375 // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 376 // motor1_pwm.write(newmotor1); // write Duty Cycle
viviien 30:390cab7cd6e6 377 //
viviien 30:390cab7cd6e6 378 // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 379 // motor2_pwm.write(newmotor2); // write Duty Cycle
viviien 30:390cab7cd6e6 380 //
viviien 30:390cab7cd6e6 381 // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 382 // motor3_pwm.write(newmotor3); // write Duty Cycle
Feike 19:9c8ab7922191 383
viviien 28:43a1d67ff8ea 384 while(true){
viviien 29:7eb028b359a1 385 pc.printf("\n\r----------------------------------------------------------");
viviien 29:7eb028b359a1 386 pc.printf("\n\r----------------------------------------------------------");
viviien 30:390cab7cd6e6 387
viviien 28:43a1d67ff8ea 388
viviien 28:43a1d67ff8ea 389 delta_calcinverse(x0,y0,z0);
viviien 26:432d3519ba86 390
viviien 28:43a1d67ff8ea 391 oldtheta1 = theta1;
viviien 28:43a1d67ff8ea 392 oldtheta2 = theta2;
viviien 28:43a1d67ff8ea 393 oldtheta3 = theta3;
viviien 30:390cab7cd6e6 394
viviien 28:43a1d67ff8ea 395 char cc = pc.getc();
viviien 28:43a1d67ff8ea 396
viviien 31:3c13f1c35ee5 397 // timer = true;
viviien 30:390cab7cd6e6 398
viviien 30:390cab7cd6e6 399
viviien 28:43a1d67ff8ea 400 if (cc=='d') {
viviien 28:43a1d67ff8ea 401 x0=x0+5.0f;
viviien 28:43a1d67ff8ea 402 if (x0>=-75 && x0<=75){
viviien 28:43a1d67ff8ea 403 movefunctioninit ();
viviien 31:3c13f1c35ee5 404 // movestop();
viviien 28:43a1d67ff8ea 405 }
viviien 28:43a1d67ff8ea 406 else {
viviien 31:3c13f1c35ee5 407 // timer = false;
viviien 28:43a1d67ff8ea 408 x0=x0-5.0f;
viviien 30:390cab7cd6e6 409
viviien 28:43a1d67ff8ea 410 }
viviien 22:9f911405e096 411 }
viviien 28:43a1d67ff8ea 412
viviien 28:43a1d67ff8ea 413 if (cc=='a') {
viviien 28:43a1d67ff8ea 414 x0=x0-5.0f;
viviien 28:43a1d67ff8ea 415 if (x0>=-75 && x0<=75){
viviien 28:43a1d67ff8ea 416 movefunctioninit ();
viviien 31:3c13f1c35ee5 417 // movestop();
viviien 28:43a1d67ff8ea 418 }
viviien 28:43a1d67ff8ea 419 else {
viviien 31:3c13f1c35ee5 420 // timer = false;
viviien 28:43a1d67ff8ea 421 x0=x0+5.0f;
viviien 21:c826abca79c3 422 }
viviien 28:43a1d67ff8ea 423 }
viviien 28:43a1d67ff8ea 424
viviien 28:43a1d67ff8ea 425 if (cc=='w'){
viviien 28:43a1d67ff8ea 426 y0=y0+5.0f;
viviien 28:43a1d67ff8ea 427 if (y0>=-75 && y0<=75){
viviien 28:43a1d67ff8ea 428 movefunctioninit ();
viviien 31:3c13f1c35ee5 429 // movestop();
viviien 28:43a1d67ff8ea 430 }
viviien 28:43a1d67ff8ea 431 else {
viviien 31:3c13f1c35ee5 432 // timer = false;
viviien 28:43a1d67ff8ea 433 y0=y0-5.0f;
viviien 21:c826abca79c3 434 }
viviien 28:43a1d67ff8ea 435 }
viviien 28:43a1d67ff8ea 436
viviien 28:43a1d67ff8ea 437 if (cc=='s'){
viviien 28:43a1d67ff8ea 438 y0=y0-5.0f;
viviien 28:43a1d67ff8ea 439 if (y0>=-75 && y0<=75) {
viviien 28:43a1d67ff8ea 440 movefunctioninit ();
viviien 31:3c13f1c35ee5 441 // movestop();
viviien 28:43a1d67ff8ea 442 }
viviien 28:43a1d67ff8ea 443 else {
viviien 31:3c13f1c35ee5 444 // timer = false;
viviien 28:43a1d67ff8ea 445 y0=y0+5.0f;
Feike 19:9c8ab7922191 446 }
viviien 28:43a1d67ff8ea 447 }
viviien 28:43a1d67ff8ea 448
viviien 28:43a1d67ff8ea 449 if (cc=='u')
viviien 28:43a1d67ff8ea 450 {
viviien 31:3c13f1c35ee5 451 z0=z0+1.0f;
viviien 28:43a1d67ff8ea 452 if (z0>=-210 && z0<=-130) {
viviien 28:43a1d67ff8ea 453 movefunctioninit ();
viviien 31:3c13f1c35ee5 454 pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);
viviien 31:3c13f1c35ee5 455 pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);
viviien 31:3c13f1c35ee5 456 pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
viviien 31:3c13f1c35ee5 457 pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3);
viviien 31:3c13f1c35ee5 458 // movestop();
viviien 30:390cab7cd6e6 459 // for (float i=0.0; i<5; i++) {
viviien 30:390cab7cd6e6 460 // pc.printf("\n\r dit is error1 %f, error2 %f, error3 %f", error1, error2, error3);
viviien 30:390cab7cd6e6 461 // wait(1.0/10.0);
viviien 30:390cab7cd6e6 462 // }
viviien 28:43a1d67ff8ea 463 }
viviien 28:43a1d67ff8ea 464 else{
viviien 31:3c13f1c35ee5 465 pc.printf("else statement");
viviien 31:3c13f1c35ee5 466 // timer = false;
viviien 28:43a1d67ff8ea 467 z0=z0-5.0f;
viviien 21:c826abca79c3 468 }
viviien 28:43a1d67ff8ea 469 }
viviien 28:43a1d67ff8ea 470
viviien 28:43a1d67ff8ea 471 if (cc=='j')
viviien 28:43a1d67ff8ea 472 {
viviien 31:3c13f1c35ee5 473 z0=z0-1.0f;
viviien 28:43a1d67ff8ea 474 if (z0>=-210 && z0<=-130){
viviien 28:43a1d67ff8ea 475 movefunctioninit ();
viviien 31:3c13f1c35ee5 476 // movestop();
viviien 28:43a1d67ff8ea 477 }
viviien 28:43a1d67ff8ea 478 else {
viviien 31:3c13f1c35ee5 479 // timer = false;
viviien 28:43a1d67ff8ea 480 z0=z0+5.0f;
viviien 28:43a1d67ff8ea 481 }
viviien 28:43a1d67ff8ea 482 }
viviien 28:43a1d67ff8ea 483 // end while
viviien 21:c826abca79c3 484 }
viviien 29:7eb028b359a1 485
viviien 28:43a1d67ff8ea 486 // end main
viviien 22:9f911405e096 487 }