feat: pid controller

Create2_proj.py

@@ -515,6 +515,28 @@ def safe_drive_monitor(self):
             if isinstance(self.collision_event, EventPause):
                 self.bot_events.put(EventResume())
 
+    @rr
+    @need_sensors
+    def right_hand_wall(self):
+        pid = PIDController(capacity=10, set_point=400, time=0.5, gain_p=2, gain_i=2, gain_d=2)
+
+        # start moving until robot hits a wall - every time wall is hit, turn left 90 degrees and start moving
+
+
+
+        # make a left turn and start following the wall. 
+
+        # 
+        # def adjust_left():
+
+        # def adjust_right():
+
+        # def right_turn():
+
+        # def left_turn():
+
+
+
 
 class World:
     """
@@ -537,6 +559,68 @@ def __init__(self):
         # total distance traveled
         self.total_dist = 0
 
+class PIDController:
+    # descrete time system PID controlelr
+
+    def __init__(self, capacity: int, set_point: int, time: float, gain_p: float, gain_i: float, gain_d: float):
+        self.set_point = set_point
+        self.time = time
+        self.gain_p = gain_p
+        self.gain_i = gain_i
+        self.gain_d = gain_d
+        self.readings = [None]*capacity
+        self.readings_errors = [None]*capacity
+        self.current_index = 0
+
+    def add_sample(self, state_sample):
+        def calc_error(state_sample):
+            return state_sample - self.set_point
+                # check index position is less than last index
+
+        if self.readings[self.current_index] == None:
+            self.readings[self.current_index] = state_sample
+            self.readings_errors[self.current_index] = calc_error(state_sample)
+        elif self.current_index < len(self.readings)-1:
+            self.current_index += 1
+        else:
+            self.current_index = 0
+        # add error calc to array for pid
+        self.readings[self.current_index] = state_sample
+        self.readings_errors[self.current_index] = calc_error(state_sample)
+
+
+        return self.current_index
+
+
+    def calculate_output(self, sample_index: int):
+        """take a state sample and calculate PID output"""
+
+        def sum_error(smpl_index):
+        # get sum of each error index up to the current index
+            sum_error = 0
+            i = 0
+            while i <= smpl_index:
+                sum_error += self.readings_errors[i]
+                i += 1
+            return sum_error
+
+        def calc_p():
+            print((self.gain_p*self.readings_errors[sample_index]))
+            return (self.gain_p*self.readings_errors[sample_index])
+        def calc_i():
+            print(self.gain_i*(self.time*sum_error(sample_index)))
+            return self.gain_i*(self.time*sum_error(sample_index))
+        def calc_d():
+            if sample_index > 0:
+                print(self.gain_d*((self.readings[sample_index] - self.readings[sample_index - 1])/self.time))
+                return self.gain_d*((self.readings[sample_index] - self.readings[sample_index - 1])/self.time)
+            else:
+                return self.gain_d*((self.readings[sample_index] - self.readings[sample_index])/self.time)
+
+        # discrete pid calculation
+        return calc_p() + calc_i() + calc_d()
+
+
 
 class MoveTimeAction:
     def __init__(self, robotref, rvel, lvel, t):