Diff: Motor_tryout.cpp

Revision:

31:3c13f1c35ee5

Parent:

30:390cab7cd6e6

Child:

32:60a71dcfdb7a

--- a/Motor_tryout.cpp	Thu Oct 31 09:56:54 2019 +0000
+++ b/Motor_tryout.cpp	Thu Oct 31 12:42:24 2019 +0000
@@ -97,7 +97,7 @@
 float derivative2 = 0;
 float derivative3 = 0;
 
-bool timer = false;
+//bool timer = false;
 
 Ticker      motor_timer; 
 void motor()
@@ -125,7 +125,7 @@
     derivative3 = error3 - lasterror3;
     
     Kp = 50;
-    Kd = potmeter1 * 100;
+    Kd = potmeter1 * 1000;
     
     
     // Proportional part
@@ -162,7 +162,7 @@
         newmotor3 = -1.0f;
         }
     
-    if (timer == true)  {
+    if (timer==true)    {
     motor1_pwm.write(fabs(newmotor1));
     motor1_dir.write(newmotor1>0);
     
@@ -173,11 +173,11 @@
     motor3_dir.write(newmotor3>0);
     }
     
-    else  {
-    motor1_pwm.write(0);
-    motor2_pwm.write(0);
-    motor3_pwm.write(0);
-        }
+//    else  {
+//    motor1_pwm.write(0);
+//    motor2_pwm.write(0);
+//    motor3_pwm.write(0);
+//        }
 }
       
      
@@ -241,17 +241,22 @@
 // Omschrijving
 float movefunctioninit ()   {   
     
-    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);       
+    theta1 = delta_calcinverse(x00,y00,z00);      
 
-    theta1 = theta1 - oldtheta1;
-    theta2 = theta2 - oldtheta2;
-    theta3 = theta3 - oldtheta3;    
-                
-    steps1 = steps1 + anglestep(theta1);
-    steps2 = steps2 + anglestep(theta2);
-    steps3 = steps3 + anglestep(theta3);
+//    theta1 = theta1 - oldtheta1;
+//    theta2 = theta2 - oldtheta2;
+//    theta3 = theta3 - oldtheta3;    
+//                
+//    steps1 = steps1 + anglestep(theta1);
+//    steps2 = steps2 + anglestep(theta2);
+//    steps3 = steps3 + anglestep(theta3);
+
+    steps1 = anglestep(theta1);
+    steps2 = anglestep(theta2);
+    steps3 = anglestep(theta3);
+    
+    pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);       
+    pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); 
     pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
     pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3);
 
@@ -303,10 +308,10 @@
 }
 
 // Omschrijving
-float movestop() {  
-wait(0.6);
-timer = false;
-} 
+//float movestop() {  
+//wait(0.6);
+//timer = false;
+//} 
 //       
 //    while (check1 || check2 || check3)    {
 //
@@ -389,17 +394,17 @@
                             
         char cc = pc.getc();
         
-        timer = true;
+ //       timer = true;
 
         
         if (cc=='d') {
             x0=x0+5.0f;
             if (x0>=-75 && x0<=75){
                 movefunctioninit ();
-                movestop();
+//                movestop();
             }
             else {
-                timer = false;
+  //              timer = false;
                 x0=x0-5.0f;
                 
             }
@@ -409,10 +414,10 @@
             x0=x0-5.0f;
             if (x0>=-75 && x0<=75){
                 movefunctioninit ();
-                movestop();
+//                movestop();
             }
             else {
-                timer = false;
+     //           timer = false;
                 x0=x0+5.0f;
             }
         }
@@ -421,10 +426,10 @@
             y0=y0+5.0f;
             if (y0>=-75 && y0<=75){
                 movefunctioninit ();
-                movestop();
+   //             movestop();
             }
             else {
-                timer = false;
+//                timer = false;
                 y0=y0-5.0f;
             }
         }
@@ -433,40 +438,45 @@
             y0=y0-5.0f;
             if (y0>=-75 && y0<=75)   {
                 movefunctioninit ();
-                movestop();
+//                movestop();
                 }
             else {
-                timer = false;
+//                timer = false;
                 y0=y0+5.0f;
             }
         }
         
         if (cc=='u') 
         {
-            z0=z0+5.0f;
+            z0=z0+1.0f;
             if (z0>=-210 && z0<=-130) {
                 movefunctioninit ();
-                movestop();
+    pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3);       
+    pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); 
+    pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3);
+    pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3);
+//                movestop();
 //                for (float i=0.0; i<5; i++)  {
 //                pc.printf("\n\r dit is error1 %f, error2 %f, error3 %f", error1, error2, error3);
 //                wait(1.0/10.0);
 //                }
             }
             else{
-                timer = false;
+                pc.printf("else statement");
+//                timer = false;
                 z0=z0-5.0f;
             }
         }
         
         if (cc=='j') 
         {
-            z0=z0-5.0f;
+            z0=z0-1.0f;
             if (z0>=-210 && z0<=-130){
                 movefunctioninit ();
-                movestop();
+  //              movestop();
             }
             else {
-                timer = false;
+//                timer = false;
                 z0=z0+5.0f;
             }
         }