Diff: Control/PIDController.cpp

Revision:

0:8f5db5085df7

diff -r 000000000000 -r 8f5db5085df7 Control/PIDController.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/PIDController.cpp	Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,111 @@
+#include "PIDController.hpp"
+
+PIDController::PIDController(Motor* motor, PwmOut* servo, FXAS21002* gyro) {
+    this->motor = motor;
+    this->servo = servo;
+    this->gyro = gyro;
+
+    initializeController();
+
+    target_angle = 0;
+    saved_velocity = 0;
+    gyro_reference = 0;
+
+    interrupt_time = 0.02;
+    Ts = interrupt_time*1000;
+}
+
+void PIDController::initializeController() {
+    P = INITIAL_P;
+    I = INITIAL_I;
+    D = INITIAL_D;
+    N = INITIAL_N;
+
+    for (int i = 0; i < 2; i++)
+	{
+		e[i] = 0;
+		ui[i] = 0;
+		ud[i] = 0;
+	}
+}
+
+void PIDController::PIDTrigger() {
+    printf("PIDCntroller \r\n");
+    PIDController::RunPID();
+}
+
+void PIDController::PIDAttach() {
+    
+    pid_trigger.attach(std::bind(&PIDController::PIDTrigger, this), 1.0);
+}
+
+void PIDController::PIDDetach() {
+    pid_trigger.detach();
+}
+
+void PIDController::setInterruptTime(float interrupt_time) {
+	this->interrupt_time = interrupt_time;
+}
+
+void PIDController::setPIDConstants(float P, float I, float D, float N) {
+    this->P = P;
+    this->I = I;
+    this->D = D;
+    this->N = N;
+}
+
+void PIDController::setTargetAngle(float target_angle) {
+    this->target_angle = target_angle;
+    if (target_angle > PI)
+        this->target_angle = target_angle - 2 * PI;
+    else if (target_angle < -PI)
+        this->target_angle = target_angle + 2 * PI;
+}
+
+void PIDController::addTargetAngle(float add_angle) {
+    this->target_angle = this->target_angle + add_angle;
+}
+
+void PIDController::Control() {
+	setServoPWM(u*100/(PI/8), *servo);
+    if(abs(e[0]) >= ERROR_THRESH && motor->getVelocity() > MINIMUM_CURVE_VELOCITY) {
+        saved_velocity = motor->getVelocity();
+        motor->setVelocity(MINIMUM_CURVE_VELOCITY);
+    }
+    else if(abs(e[0]) < ERROR_THRESH && motor->getVelocity() != saved_velocity) {
+        motor->setVelocity(saved_velocity);
+    }
+}
+/* PID controller for angular position */
+void PIDController::RunPID() {
+    float feedback = gyro->get_angle() - gyro_reference;
+    	e[0]= e[0] - 2*PI;
+    if(e[0] < -PI)
+    	e[0] = e[0] + 2*PI;
+
+    /* Proportinal Part */
+    up[0] = e[0]*P;  
+
+    /* Integral Part */
+    ui[1] = ui[0];
+    if(abs(u) < PI/8){
+    	ui[0] = (P*I*Ts)*e[1] + ui[1];    
+    }
+    else if(u > 0)
+    	ui[0] = PI/8 - up[0];
+    else if(u < 0)
+    	ui[0] = -PI/8 - up[0]; 
+
+    /* Derivative Part */
+    ud[1] = ud[0];
+    ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1); 
+    
+    /** Sum of all parts **/ 
+    u = up[0] + ud[0] + ui[0];
+
+    Control();
+}
+
+void PIDController::setGyroReference(float gyro_ref) {
+	this->gyro_reference = gyro_ref;
+}