Vivien van Veldhuizen / Mbed 2 deprecated Signa-bot

Signa-bot code for project BioRobotics, at University of Twente.

Dependencies:   mbed QEI MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@30:390cab7cd6e6, 2019-10-31 (annotated)

Committer:
viviien
Date:
Thu Oct 31 09:56:54 2019 +0000
Revision:
30:390cab7cd6e6
Parent:
29:7eb028b359a1
Child:
31:3c13f1c35ee5
Helemaal werkend

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 30:390cab7cd6e6 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 30:390cab7cd6e6 128 Kd = potmeter1 * 100;
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 30:390cab7cd6e6 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 30:390cab7cd6e6 176 else {
viviien 30:390cab7cd6e6 177 motor1_pwm.write(0);
viviien 30:390cab7cd6e6 178 motor2_pwm.write(0);
viviien 30:390cab7cd6e6 179 motor3_pwm.write(0);
viviien 30:390cab7cd6e6 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 22:9f911405e096 244 theta1 = delta_calcinverse(x00,y00,z00);
viviien 21:c826abca79c3 245 pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);
viviien 21:c826abca79c3 246 pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);
viviien 21:c826abca79c3 247
viviien 21:c826abca79c3 248 theta1 = theta1 - oldtheta1;
viviien 21:c826abca79c3 249 theta2 = theta2 - oldtheta2;
viviien 21:c826abca79c3 250 theta3 = theta3 - oldtheta3;
viviien 21:c826abca79c3 251
viviien 30:390cab7cd6e6 252 steps1 = steps1 + anglestep(theta1);
viviien 30:390cab7cd6e6 253 steps2 = steps2 + anglestep(theta2);
viviien 30:390cab7cd6e6 254 steps3 = steps3 + anglestep(theta3);
viviien 27:3eb181cbe183 255 pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
viviien 30:390cab7cd6e6 256 pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3);
viviien 30:390cab7cd6e6 257
viviien 28:43a1d67ff8ea 258
viviien 28:43a1d67ff8ea 259
viviien 30:390cab7cd6e6 260 // pc.printf("\n\r error1: %f", error1);
viviien 30:390cab7cd6e6 261
viviien 27:3eb181cbe183 262
viviien 21:c826abca79c3 263 // Set the direction of the motors.
viviien 30:390cab7cd6e6 264 // if (theta1 < 0) {
viviien 30:390cab7cd6e6 265 // motor1_dir.write(1);
viviien 30:390cab7cd6e6 266 // }
viviien 30:390cab7cd6e6 267 // else {
viviien 30:390cab7cd6e6 268 // motor1_dir.write(0);
viviien 30:390cab7cd6e6 269 // }
viviien 30:390cab7cd6e6 270 //
viviien 30:390cab7cd6e6 271 // if (theta2 < 0) {
viviien 30:390cab7cd6e6 272 // motor2_dir.write(0);
viviien 30:390cab7cd6e6 273 // }
viviien 30:390cab7cd6e6 274 // else {
viviien 30:390cab7cd6e6 275 // motor2_dir.write(1);
viviien 30:390cab7cd6e6 276 // }
viviien 30:390cab7cd6e6 277 //
viviien 30:390cab7cd6e6 278 // if (theta3 < 0) {
viviien 30:390cab7cd6e6 279 // motor3_dir.write(0);
viviien 30:390cab7cd6e6 280 // }
viviien 30:390cab7cd6e6 281 // else {
viviien 30:390cab7cd6e6 282 // motor3_dir.write(1);
viviien 30:390cab7cd6e6 283 // }
viviien 30:390cab7cd6e6 284 //
viviien 30:390cab7cd6e6 285 // int frequency_pwm = 10000; //10 kHz PWM
viviien 30:390cab7cd6e6 286 // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 287 // motor1_pwm.write(0.57); // write Duty Cycle
viviien 30:390cab7cd6e6 288 //
viviien 30:390cab7cd6e6 289 // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 290 // motor2_pwm.write(0.57); // write Duty Cycle
viviien 30:390cab7cd6e6 291 //
viviien 30:390cab7cd6e6 292 // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 293 // motor3_pwm.write(0.57); // write Duty Cycle
viviien 30:390cab7cd6e6 294 //
viviien 30:390cab7cd6e6 295 // check1 = true;
viviien 30:390cab7cd6e6 296 // check2 = true;
viviien 30:390cab7cd6e6 297 // check3 = true;
viviien 30:390cab7cd6e6 298 //
viviien 30:390cab7cd6e6 299 // Encoder1.reset();
viviien 30:390cab7cd6e6 300 // Encoder2.reset();
viviien 30:390cab7cd6e6 301 // Encoder3.reset();
viviien 30:390cab7cd6e6 302 //
viviien 28:43a1d67ff8ea 303 }
viviien 25:eb3204e45d33 304
viviien 28:43a1d67ff8ea 305 // Omschrijving
viviien 30:390cab7cd6e6 306 float movestop() {
viviien 30:390cab7cd6e6 307 wait(0.6);
viviien 30:390cab7cd6e6 308 timer = false;
viviien 30:390cab7cd6e6 309 }
viviien 30:390cab7cd6e6 310 //
viviien 30:390cab7cd6e6 311 // while (check1 || check2 || check3) {
viviien 30:390cab7cd6e6 312 //
viviien 30:390cab7cd6e6 313 // int frequency_pwm = 10000; //10 kHz PWM
viviien 30:390cab7cd6e6 314 // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 315 // motor1_pwm.write(newmotor1); // write Duty Cycle
viviien 30:390cab7cd6e6 316 //
viviien 30:390cab7cd6e6 317 // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 318 // motor2_pwm.write(newmotor2); // write Duty Cycle
viviien 30:390cab7cd6e6 319 //
viviien 30:390cab7cd6e6 320 // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 321 // motor3_pwm.write(newmotor3); // write Duty Cycle
viviien 30:390cab7cd6e6 322 //
viviien 30:390cab7cd6e6 323 // if (error1==0) {
viviien 30:390cab7cd6e6 324 // check1=false;
viviien 30:390cab7cd6e6 325 // }
viviien 30:390cab7cd6e6 326 // if (error2==0) {
viviien 30:390cab7cd6e6 327 // check2=false;
viviien 30:390cab7cd6e6 328 // }
viviien 30:390cab7cd6e6 329 // if (error3==0) {
viviien 30:390cab7cd6e6 330 // check3=false;
viviien 30:390cab7cd6e6 331 // }
viviien 30:390cab7cd6e6 332 //
viviien 30:390cab7cd6e6 333 //// if(abs(counts1)>=abs(steps1)) {
viviien 30:390cab7cd6e6 334 //// pc.printf("\n\r 1 is false");
viviien 30:390cab7cd6e6 335 //// motor1_pwm.write(0);
viviien 30:390cab7cd6e6 336 //// check1=false;
viviien 30:390cab7cd6e6 337 //// pc.printf("\n\rcounts1 %i", counts1);
viviien 30:390cab7cd6e6 338 //// }
viviien 30:390cab7cd6e6 339 ////
viviien 30:390cab7cd6e6 340 //// if (abs(counts2)>=abs(steps2)) {
viviien 30:390cab7cd6e6 341 //// pc.printf("\n\r 2 is false");
viviien 30:390cab7cd6e6 342 //// motor2_pwm.write(0);
viviien 30:390cab7cd6e6 343 //// check2=false;
viviien 30:390cab7cd6e6 344 //// pc.printf("\n\rcounts2 %i", counts2);
viviien 30:390cab7cd6e6 345 //// }
viviien 30:390cab7cd6e6 346 ////
viviien 30:390cab7cd6e6 347 //// if (abs(counts3)>=abs(steps3)) {
viviien 30:390cab7cd6e6 348 //// pc.printf("\n\r 3 is false");
viviien 30:390cab7cd6e6 349 //// motor3_pwm.write(0);
viviien 30:390cab7cd6e6 350 //// check3=false;
viviien 30:390cab7cd6e6 351 //// pc.printf("\n\rcounts3 %i", counts3);
viviien 30:390cab7cd6e6 352 //// }
viviien 30:390cab7cd6e6 353 // }
viviien 30:390cab7cd6e6 354 // wait(1.0);
viviien 30:390cab7cd6e6 355 // pc.printf("\n\rdone moving");
viviien 30:390cab7cd6e6 356 //}
viviien 22:9f911405e096 357
viviien 28:43a1d67ff8ea 358
viviien 28:43a1d67ff8ea 359 ///////////////////
viviien 28:43a1d67ff8ea 360 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 361 ///////////////////
viviien 28:43a1d67ff8ea 362
viviien 22:9f911405e096 363 int main(void) {
viviien 30:390cab7cd6e6 364 motor_timer.attach(&motor, 0.002);
viviien 26:432d3519ba86 365
viviien 28:43a1d67ff8ea 366 pc.baud(115200);
Feike 17:6da57acb7bea 367 char cc = pc.getc();
viviien 30:390cab7cd6e6 368
viviien 30:390cab7cd6e6 369 int frequency_pwm = 10000; //10 kHz PWM
viviien 30:390cab7cd6e6 370 // motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 371 // motor1_pwm.write(newmotor1); // write Duty Cycle
viviien 30:390cab7cd6e6 372 //
viviien 30:390cab7cd6e6 373 // motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 374 // motor2_pwm.write(newmotor2); // write Duty Cycle
viviien 30:390cab7cd6e6 375 //
viviien 30:390cab7cd6e6 376 // motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
viviien 30:390cab7cd6e6 377 // motor3_pwm.write(newmotor3); // write Duty Cycle
Feike 19:9c8ab7922191 378
viviien 28:43a1d67ff8ea 379 while(true){
viviien 29:7eb028b359a1 380 pc.printf("\n\r----------------------------------------------------------");
viviien 29:7eb028b359a1 381 pc.printf("\n\r----------------------------------------------------------");
viviien 30:390cab7cd6e6 382
viviien 28:43a1d67ff8ea 383
viviien 28:43a1d67ff8ea 384 delta_calcinverse(x0,y0,z0);
viviien 26:432d3519ba86 385
viviien 28:43a1d67ff8ea 386 oldtheta1 = theta1;
viviien 28:43a1d67ff8ea 387 oldtheta2 = theta2;
viviien 28:43a1d67ff8ea 388 oldtheta3 = theta3;
viviien 30:390cab7cd6e6 389
viviien 28:43a1d67ff8ea 390 char cc = pc.getc();
viviien 28:43a1d67ff8ea 391
viviien 30:390cab7cd6e6 392 timer = true;
viviien 30:390cab7cd6e6 393
viviien 30:390cab7cd6e6 394
viviien 28:43a1d67ff8ea 395 if (cc=='d') {
viviien 28:43a1d67ff8ea 396 x0=x0+5.0f;
viviien 28:43a1d67ff8ea 397 if (x0>=-75 && x0<=75){
viviien 28:43a1d67ff8ea 398 movefunctioninit ();
viviien 30:390cab7cd6e6 399 movestop();
viviien 28:43a1d67ff8ea 400 }
viviien 28:43a1d67ff8ea 401 else {
viviien 30:390cab7cd6e6 402 timer = false;
viviien 28:43a1d67ff8ea 403 x0=x0-5.0f;
viviien 30:390cab7cd6e6 404
viviien 28:43a1d67ff8ea 405 }
viviien 22:9f911405e096 406 }
viviien 28:43a1d67ff8ea 407
viviien 28:43a1d67ff8ea 408 if (cc=='a') {
viviien 28:43a1d67ff8ea 409 x0=x0-5.0f;
viviien 28:43a1d67ff8ea 410 if (x0>=-75 && x0<=75){
viviien 28:43a1d67ff8ea 411 movefunctioninit ();
viviien 30:390cab7cd6e6 412 movestop();
viviien 28:43a1d67ff8ea 413 }
viviien 28:43a1d67ff8ea 414 else {
viviien 30:390cab7cd6e6 415 timer = false;
viviien 28:43a1d67ff8ea 416 x0=x0+5.0f;
viviien 21:c826abca79c3 417 }
viviien 28:43a1d67ff8ea 418 }
viviien 28:43a1d67ff8ea 419
viviien 28:43a1d67ff8ea 420 if (cc=='w'){
viviien 28:43a1d67ff8ea 421 y0=y0+5.0f;
viviien 28:43a1d67ff8ea 422 if (y0>=-75 && y0<=75){
viviien 28:43a1d67ff8ea 423 movefunctioninit ();
viviien 30:390cab7cd6e6 424 movestop();
viviien 28:43a1d67ff8ea 425 }
viviien 28:43a1d67ff8ea 426 else {
viviien 30:390cab7cd6e6 427 timer = false;
viviien 28:43a1d67ff8ea 428 y0=y0-5.0f;
viviien 21:c826abca79c3 429 }
viviien 28:43a1d67ff8ea 430 }
viviien 28:43a1d67ff8ea 431
viviien 28:43a1d67ff8ea 432 if (cc=='s'){
viviien 28:43a1d67ff8ea 433 y0=y0-5.0f;
viviien 28:43a1d67ff8ea 434 if (y0>=-75 && y0<=75) {
viviien 28:43a1d67ff8ea 435 movefunctioninit ();
viviien 30:390cab7cd6e6 436 movestop();
viviien 28:43a1d67ff8ea 437 }
viviien 28:43a1d67ff8ea 438 else {
viviien 30:390cab7cd6e6 439 timer = false;
viviien 28:43a1d67ff8ea 440 y0=y0+5.0f;
Feike 19:9c8ab7922191 441 }
viviien 28:43a1d67ff8ea 442 }
viviien 28:43a1d67ff8ea 443
viviien 28:43a1d67ff8ea 444 if (cc=='u')
viviien 28:43a1d67ff8ea 445 {
viviien 28:43a1d67ff8ea 446 z0=z0+5.0f;
viviien 28:43a1d67ff8ea 447 if (z0>=-210 && z0<=-130) {
viviien 28:43a1d67ff8ea 448 movefunctioninit ();
viviien 30:390cab7cd6e6 449 movestop();
viviien 30:390cab7cd6e6 450 // for (float i=0.0; i<5; i++) {
viviien 30:390cab7cd6e6 451 // pc.printf("\n\r dit is error1 %f, error2 %f, error3 %f", error1, error2, error3);
viviien 30:390cab7cd6e6 452 // wait(1.0/10.0);
viviien 30:390cab7cd6e6 453 // }
viviien 28:43a1d67ff8ea 454 }
viviien 28:43a1d67ff8ea 455 else{
viviien 30:390cab7cd6e6 456 timer = false;
viviien 28:43a1d67ff8ea 457 z0=z0-5.0f;
viviien 21:c826abca79c3 458 }
viviien 28:43a1d67ff8ea 459 }
viviien 28:43a1d67ff8ea 460
viviien 28:43a1d67ff8ea 461 if (cc=='j')
viviien 28:43a1d67ff8ea 462 {
viviien 28:43a1d67ff8ea 463 z0=z0-5.0f;
viviien 28:43a1d67ff8ea 464 if (z0>=-210 && z0<=-130){
viviien 28:43a1d67ff8ea 465 movefunctioninit ();
viviien 30:390cab7cd6e6 466 movestop();
viviien 28:43a1d67ff8ea 467 }
viviien 28:43a1d67ff8ea 468 else {
viviien 30:390cab7cd6e6 469 timer = false;
viviien 28:43a1d67ff8ea 470 z0=z0+5.0f;
viviien 28:43a1d67ff8ea 471 }
viviien 28:43a1d67ff8ea 472 }
viviien 28:43a1d67ff8ea 473 // end while
viviien 21:c826abca79c3 474 }
viviien 29:7eb028b359a1 475
viviien 28:43a1d67ff8ea 476 // end main
viviien 22:9f911405e096 477 }