diff --git a/controller b/controller new file mode 100755 index 0000000000000000000000000000000000000000..45f3de0ce3850a41fa2035866ba49fdfac4c9c5a Binary files /dev/null and b/controller differ diff --git a/draiscontrol/control.py b/draiscontrol/control.py index a5dbda79fda7b5018f6ff276dd78d3e6978b843d..d6d4bdfd97165a50beda65ed63399823500bb09e 100755 --- a/draiscontrol/control.py +++ b/draiscontrol/control.py @@ -11,6 +11,7 @@ import array import signal import sys import time +import subprocess import config as cfg import readgyro @@ -21,9 +22,9 @@ from tachometer import Tachometer, InterruptLogger class Control(object): - def __init__(self, logfile=None): + def __init__(self, logfile=None, precision=150): self.lookback = 2 - self.precision = 150 + self.precision = precision # we are only fast enough if we don't read the gyro self.period = 1.0 / self.precision self.accG = 9.81 # hall logging @@ -67,7 +68,7 @@ class Control(object): ltt = self.tacho.last_time t = time.time() (_, aY, _) = force = self.mpu.readForce() - gyro = self.mpu.readGyro() + # gyro = self.mpu.readGyro() # not reading the gyro speads up the loop aY *= self.accG self.accY.appendSample(t, aY) self.hall.appendSample(t, ltt) @@ -123,12 +124,68 @@ class Control(object): sys.stdout.write(255) sys.stdout.flush() + def accelerationToByte(self, a): + a /= self.accG # convert back to g-force + a *= 16384.0 # convert back to raw 16-bit word + a = int(a / (2**8)) # truncate to byte + a = min(max(a, -(2**7 - 1)), 2**7 - 1) # enforce range + return a + + def speedToByte(self, s): + s *= 25 # stretch to reasonable range + s = int(s) # truncate to byte + s = min(max(s, 0), 2**8-1) # enforce range + return s + + def savedloop(self, human=False): + # old duty signal + # | + # | acceleration history + # | /-----------------\ + state = "0 0 0 0 0 0 0 0 0 0 0" + t = time.time() + + controller = subprocess.Popen(['/opt/draisine-hypro/tool/build/bin/controller', '--continuous'], stdin=subprocess.PIPE, stdout=subprocess.PIPE) + + while True: + self.refreshSensors() + + a = self.accY.read(t) + s = self.hallSpeed.read(t) + + controllerInput = '{state}\n{a} {s}\n'.format(state = state, a = self.accelerationToByte(a), s = self.speedToByte(s)) + + controller.stdin.write(controllerInput) + + state = controller.stdout.readline().rstrip('\n') + d = int(controller.stdout.readline().rstrip('\n')) + b = max(0, min(254, int(d))) + if human: + print 'accY = {v}, as byte: {b}'.format(v=a, b=self.accelerationToByte(a)) + print 'speed = {s}, as byte: {b}'.format(s=s, b=self.speedToByte(s)) + print 'duty = {d}'.format(d=b) + else: + dutycmd = array.array('B', [b]) + self.logDuty(dutycmd) + dutycmd.tofile(sys.stdout) + sys.stdout.flush() + t += self.period + sleeptime = t - time.time() + if sleeptime >= 0: + time.sleep(sleeptime) + else: + if human: print 'WARNING: Lag detected, skipping ahead {l}ms'.format(l = -sleeptime * 1000) + t += -sleeptime # skip ahead + sys.stdout.write(255) + sys.stdout.flush() + if __name__ == '__main__': def usage(): import os u = """%(name)s usage: -h: Human readable mode +-s: use alternate controller logic of SAVED project -f filename: Control mode. Filename specifies log file base. """ % {'name': os.path.basename(__file__)} @@ -139,13 +196,23 @@ if __name__ == '__main__': human = False filename = None - if len(sys.argv) > 2: - if sys.argv[1] == "-f": - filename = sys.argv[2] - else: usage() - elif len(sys.argv) > 1: - if sys.argv[1] == "-h": + saved = False + + i = 1 + while i < len(sys.argv): + if sys.argv[i] == "-h": human = True - else: usage() - dc = Control(logfile=filename) - dc.mainloop(human) + if sys.argv[i] == "-s": + saved = True + if sys.argv[i] == "-f": + i += 1 + if i < len(sys.argv): + filename = sys.argv[i] + else: usage() + i += 1 + + dc = Control(logfile=filename, precision=(50 if saved else 150)) + if (saved): + dc.savedloop(human) + else: + dc.mainloop(human) diff --git a/logging.sh b/logging.sh index 085fad3f3ce94801b2d85c5895eff8cafc37fee3..453d148e0c3169747fdedc4a35defd7377275683 100755 --- a/logging.sh +++ b/logging.sh @@ -127,7 +127,7 @@ tar) do_tar ;; *) - echo "Usage: $0 [start|run [logname]] [stop|stop-run|upload]" + echo "Usage: $0 [start|run [logname]] [stop|upload]" esac diff --git a/saved.sh b/saved.sh new file mode 100755 index 0000000000000000000000000000000000000000..58ebea5e0ce8f90e4e084e00b87a5edfec3ebe2a --- /dev/null +++ b/saved.sh @@ -0,0 +1,2 @@ +#!/bin/bash +draiscontrol/control.py -s | "MCU logging/BLDC/run"