Vivien van Veldhuizen
Signa-bot code for project BioRobotics, at University of Twente.
Dependencies: mbed QEI MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 21:c826abca79c3
- Parent:
- 19:9c8ab7922191
- Child:
- 22:9f911405e096
diff -r ff45b4e1a475 -r c826abca79c3 Motor_tryout.cpp
--- a/Motor_tryout.cpp Fri Oct 25 07:21:41 2019 +0000
+++ b/Motor_tryout.cpp Fri Oct 25 11:23:15 2019 +0000
@@ -49,6 +49,9 @@
float z0=-172;
float y0=0;
float x0=0;
+float x00;
+float y00;
+float z00;
float theta1;
float theta2;
@@ -57,14 +60,14 @@
float oldtheta2=0;
float oldtheta3=0;
-float delta_calcangleyz(float x0, float y0, float z0) {
- float y2 = y0 + le;
+float delta_calcangleyz(float x00, float y00, float z00) {
+ float y2 = y00 + le;
float y1 = f;
float z1 = 0.0;
- float z2 = z0;
- float rje2 = re*re - x0*x0;
+ float z2 = z00;
+ float rje2 = re*re - x00*x00;
float rje = sqrt(rje2);
- float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0);
+ float r2 = (y1-y2)(y1-y2) + (z1-z00)(z1-z00);
float r = sqrt(r2);
if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
@@ -82,19 +85,25 @@
}
}
-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 theta1, theta2, theta3;
+float delta_calcinverse(float x00, float y00, float z00) {
+ theta1 = theta2 = theta3 = 0;
+ theta1 = delta_calcangleyz(x00, y00, z00);
+
+ if (check == 0) {
+ x00=x0*cospi+y0*sinpi;
+ y00=y0*cospi-x0*sinpi;
+ z00=z0;
+ theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0);
+ x00=x0*cospi-y0*sinpi;
+ y00=y0*cospi+y0*sinpi;
+ z00=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;
+//Ticker pulse;
void pulseget() {
counts1 = Encoder1.getPulses();
counts2 = Encoder2.getPulses();
@@ -107,7 +116,80 @@
return steps;
}
-// DE MAIN FUNCTIE
+
+float move_steps() {
+
+ 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);
+
+ // Set the direction of the motors.
+ if (theta1 < 0) {
+ motor1_dir.write(1);
+ }
+ else {
+ motor1_dir.write(0);
+ }
+
+ if (theta2 < 0) {
+ motor2_dir.write(0);
+ }
+ else {
+ motor2_dir.write(1);
+ }
+
+ if (theta3 < 0) {
+ motor3_dir.write(0);
+ }
+ else {
+ motor3_dir.write(1);
+ }
+
+ 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);
+
+ 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);
+ }
+ }
int main(void)
{
pc.baud(115200);
@@ -123,158 +205,68 @@
oldtheta1 = theta1;
oldtheta2 = theta2;
- oldtheta3 = theta3;
-
+ oldtheta3 = theta3;
char cc = pc.getc();
-
-if (cc=='a') {
- check1=true;
- check2=true;
- check3=true;
-
- z0=z0+2.0f;
-
- theta1 = delta_calcinverse(x0,y0,z0);
-
- theta1 = theta1 - oldtheta1;
- theta2 = theta2 - oldtheta2;
- theta3 = theta3 - oldtheta3;
-
- float steps1 = anglestep(theta1);
- float steps2 = anglestep(theta2);
- float steps3 = anglestep(theta3);
-
- motor1_dir.write(1); // positief
- motor2_dir.write(0); // positief
- motor3_dir.write(1); // positief
-
- if (steps1>0) {
- motor1_dir.write(0); // positief
+if (cc=='d') {
+x0=x0+2.0f;
+if (x0>=-75 && x0<=75) {
+ movefunction ();
+ }
+ else {
+ x0=x0-2.0f;
}
- 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
- 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
-
- wait(1.0/100);
+}
-
- 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=='a') {
+x0=x0-2.0f;
+if (x0>=-75 && x0<=75) {
+ movefunction ();
+ }
+ else {
+ x0=x0+2.0f;
+ }
+}
+
+if (cc=='w') {
+y0=y0+2.0f;
+if (y0>=-75 && y0<=75) {
+ movefunction ();
+ }
+ else {
+ y0=y0-2.0f;
+ }
}
-
-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 (cc=='s') {
+y0=y0-2.0f;
+if (y0>=-75 && y0<=75) {
+ movefunction ();
+ }
+ else {
+ y0=y0+2.0f;
}
- 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);
- }
- }
+}
+
+if (cc=='u') {
+z0=z0+2.0f;
+if (z0>=-210 && z0<=-130) {
+ movefunction ();
+ }
+ else {
+ z0=z0-2.0f;
+ }
+}
-}
-
-
- wait(1.0/100);
+if (cc=='j') {
+z0=z0-2.0f;
+if (z0>=-210 && z0<=-130) {
+ movefunction ();
+ }
+ else {
+ z0=z0+2.0f;
}
-}
\ No newline at end of file
+}
+
+
+
\ No newline at end of file