Diff: main.cpp

Revision:

0:447a347725bb

Child:

1:e10ae03926e6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 26 17:46:54 2018 +0000
@@ -0,0 +1,110 @@
+
+// ------------- INITIALIZING -----------------
+
+#include "mbed.h"
+#include "BiQuad.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "HIDScope.h"
+
+PwmOut pwmpin1(D6);
+DigitalOut directionpin1(D7);
+QEI encoder1 (D9, D8, NC, 8400, QEI::X4_ENCODING);
+Ticker ref_rot;
+Ticker show_counts; 
+Ticker Scope_Data;
+MODSERIAL pc(USBTX, USBRX);
+HIDScope scope(2);
+
+// INITIALIZING GLOBAl VALUES
+double Kp = 50; //200
+double Ki = 0.5;   //1
+double Kd = 10; //200
+double Ts = 0.1; // Sample time in seconds
+volatile double reference_rotation = 8400;
+double motor_position;
+bool AlwaysTrue;
+
+//----------- FUNCTIONS ----------------------
+
+
+// PID CONTROLLER FUNCTION
+double PID_controller(double error)
+{
+  static double error_integral = 0;
+  static double error_prev = error; // initialization with this value only done once!
+  static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+
+  // Proportional part:
+  double u_k = Kp * error;
+
+  // Integral part
+  error_integral = error_integral + error * Ts;
+  double u_i = Ki * error_integral;
+
+  // Derivative part
+  double error_derivative = (error - error_prev)/Ts;
+  double filtered_error_derivative = LowPassFilter.step(error_derivative);
+  double u_d = Kd * filtered_error_derivative;
+  error_prev = error;
+
+  // Sum all parts and return it
+  return u_k + u_i + u_d;   
+}
+
+
+// DIRECTON AND SPEED CONTROL
+void moter_control(double u)
+{
+    directionpin1= u > 0.0f; //eithertrueor false
+    pwmpin1= fabs(u); //pwmduty cycle canonlybepositive, floatingpoint absolute value            
+}
+
+
+// CONTROLLING THE MOTOR
+void Motor_mover()
+{
+    double motor_position = encoder1.getPulses();
+    double error = reference_rotation - motor_position;
+    double u = PID_controller(error);
+    moter_control(u);
+}
+
+//PRINT TICKER
+void PrintFlag()
+{
+    AlwaysTrue = true;
+    }
+    
+// HIDSCOPE
+void ScopeData()
+{
+    double y = encoder1.getPulses();
+    scope.set(0, y);
+    scope.send();
+    }
+
+//------------------ EXECUTION ---------------
+
+int main()
+{
+  pwmpin1.period_us(60);
+  pc.printf("test");
+  pc.baud(9600);
+  //show_counts.attach(PrintFlag, 0.2);
+  ref_rot.attach(Motor_mover, 0.01);
+  Scope_Data.attach(ScopeData, 0.1);
+    
+    while (true) {
+        /*
+        PrintFlag();
+        while (AlwaysTrue){
+            float q = encoder1.getPulses();
+            pc.printf("%f \r\n", q);
+            AlwaysTrue = false;
+            wait(0.2f);
+            }
+            */
+            
+    }
+}
\ No newline at end of file