Diff: Motor_tryout.cpp

Revision:

22:9f911405e096

Parent:

21:c826abca79c3

Child:

23:18b0be02187f

--- a/Motor_tryout.cpp	Fri Oct 25 11:23:15 2019 +0000
+++ b/Motor_tryout.cpp	Fri Oct 25 11:44:33 2019 +0000
@@ -37,6 +37,8 @@
 const float re = 174.0;
 const float rf = 50.0;
 const float pi = 3.14159265358979323846;
+const float cospi = -0.5;
+const float sinpi = 0.8660254;
 float y2;
 float y1;
 float z1;
@@ -67,7 +69,7 @@
     float z2 = z00;
     float rje2 = re*re - x00*x00;
     float rje = sqrt(rje2);
-    float r2 = (y1-y2)(y1-y2) + (z1-z00)(z1-z00);
+    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,22 +84,25 @@
                     }
                 float theta1 = (beta - alpha)*180.0/pi;
                 return theta1;
-        }
+}
 }
 
 float delta_calcinverse(float x00, float y00, float z00) {
     theta1 = theta2 = theta3 = 0;
+    x00=x0;
+    y00=y0;
+    z00=z0;
     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);
+            theta2 = delta_calcangleyz(x00, y00, z00);
             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);
+            theta3 = delta_calcangleyz(x00, y00, z00);
             }
             
     return theta1, theta2, theta3;
@@ -117,9 +122,9 @@
     }
 
 
-float move_steps() {
+float movefunction() {
     
-    theta1 = delta_calcinverse(x0,y0,z0);
+    theta1 = delta_calcinverse(x00,y00,z00);
     pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);       
     pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0);       
 
@@ -161,7 +166,11 @@
     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   
+    motor3_pwm.write(0.7); // write Duty Cycle  
+    
+    check1 = true;
+    check2 = true;
+    check3 = true; 
                 
     while (check1 || check2 || check3)    {
         counts1 = Encoder1.getPulses();
@@ -190,8 +199,9 @@
             wait(1.0/100);
             }
     }
-int main(void)
-{    
+}
+
+int main(void)  {    
     pc.baud(115200);
         
     char cc = pc.getc();
@@ -209,13 +219,13 @@
 char cc = pc.getc();
 
 if (cc=='d') {
-x0=x0+2.0f;
-if (x0>=-75 && x0<=75)   {
-    movefunction ();
-    }
-    else    {
-            x0=x0-2.0f;
-            }
+    x0=x0+2.0f;
+    if (x0>=-75 && x0<=75)   {
+        movefunction ();
+        }
+            else    {
+                x0=x0-2.0f;
+                }
 }
 
 if (cc=='a') {
@@ -267,6 +277,5 @@
             z0=z0+2.0f;
             }
 }                                       
-
- 
- 
\ No newline at end of file
+}
+}  
\ No newline at end of file