Fix stuff

3 files changed, 145 insertions, 47 deletions

diff --git a/main/main.ino b/main/main.ino
index 8965de9..2c5d550 100644
--- a/main/main.ino
+++ b/main/main.ino
@@ -79,5 +79,5 @@ void loop() {
 
     //next_state_time = time + state_times[state];
   }
-  delay(20);
+  delay(20); # 50 Hz
 }
diff --git a/tools/controller.py b/tools/controller.py
index 93ac46b..59ff284 100755
--- a/tools/controller.py
+++ b/tools/controller.py
@@ -10,6 +10,7 @@ import serial
 import threading
 import sys
 import logging
+import matplotlib.pyplot as plt
 
 # 0 = accel, 1 = coast, 2 = brake, 3 = coast
 global_state = -1
@@ -17,6 +18,7 @@ global_millis = -1
 global_current = 0
 state_lock = threading.Lock()
 
+I=0.5300 # kg m ^ 2
 MAX_STATE = 4 # max + 1 so modulo works
 coast_time = .050 # coast time before regen brake
 
@@ -32,7 +34,7 @@ def readStatus(ard):
     split = status.split(' ')
 
     mVPerAmp = 66
-    OFFSET=2500
+    OFFSET=2497
 
 
     if len(split) == 3:
@@ -68,7 +70,7 @@ def backgroundPoll(ard):
         # probably what we want.
         with state_lock:
             global_millis, global_current, global_state = readStatusBlocking(ard)
-            logging.info((global_state, global_millis, global_current))
+            #logging.info((global_state, global_millis, global_current))
         time.sleep(1e-3)
 
 # wait for a state transition to `state' (blocking)
@@ -79,10 +81,9 @@ def waitState(state):
         with state_lock:
             if global_state == state:
                 return
-            print(global_state, state)
+            #print(global_state, state)
 
-# jump to `state' by constantly writing a `force' command until we get
-# what we want.
+# jump straight to a state
 def jumpState(ard, state):
     logging.info("Forcing state " + str(state))
     writeCmd(ard, 'force' + str(state) + str(state))
@@ -129,41 +130,112 @@ def doTrial(ard, accel_time):
 
     logging.info("Coasting...")
     time.sleep(coast_time)
+
+    last_millis = 0
+    with state_lock:
+        last_millis = global_millis
+    start_millis = -1
+
+    end_millis = last_millis + 60 * 1000 # time out after 30s
+
     forceNextState(ard)
     logging.info("Braking...")
 
+    curr_status = -1, last_millis, 0
+
+    xdata = []
+    ydata = []
+
+    AVG_WINDOW = 25 # 10 samples
+    ZERO_THRESHOLD = .050 # amperes
+
+    done = False
     # measure current
-    for i in range(100):
-        with state_lock:
-            status = global_state, global_millis, global_current
-            logging.info(status)
+    #while last_millis < end_millis:
+    while not done and last_millis < end_millis:
+        while last_millis == curr_status[1]:
+            with state_lock:
+                curr_status = global_state, global_millis, global_current
+        last_millis = curr_status[1]
+        if start_millis == -1:
+            start_millis = curr_status[1]
+
+        logging.info(curr_status)
+        xdata.append((curr_status[1] - start_millis) / 1000.0)
+        ydata.append(curr_status[2])
+
+        # check if we're done
+        # average the last AVG_WINDOW samples and see if they're close to zero
+        if len(ydata) >= AVG_WINDOW and sum(ydata[-AVG_WINDOW:-1]) / AVG_WINDOW < ZERO_THRESHOLD:
+            done = True
+
+    R = 0.6 # ohms, measured by multimeter
+    power = [ I*I*R for I in ydata ]
+
+    # integrate power
+    E = 0
+    for i in range(len(xdata) - 1):
+        E += power[i] * (xdata[i + 1] - xdata[i + 0])
+
+    logging.info("Recovered energy: " + str(E) + " joules")
+
+    ax = plt.subplot(1, 2, 1)
+    ax.plot(xdata, ydata)
+    ax.grid(True, which='both')
+
+    ax.axhline(y=0, color='k')
+    ax.axvline(x=0, color='k')
+
+    ax.set_title('Current vs. time')
+    ax.set_xlabel('time after braking (s)')
+    ax.set_ylabel('regen current (A)')
+
+    ax = plt.subplot(1, 2, 2)
+    ax.plot(xdata, power)
+    ax.grid(True, which='both')
+
+    ax.axhline(y=0, color='k')
+    ax.axvline(x=0, color='k')
+
+    ax.set_title('Power vs. time')
+    ax.set_xlabel('time after braking (s)')
+    ax.set_ylabel('regen power (W)')
+    plt.show()
+
 
 PORT='/dev/ttyACM0'
 BAUD=19200
 
 if __name__ == "__main__":
