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:
- 37:15446e49ff3d
- Parent:
- 35:4cb2ed6dd2d2
--- a/Motor_tryout.cpp Fri Nov 01 08:24:42 2019 +0000
+++ b/Motor_tryout.cpp Sat Nov 02 14:48:42 2019 +0000
@@ -218,11 +218,9 @@
//z0 = 158 - 60* sin(3*time_sin);
-if (sign == 1) {
float r = 20*(1-1/(1+(time_sin)));
- x0 = savedX0 - r*sin(3*time_sin);
- y0 = savedY0 - r*cos(3*time_sin);
-}
+ x0 = r*sin(3*time_sin);
+ y0 = r*cos(3*time_sin);
delta_calctheta1 ();
@@ -294,8 +292,6 @@
delta_calctheta3 ();
pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-
- char cc = pc.getc();
motor_timer.attach(&motor, 0.002);
@@ -308,6 +304,9 @@
pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+
+ char cc = pc.getc();
+
@@ -315,83 +314,83 @@
while(true){
- while (boxcheckC == 0) {
- if (boxcheckA == 1 && x0>=-76 && x0<=75) {
- x0=x0+1.0f ;
- wait(1.0/20);
- }
- if (boxcheckB== 1 && x0>=-75 && x0<=76) {
- x0=x0-1.0f;
- wait(1.0/20);
- }
- }
-
- wait(1.5);
- savedX = x0;
-
- while (boxcheckC == 0) {
- if (boxcheckA==1 && y0>=-76 && y0<=75){
- y0=y0+1.0f;
- wait(1.0/20);
- }
- if (boxcheckB==1 && y0>=-75 && y0<=76){
- y0=y0-1.0f;
- wait(1.0/20);
- }
- }
-
- wait(1.5);
- savedY = y0;
-
- while (boxcheckC == 0) {
- if (boxcheckA == 1 && z0>=-179 && z0<=-158){
- z0=z0+1.0f;
- wait(1.0/20);
- }
- if (boxcheckB == 1 && z0>=-178 && z0<=-157){
- z0=z0-1.0f;
- wait(1.0/20);
- }
- }
-
- wait(1.5);
- savedZ = z0;
-
- if (boxcheckC == 1) {
- sign = 1;
- }
-
- wait(5.0);
-
- sign = 0;
-
- float diffZ = -158 - z0;
- float diffX = 0 - x0;
- float diffY = 0 - y0;
-
- for (int i=0; i<=diffZ; i++) {
- z0 = z0+1;
- }
-
- for (int i=0; i<=diffY; i++) {
- if (diffY>0) {
- y0 = y0+1;
- }
- if (diffY<0) {
- y0 = y0-1;
- }
- }
-
- for (int i=0; i<=diffX; i++) {
- if (diffX>0) {
- x0 = x0+1;
- }
- if (diffX<0) {
- x0 = x0-1;
- }
- }
-
-break;
+// while (boxcheckC == 0) {
+// if (boxcheckA == 1 && x0>=-76 && x0<=75) {
+// x0=x0+1.0f ;
+// wait(1.0/20);
+// }
+// if (boxcheckB== 1 && x0>=-75 && x0<=76) {
+// x0=x0-1.0f;
+// wait(1.0/20);
+// }
+// }
+//
+// wait(1.5);
+// savedX = x0;
+//
+// while (boxcheckC == 0) {
+// if (boxcheckA==1 && y0>=-76 && y0<=75){
+// y0=y0+1.0f;
+// wait(1.0/20);
+// }
+// if (boxcheckB==1 && y0>=-75 && y0<=76){
+// y0=y0-1.0f;
+// wait(1.0/20);
+// }
+// }
+//
+// wait(1.5);
+// savedY = y0;
+//
+// while (boxcheckC == 0) {
+// if (boxcheckA == 1 && z0>=-179 && z0<=-158){
+// z0=z0+1.0f;
+// wait(1.0/20);
+// }
+// if (boxcheckB == 1 && z0>=-178 && z0<=-157){
+// z0=z0-1.0f;
+// wait(1.0/20);
+// }
+// }
+//
+// wait(1.5);
+// savedZ = z0;
+//
+// if (boxcheckC == 1) {
+// sign = 1;
+// }
+//
+// wait(5.0);
+//
+// sign = 0;
+//
+// float diffZ = -158 - z0;
+// float diffX = 0 - x0;
+// float diffY = 0 - y0;
+//
+// for (int i=0; i<=diffZ; i++) {
+// z0 = z0+1;
+// }
+//
+// for (int i=0; i<=diffY; i++) {
+// if (diffY>0) {
+// y0 = y0+1;
+// }
+// if (diffY<0) {
+// y0 = y0-1;
+// }
+// }
+//
+// for (int i=0; i<=diffX; i++) {
+// if (diffX>0) {
+// x0 = x0+1;
+// }
+// if (diffX<0) {
+// x0 = x0-1;
+// }
+// }
+//
+//break;
@@ -400,44 +399,44 @@
// wait(0.1);
-// char cc = pc.getc();
-// if (cc=='d' && x0>=-76 && x0<=75) {
-// x0=x0+1.0f ;
-// }
-//
-// if (cc=='a' && x0>=-75 && x0<=76) {
-//
-// x0=x0-1.0f;
-// }
-//
-// if (cc=='w' && y0>=-76 && y0<=75){
-// y0=y0+1.0f;
-// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-//
-// }
-//
-// if (cc=='s' && y0>=-75 && y0<=76){
-// y0=y0-1.0f;
-// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-// pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-// }
-//
-// if (cc=='u' && z0>=-211 && z0<=-130){
-// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-// z0=z0+1.0f;
-// }
-//
-// if (cc=='j' && z0>=-210 && z0<=-129){
-// pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
-// pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
-// pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
-// z0=z0-1.0f;
-// }
+ char cc = pc.getc();
+ if (cc=='d' && x0>=-76 && x0<=75) {
+ x0=x0+1.0f ;
+ }
+
+ if (cc=='a' && x0>=-75 && x0<=76) {
+
+ x0=x0-1.0f;
+ }
+
+ if (cc=='w' && y0>=-76 && y0<=75){
+ y0=y0+1.0f;
+ pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+ pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+ pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+
+ }
+
+ if (cc=='s' && y0>=-75 && y0<=76){
+ y0=y0-1.0f;
+ pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+ pc .printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+ pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+ }
+
+ if (cc=='u' && z0>=-211 && z0<=-130){
+ pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+ pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+ pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+ z0=z0+1.0f;
+ }
+
+ if (cc=='j' && z0>=-210 && z0<=-129){
+ pc.printf("\n\rtheta1 %f, theta2 %f, theta3 %f", theta1, theta2, theta3);
+ pc.printf("\n\rangle1 %f, angle2 %f, angle3 %f", angle1, angle2, angle3);
+ pc.printf("\n\rposition (%f %f %f)", x0, y0, z0);
+ z0=z0-1.0f;
+ }
}
//END MAIN
}
\ No newline at end of file