Diff: Control/PIDController.hpp

Revision:

0:8f5db5085df7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/PIDController.hpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,74 @@
+#ifndef __PIDCONTROLLER__
+#define __PIDCONTROLLER__
+
+#include "mbed.h"
+#include "Motor.hpp"
+#include "FXAS21002.h"
+#include "FXOS8700Q.h"
+#include "rtos.h"
+
+#define PI 3.141593
+
+#define INITIAL_P 0.452531214933414
+#define INITIAL_I 5.45748932024049
+#define INITIAL_D 0.000233453623255507
+#define INITIAL_N 51.0605584484153
+
+#define ERROR_THRESH    PI/5
+#define MINIMUM_CURVE_VELOCITY 19
+
+class PIDController
+{
+    private:
+    
+        PwmOut* servo;
+        Motor* motor;
+        FXAS21002* gyro;
+      
+        float target_angle;
+        float P;
+        float I;
+        float D;
+        float N;
+        float e[2];
+        float up[2];
+        float ui[2];
+        float ud[2];
+        float u;
+        float saved_velocity;
+        float gyro_reference;
+    
+        Ticker pid_trigger; 
+        float interrupt_time;
+        float Ts;
+    
+        void PIDTrigger();
+    
+        void Control();
+    
+        void RunPID();
+        
+    public:
+        PIDController(Motor* motor, PwmOut* servo, FXAS21002* gyro);
+    
+        void initializeController();
+        // Atach PIDController.
+        void PIDAttach();
+    
+        // Detach PIDController.
+        void PIDDetach();
+    
+        void setInterruptTime(float interrupt_time);
+    
+        void setPIDConstants(float P, float I, float D, float N);
+    
+        void setTargetAngle(float target_angle);
+    
+        void addTargetAngle(float add_angle);
+        
+        void setGyroReference(float gyro_ref);
+        
+
+};
+
+#endif //__PIDCONTROLLER__
\ No newline at end of file