Diff: main.cpp

Revision:

0:7c204101adb0

Child:

1:28377623e8c9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 18 12:37:34 2018 +0000
@@ -0,0 +1,87 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+
+const float PI = 3.14159265359;
+const int PULSES_PER_ROTATION = 6533; // Amount of motor encoder pulses per rotation. When using X4 encoding.
+
+Ticker motorTicker; // 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);
+
+// 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) {
+    // 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 = abs(speed);
+}
+
+float 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) {
+    // 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; }
+    return pwm_speed;
+}
+
+// Normalizes a potmeter value from it's original range of [0, 1] to [-1, 1]
+float normalize_pot(float 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]
+        
+        int pulses = encoder2.getPulses();
+        float current_angle = encoder_pulses_to_radians(pulses);
+        
+        float error = current_angle - desired_angle;
+        float speed_rps = P_controller(error);
+        
+        int 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);
+        
+        update_motor(directionpin2, pwmpin2, speed_pwm);
+}
+
+
+int main()
+{
+    pc.printf("Starting.");
+    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