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

Begin motor sync control loop

Browse Source
This commit is contained in:
Tanner 2025-04-02 12:30:43 -07:00
parent a1fcd61c53
commit 3fe0a5757c
1 changed files with 56 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
main.py
View File

@ -27,6 +27,7 @@ dmm2 = None
MOTOR_EN1 = 1
MOTOR_EN2 = 2
DISABLED = None
RPM_TARGET = 0
def enable_motors():
global DISABLED
@ -76,6 +77,8 @@ def set_motors(rpm):
async def process_mqtt(message):
global RPM_TARGET
try:
text = message.payload.decode()
except UnicodeDecodeError:
@ -86,7 +89,7 @@ async def process_mqtt(message):
if topic == 'motion/set_rpm':
rpm = int(float(text))
set_motors(rpm)
RPM_TARGET = rpm
async def monitor_mqtt():
@ -143,7 +146,7 @@ async def check_sync(rev1, rev2):
difference = rev1 - rev2
if abs(difference) > 1.0:
if abs(difference) > 2.0:
# out of sync, kill both motors
set_motors(0)
disable_motors()
@ -154,9 +157,13 @@ async def check_sync(rev1, rev2):
logging.error('MOTOR READINGS OUT OF SYNC! Motor1: %s, Motor2: %s, difference: %s', rev1, rev2, difference)
await send_mqtt('server/motor_status', 'Motor desync')
return False
return True
async def monitor_dyn4():
global dmm1, dmm2
global dmm1, dmm2, RPM_TARGET
while True:
await asyncio.sleep(0.1)
@ -173,20 +180,63 @@ async def monitor_dyn4():
if pos1 is False:
dmm1 = False
continue
rev1 = pos1 / ENCODER_PPR
rev1 = round(pos1 / ENCODER_PPR, 4)
if ONE_MOTOR:
revolutions = rev1
motor1_rpm = RPM_TARGET
else:
pos2 = await read_motor(dmm2, 'Motor2')
if pos2 is False:
dmm2 = False
continue
rev2 = pos2 / ENCODER_PPR
rev2 = round(pos2 / ENCODER_PPR, 4)
revolutions = (rev1 + rev2) / 2.0
await check_sync(rev1, rev2)
if not await check_sync(rev1, rev2):
continue
motor1_scaler = 1.0
motor2_scaler = 1.0
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)
motor1_rpm = int(RPM_TARGET * motor1_scaler)
motor2_rpm = int(RPM_TARGET * motor2_scaler)
if abs(difference) > 0.25:
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)
if ONE_MOTOR:
logging.debug('Setting motor1: %s', motor1_rpm)
dmm1.set_speed(motor1_rpm)
else:
logging.debug('Setting motor1: %s, motor2: %s', motor1_rpm, motor2_rpm)
dmm1.set_speed(motor1_rpm)
dmm2.set_speed(motor2_rpm)
topic = 'server/position'
message = str(round(revolutions, 5))