-    format = "%(asctime)s: %(message)s"
+    format = "[%(asctime)s] %(message)s"
     logging.basicConfig(format=format, level=logging.INFO,
                         datefmt="%H:%M:%S")
 
+    logging.info("=== Started E-bike Regen Tester ===")
+    logging.info("*** WARNING ***")
+    logging.info("Operating a high-energy test device is inherently dangerous.\nYou assume all responsibility for the safe operation of this device.")
+    logging.info("TO EMERGENCY STOP: PRESS CTRL+C AT ANY TIME")
+
     ard = serial.Serial(PORT, BAUD, timeout=5, writeTimeout=0)
-    logging.info("Creating background thread...")
     bg_thread = threading.Thread(target=backgroundPoll, args=(ard,))
     bg_thread.start()
 
     logging.info("Initializing...")
 
-    time.sleep(.500)
-
-    logging.info(threading.enumerate(), bg_thread.is_alive())
+    #time.sleep(.500)
 
-    forceReset(ard)
+    try:
+        forceReset(ard)
 
-    accel_time = float(input("Enter acceleration time (s): "))
+        accel_time = float(input("Enter acceleration time (s): "))
 
-    # begin trial
-    doTrial(ard, accel_time)
+        logging.info("ACTION REQUIRED: Verify that cover is in place.")
+        input("Press enter to begin acceleration...")
 
-    kill_thread = True
+        # begin trial
+        doTrial(ard, accel_time)
 
-    bg_thread.join()
+    finally:
+        forceReset(ard)
+        kill_thread = True
+        bg_thread.join()
diff --git a/tools/tachometer.py b/tools/tachometer.py
index 5010b94..70c0abd 100755
--- a/tools/tachometer.py
+++ b/tools/tachometer.py
@@ -55,18 +55,17 @@ TICS_PER_REV = 1
 
 # desired RPM range
 RPM_LOW = 25
-RPM_HIGH = 180
+RPM_HIGH = 400
 
 # RPMs below this get mapped to zero
 ZERO_RPM_THRES = 15
 
 
-def update_plot(ax, fig, x, y):
+def update_plot(ax, x, y):
     ax.lines[0].set_data( x, y ) # set plot data
     ax.relim()                  # recompute the data limits
     ax.set_ylim(ymin=0, ymax=max(y) * 1.5)
     ax.autoscale_view()         # automatic axis scaling
-    fig.canvas.flush_events()   # update the plot and take care of window eve
 
 def show_webcam(mirror=False):
     cam = cv2.VideoCapture(CAMERA_IDX)
@@ -96,7 +95,7 @@ def getFFT(data, rate):
     freq = np.fft.fftfreq(len(fft), 1.0 / rate)
     return freq[:int(len(freq) / 2)], fft[:int(len(fft) / 2)]
 
-def getRPM(x_data, y_data, ax, fig):
+def getRPM(x_data, y_data, ax):
     delta_t = x_data[-1] - x_data[0]
     nsamples = len(x_data)
     supersample = 3
@@ -133,13 +132,19 @@ def getRPM(x_data, y_data, ax, fig):
 
     interpolated_freqs = freq_interp(freqs)
 
-    update_plot(ax, fig, freqs, interpolated_freqs)
+    update_plot(ax, freqs, interpolated_freqs)
 
     peakRPM = np.where(interpolated_freqs == np.amax(interpolated_freqs))
     peakRPM=freqs[peakRPM[0]][0]
     print(peakRPM)
     return peakRPM if peakRPM >= ZERO_RPM_THRES else 0
 
+def avgColor(extracted_region):
+    gray=cv2.cvtColor(extracted_region, cv2.COLOR_BGR2GRAY)
+    avg=averageColorOfRegion(extracted_region, 0, extracted_region.shape[0], 0, extracted_region.shape[1])
+    avg=np.average(avg, axis=0)
+    return avg
+
 def main():
     cam = cv2.VideoCapture(CAMERA_IDX)
     cam.set(cv2.CAP_PROP_FRAME_HEIGHT, HEIGHT)
