tanner/slacksnap-motor-control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: tanner:master
Branches Tags
tanner:master
tanner:sync-control
..
compare: tanner:sync-control
Branches Tags
tanner:master
tanner:sync-control

No commits in common. "master" and "sync-control" have entirely different histories.
master ... sync-contr

2 changed files with 21 additions and 77 deletions
Show Stats Expand all files Collapse all files

28
limits.py
View File

@ -1,28 +0,0 @@
import time
import sys
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM)
GPIO.setup(19, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(20, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(21, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# 1 = optical path blocked, 0 = open
def test():
while True:
try:
print(
'GPIO19:', GPIO.input(19),
'GPIO20:', GPIO.input(20),
'GPIO21:', GPIO.input(21),
)
time.sleep(1)
except KeyboardInterrupt as e:
sys.exit()
if __name__ == '__main__':
test()

70
main.py
View File

@ -128,48 +128,26 @@ async def init_motor(path, name):
async def read_motor(dmm, name):
global RPM_TARGET
try:
return dmm.read_AbsPos32()
except BaseException as e:
# any problems, kill both motors
RPM_TARGET = 0
set_motors(0)
disable_motors()
logging.error('Problem reading position of %s: %s - %s', name, e.__class__.__name__, e)
await send_mqtt('server/motor_status', name + ' disconnected')
dmm = False
return False
async def check_status(dmm, name):
global RPM_TARGET
try:
status = dmm.read_Status()
current = dmm.read_TrqCurrent()
logging.info('%s status, alarm: %s, current: %s', name, status['alarm'] or 'None', current)
except BaseException as e:
# any problems, kill both motors
RPM_TARGET = 0
set_motors(0)
disable_motors()
logging.error('Problem reading status of %s: %s - %s', name, e.__class__.__name__, e)
logging.error('Problem reading %s: %s - %s', name, e.__class__.__name__, e)
await send_mqtt('server/motor_status', name + ' disconnected')
dmm = False
return False
async def check_sync(rev1, rev2):
global dmm1, dmm2, RPM_TARGET
global dmm1, dmm2
difference = rev1 - rev2
if abs(difference) > 2.0:
# out of sync, kill both motors
RPM_TARGET = 0
set_motors(0)
disable_motors()
@ -198,12 +176,6 @@ async def monitor_dyn4():
dmm2 = await init_motor('/dev/ttyUSB1', 'Motor2')
continue
if dmm1:
await check_status(dmm1, 'Motor1')
if dmm2:
await check_status(dmm2, 'Motor2')
pos1 = await read_motor(dmm1, 'Motor1')
if pos1 is False:
dmm1 = False
@ -230,25 +202,25 @@ async def monitor_dyn4():
kp = 0.01
difference = rev1 - rev2
#if abs(difference) > 0.25:
# if RPM_TARGET > 0:
# if rev1 > rev2:
# motor1_scaler = 1.0 - kp * abs(difference)
# motor1_scaler = min(motor1_scaler, 1.0) # clamp to range 0.5 - 1.0
# motor1_scaler = max(motor1_scaler, 0.5)
# elif rev2 > rev1:
# motor2_scaler = 1.0 - kp * abs(difference)
# motor2_scaler = min(motor2_scaler, 1.0) # clamp to range 0.5 - 1.0
# motor2_scaler = max(motor2_scaler, 0.5)
# elif RPM_TARGET < 0:
# if rev1 < rev2:
# motor1_scaler = 1.0 - kp * abs(difference)
# motor1_scaler = min(motor1_scaler, 1.0) # clamp to range 0.5 - 1.0
# motor1_scaler = max(motor1_scaler, 0.5)
# elif rev2 < rev1:
# motor2_scaler = 1.0 - kp * abs(difference)
# motor2_scaler = min(motor2_scaler, 1.0) # clamp to range 0.5 - 1.0
# motor2_scaler = max(motor2_scaler, 0.5)
if abs(difference) > 0.25:
if RPM_TARGET > 0:
if rev1 > rev2:
motor1_scaler = 1.0 - kp * abs(difference)
motor1_scaler = min(motor1_scaler, 1.0) # clamp to range 0.5 - 1.0
motor1_scaler = max(motor1_scaler, 0.5)
elif rev2 > rev1:
motor2_scaler = 1.0 - kp * abs(difference)
motor2_scaler = min(motor2_scaler, 1.0) # clamp to range 0.5 - 1.0
motor2_scaler = max(motor2_scaler, 0.5)
elif RPM_TARGET < 0:
if rev1 < rev2:
motor1_scaler = 1.0 - kp * abs(difference)
motor1_scaler = min(motor1_scaler, 1.0) # clamp to range 0.5 - 1.0
motor1_scaler = max(motor1_scaler, 0.5)
elif rev2 < rev1:
motor2_scaler = 1.0 - kp * abs(difference)
motor2_scaler = min(motor2_scaler, 1.0) # clamp to range 0.5 - 1.0
motor2_scaler = max(motor2_scaler, 0.5)
motor1_rpm = int(RPM_TARGET * motor1_scaler)
motor2_rpm = int(RPM_TARGET * motor2_scaler)