Diff: main.cpp

Revision:

3:c4c2d42cedd4

Parent:

2:323606a75d64

Child:

4:b8c8b94aa890

--- a/main.cpp	Tue Oct 13 13:42:53 2015 +0000
+++ b/main.cpp	Tue Oct 13 14:51:42 2015 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "encoder.h"
+#include "HIDScope.h"
 
 Ticker control_ticker;
 PwmOut pwm_motor1(D5); //onderste motor
@@ -7,17 +8,21 @@
 AnalogIn potmeter1(A4);
 Encoder encoder1(D13,D12,true);
 double pos_1;
+HIDScope scope(1);
 
 // Sample time (motor1−timestep)
 const double m1_Ts = 0.01;
 // Controller gains (motor1−Kp,−Ki,...)
-const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 0.5;
+const double m1_Kp = 5, m1_Ki = 0.005, m1_Kd = 2;
 double m1_err_int = 0, m1_prev_err = 0;
 // Derivative filter coeicients (motor1−filter−b0,−b1,...)
-const double m1_f_a1 = 1.0, m1_f_a2 = 2.0, m1_f_b0 = 1.0, m1_f_b1 = 3.0, m1_f_b2 = 4.0;
+//const double m1_f_a1 = 1.0, m1_f_a2 = 2.0, m1_f_b0 = 1.0, m1_f_b1 = 3.0, m1_f_b2 = 4.0;
 // Filter variables (motor1−filter−v1,−v2)
 double m1_f_v1 = 0, m1_f_v2 = 0;
 
+    const double BiGain1 = 0.016955;
+    const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
+
 // Biquad filter (See slide 14)
 double biquad( double u, double &v1, double &v2, const double a1, const double a2,
                const double b0, const double b1, const double b2 )
@@ -42,22 +47,33 @@
     return Kp * e + Ki * e_int + Kd * e_der;
 }
 
+
+
 void m1_Controller()
 {
-    double reference = potmeter1.read();
-    double position = 0.002991*encoder1.getPosition(); // Don’t use magic numbers!
-    pos_1 = PID( reference - position, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
+    double reference = (potmeter1.read()-0.5)*4200;
+    double position = encoder1.getPosition(); // Don’t use magic numbers!
+    double error = reference - position;
+    pos_1 = PID( error, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
+    //pos_1 = (reference - position) *0.001;
     if(pos_1 > 0)
         dir_motor1 = 0;
     else
         dir_motor1 = 1;
 
     pwm_motor1.write(abs(pos_1));
+
+   scope.set(0,position);
+    scope.send();
+
 } // Attach this function to a Ticker
 
 int main()
 {
+    pwm_motor1.period(0.0001);
     control_ticker.attach(&m1_Controller, 0.01);
-    
-    while(1){}
+
+    while(1) {
+
+    }
 }
\ No newline at end of file