move stuff around and add servos

Software/python/hwtest.py

@@ -16,28 +16,44 @@ from bokeh.plotting import figure
 
 DEBUG = True
 
+# Configure MUX for ADC
 mux_io = [None] * 4
 mux_io[0] = digitalio.DigitalInOut(board.D23)
 mux_io[1] = digitalio.DigitalInOut(board.D22)
 mux_io[2] = digitalio.DigitalInOut(board.D27)
 mux_io[3] = digitalio.DigitalInOut(board.D17)
-
 for ii, io in enumerate(mux_io):
     io.switch_to_output()
 
+# Configure ADC
 i2c = busio.I2C(board.SCL, board.SDA)
 adc = ADS.ADS1015(i2c)
 adc_mux = AnalogIn(adc, ADS.P0)
+adc_lock = threading.Lock()
+
+# Configure Servo Driver
+servos = ServoKit(channels=16).continuous_servo
+servos[0].throttle = 0
+servos[1].throttle = 0
+servos[2].throttle = 0
+
+# Initialize calibration
+# TODO: save cal and load from file by default
 white_cal = [0]*8
 black_cal = [5]*8
 
-adc_lock = threading.Lock()
+# Initialize Brightness
+brightness_idx = np.arange(8)
+brightness = [get_normalized_reflectivity(c) for c in range(8)]
+
+# Initialize Log
+time_data = np.empty((0, 3)) # [[t, e, c]]
 
 def get_reflectivity(chan):
-    chan = int(chan)
     global mux_io
     global adc_mux
     global adc_lock
+    chan = int(chan)
     mux = 1-np.array(list(f"{chan:04b}"), dtype=int)
     adc_lock.acquire()
     for ii, io in enumerate(mux_io):
@@ -49,31 +65,28 @@ def get_reflectivity(chan):
 def get_normalized_reflectivity(chan):
     global white_cal
     global black_cal
-    
     return (get_reflectivity(chan) - black_cal[chan]) / (white_cal[chan] - black_cal[chan])
 
-brightness_idx = np.arange(8)
-brightness = [get_normalized_reflectivity(c) for c in range(8)]
-time_data = np.empty((0,2))
-
+# Create sources for plots
 brightness_plot_source = ColumnDataSource(data=dict(sensor=brightness_idx, brightness=brightness))
-time_plot_source = ColumnDataSource(data=dict(t=time_data[:,0], e=time_data[:,1]))
+time_plot_source = ColumnDataSource(data=dict(t=time_data[:,0], e=time_data[:,1], c=time_data[:,2]))
 
 # Set up plots
 brightness_plot = figure(plot_height=150, plot_width=400, x_range=[0, 7], y_range=[0, 1])
 brightness_plot.line('sensor', 'brightness', source=brightness_plot_source, line_width=3)
 brightness_plot.circle('sensor', 'brightness', source=brightness_plot_source, size=8, fill_color="white", line_width=2)
 
-time_plot = figure(plot_height=400, plot_width=800, y_range=[-3.5, 3.5])
+time_plot = figure(plot_height=400, plot_width=800, y_range=[-1, 1])
 time_plot.line('t', 'e', source=time_plot_source, line_width=3, line_alpha=0.6, legend_label="e(t)")
+time_plot.line('t', 'c', source=time_plot_source, line_width=3, line_alpha=0.6, legend_label="c(t)")
 
+# Callback functions
 def update_plots(attrname=None, old=None, new=None):
     global brightness
     global time_data
     global brightness_plot_source
     brightness_plot_source.data = dict(sensor=brightness_idx, brightness=brightness)
-    time_plot_source.data = dict(t=time_data[:,0], e=time_data[:,1])
-
+    time_plot_source.data = dict(t=time_data[:,0], e=time_data[:,1], c=time_data[:,2])
 
 def cal_white(attrname=None, old=None, new=None):
     global white_cal
@@ -85,6 +98,7 @@ def cal_black(attrname=None, old=None, new=None):
     black_cal = [get_reflectivity(c) for c in range(8)]
     update_plots()
 
+# GUI elements
 cal_white_button = Button(label="Cal White")
 cal_white_button.on_click(cal_white)
 cal_black_button = Button(label="Cal Black")
@@ -95,10 +109,17 @@ curdoc().add_root(column(row(controls, brightness_plot, width=800), time_plot))
 curdoc().title = "TriangleBot Control Panel"
 curdoc().add_periodic_callback(update_plots, 250)
 
+# Controller
 def control_thread():
     global brightness
     global time_data
+    global servos
+
     sample_interval = 0.01
+    base_speed = 0.1
+    fir_taps = [0, 0, 0]
+    iir_taps = [0, 0]
+
     while True:
         # TODO: replace sleep statement with something that doesn't depend on execution time of loop
         time.sleep(sample_interval)
@@ -107,15 +128,30 @@ def control_thread():
         else: 
             this_time = time_data[-1, 0] + sample_interval
 
+        # Precompute as much as possible
+        c = time_data[:,2]
+        np.append(c, fir_taps[1:] * e[-len(fir_taps)+1:] + iir_taps * c[-len(iir_taps):])
+        motor_speed = np.array(-1, 1, 0) * base_speed
+        
+        # Read error
         brightness = np.clip([get_normalized_reflectivity(c) for c in range(8)], 0, 1)
-        line_position = np.sum((1 - brightness) * (np.arange(8) - 3.5))/np.sum(1-brightness)
+        line_position = np.sum((1 - brightness) * (np.arange(8) - 3.5)) / np.sum(1-brightness) / 3.5
         if np.isnan(line_position):
             line_position = 0
-        # TODO: implement control stuff and drive outputs
 
+        # Calculate output
+        c[-1] += fir_taps[0] * line_position
+        motor_speed += c[-1]
+
+        # Update motors
+        for ii in range(3):
+            servos[ii].throttle = motor_speed[ii]
+
+        # Log data
         new_time_data = [[this_time, line_position]]
         time_data = np.concatenate((time_data, new_time_data))
 
+        # Print data
         if DEBUG:
             for b in brightness:
                 print(f"{b:1.2f}\t", end="")
@@ -123,20 +159,7 @@ def control_thread():
             print()
 
 
+# Start controller
+# TODO: add start/stop/reset capability to GUI
 control_thread = threading.Thread(target=control_thread)
 control_thread.start()
-
-
-# servos = ServoKit(channels=16).continuous_servo
-# servos[0].throttle = 0
-# servos[1].throttle = 0
-# servos[2].throttle = 0
-# time.sleep(1)
-# servos[0].throttle = 20
-# servos[1].throttle = 20
-# servos[2].throttle = 20
-# time.sleep(1)
-# servos[0].throttle = 0
-# servos[1].throttle = 0
-# servos[2].throttle = 0
-