Diff: Motor_tryout.cpp

Revision:

32:60a71dcfdb7a

Parent:

31:3c13f1c35ee5

Child:

33:88fbf14d8aaf

diff -r 3c13f1c35ee5 -r 60a71dcfdb7a Motor_tryout.cpp
--- a/Motor_tryout.cpp	Thu Oct 31 12:42:24 2019 +0000
+++ b/Motor_tryout.cpp	Thu Oct 31 14:30:59 2019 +0000
@@ -57,9 +57,6 @@
 float z0=-172;
 float y0=0;
 float x0=0;
-float x00;
-float y00;
-float z00;
 
 float theta1;
 float theta2;
@@ -97,49 +94,126 @@
 float derivative2 = 0;
 float derivative3 = 0;
 
-//bool timer = false;
+float angle1 = 0;
+float angle2 = 0;
+float angle3 = 0;
+
+bool timer = false;
+float time_sin = 0;
+
+     
+void delta_calctheta1 ()   {
+    float y2 = y0 + le;
+    float y1 = f;
+    float z1 = 0.0;
+    float z2 = z0;
+    float rje2 = re*re - x0*x0;
+    float rje = sqrt(rje2);
+    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)  {
+                    beta = beta + pi;
+                    }
+                theta1 = (beta - alpha);
+    }
+}
+
+void delta_calctheta2 ()   {
+    float xref = x0*cospi+y0*sinpi;
+    float yref = y0*cospi-x0*sinpi;
+    float zref = z0;
+    float y2 = yref + le;
+    float y1 = f;
+    float z1 = 0.0;
+    float z2 = zref;
+    float rje2 = re*re - xref*xref;
+    float rje = sqrt(rje2);
+    float r2 = (y1-y2)*(y1-y2) + (z1-zref)*(z1-zref);
+    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)  {
+                    beta = beta + pi;
+                    }
+                theta2 = (beta - alpha);
+    }
+}
+
+void delta_calctheta3 ()   {
+    float xreff = x0*cospi-y0*sinpi;
+    float yreff = y0*cospi+y0*sinpi;
+    float zreff = z0;
+    float y2 = yreff + le;
+    float y1 = f;
+    float z1 = 0.0;
+    float z2 = zreff;
+    float rje2 = re*re - xreff*xreff;
+    float rje = sqrt(rje2);
+    float r2 = (y1-y2)*(y1-y2) + (z1-zreff)*(z1-zreff);
+    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)  {
+                    beta = beta + pi;
+                    }
+                theta3 = (beta - alpha);
+    }
+}
+
+
+  
 
 Ticker      motor_timer; 
-void motor()
-{ 
+void motor(){ 
+
     counts1 = Encoder1.getPulses();
     counts2 = Encoder2.getPulses();
     counts3 = Encoder3.getPulses();
-
-//    angle1 = counts1/(8400.0)*2.0*pie; 
-//    angle2 = counts2/(8400.0)*2.0*pie; 
-//    angle3 = counts3/(8400.0)*2.0*pie; 
-
-//    sinewave = sin(time_sin);
+    
+    z0 = -172.0 + 10* sin(time_sin);  
 
-    lasterror1 = error1;
-    lasterror2 = error2;
-    lasterror3 = error3;
+    delta_calctheta1 ();  
+    delta_calctheta2 ();
+    delta_calctheta3 ();
     
-    error1 = counts1 - steps1;
-    error2 = counts2 - steps2;
-    error3 = counts3 - steps3;
+    angle1 = counts1/(8400.0)*2.0*pi; 
+    angle2 = counts2/(8400.0)*2.0*pi; 
+    angle3 = counts3/(8400.0)*2.0*pi; 
     
-    derivative1 = error1 - lasterror1;
-    derivative2 = error2 - lasterror2;
-    derivative3 = error3 - lasterror3;
+    error1 = theta1 - angle1;
+    error2 = theta2 - angle2;
+    error3 = theta3 - angle3;
     
-    Kp = 50;
+    Kp = 10;
     Kd = potmeter1 * 1000;
     
-    
-    // Proportional part
-    u_k1 = Kp * error1;
+        u_k1 = Kp * error1;
     u_k2 = Kp * error2;
     u_k3 = Kp * error3;
-    
-    u_d1 = Kd * derivative1;
-    u_d2 = Kd * derivative2;
-    u_d3 = Kd * derivative3;
 
-    newmotor1 = u_k1 + u_d1;
-    newmotor2 = u_k2 + u_d2;
-    newmotor3 = u_k3 + u_d3;
+    newmotor1 = u_k1;
+    newmotor2 = u_k2;
+    newmotor3 = u_k3;
 
     if (newmotor1>1.0f){
         newmotor1 =1.0f;
@@ -162,204 +236,19 @@
         newmotor3 = -1.0f;
         }
     
-    if (timer==true)    {
     motor1_pwm.write(fabs(newmotor1));
-    motor1_dir.write(newmotor1>0);
+    motor1_dir.write(newmotor1<0);
     
     motor2_pwm.write(fabs(newmotor2));
-    motor2_dir.write(newmotor2<0);
+    motor2_dir.write(newmotor2>0);
     
     motor3_pwm.write(fabs(newmotor3));
-    motor3_dir.write(newmotor3>0);
-    }
+    motor3_dir.write(newmotor3<0);
     
-//    else  {
-//    motor1_pwm.write(0);
-//    motor2_pwm.write(0);
-//    motor3_pwm.write(0);
-//        }
+    time_sin += 0.002;
 }
       
-     
-float delta_calcangleyz(float x00, float y00, float z00)   {
-    float y2 = y00 + le;
-    float y1 = f;
-    float z1 = 0.0;
-    float z2 = z00;
-    float rje2 = re*re - x00*x00;
-    float rje = sqrt(rje2);
-    float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00);
-    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)  {
-                    beta = beta + pi;
-                    }
-                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(x00, y00, z00);
-            x00=x0*cospi-y0*sinpi;
-            y00=y0*cospi+y0*sinpi;
-            z00=z0;
-            theta3 = delta_calcangleyz(x00, y00, z00);
-            }
-            
-    return theta1, theta2, theta3;
-}
-            
-//double error;
-
-// Calculate the steps from angle
-float anglestep(float angle)    {
-    float steps;
-    steps = angle / 360 * 8400;
-    return steps;
-    }
-
-// Omschrijving
-float movefunctioninit ()   {   
-    
-    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);
-
-    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);
-
-    
-
-//    pc.printf("\n\r error1: %f", error1);
-
-
-    // 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.57); // write Duty Cycle  
-//    
-//    motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-//    motor2_pwm.write(0.57); // write Duty Cycle  
-//    
-//    motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-//    motor3_pwm.write(0.57); // write Duty Cycle  
-//    
-//    check1 = true;
-//    check2 = true;
-//    check3 = true; 
-//
-//    Encoder1.reset();
-//    Encoder2.reset();
-//    Encoder3.reset(); 
-//    
-}
-
-// Omschrijving
-//float movestop() {  
-//wait(0.6);
-//timer = false;
-//} 
-//       
-//    while (check1 || check2 || check3)    {
-//
-//        int frequency_pwm = 10000; //10 kHz PWM
-//        motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-//        motor1_pwm.write(newmotor1); // write Duty Cycle
-//        
-//        motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-//        motor2_pwm.write(newmotor2); // write Duty Cycle
-//        
-//        motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-//        motor3_pwm.write(newmotor3); // write Duty Cycle
-//        
-//        if (error1==0)  {
-//            check1=false;
-//            }
-//        if (error2==0)  {
-//            check2=false;
-//            }
-//        if (error3==0)  {
-//            check3=false;
-//            }
-//
-////       if(abs(counts1)>=abs(steps1)) {
-////            pc.printf("\n\r 1 is false");
-////            motor1_pwm.write(0);
-////            check1=false;  
-////            pc.printf("\n\rcounts1 %i", counts1);
-////            }
-////            
-////        if (abs(counts2)>=abs(steps2))   {
-////            pc.printf("\n\r 2 is false");
-////            motor2_pwm.write(0);
-////            check2=false;
-////            pc.printf("\n\rcounts2 %i", counts2);
-////            }
-////            
-////        if (abs(counts3)>=abs(steps3))  {
-////            pc.printf("\n\r 3 is false");
-////            motor3_pwm.write(0);
-////            check3=false;
-////            pc.printf("\n\rcounts3 %i", counts3);
-////            }
-//    }
-//            wait(1.0);
-//        pc.printf("\n\rdone moving");
-//}
-
+          
 
 ///////////////////
 // MAIN FUNCTION //
