Feike van Veen
Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 19:9c8ab7922191
- Parent:
- 18:ab0fe311e7f3
- Child:
- 21:c826abca79c3
diff -r ab0fe311e7f3 -r 9c8ab7922191 Motor_tryout.cpp
--- a/Motor_tryout.cpp Mon Oct 21 11:01:18 2019 +0000
+++ b/Motor_tryout.cpp Fri Oct 25 07:18:00 2019 +0000
@@ -19,16 +19,18 @@
QEI2 Encoder2(D10,D11,NC,64,QEI2::X4_ENCODING);
QEI Encoder(D2,D3,NC,64,QEI::X4_ENCODING);
+int check;
int quit;
+int limit_pos = 8400;
+float steps;
+int g = 0;
+
+bool check1;
+bool check2;
+bool check3;
int counts1 = 0;
int counts2 = 0;
int counts3 = 0;
-int limit_pos = 8400;
-int x;
-int y;
-int z;
-float steps;
-int g = 0;
const float le = 15.0;
const float f = 37.5;
@@ -43,17 +45,18 @@
float rje;
float r2;
float r;
+
float z0=-172;
float y0=0;
float x0=0;
-int check;
+
float theta1;
float theta2;
float theta3;
float oldtheta1=0;
float oldtheta2=0;
float oldtheta3=0;
-int direction;
+
float delta_calcangleyz(float x0, float y0, float z0) {
float y2 = y0 + le;
float y1 = f;
@@ -64,51 +67,41 @@
float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0);
float r = sqrt(r2);
-
- if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r))
- {
- int check = 1;
- pc.printf("\n\rPunt bestaat niet");
- }
- else
- {
- float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
- float beta = atan((z1-z2)/(y1-y2));
-
- if(beta<0) {
+ if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
+ int check = 1;
+ pc.printf("\n\rPunt bestaat niet");
+ }
+ else {
+ float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
+ float beta = atan((z1-z2)/(y1-y2));
+ if(beta<0) {
beta = beta + pi;
}
- float theta1 = (beta - alpha)*180.0/pi;
- int check = 0;
- return theta1;
+ float theta1 = (beta - alpha)*180.0/pi;
+ return theta1;
}
}
float delta_calcinverse(float x0, float y0, float z0) {
- theta1 = theta2 = theta3 = 0;
- theta1 = delta_calcangleyz(x0, y0, z0);
- if (check == 0)
- {
- theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0);
- theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0);
- //return theta2;
- //return theta3;
- }
+theta1 = theta2 = theta3 = 0;
+theta1 = delta_calcangleyz(x0, y0, z0);
+
+ if (check == 0) {
+ theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0);
+ theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0);
+ }
+
return theta1, theta2, theta3;
-
}
-
Ticker pulse;
-void pulseget()
-{
+void pulseget() {
counts1 = Encoder1.getPulses();
counts2 = Encoder2.getPulses();
counts3 = Encoder.getPulses();
}
-int anglestep(float angle)
-{
+float anglestep(float angle) {
float steps;
steps = angle / 360 * 8400;
return steps;
@@ -120,99 +113,51 @@
pc.baud(115200);
char cc = pc.getc();
- pc.printf("\n\r cc check");
- //char key = term.getc();
- pc.printf("\n\r term check");
+
+ while(true) {
+ counts1 = 0;
+ counts2 = 0;
+ counts3 = 0;
- float angle = 360;
- while(true)
- {
- counts1 = 0;
- counts2 = 0;
- counts3 = 0;
-
- float steps1;
- float steps2;
- float steps3;
-
- counts1 = Encoder1.getPulses();
- counts2 = Encoder2.getPulses();
- counts3 = Encoder.getPulses();
-
- delta_calcinverse(x0,y0,z0);
-
- oldtheta1 = theta1;
- oldtheta2 = theta2;
- oldtheta3 = theta3;
-
+ delta_calcinverse(x0,y0,z0);
+
+ oldtheta1 = theta1;
+ oldtheta2 = theta2;
+ oldtheta3 = theta3;
+
char cc = pc.getc();
if (cc=='a') {
- pc.printf("\n\rleft");
- z0=z0-1.0f;
-
+ check1=true;
+ check2=true;
+ check3=true;
+
+ z0=z0+2.0f;
+
theta1 = delta_calcinverse(x0,y0,z0);
- theta1 = delta_calcangleyz(x0,y0,z0);
theta1 = theta1 - oldtheta1;
theta2 = theta2 - oldtheta2;
- theta3 = theta3 - oldtheta3;
-
- steps1 = anglestep(theta1);
- steps2 = anglestep(theta2);
- steps3 = anglestep(theta3);
-
- int frequency_pwm = 10000; //10 kHz PWM
- motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
- motor1_dir.write(1); // positief
- motor1_pwm.write(0.7); // write Duty Cycle
-
- motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
- motor2_dir.write(0); // positief
- motor2_pwm.write(0.7); // write Duty Cycle
-
- motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
- motor3_dir.write(1); // positief
- motor3_pwm.write(0.7); // write Duty Cycle
-
-
-}
-if (cc=='d') {
- pc.printf("\n\rright");
+ theta3 = theta3 - oldtheta3;
- z0=z0-10.0f;
-
- theta1 = delta_calcinverse(x0,y0,z0);
- theta1 = delta_calcangleyz(x0,y0,z0);
-
- theta1 = theta1 - oldtheta1;
- theta2 = theta2 - oldtheta2;
- theta3 = theta3 - oldtheta3;
-
float steps1 = anglestep(theta1);
float steps2 = anglestep(theta2);
float steps3 = anglestep(theta3);
- pc.printf("\n\r the difference in angles are (%f, %f, %f)", theta1, theta2, theta3);
- pc.printf("\n\r the steps are (%f, %f, %f)", steps1, steps2, steps3);
+ motor1_dir.write(1); // positief
+ motor2_dir.write(0); // positief
+ motor3_dir.write(1); // positief
+ if (steps1>0) {
motor1_dir.write(0); // positief
- motor2_dir.write(1); // positief
- motor3_dir.write(0); // positief
-
- if (steps1<0) {
- motor1_dir.write(1); // positief
}
-
- if (steps1<0) {
- motor2_dir.write(0); // positief
-
- }
-
- if (steps1<0) {
- motor3_dir.write(1); // positief
- }
+ if (steps1>0) {
+ motor2_dir.write(1); // positief
+ }
+ if (steps1>0) {
+ motor3_dir.write(0); // positief
+ }
int frequency_pwm = 10000; //10 kHz PWM
motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
@@ -223,17 +168,107 @@
motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
motor3_pwm.write(0.7); // write Duty Cycle
+
+ wait(1.0/100);
+
- while (abs(counts1)<=abs(steps1) || abs(counts2)<=abs(steps2) || abs(counts3)<=abs(steps3)) {
- pc.printf("while");
+ while (check1 || check2 || check3) {
+ counts1 = Encoder1.getPulses();
+ counts2 = Encoder2.getPulses();
+ counts3 = Encoder.getPulses();
+
+ wait(1.0/1000);
+ //pc.printf("while");
if(abs(counts1)>=abs(steps1)) {
motor1_pwm.write(0);
+ check1=false;
+ counts1=0;
}
if (abs(counts2)>=abs(steps2)) {
motor2_pwm.write(0);
+ check2=false;
+ counts2=0;
}
if (abs(counts3)>=abs(steps3)) {
motor3_pwm.write(0);
+ check3=false;
+ counts3=0;
+ }
+ }
+}
+
+if (cc=='d') {
+
+ check1=true;
+ check2=true;
+ check3=true;
+
+ x0=x0-2.0f;
+
+ theta1 = delta_calcinverse(x0,y0,z0);
+ pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);
+ pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);
+
+ theta1 = theta1 - oldtheta1;
+ theta2 = theta2 - oldtheta2;
+ theta3 = theta3 - oldtheta3;
+
+ float steps1 = anglestep(theta1);
+ float steps2 = anglestep(theta2);
+ float steps3 = anglestep(theta3);
+
+ //pc.printf("\n\r (%f, %f, %f)", steps1, steps2, steps3);
+
+
+ motor1_dir.write(0); // positief
+ motor2_dir.write(1); // positief
+ motor3_dir.write(0); // positief
+
+ if (steps1<0) {
+ motor1_dir.write(1); // positief
+ }
+ if (steps1<0) {
+ motor2_dir.write(0); // positief
+ }
+ if (steps1<0) {
+ motor3_dir.write(1); // positief
+ }
+
+ int frequency_pwm = 10000; //10 kHz PWM
+ motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+ motor1_pwm.write(0.7); // write Duty Cycle
+
+ motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+ motor2_pwm.write(0.7); // write Duty Cycle
+
+ motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+ motor3_pwm.write(0.7); // write Duty Cycle
+
+ while (check1 || check2 || check3) {
+ counts1 = Encoder1.getPulses();
+ counts2 = Encoder2.getPulses();
+ counts3 = Encoder.getPulses();
+
+ wait(1.0/500);
+ //pc.printf("while");
+ if(abs(counts1)>=abs(steps1)) {
+ motor1_pwm.write(0);
+ check1=false;
+ counts1=0;
+ wait(1.0/100);
+ }
+ if (abs(counts2)>=abs(steps2)) {
+ motor2_pwm.write(0);
+ check2=false;
+ counts2=0;
+ wait(1.0/100);
+ }
+ if (abs(counts3)>=abs(steps3)) {
+ motor3_pwm.write(0);
+ check3=false;
+ //pc.printf("\n\rsteps %f, counts %f", steps3, counts3);
+ counts3=0;
+ wait(1.0/100);
}
}
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 30 Sep 2019 |
| Imports: | 7 |
| Forks: | 0 |
| Commits: | 37 |
| Dependents: | 0 |
| Dependencies: | 7 |
| Followers: | 5 |
Developers
Feike van Veen
Angelique van Leeuwen Vivien van Veldhuizen Maarten van Bourgondien Nina Gesthuizen