Diff: Motor_tryout.cpp

Revision:

18:ab0fe311e7f3

Parent:

17:6da57acb7bea

Child:

19:9c8ab7922191

--- a/Motor_tryout.cpp	Mon Oct 14 09:05:05 2019 +0000
+++ b/Motor_tryout.cpp	Mon Oct 21 11:01:18 2019 +0000
@@ -1,70 +1,245 @@
 #include "mbed.h" 
 #include "MODSERIAL.h"
+#include "QEI1.h"
+#include "QEI2.h"
 #include "QEI.h"
+#include "Math.h"
+#include "ttmath.h"
+
 MODSERIAL pc(USBTX, USBRX);
+//Serial term (USBTX, USBRX);
 PwmOut motor1_pwm(PTC2);
 DigitalOut motor1_dir(PTC3);
-PwmOut motor3_pwm(PTA1);
-DigitalOut motor3_dir(PTB9);
+PwmOut motor2_pwm(PTA2);
+DigitalOut motor2_dir(PTB23);
+PwmOut motor3_pwm(PTC4);
+DigitalOut motor3_dir(PTC12);
 
-QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING);
+QEI1 Encoder1(D12,D13,NC,64,QEI1::X4_ENCODING);
+QEI2 Encoder2(D10,D11,NC,64,QEI2::X4_ENCODING);
+QEI  Encoder(D2,D3,NC,64,QEI::X4_ENCODING);
 
 int quit;
-int counts = 0;
+int counts1 = 0;
+int counts2 = 0;
+int counts3 = 0;
 int limit_pos = 8400;
+int x;
+int y;
+int z;
+float steps;
+int g = 0;
 
+const float le = 15.0;
+const float f = 37.5;
+const float re = 174.0;
+const float rf = 50.0;
+const float pi = 3.14159265358979323846;
+float y2;
+float y1;
+float z1;
+float z2;
+float rje2;
+float rje;
+float r2;
+float r;
+float z0=-172;
+float y0=0;
+float x0=0;
+int check;
+float theta1;
+float theta2;
+float theta3;
+float oldtheta1=0;
+float oldtheta2=0;
+float oldtheta3=0;
+int direction;
+float delta_calcangleyz(float x0, float y0, float z0)   {
+    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;
+                    }
+        float theta1 = (beta - alpha)*180.0/pi;
+        int check = 0;
+        return theta1;
+        }
+}
+
+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 theta2;
+                //return theta3;
+                }
+return theta1, theta2, theta3;
+
+}
+            
+            
 Ticker pulse;
 void pulseget()
 {
-    counts = Encoder.getPulses();
-    if(counts > limit_pos)
-        {
-            quit = 1;
-        }
+    counts1 = Encoder1.getPulses();
+    counts2 = Encoder2.getPulses();
+    counts3 = Encoder.getPulses();
     }
 
-Ticker show;
-void pulseshow()
+int anglestep(float angle)
 {
-    pc.printf("The counts is %i",counts);
+    float steps;
+    steps = angle / 360 * 8400;
+    return steps;
     }
 
-
 // DE MAIN FUNCTIE
 int main(void)
-{
-    pc.baud(115200);    
+{    
+    pc.baud(115200);
+        
     char cc = pc.getc();
+    pc.printf("\n\r cc check");  
+    //char key = term.getc();
+    pc.printf("\n\r term check");  
+
+    float angle = 360;
     while(true)
     {
-        quit = 0;
-        counts = 0;
-        {
-            pc.printf("in while");
+       counts1 = 0;
+       counts2 = 0;
+       counts3 = 0;
+       
+        float steps1;
+        float steps2;
+        float steps3;
+                     
+            counts1 = Encoder1.getPulses();
+            counts2 = Encoder2.getPulses();
+            counts3 = Encoder.getPulses();
+            
+            delta_calcinverse(x0,y0,z0);
+            
+            oldtheta1 = theta1;
+            oldtheta2 = theta2;
+            oldtheta3 = theta3;
+            
+char cc = pc.getc();
+
+                        
+if (cc=='a') {
+            pc.printf("\n\rleft");
+            z0=z0-1.0f;
+            
+            theta1 = delta_calcinverse(x0,y0,z0);
+            theta1 = delta_calcangleyz(x0,y0,z0); 
+            
+            theta1 = theta1 - oldtheta1;
+            theta2 = theta2 - oldtheta2;
+            theta3 = theta3 - oldtheta3;                     
+            
+             steps1 = anglestep(theta1);
+             steps2 = anglestep(theta2);
+             steps3 = anglestep(theta3);
+            
             int frequency_pwm = 10000; //10 kHz PWM
             motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-            motor1_dir.write(0); // positief
-            motor1_pwm.write(0.8); // write Duty Cycle  
+            motor1_dir.write(1); // positief
+            motor1_pwm.write(0.7); // write Duty Cycle  
+            
+            motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
+            motor2_dir.write(0); // positief
+            motor2_pwm.write(0.7); // write Duty Cycle  
             
             motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
-            motor3_dir.write(0); // positief
-            motor3_pwm.write(0.8); // write Duty Cycle  
+            motor3_dir.write(1); // positief
+            motor3_pwm.write(0.7); // write Duty Cycle  
+            
+        
+}                
+if (cc=='d') {
+            pc.printf("\n\rright");
+                        
+            z0=z0-10.0f;
+             
+            theta1 = delta_calcinverse(x0,y0,z0);
+            theta1 = delta_calcangleyz(x0,y0,z0); 
             
-            counts = Encoder.getPulses();
-            pc.printf("The counts is %i\n\r",counts);
+            theta1 = theta1 - oldtheta1;
+            theta2 = theta2 - oldtheta2;
+            theta3 = theta3 - oldtheta3;                     
+            
+            float steps1 = anglestep(theta1);
+            float steps2 = anglestep(theta2);
+            float steps3 = anglestep(theta3);
             
-            //pulse.attach(pulseget,1.0/10000);
-            // show.attach(pulseshow,1.0/10000);
+            pc.printf("\n\r the difference in angles are (%f, %f, %f)", theta1, theta2, theta3);
+            pc.printf("\n\r the steps are (%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(quit == 1)
-            //{
-            //    pulse.detach();
-            //    motor1_pwm.write(0);
-            //    motor3_pwm.write(0);
-            //    pc.printf("The motor is off with counts is %i\n\r",counts);               
-            //    break;
-            //    }
-            wait(1.0/10);
+            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 (abs(counts1)<=abs(steps1) || abs(counts2)<=abs(steps2) || abs(counts3)<=abs(steps3)) {
+             pc.printf("while");
+             if(abs(counts1)>=abs(steps1)) {
+                  motor1_pwm.write(0);
+                }
+                if (abs(counts2)>=abs(steps2))   {
+                    motor2_pwm.write(0);
+                    }
+                        if (abs(counts3)>=abs(steps3))  {
+                            motor3_pwm.write(0);
+                            }
         }
-    }
\ No newline at end of file
+
+}                
+               
+            
+            wait(1.0/100);
+            }
+}
\ No newline at end of file