@@ -150,62 +155,83 @@ def main():
     roi = getROI(cam)
     x, y, w, h = roi
 
+    roi2 = getROI(cam)
+    x2, y2, w2, h2 = roi2
+
     plt.ion()
 
     fig = plt.figure()
-    ax = plt.subplot(1,1,1)
-    ax.set_xlabel('time')
-    ax.set_ylabel('measured rpm')
+    ax = fig.add_subplot(2, 3, 1, xlabel='time', ylabel='measured rpm1')
 
-    fig_fft = plt.figure()
-    ax_fft = plt.subplot(1,1,1)
+    ax_rpm2 = fig.add_subplot(2,3,3 + 1, xlabel='time', ylabel='measured rpm2')
+
+    ax_fft = fig.add_subplot(2,3,2)
     ax_fft.set_xlabel('frequency (rpm)')
     ax_fft.set_ylabel('amplitude')
 
-    fig_brt = plt.figure()
-    ax_brt = plt.subplot(1,1,1)
+    ax_fft2 = fig.add_subplot(2, 3, 3 + 2, xlabel='frequency (rpm)', ylabel='amplitude')
+
+    ax_brt = fig.add_subplot(2,3,3)
     ax_brt.set_xlabel('time')
     ax_brt.set_ylabel('region brightness')
 
+    ax_brt2 = fig.add_subplot(2, 3, 3 + 3, xlabel='time', ylabel='region brightness2')
+
     x_data, y_data = [], []
+    y_data2 = []
     rpm_times, rpm_data = [], []
-    ax.plot(x_data , y_data , ',-k' , markersize = 10 ) # add an empty line to the plot
-    fig.show() # show the window (figure will be in foreground, but the user may move it to background)
-
+    rpm_data2 = []
+    ax.plot(x_data , y_data , ',-k') # add an empty line to the plot
+    ax_brt2.plot(x_data, y_data2, ',-k')
+    ax_rpm2.plot(x_data, y_data2, ',-k')
     ax_fft.plot(x_data, y_data, ',-k')
-    fig_fft.show()
-
+    ax_fft2.plot(x_data, y_data, ',-k')
     ax_brt.plot(x_data, y_data, ',-k')
-    fig_brt.show()
 
     while True:
         # sample.
         ret_val, img = cam.read()
         cv2.imshow('my webcam', img)
+
         extracted_region = img[y:y+h, x:x+w]
-        cv2.imshow('extracted', extracted_region)
-        gray=cv2.cvtColor(extracted_region, cv2.COLOR_BGR2GRAY)
-        avg=averageColorOfRegion(extracted_region, 0, h, 0, w)
-        avg=np.average(avg, axis=0)
+        cv2.imshow('extracted1', extracted_region)
+
+        extracted_region2 = img[y2:y2+h2, x2:x2+w2]
+        cv2.imshow('extracted2', extracted_region2)
+
+        avg = avgColor(extracted_region)
+        avg2 = avgColor(extracted_region2)
 
         t = time.time()
         x_data.append(t)
         y_data.append(avg)
+        y_data2.append(avg2)
+
         if len(x_data) > WINDOW:
             x_data.pop(0)
             y_data.pop(0)
+            y_data2.pop(0)
 
-
-        #update_plot(ax_brt, fig_brt, x_data, y_data)
+        update_plot(ax_brt, x_data, y_data)
+        update_plot(ax_brt2, x_data, y_data2)
 
         if len(x_data) >= WINDOW:
-            rpm = getRPM(x_data, y_data, ax_fft, fig_fft)
+            rpm = getRPM(x_data, y_data, ax_fft)
             rpm_times.append(t)
             rpm_data.append(rpm)
+
+            rpm2 = getRPM(x_data, y_data2, ax_fft2)
+            rpm_data2.append(rpm2)
+
             if(len(rpm_times) > WINDOW):
                 rpm_times.pop(0)
                 rpm_data.pop(0)
-            update_plot(ax, fig, rpm_times, rpm_data)
+                rpm_data2.pop(0)
+
+            update_plot(ax, rpm_times, rpm_data)
+            update_plot(ax_rpm2, rpm_times, rpm_data2)
+
+        fig.canvas.flush_events()   # update the plot and take care of window eve
 
         if cv2.waitKey(1) == 27:
             break  # esc to quit