Initial working prototype

2 files changed, 202 insertions, 32 deletions

diff --git a/main/main.ino b/main/main.ino
index 2bf87a2..8965de9 100644
--- a/main/main.ino
+++ b/main/main.ino
@@ -8,9 +8,9 @@ void setup() {
   digitalWrite(2, LOW);
 
   pinMode(3, OUTPUT);
-  digitalWrite(3, LOW);
+  digitalWrite(3, HIGH);
 
-  Serial.println("I_in I_out");
+  //Serial.println("I_in I_out");
 }
 
 #define ACCEL  0
@@ -23,7 +23,7 @@ int state = LAST_STATE - 1; // begin with acceleration
 int state_times[] = { 20000, 250, 5000, 250 };
 unsigned long next_state_time = 0;
 
-const int CURRENT_OUT_SENSOR = A0;
+const int CURRENT_OUT_SENSOR = A1;
 const double mVPerAmp = 66;
 const int OFFSET=2500;
 
@@ -31,34 +31,53 @@ void loop() {
   unsigned long time = millis();
 
   int raw = analogRead(CURRENT_OUT_SENSOR);
-  float mV = raw * 5000.0 / 1024.0;
-  float current = (mV - OFFSET) / mVPerAmp;
-  Serial.println(current, 3);
+  //float mV = raw * 5000.0 / 1024.0;
+  //float current = (mV - OFFSET) / mVPerAmp;
+  
+  char buf[64];
+  sprintf(buf, "%lu %d %d\n", time, raw, state); 
+  Serial.print(buf);
 
-  //if(Serial.available() > 0)
-  if(time > next_state_time)
+  if(Serial.available() > 0)
+  //if(time > next_state_time)
   {
-  //  while(Serial.available() > 0)
-  //    Serial.read();
-    // advance state
-    state++;
-    state %= LAST_STATE;
+    String op = Serial.readStringUntil('\n');
+
+    if(op == "next") {
+      // advance state
+      state++;
+      state %= LAST_STATE;
+    } else if(op == "force00") {
+      state = ACCEL;
+    } else if(op == "force11") {
+      state = COAST1;
+    } else if(op == "force22") {
+      state = BRAKE;
+    } else if(op == "reset" || op == "force33") {
+      state = COAST2;
+    }
+    
     switch(state) {
-      case ACCEL:
-        digitalWrite(2, HIGH);
-        break;
-      case COAST1:
-        digitalWrite(2, LOW);
-        break;
-      case BRAKE:
-        digitalWrite(3, HIGH);
-        break;
-      case COAST2:
-        digitalWrite(3, LOW);
-        break;
+    case ACCEL:
+      digitalWrite(3, LOW); // regen off
+      digitalWrite(2, HIGH); // motor on
+      break;
+    case BRAKE:
+      digitalWrite(2, LOW); // motor off
+      digitalWrite(3, HIGH); // regen on
+      break;
+    case COAST1:
+    case COAST2:
+      digitalWrite(3, LOW); // motor off
+      digitalWrite(2, LOW); // regen on
+      break;
+    default:
+      break;
     }
-    //Serial.println("State advance");
+    //sprintf(buf, "S%d", state);
+    //Serial.println(buf);
 
-    next_state_time = time + state_times[state];
+    //next_state_time = time + state_times[state];
   }
+  delay(20);
 }
