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

Read motor alarms, comment out sync code

Browse Source
This commit is contained in:
Tanner 2025-05-28 11:16:04 -07:00
parent 3fe0a5757c
commit 3443bd1f0b
1 changed files with 49 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
main.py
View File

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