From 2e5c2ad8dbc6a8438be308910bc327f9fa9639c5 Mon Sep 17 00:00:00 2001
From: Brendan Haines <brendan.haines@gmail.com>
Date: Mon, 2 Mar 2020 21:41:43 -0700
Subject: [PATCH] rearrange code in main loop

---
 Software/python/hwtest.py | 32 ++++++++++++++++++--------------
 1 file changed, 18 insertions(+), 14 deletions(-)

diff --git a/Software/python/hwtest.py b/Software/python/hwtest.py
index a870d58..959c14c 100644
--- a/Software/python/hwtest.py
+++ b/Software/python/hwtest.py
@@ -121,21 +121,15 @@ def control_thread():
     fir_taps = [1, -1, 0]
     iir_taps = [0, 0]
     time_data = np.zeros((max(len(fir_taps), len(iir_taps)), time_data.shape[1]))
+    motor_directions = [-1, 1, 0]
+    steering_sign = 1
+
+    # Precompute
+    this_time = 0
+    new_c = 0
+    motor_speed = np.array(motor_directions) * base_speed
 
     while True:
-        # TODO: replace sleep statement with something that doesn't depend on execution time of loop
-        time.sleep(sample_interval)
-        if time_data.shape[0] == 0:
-            this_time = 0
-        else: 
-            this_time = time_data[-1, 0] + sample_interval
-
-        # Precompute as much as possible
-        c = time_data[:,2]
-        e = time_data[:,1]
-        new_c = np.sum(fir_taps[1:] * e[-len(fir_taps)+1:]) - np.sum(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) / 3.5
@@ -144,7 +138,7 @@ def control_thread():
 
         # Calculate output
         new_c += fir_taps[0] * line_position
-        motor_speed += new_c
+        motor_speed += steering_sign * new_c
 
         # Update motors
         # for ii in range(3):
@@ -161,6 +155,16 @@ def control_thread():
             print(f"{line_position:+1.2f}", end="")
             print()
 
+        # Precompute for next iteration
+        this_time = time_data[-1, 0] + sample_interval
+        c = time_data[:,2]
+        e = time_data[:,1]
+        new_c = np.sum(fir_taps[1:] * e[-len(fir_taps)+1:]) - np.sum(iir_taps * c[-len(iir_taps):])
+        motor_speed = np.array(motor_directions) * base_speed
+
+        # TODO: replace sleep statement with something that doesn't depend on execution time of loop
+        time.sleep(sample_interval)
+
 
 # Start controller
 # TODO: add start/stop/reset capability to GUI