Diff: main.cpp

Revision:

1:28377623e8c9

Parent:

0:7c204101adb0

Child:

2:3be8cd780b3d

--- a/main.cpp	Thu Oct 18 12:37:34 2018 +0000
+++ b/main.cpp	Fri Oct 19 07:58:26 2018 +0000
@@ -3,10 +3,17 @@
 #include "MODSERIAL.h"
 #include "QEI.h"
 
+#include "pid.h"
+
 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.
 
-Ticker motorTicker; // Ticker function
+const double Kp = 10.0;
+const double Ki = 0.5;
+const double Kd = 2.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)
 FastPWM pwmpin2(D6); // 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)
 DigitalOut directionpin1(D4); // SPECIFIC PIN (hoeft niet aangesloten te worden) Direction value (0-1) that the mbed will give the motor: in which direction the motor must rotate
@@ -17,71 +24,78 @@
 QEI encoder2(D13, D12, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 2 to D12 and D13; NC: not connected pin (for X4); 6533 prm (counts per rotation)
 Serial pc(USBTX, USBRX);
 
+PID pid(pid_period);
+
 // Updates a motor connected to the specified pins with the given speed.
-// The speed can be both positive and negative.
-void update_motor(DigitalOut dir, FastPWM pwm, int speed) {
+// The speed can be both positive and negative, in the range [-1, 1].
+void update_motor(DigitalOut* dir, FastPWM* pwm, double speed) {
+    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;
+    }
+    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;
+    }
+    
     // either true or false, determines direction (0 or 1)
-    dir = speed > 0;
+    *dir = speed > 0;
     // pwm duty cycle can only be positive, floating point absolute value (if value is >0, the there still will be a positive value).
-    pwm = abs(speed);
+    *pwm = fabs(speed);
 }
 
-float encoder_pulses_to_radians(int pulses) {
-    return (pulses/(float)PULSES_PER_ROTATION) * 2.0f*PI;
+double encoder_pulses_to_radians(int pulses) {
+    return (pulses/float(PULSES_PER_ROTATION)) * 2.0f*PI;
 }
 
 // Converts radians/s values into PWM values for motor controll.
 // Both positive and negative values.
-int radians_per_second_to_pwm(float rps) {
+double radians_per_second_to_pwm(double rps) {
     // With our specific motor, full PWM is equal to 1 round per second.
     // Or 2PI radians per second.
-    int pwm_speed = (rps / (2*PI)) * 255;
-    if (pwm_speed > 255) { pwm_speed = 255; }
-    if (pwm_speed < -255) { pwm_speed = -255; }
+    double pwm_speed = rps / (2*PI);
+    
+    if (pwm_speed > 1) { pwm_speed = 1; }
+    if (pwm_speed < -1) { pwm_speed = -1; }
     return pwm_speed;
 }
 
 // Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1]
-float normalize_pot(float pot_value) {
+double normalize_pot(double pot_value) {
     // scales value potmeter from 0-1 to -1 - 1.
     return pot_value * 2 - 1;
 };
 
-double P_controller(double error) {
-    double Kp = 10;
-    
-    // Proportional part:
-    double u_k = Kp * error;
-    
-    return u_k;
-}
-
 
 void motorfunction() {
         // reads out value potmeter 1 between 0-1
-        float pot = potmeter2.read();
-        float desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
+        double pot = potmeter2.read();
+        double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
         
         int pulses = encoder2.getPulses();
-        float current_angle = encoder_pulses_to_radians(pulses);
+        double current_angle = encoder_pulses_to_radians(pulses);
         
-        float error = current_angle - desired_angle;
-        float speed_rps = P_controller(error);
+        double error = current_angle - desired_angle;
+        // PID controll.
+        double speed_rps = pid.update(error);
         
-        int speed_pwm = radians_per_second_to_pwm(speed_rps);
+        double speed_pwm = radians_per_second_to_pwm(speed_rps);
         
-        pc.printf("cur_angle: %f, des_angle: %f, rps: %f, pwm: %i\n", current_angle, desired_angle, speed_rps, speed_pwm);
+        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);
         
-        update_motor(directionpin2, pwmpin2, speed_pwm);
+        update_motor(&directionpin2, &pwmpin2, speed_pwm);
 }
 
 
 int main()
 {
+    pc.baud(115200);
     pc.printf("Starting.");
+    pid.set_k_values(Kp, Ki, Kd);
+    pidTicker.attach(motorfunction, pid_period);
     pwmpin1.period_us(60.0); // 60 microseconds PWM period, 16.7 kHz, defines all PWM pins (only needs to be done once)
     while(true){
-        motorfunction();
-        wait(0.1);
     } //Lege while loop zodat functie niet afloopt
 }
\ No newline at end of file