Diff: main.cpp

Revision:

2:3be8cd780b3d

Parent:

1:28377623e8c9

Child:

3:689f3f2e78e8

diff -r 28377623e8c9 -r 3be8cd780b3d main.cpp
--- a/main.cpp	Fri Oct 19 07:58:26 2018 +0000
+++ b/main.cpp	Fri Oct 19 08:43:25 2018 +0000
@@ -7,11 +7,16 @@
 
 const float PI = 3.14159265359;
 const int PULSES_PER_ROTATION = 6533; // Amount of motor encoder pulses per rotation. When using X4 encoding.
-const float pid_period = 0.1; // PID sample period in seconds.
+const float pid_period = 0.0001; // PID sample period in seconds.
 
-const double Kp = 10.0;
-const double Ki = 0.5;
-const double Kd = 2.0;
+const double Kp = 5.0;
+const double Ki = 0.1;
+const double Kd = 0.1;
+
+const double motor_threshold_rps = 0.3; // Rad/s under which we send 0 to the motor, to prevent it from jittering around.
+const double motor_stall_pwm = 0.5; // PWM fraction above which the motor starts to move.
+
+int printcount = 0;
 
 Ticker pidTicker; // Ticker function
 FastPWM pwmpin1(D5); // SPECIFIC PIN (hoeft niet aangesloten te worden) Tells you how fast the motor has to go (later: pwmpin.write will tell you the duty cycle, aka how much voltage the motor gets)
@@ -32,12 +37,12 @@
     if (speed < 1.0 && speed > 0) {
         // Speed is in the range [0, 1] but the motor only moves
         // in the range [0.5, 1]. Rescale for this.
-        speed = (speed * 0.5) + 0.5;
+        speed = (speed * (1-motor_stall_pwm)) + motor_stall_pwm;
     }
     if (speed > -1.0 && speed < 0) {
         // Speed is in the range [-1, 0] but the motor only moves
         // in the range [-1, -0.5]. Rescale for this.
-        speed = (speed * 0.5) - 0.5;
+        speed = (speed * (1-motor_stall_pwm)) - motor_stall_pwm;
     }
     
     // either true or false, determines direction (0 or 1)
@@ -53,10 +58,20 @@
 // Converts radians/s values into PWM values for motor controll.
 // Both positive and negative values.
 double radians_per_second_to_pwm(double rps) {
+    // If the rad/s is below the anti-jitter treshold, it is simply 0.
+    if (rps > 0 && rps < motor_threshold_rps) {
+        rps = 0;
+    }
+    if (rps < 0 && rps > motor_threshold_rps) {
+        rps = 0;
+    }
+    
+    
     // With our specific motor, full PWM is equal to 1 round per second.
     // Or 2PI radians per second.
     double pwm_speed = rps / (2*PI);
     
+    // PWM speeds can only go between [-1, 1]
     if (pwm_speed > 1) { pwm_speed = 1; }
     if (pwm_speed < -1) { pwm_speed = -1; }
     return pwm_speed;
@@ -83,7 +98,11 @@
         
         double speed_pwm = radians_per_second_to_pwm(speed_rps);
         
-        pc.printf("puls: %i, c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", pulses, current_angle, desired_angle, error, speed_rps, speed_pwm);
+        printcount++;
+        if (printcount >= 0.1L/pid_period) {
+            pc.printf("c_angle: %f, d_angle: %f, error: %f, rps: %f, speed: %f\n", current_angle, desired_angle, error, speed_rps, speed_pwm);
+            printcount = 0;
+        }
         
         update_motor(&directionpin2, &pwmpin2, speed_pwm);
 }