@@ -372,116 +261,8 @@
     char cc = pc.getc();
     
     int frequency_pwm = 10000; //10 kHz PWM
-//    motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-//    motor1_pwm.write(newmotor1); // write Duty Cycle
-//        
-//    motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-//    motor2_pwm.write(newmotor2); // write Duty Cycle
-//        
-//    motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-//    motor3_pwm.write(newmotor3); // write Duty Cycle
 
     while(true){
-    pc.printf("\n\r----------------------------------------------------------");
-    pc.printf("\n\r----------------------------------------------------------");
-
-        
-        delta_calcinverse(x0,y0,z0);
-    
-        oldtheta1 = theta1;
-        oldtheta2 = theta2;
-        oldtheta3 = theta3; 
-                            
-        char cc = pc.getc();
-        
- //       timer = true;
-
-        
-        if (cc=='d') {
-            x0=x0+5.0f;
-            if (x0>=-75 && x0<=75){
-                movefunctioninit ();
-//                movestop();
-            }
-            else {
-  //              timer = false;
-                x0=x0-5.0f;
-                
-            }
         }
-        
-        if (cc=='a') {
-            x0=x0-5.0f;
-            if (x0>=-75 && x0<=75){
-                movefunctioninit ();
-//                movestop();
-            }
-            else {
-     //           timer = false;
-                x0=x0+5.0f;
-            }
-        }
-        
-        if (cc=='w'){
-            y0=y0+5.0f;
-            if (y0>=-75 && y0<=75){
-                movefunctioninit ();
-   //             movestop();
-            }
-            else {
-//                timer = false;
-                y0=y0-5.0f;
-            }
-        }
-        
-        if (cc=='s'){
-            y0=y0-5.0f;
-            if (y0>=-75 && y0<=75)   {
-                movefunctioninit ();
-//                movestop();
-                }
-            else {
-//                timer = false;
-                y0=y0+5.0f;
-            }
-        }
-        
-        if (cc=='u') 
-        {
-            z0=z0+1.0f;
-            if (z0>=-210 && z0<=-130) {
-                movefunctioninit ();
-    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{
-                pc.printf("else statement");
-//                timer = false;
-                z0=z0-5.0f;
-            }
-        }
-        
-        if (cc=='j') 
-        {
-            z0=z0-1.0f;
-            if (z0>=-210 && z0<=-130){
-                movefunctioninit ();
-  //              movestop();
-            }
-            else {
-//                timer = false;
-                z0=z0+5.0f;
-            }
-        }
-    // end while                                           
-    }
-     
-// end main
+//END MAIN
 }  
\ No newline at end of file