diff --git a/tools/controller.py b/tools/controller.py
index 0bc1f73..93ac46b 100755
--- a/tools/controller.py
+++ b/tools/controller.py
@@ -5,14 +5,165 @@ All-in-one controller program for regen test stand.
 Handles motor control, tachometer, and current integration.
 """
 
-import serial
 import time
+import serial
+import threading
+import sys
+import logging
+
+# 0 = accel, 1 = coast, 2 = brake, 3 = coast
+global_state = -1
+global_millis = -1
+global_current = 0
+state_lock = threading.Lock()
+
+MAX_STATE = 4 # max + 1 so modulo works
+coast_time = .050 # coast time before regen brake
+
+# returns arduino timestamp (ms, int), current (A, float), state (0-3, int)
+# blocks until data received
+def readStatus(ard):
+    try:
+        status = ard.readline().decode('UTF-8')
+        #logging.info(status)
+    except UnicodeDecodeError:
+        return None
+
+    split = status.split(' ')
+
+    mVPerAmp = 66
+    OFFSET=2500
+
+
+    if len(split) == 3:
+        try:
+            millis, rawCurrent, state = status.split(' ')
+            millis = int(millis)
+            rawCurrent = float(rawCurrent)
+            state = int(state)
+        except ValueError:
+            return None
+
+        mV = rawCurrent * 5000 / 1024.0
+        current = (mV - OFFSET) / mVPerAmp
+
+        return millis, current, state
+    return None
+
+# blocking equivalent, guaranteed to return valid values
+def readStatusBlocking(ard):
+    while True:
+        status = readStatus(ard)
+        if status is not None and len(status) == 3:
+            return status
+
+kill_thread = False
+
+def backgroundPoll(ard):
+    logging.info("Starting background thread...")
+    global kill_thread
+    global global_state, global_millis, global_current
+    while not kill_thread:
+        # this will hold the lock until we get data... this is
+        # probably what we want.
+        with state_lock:
+            global_millis, global_current, global_state = readStatusBlocking(ard)
+            logging.info((global_state, global_millis, global_current))
+        time.sleep(1e-3)
+
+# wait for a state transition to `state' (blocking)
+def waitState(state):
+    logging.info("Waiting for " + str(state))
+    global global_state
+    while True:
+        with state_lock:
+            if global_state == state:
+                return
+            print(global_state, state)
+
+# jump to `state' by constantly writing a `force' command until we get
+# what we want.
+def jumpState(ard, state):
+    logging.info("Forcing state " + str(state))
+    writeCmd(ard, 'force' + str(state) + str(state))
+    waitState(state)
+
+def writeCmd(ard, cmd):
+    b = (str(cmd) + '\n').encode('ascii')
+    logging.info(bytes(b))
+    ard.write(b)
+
+# reset to state 3 - coast 2 (blocking)
+def waitReset(ard):
+    logging.info("Waiting for reset...")
+    writeCmd(ard, 'reset')
+    waitState(3)
+
+def forceReset(ard):
+    jumpState(ard, 3)
+
+# advance state (blocking, assumes global_state is set properly)
+def nextState(ard):
+    global global_state
+    writeCmd(ard, 'next')
+    global_state = (global_state + 1) % MAX_STATE
+    waitState(global_state)
+    logging.info("State transition -> " + str(global_state))
+
+def forceNextState(ard):
+    global global_state
+    global_state = (global_state + 1) % MAX_STATE
+    jumpState(ard, global_state)
+    logging.info("State transition -> " + str(global_state))
+
+def doTrial(ard, accel_time):
+    forceReset(ard)
+
+    forceNextState(ard)
+    logging.info("Accelerating...")
+
+    # we are accelerating now
+    time.sleep(accel_time)
+
+    forceNextState(ard)
+
+    logging.info("Coasting...")
+    time.sleep(coast_time)
+    forceNextState(ard)
+    logging.info("Braking...")
+
+    # measure current
+    for i in range(100):
+        with state_lock:
+            status = global_state, global_millis, global_current
+            logging.info(status)
 
 PORT='/dev/ttyACM0'
 BAUD=19200
 
-ard = serial.Serial(PORT, BAUD, timeout=5)
+if __name__ == "__main__":
+    format = "%(asctime)s: %(message)s"
+    logging.basicConfig(format=format, level=logging.INFO,
+                        datefmt="%H:%M:%S")
+
+    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())
+
+    forceReset(ard)
+
+    accel_time = float(input("Enter acceleration time (s): "))
+
+    # begin trial
+    doTrial(ard, accel_time)
+
+    kill_thread = True
 
-while True:
-    current = ard.readline()
-    print(current.decode('UTF-8'))
+    bg_thread.join()