56 lines
1.6 KiB
Python
56 lines
1.6 KiB
Python
|
import time
|
||
|
|
import odrive
|
||
|
|
from odrive.enums import *
|
||
|
|
|
||
|
|
accel=10.
|
||
|
|
vel=3.
|
||
|
|
calibration=False
|
||
|
|
|
||
|
|
odrv0=odrive.find_any(serial_number='205C3690424D')
|
||
|
|
|
||
|
|
if calibration:
|
||
|
|
print("Calibration...", end='', flush=True)
|
||
|
|
odrv0.axis0.requested_state=4
|
||
|
|
odrv0.axis1.requested_state=4
|
||
|
|
|
||
|
|
while odrv0.axis0.current_state != AXIS_STATE_IDLE:
|
||
|
|
time.sleep(0.1)
|
||
|
|
while odrv0.axis1.current_state != AXIS_STATE_IDLE:
|
||
|
|
time.sleep(0.1)
|
||
|
|
|
||
|
|
print("OK")
|
||
|
|
|
||
|
|
odrv0.axis0.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
|
||
|
|
odrv0.axis0.controller.config.input_mode = INPUT_MODE_VEL_RAMP
|
||
|
|
odrv0.axis1.controller.config.control_mode = CONTROL_MODE_VELOCITY_CONTROL
|
||
|
|
odrv0.axis1.controller.config.input_mode = INPUT_MODE_VEL_RAMP
|
||
|
|
|
||
|
|
odrv0.axis0.controller.config.vel_ramp_rate=accel
|
||
|
|
odrv0.axis1.controller.config.vel_ramp_rate=accel
|
||
|
|
|
||
|
|
odrv0.axis0.requested_state=AXIS_STATE_CLOSED_LOOP_CONTROL
|
||
|
|
odrv0.axis1.requested_state=AXIS_STATE_CLOSED_LOOP_CONTROL
|
||
|
|
|
||
|
|
odrv0.axis0.controller.input_vel=vel
|
||
|
|
odrv0.axis1.controller.input_vel=0
|
||
|
|
|
||
|
|
evenement=time.time()
|
||
|
|
id=0
|
||
|
|
while True:
|
||
|
|
if id==0:
|
||
|
|
torque=odrv0.axis0.motor.current_control.Iq_setpoint*odrv0.axis0.motor.config.torque_constant
|
||
|
|
else:
|
||
|
|
torque=odrv0.axis1.motor.current_control.Iq_setpoint*odrv0.axis1.motor.config.torque_constant
|
||
|
|
if abs(torque)>0.2 and (time.time()-evenement)>1:
|
||
|
|
if id==0:
|
||
|
|
odrv0.axis0.controller.input_vel=0
|
||
|
|
odrv0.axis1.controller.input_vel=vel
|
||
|
|
id=1
|
||
|
|
else:
|
||
|
|
odrv0.axis1.controller.input_vel=0
|
||
|
|
odrv0.axis0.controller.input_vel=vel
|
||
|
|
id=0
|
||
|
|
evenement0=time.time()
|
||
|
|
|
||
|
|
|