Pid fixes

onboard/tests/PIDTests.py → onboard/structures/PIDSims.py

@@ -1,5 +1,8 @@
-from structures.Buffer import Buffer
-from structures.pid import PID
+"""
+Code to test a vanilla PID controller with fixed parameters.
+"""
+from Buffer import Buffer
+from pid import PID
 import matplotlib.pyplot as plt
 import numpy as np
 import datetime as dt
@@ -51,7 +54,7 @@ def adjust(self, u):
     y = system.following_function()
     iPID.setpoint = y
     following.append(y)
-    u = iPID.update(dt.datetime.now())
+    u = iPID.update()
     u_vals.append(u)
     system.adjust(u)
 

onboard/structures/pid.py

@@ -1,4 +1,21 @@
 import numpy as np
+from icecream import ic
+
+
+def timestamp_range_search(l, low: float, high: float) -> list:
+    """return a sublist with timestamps within a given range"""
+    highindex = len(l)-1
+    lowindex = 0
+    for i in range(highindex,-1,-1):
+        end = True
+        if(l[i][2] >= high):
+            highindex = i
+        if(l[i][2] >= low):
+            lowindex = i
+            end = False
+        if end:
+            break
+    return l[lowindex:highindex]
 
 class PID:
 
@@ -18,9 +35,9 @@ def __init__(self, setpoint: float, buffer, kp: float, ki: float, kd: float):
         self.ki = ki
         self.kd = kd
         self.integral = 0
-        self.most_recent_index_cached = 0 # the most recent timstep included in the integral.
+        self.most_recent_index_cached = 0.0 # the most recent timstep included in the integral.
 
-    def update(self, current_time) -> float:
+    def update(self) -> float:
         """_summary_
 
         Args:
@@ -29,10 +46,10 @@ def update(self, current_time) -> float:
         Returns:
             float: _description_
         """
-
-        y = self.buffer[current_time][1] #current value. TODO: adjust this to access the buffer properly. also, make sure that the correct timstep is accessed (ie: it exists)
+        y = self.buffer[-1][1] #current value. TODO: adjust this to access the buffer properly. also, make sure that the correct timstep is accessed (ie: it exists)
+        current_time = self.buffer[-1][2]
         e = self.setpoint - y
-        data = self.buffer[self.most_recent_index_cached: current_time]
+        data = timestamp_range_search(self.buffer, self.most_recent_index_cached, current_time)
 
         # data = [
         #     (value, smoothvalue, self.most_recent_index_cached)
@@ -42,15 +59,14 @@ def update(self, current_time) -> float:
         # ]
 
         for i in range(len(data)-1): 
-            time_diff = (data[i+1][2]-data[i][2]).total_seconds()
+            time_diff = (data[i+1][2]-data[i][2])
 
             value_sum = data[i+1][1]-data[i][1]
             self.integral += time_diff*0.5*value_sum
 
-
         P = self.kp * e
         I = self.ki *  self.integral
-        D = self.kd * (data[-1][1]-data[-2][1])/(data[-1][2]-data[-2][2]).total_seconds()
+        D = self.kd * (data[-1][1]-data[-2][1])/(data[-1][2]-data[-2][2])
         # we can always change how this grad is computed (ie: number of points to lookback)
 
         self.most_recent_index_cached = current_time;
@@ -61,23 +77,23 @@ def update(self, current_time) -> float:
 
 
 def test():
-    from Buffer import Buffer
     import datetime as dt
     import matplotlib.pyplot as plt
     import time
 
-    BUFF_MAX = 100
-
-
-    buff = Buffer(BUFF_MAX, [('value', np.float64), ('corr_value', np.float64), ('timestamp', dt.datetime)])
+    BUFF_MAX = 10
+
+    buff = []
     for i in range(BUFF_MAX):
-        time.sleep(0.1)
-        buff += (i, i, dt.datetime.now())
-
-
+        time.sleep(0.01)
+        buff.append( (i, i, dt.datetime.timestamp(dt.datetime.now())))
 
     pid = PID(5, buff, 1, 2, 3)
-    print(pid.update(BUFF_MAX - 1))
+    for i in range(10):
+        for j in range(10):
+            time.sleep(0.01)
+            buff.append( (j, j, dt.datetime.timestamp(dt.datetime.now())))
+        ic(pid.update())
 
 
 if __name__ == "__main__":  # only runs when this file is run directly in terminal; if imported to somewhere else this doesn't run

onboard/tests/BufferTests.py

@@ -120,7 +120,7 @@ def __init__(self, methodName: str = "runTest") -> None:
 
         self._size = 1000
         self._num_ops = 500 * 12 * 2
-        self._max_time = 1
+        self._max_time = 100
 
         self.buffer = Buffer(
             self._size,