Diff: Der_design.cpp

Revision:

0:588962464765

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Der_design.cpp	Fri Oct 26 10:41:14 2018 +0000
@@ -0,0 +1,186 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "MODSERIAL.h"
+#include "QEI.h"
+#include "math.h"  
+#include "HIDScope.h"
+
+#define PI 3.14159265
+
+// Declare variables
+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);
+
+// Define the HIDScope and Ticker objects
+HIDScope    scope(1);
+Ticker      scopeTimer;
+
+MODSERIAL pc(USBTX, USBRX);
+
+int count = 0;
+
+// Tickers
+Ticker MotorTicker;
+
+
+// Functions
+
+void read_pots(volatile double &pot1_value, volatile 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 comp_error (double meas_pos, double ref_pos)
+{
+    return ref_pos - meas_pos;
+}
+
+double comp_integral_error (double error) // compute the integral error
+{
+    double T = 1.0 / 400.0;
+    volatile double error_integral = error_integral + error * T;
+    
+    return error_integral;
+}
+
+double comp_derivative_error (double error)
+{
+    double T = 1.0 / 400.0;
+    volatile double error_prev;
+    double error_der = (error - error_prev)/T;
+    error_prev = error;
+    
+    return error_der;
+}
+
+// PID controller
+double PID_controller (double pot2_val, double error, double integral_error, double error_der)
+{
+    
+    // Proportional control
+    double u_k = 30.0 * error; //20.0 * pot2_val * error;
+    
+    
+    // Integral control
+    double u_i = 20.0 * pot2_val * integral_error;
+    
+    // Derivative
+    
+    double u_d = 30.0 * error_der;
+    
+    
+    //u definieren
+    double u = u_k; //+ u_d; //+ u_i;
+    
+    // Signaal binnen de perken houden.
+    if (u > 1.0)
+        {
+            u = 1.0;
+        }
+            
+    else if (u < -1.0)
+        {
+            u = -1.0;
+        }
+    
+    
+    
+    
+    return u;
+}
+
+
+//Move motor
+void move_mot(double u)
+{
+    
+        if (u <= 0) 
+        {
+            motor1_richting = 0;
+            motor1_pwm.write(-u); //write Duty cycle 
+        }
+        
+        if (u >= 0) 
+        {
+            motor1_richting = 1;
+            motor1_pwm.write(u); //write Duty cycle 
+        }
+}
+
+//sinefunction
+double sinefunct()
+{   
+    static int t;     
+    double pos_sine = sin((double)t*0.0025*0.5*PI);
+    t++;
+    return (pos_sine + 1.0) / 2.0;
+}
+
+// Alles samenvoegen
+void Motor_control()
+{      
+    
+    volatile double pot_value;
+    
+    volatile double pot2_val;
+    
+    read_pots(pot_value, pot2_val);
+    
+    volatile double yref = sinefunct(); // reference position
+    
+    volatile double y = meas_pos(); // measured position
+    
+    volatile double error = comp_error (y, yref);
+    
+    volatile double integral_error = comp_integral_error(error);
+    
+    volatile double error_der = comp_derivative_error(error);
+    
+    scope.set(0, error_der);
+    
+    //scope.set(0, meas_pos());
+    
+    volatile double u = PID_controller(pot2_val, error, integral_error, error_der); // output PID controller
+    
+    move_mot(u); //functie die motor laat bewegen.
+       
+    count++;
+    //pc.printf("count = %i", count);
+    
+    if (count == 400) // print 1x per seconde waardes.
+    {
+        pc.printf("u = %lf, measured position y = %lf, reference position yref = %lf, gain = %lf, error = %lf, integral error = %lf, derivative error = %lf \n\r", u, y, yref, pot2_val*20.0, error, integral_error, error_der);
+        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);
+    scopeTimer.attach_us(&scope, &HIDScope::send, 2e4); 
+    
+    while (true) 
+    {}
+}
\ No newline at end of file