Diff: main.cpp

Revision:

2:2563d1d8461f

Parent:

1:13d8940f0fd4

Child:

3:b353ee86230a

--- a/main.cpp	Fri Oct 27 08:59:41 2017 +0000
+++ b/main.cpp	Fri Oct 27 09:29:18 2017 +0000
@@ -2,6 +2,7 @@
 #include "QEI.h"
 #include "MODSERIAL.h"
 #include "math.h"
+#include "FastPWM.h"
 
 
 
@@ -10,9 +11,9 @@
 DigitalOut ledr(LED_RED);
 DigitalOut ledg(LED_GREEN);
 DigitalOut motor1DC(D7);
-PwmOut motor1PWM(D6);
+FastPWM motor1PWM(D6);
 DigitalOut motor2DC(D4);
-PwmOut motor2PWM(D5);
+FastPWM motor2PWM(D5);
 
 AnalogIn   potMeter1(A0);
 AnalogIn   potMeter2(A1);
@@ -27,9 +28,11 @@
 
 int potmultiplier = 8000; // Multiplier for the pot meter reference which is normally between 0 and 1
 
+int frequency_pwm = 1000;
+
 //Start constants for the PID ------------------------------- 
 const double pi = 3.1415926535897;
-const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
+const double M1_TS = 1/100; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
 
 //verplaatst
 const float RAD_PER_PULSE = (2*pi)/4200;
@@ -87,8 +90,8 @@
     double reference_motor1 = atan(y/x); // reference for Theta
     double reference_motor2 = sqrt((x*x+y*y));  // reference for radius
     
-    int position_motor1 = RAD_PER_PULSE*Encoder1.getPulses(); // current position for theta
-    int position_motor2 = RAD_PER_PULSE*Encoder2.getPulses(); // current position for the radius
+    int position_motor1 = Encoder1.getPulses(); // current position for theta
+    int position_motor2 = Encoder2.getPulses(); // current position for the radius
     
     double motor1 = PID(reference_motor1 - position_motor1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
     double motor2 = PID(reference_motor2 - position_motor2, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
@@ -96,8 +99,8 @@
     pc.baud(115200);
     pc.printf("\r Position(X,Y)=(%f,%f), Ref(Theta,R): (%f,%f), Pos(Theta,R):(%i,%i), Motor Value(M1,M2):(%f,%f).\n",x, y, reference_motor1, reference_motor2, position_motor1, position_motor2, motor1, motor2);
     
-    motor1PWM = motor1;
-    motor2PWM = motor2;
+    //motor1PWM = motor1;
+    //motor2PWM = motor2;
 
     if(motor1 > 0.3){
         motor1DC = 1;