Diff: main.cpp

Revision:

0:55255afaa7a4

Child:

1:a602cde74178

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 15 10:25:05 2018 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+
+DigitalOut gpo(D0);
+DigitalOut led(LED_RED);
+AnalogIn pot1(A0);
+AnalogIn pot2(A1);
+QEI encoder1(D10, D11, NC, 32);
+QEI encoder2(D12, D13, NC, 32);
+FastPWM motor1_pwm(D6);
+DigitalOut motor1_richting(D7);
+
+MODSERIAL pc(USBTX, USBRX);
+
+int count = 0;
+
+Ticker MotorTicker;
+
+void read_pots(double &pot1_value, double &pot2_value) // read pot 1 en pot 2
+{
+    pot1_value = pot1.read();
+    pot2_value = pot2.read();
+}
+
+double det_ref(double ref_value) //zet referentiewaarde om in positie
+{
+    return ref_value * 2 * 3.1416;
+}
+
+double meas_pos () //Encoder functie
+{
+    return encoder1.getPulses()/32.0/131.25*2.0*3.1416;
+}
+
+double PID_controller (meas_pos, ref_pos, gain)
+{
+    double error = ref_pos - meas_pos;
+    
+    // Proportional control
+    double u = gain * error;
+    
+        if (u > 1.0)
+            {
+                u = 1.0;
+            }
+            
+        else if (u < -1.0)
+            {
+                u = -1.0);
+            }
+    return u;
+}
+
+void move_mot(double u)
+{
+    
+        if (u <= 0) 
+        {
+            motor1_richting = 0;
+            motor1_pwm.write(-AnalogVoltage1); //write Duty cycle 
+        }
+        
+        if (u >= 0) 
+        {
+            motor1_richting = 1;
+            motor1_pwm.write(u); //write Duty cycle 
+        }
+}
+
+
+void Motor_control()
+{    
+    volatile double pot_value, volatile double gain;
+    
+    read_pots(pot_value, gain);
+    
+    volatile double yref = det_ref(pot_value); // reference position
+    
+    volatile double y = meas_pos(); // measured position
+    
+    volatile double u = PID_controller(y, yref, gain); // output PID controller
+    
+    move_mot(u); //functie die motor laat bewegen.
+       
+    count++;
+    
+    if (count == 400) // print 1x per seconde waardes.
+    {
+        pc.printf("u = %d, measure position y = %d, reference position yref = %d, gain = %d", u, y, yref, gain);
+        count = 0;   
+    }
+}
+
+
+int main()
+{
+    pc.baud(115200);
+    
+    int frequency_pwm = 16700; //16.7 kHz PWM
+    motor1_pwm.period(1.0/frequency_pwm); // T = 1/f
+    MotorTicker.attach(Motor_control, 0.0025);
+    
+    while (true) 
+    {}
+}
\ No newline at end of file