From: Federica Di Lauro <federicadilauro1998@gmail.com>
Date: Wed, 22 Jan 2020 13:24:38 +0000 (+0100)
Subject: refactor pid
X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=a644e744bf92d33c2a32269ca6916ad109471722;p=pioneer-stm32

refactor pid
---

diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h b/utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h
index 5a0b529..6ede504 100644
--- a/utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h
+++ b/utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h
@@ -8,7 +8,6 @@ class Odometry {
  public:
   Encoder left_encoder_;
   Encoder right_encoder_;
-  float kBaseline;
 
   float linear_velocity;
   float angular_velocity;
@@ -17,14 +16,12 @@ class Odometry {
   Odometry() {
     left_encoder_ = NULL;
     right_encoder_ = NULL;
-    kBaseline = 0.35;  //in meters
   }
 
   Odometry(Encoder left, Encoder right) {
 
     left_encoder_ = left;
     right_encoder_ = right;
-    kBaseline = 0.35;  //in meters
   }
 
   void UpdateValues() {
diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h b/utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h
index 645cced..66d2b1e 100644
--- a/utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h
+++ b/utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h
@@ -1,7 +1,7 @@
 #ifndef PID_H
 #define PID_H
 
-#define WINDOW_SIZE 500
+#include "constants.h"
 
 class Pid {
  public:
@@ -13,9 +13,8 @@ class Pid {
   float error_;
   float setpoint_;
 
-  //needed for integral term
-  float error_sum_array_[WINDOW_SIZE];
-  int error_sum_index_;
+  //needed for integrative term
+  int error_sum_;
 
   //needed for derivative term
   float previous_error_;
@@ -32,14 +31,10 @@ class Pid {
     this->setpoint_ = 0;
 
     this->previous_error_ = 0;
-    this->error_sum_index_ = 0;
+    this->error_sum_ = 0;
 
-    for (int i = 0; i < WINDOW_SIZE; i++) {
-      this->error_sum_array_[i] = 0;
-    }
-
-    this->min_ = -790;
-    this->max_ = 790;
+    this->min_ = -MAX_DUTY_CYCLE;
+    this->max_ = MAX_DUTY_CYCLE;
 
   }
 
@@ -52,11 +47,8 @@ class Pid {
     this->setpoint_ = 0;
 
     this->previous_error_ = 0;
-    this->error_sum_index_ = 0;
+    this->error_sum_ = 0;
 
-    for (int i = 0; i < WINDOW_SIZE; i++) {
-      this->error_sum_array_[i] = 0;
-    }
   }
 
   void set(float setpoint) {
@@ -70,28 +62,9 @@ class Pid {
     //proportional term
     float output = this->error_ * this->kp_;
 
-    //integral term with antiwindup
-//    if (this->error_sum_index_ == WINDOW_SIZE) {
-//      this->error_sum_array_[0] = this->error_;
-//      this->error_sum_index_ = 1;
-//    } else {
-//      this->error_sum_array_[this->error_sum_index_] = this->error_;
-//      this->error_sum_index_++;
-//    }
-//
-//    float error_sum = 0;
-//    for (int i = 0; i < WINDOW_SIZE; i++) {
-//      error_sum += this->error_sum_array_[i];
-//    }
-//
-//    output += error_sum * this->ki_;
-
     //integral term without windup
-
-    error_sum_array_[0] += this->error_;
-    output += error_sum_array_[0] * this->ki_;
-
-
+    error_sum_ += this->error_;
+    output += error_sum_ * this->ki_;
 
     //derivative term
     output += (this->error_ - this->previous_error_) * kd_;