Diff: Motor_tryout.cpp

Revision:

33:88fbf14d8aaf

Parent:

32:60a71dcfdb7a

Child:

34:0af87f9cfb7b

Child:

35:4cb2ed6dd2d2

--- a/Motor_tryout.cpp	Thu Oct 31 14:30:59 2019 +0000
+++ b/Motor_tryout.cpp	Thu Oct 31 15:02:17 2019 +0000
@@ -3,14 +3,15 @@
 #include "QEI.h"
 #include "Math.h"
 #include "ttmath.h"
+#include "FastPWM.h"
 
 MODSERIAL pc(USBTX, USBRX);
 //Serial term (USBTX, USBRX);
-PwmOut motor1_pwm(PTC2);
+FastPWM motor1_pwm(PTC2);
 DigitalOut motor1_dir(PTC3);
-PwmOut motor2_pwm(PTA2);
+FastPWM motor2_pwm(PTA2);
 DigitalOut motor2_dir(PTB23);
-PwmOut motor3_pwm(PTC4);
+FastPWM motor3_pwm(PTC4);
 DigitalOut motor3_dir(PTC12);
 
 AnalogIn potmeter1(A1);
@@ -190,7 +191,12 @@
     counts2 = Encoder2.getPulses();
     counts3 = Encoder3.getPulses();
     
-    z0 = -172.0 + 10* sin(time_sin);  
+//    z0 = -172.0 + 10* sin(time_sin); 
+
+float r = 20*(1-1/(1+(time_sin)));
+    x0 = r*sin(6.28 * time_sin);
+    y0 = r*cos(6.28 * time_sin);
+     
 
     delta_calctheta1 ();  
     delta_calctheta2 ();
@@ -204,10 +210,9 @@
     error2 = theta2 - angle2;
     error3 = theta3 - angle3;
     
-    Kp = 10;
-    Kd = potmeter1 * 1000;
+    Kp = potmeter1 * 25;
     
-        u_k1 = Kp * error1;
+    u_k1 = Kp * error1;
     u_k2 = Kp * error2;
     u_k3 = Kp * error3;
 
@@ -261,8 +266,13 @@
     char cc = pc.getc();
     
     int frequency_pwm = 10000; //10 kHz PWM
+    motor1_pwm.period_us(6);
+    motor2_pwm.period_us(6);
+    motor3_pwm.period_us(6);
 
     while(true){
+        
+        
         }
 //END MAIN
 }  
\ No newline at end of file