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

Compare commits

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

4 Commits
sync-contr ... d943874797

Author SHA1 Message Date
Tanner
d943874797 Add limit switch code 2025-10-24 12:46:15 -07:00
Tanner
f99eaacfec Move limit switch numbers to constants 2025-10-24 12:19:28 -07:00
Tanner
576f9ee94e Add limit switch test code 2025-05-30 11:47:29 -07:00
Tanner
3443bd1f0b Read motor alarms, comment out sync code 2025-05-28 11:16:04 -07:00
2 changed files with 116 additions and 21 deletions
Expand all files Collapse all files

37
limits.py Normal file
View File

@@ -0,0 +1,37 @@
import time
import sys
import RPi.GPIO as GPIO
PATH_OPEN = 0
PATH_BLOCKED = 1
LIMIT_LOWER_RIGHT = 19
LIMIT_LOWER_LEFT = 20
LIMIT_UPPER_RIGHT = 21
GPIO.setmode(GPIO.BCM)
GPIO.setup(LIMIT_LOWER_RIGHT, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(LIMIT_LOWER_LEFT, GPIO.IN, pull_up_down=GPIO.PUD_UP)
GPIO.setup(LIMIT_UPPER_RIGHT, GPIO.IN, pull_up_down=GPIO.PUD_UP)
# True = optical path blocked, False = open
def test():
while True:
try:
print(
'Lower Right (GPIO19):', GPIO.input(LIMIT_LOWER_RIGHT) == PATH_BLOCKED,
'| Lower Left (GPIO20):', GPIO.input(LIMIT_LOWER_LEFT) == PATH_BLOCKED,
'| Upper Right (GPIO21):', GPIO.input(LIMIT_UPPER_RIGHT) == PATH_BLOCKED,
)
time.sleep(1)
except KeyboardInterrupt as e:
sys.exit()
if __name__ == '__main__':
test()

100
main.py
View File

@@ -12,9 +12,11 @@ import aiomqtt
import serial
import glob
import time
import RPi.GPIO as GPIO
import dyn4
import relays
import limits
ENCODER_PPR = 65536
@@ -24,6 +26,13 @@ dmm1 = None
dmm2 = None
# 2025-10-24 facts:
# Motor1 is left
# Motor2 is right
# Positive RPM moves the crosshead down
# Negative RPM moves the crosshead up
MOTOR_EN1 = 1
MOTOR_EN2 = 2
DISABLED = None
@@ -128,26 +137,48 @@ 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 %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')
dmm = False
return False
async def check_sync(rev1, rev2):
global dmm1, dmm2
global dmm1, dmm2, RPM_TARGET
difference = rev1 - rev2
if abs(difference) > 2.0:
# out of sync, kill both motors
RPM_TARGET = 0
set_motors(0)
disable_motors()
@@ -176,6 +207,12 @@ 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
@@ -202,25 +239,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)
@@ -229,6 +266,27 @@ async def monitor_dyn4():
logging.info('Pos difference: %s, rev1: %s, rev2: %s, scaler1: %s, scaler2: %s, target: %s, rpm1: %s, rpm2: %s',
difference, rev1, rev2, motor1_scaler, motor2_scaler, RPM_TARGET, motor1_rpm, motor2_rpm)
# check limit switches
if ONE_MOTOR:
pass
else:
GOING_DOWNWARD = motor1_rpm > 0 or motor2_rpm > 0
GOING_UPWARD = motor1_rpm < 0 or motor2_rpm < 0
if GOING_DOWNWARD:
if GPIO.input(limits.LIMIT_LOWER_RIGHT) == limits.PATH_BLOCKED or GPIO.input(limits.LIMIT_LOWER_LEFT) == limits.PATH_BLOCKED:
motor1_rpm = 0
motor2_rpm = 0
logging.info('Lower limit switch hit, preventing downward travel.')
if GOING_UPWARD:
if GPIO.input(limits.LIMIT_UPPER_RIGHT) == limits.PATH_BLOCKED:
motor1_rpm = 0
motor2_rpm = 0
logging.info('Upper limit switch hit, preventing upward travel.')
if ONE_MOTOR:
logging.debug('Setting motor1: %s', motor1_rpm)
dmm1.set_speed(motor1_rpm)