Tweak and refactor
@@ -14,11 +14,11 @@ All text above must be included in any redistribution.
"""
-import busio
-import board
import time
import array
import math
+import busio
+import board
from adafruit_seesaw.seesaw import Seesaw
from adafruit_seesaw.pwmout import PWMOut
from adafruit_motor import servo
@@ -59,6 +59,15 @@ servo1.throttle = SERVO1_ZERO_ADJUST
servo2.throttle = SERVO2_ZERO_ADJUST
+def adjust(original, op, value):
+ if op == b'=':
+ return value
+ elif op == b'+':
+ return original + value
+ else:
+ return original - value
+
+
# A line was received via the UART, process it
def process_command(cmd, kp, ki, kd):
@@ -73,7 +82,7 @@ def process_command(cmd, kp, ki, kd):
"""Inner function to write the constants to the UART"""
uart.write("KP: {0: 0.3f} KI: {1: 0.3f} KD: {2: 0.3f}\r\n".format(kp, ki, kd))
- if cmd[0] == 63:
+ if cmd[0] == b'?':
report(kp, ki, kd)
elif cmd[0] not in [b'p', b'i', b'd']:
uart.write("Bad parameter\r\n")
@@ -92,26 +101,11 @@ def process_command(cmd, kp, ki, kd):
return (kp, ki, kd)
if var == b'p':
- if op == b'=':
- kp = value
- elif op == b'+':
- kp += value
- else:
- kp -= value
+ kp = adjust(kp, op, value)
elif var == b'i':
- if op == b'=':
- ki = value
- elif op == b'+':
- ki += value
- else:
- ki -= value
- else:
- if op == b'=':
- kd = value
- elif op == b'+':
- kd += value
+ ki = adjust(ki, op, value)
else:
- kd -= value
+ kd = adjust(kd, op, value)
report(kp, ki, kd)
else:
uart.write("Bad command\r\n")
@@ -123,7 +117,6 @@ def limit(x):
"""Limit the argument to the range -1.0 to 1.0"""
return max([-1.0, min([1.0, x])])
-def run():
kp = 2.000
ki = 1.000
kd = 0.050
@@ -205,9 +198,4 @@ def run():
previous_error = error
previous_time = loop_start_time
-
- # print("error: {0: 0.3f}, iterm: {1: 0.3f}, dterm: {2: 0.3f}, output: {3: 0.3f}".format(error, iterm, dterm, output))
-
time.sleep(SAMPLE_INTERVAL - (time.monotonic() - loop_start_time))
-
-run()
