Diff: main.cpp

Revision:

5:fd4eaf3cb09d

Parent:

4:fce8c7458e97

Child:

6:cc3e47c7aac2

diff -r fce8c7458e97 -r fd4eaf3cb09d main.cpp
--- a/main.cpp	Fri Oct 19 09:31:11 2018 +0000
+++ b/main.cpp	Fri Oct 19 10:51:16 2018 +0000
@@ -1,81 +1,21 @@
 #include "mbed.h"
-#include "FastPWM.h"
 #include "MODSERIAL.h"
-#include "QEI.h"
 
 #include "pid.h"
+#include "motor.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.001; // PID sample period in seconds.
 
 const double Kp = 10.0;
 const double Ki = 0.1;
 const double Kd = 0.5;
 
-const double motor_threshold_rps = 0.1; // Rad/s under which we send 0 to the motor, to prevent it from jittering around.
-const double motor_stall_pwm = 0.45; // 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)
-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
-DigitalOut directionpin2(D7); // 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
 AnalogIn potmeter1(A1); // Analoge input van potmeter 1 -> Motor 1
 AnalogIn potmeter2(A2); // Analoge input van potmeter 2 -> Motor 2
-QEI encoder1(D11, D10, NC, PULSES_PER_ROTATION, QEI::X4_ENCODING); // Reads encoder, connect pins of encoder 1 to D12 and D13; NC: not connected pin (for X4); 6533 prm (counts per rotation)
-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, 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 * (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 * (1-motor_stall_pwm)) - motor_stall_pwm;
-    }
-    
-    // either true or false, determines direction (0 or 1)
-    *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 = fabs(speed);
-}
+Motor motor(D6, D7, D13, D12, &pc);
 
-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.
-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;
-}
 
 // Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1]
 double normalize_pot(double pot_value) {
@@ -84,37 +24,20 @@
 };
 
 
-void motorfunction() {
-        // reads out value potmeter 1 between 0-1
-        double pot = potmeter2.read();
-        double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
-        
-        int pulses = encoder2.getPulses();
-        double current_angle = encoder_pulses_to_radians(pulses);
-        
-        double error = current_angle - desired_angle;
-        // PID controll.
-        double speed_rps = pid.update(error);
-        
-        double speed_pwm = radians_per_second_to_pwm(speed_rps);
-        
-        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);
-}
-
-
 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)
+    motor.set_pid_k_values(Kp, Ki, Kd);
+    // Start the motor controller at the desired frequency.
+    motor.start(pid_period);
+    
     while(true){
+        // reads out value potmeter 1 between 0-1
+        double pot = potmeter2.read();
+        double desired_angle = normalize_pot(pot) * PI; // Scale the potmeter to [-PI, PI]
+        // Update the motor controller with the new angle.
+        motor.set_target_angle(desired_angle);
+        wait(0.1);
     } //Lege while loop zodat functie niet afloopt
 }
\ No newline at end of file