总体介绍
使用两块ordive控制ros-mobile app进行控制，odrive通过python可以轻松控制，ros-mobile可以进行与电脑的ros连接充当一个遥控器。
记录代码
读取rosmobile的遥控数据
#!/usr/bin/env python3import threading
import time
from queue import Queueimport rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from car_contral import * cQue = Queue(20)def CarRun():# init CarmyCar = Car()myCar.requestStatus()# init speedspeedvx = 0.speedvy = 0.speedw = 0.kv = 40kw = 100while not rospy.is_shutdown():v = cQue.get()if(len(v)==1):speedw = v[0]if(len(v)==2):speedvx = v[0]speedvy = v[1]print(speedvx, speedvy, speedw)myCar.carModel(kv * speedvx, kv * speedvy, kw * speedw)def callbackLeft(data):# rospy.loginfo(data)v1 = data.linear.x * 0.5v2 = data.linear.y * 0.5 cQue.put([v2, -v1])# 这里是摇杆与小车之间坐标系需要反一下def callbackRight(data):angular = data.angular.z * 0.5cQue.put([-angular])# 这里也是反的def listener():rospy.init_node('listener', anonymous=True)rospy.Subscriber("cmd_vel", Twist, callbackLeft)rospy.Subscriber("cmd_vel2", Twist, callbackRight)rospy.spin()if __name__ == '__main__':carThread = threading.Thread(target=CarRun)carThread.start()listener()odrive控制
(参考知乎-华北舵狗王)
#!/usr/bin/env python3from __future__ import print_functionimport odrive
from odrive.enums import *
import time
import math
from fibre import Logger, Event
from odrive.utils import OperationAbortedException
from fibre.protocol import ChannelBrokenException
import sysdef set_gains(odrv,axis):"""Sets the nested PID gains to a good default"""axis.controller.config.pos_gain = 100.0 #f [(counts/s) / counts]axis.controller.config.pos_gain = 0.01 #f [(counts/s) / counts]axis.controller.config.vel_gain = 5.0 / 10000.0 #[A/(counts/s)]axis.controller.config.vel_limit = 50000.0axis.controller.config.vel_integrator_gain = 0 #[A/((counts/s) * s)]axis.controller.config.kp_theta = 0.04 * 6000 / (2 * math.pi)axis.controller.config.kd_theta = 5.0 / 10000.0 * 6000 / (2 * math.pi)axis.controller.config.kp_gamma = 0.0 * 6000 / (2 * math.m2)axis.controller.config.kd_gamma = 5.0 / 10000.0 * 6000 / (2 * math.pi)def calibrate_motor(odrv, axis):# time.sleep(0.5)print('Calibrating motor...',end='',flush=True)run_state(axis, AXIS_STATE_MOTOR_CALIBRATION, True)axis.motor.config.pre_calibrated = True# then set motor pre calibrated to trueprint('Done');def calibrate_motor_and_zencoder(odrv,axis):"""Runs motor and encoder calibration (with z index) and savesEnables closed loop control on startup and encoder search on startup"""calibrate_motor(odrv, axis)print('Calibrating encoder...',end='',flush=True)axis.encoder.config.use_index = Truerun_state(axis, AXIS_STATE_ENCODER_INDEX_SEARCH, True)run_state(axis, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, True)axis.encoder.config.pre_calibrated = Trueprint('Done');print('Setting startup flags...',end='',flush=True)# axis.config.startup_encoder_index_search = Trueaxis.config.startup_encoder_index_search = Trueaxis.config.startup_closed_loop_control = Trueprint('Done with axis.')def run_state(axis, requested_state, wait):"""Sets the requested state on the given axis. If wait is True, the methodwill block until the state goes back to idle.Returns whether the odrive went back to idle in the given timeout period,which is 10s by default."""axis.requested_state = requested_statetimeout_ctr = 100; # 20s timeout for encoder calibration to finishif wait:while(timeout_ctr > 0): # waits until state goes back to idle to continuetime.sleep(0.2)timeout_ctr -= 1if axis.current_state == AXIS_STATE_IDLE:breakreturn timeout_ctr > 0def reboot_odrive(odrv):"""Reboot odrive"""try:odrv.reboot()except ChannelBrokenException:print('Lost connection because of reboot...')def reset_odrive(odrv):"""Erase config"""print('Erasing config and rebooting...')odrv.erase_configuration()reboot_odrive(odrv)def init_odrive(odrv):"""NOTE: does not save"""print('Initializing ODrive...')odrv.config.brake_resistance = 0odrv.axis0.motor.config.pole_pairs = 11odrv.axis1.motor.config.pole_pairs = 11odrv.axis0.motor.config.resistance_calib_max_voltage = 4.0odrv.axis1.motor.config.resistance_calib_max_voltage = 4.0odrv.axis0.encoder.config.cpr = 4000odrv.axis1.encoder.config.cpr = 4000odrv.axis0.motor.config.current_lim = 25 #40odrv.axis1.motor.config.current_lim = 25 #40odrv.axis0.motor.config.calibration_current = 10odrv.axis1.motor.config.calibration_current = 10odrv.axis0.motor.config.pre_calibrated = Falseodrv.axis1.motor.config.pre_calibrated = Falseodrv.axis0.encoder.config.pre_calibrated = Falseodrv.axis1.encoder.config.pre_calibrated = Falseprint('Done.')def set_odrive_limits(odrv,axis,cur_lim):"""Set motor current limits"""axis.motor.config.current_lim = current_limdef calibrate_odrive(odrv):"""Calibrate motors and encoders (including index) on both axes"""print('Calibrating axes...')calibrate_motor_and_zencoder(odrv,odrv.axis0)calibrate_motor_and_zencoder(odrv,odrv.axis1)print('Done.')def set_odrive_gains(odrv):"""Set position control gains for both motors"""print('Setting gains...')set_gains(odrv,odrv.axis0)set_gains(odrv,odrv.axis1)print('Done.')def test_odrive_motors(odrv):"""Give two position commands to both axis"""print('Testing motors...')odrv.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROLodrv.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROLtime.sleep(0.5)odrv.axis0.controller.set_pos_setpoint(1000.0, 0.0, 0.0)odrv.axis1.controller.set_pos_setpoint(-1000.0, 0.0, 0.0)time.sleep(1)odrv.axis0.controller.set_pos_setpoint(0.0, 0.0, 0.0)odrv.axis1.controller.set_pos_setpoint(0.0, 0.0, 0.0)print('Done.')def get_odrive(shutdown_token):"""Look for and return an odrive connected via usb"""print('Looking for ODrive...')odrv = odrive.find_any(serial_number='385B38543439')print('Found.')return odrvdef print_configs(odrv0):"""Print configuration info"""print('\n\nMOTOR CONFIG')print(odrv0.axis0.motor.config)print('\n')print(odrv0.axis1.motor.config)print('\n\n ENC CONFIG')print(odrv0.axis0.encoder.config)print('\n')print(odrv0.axis1.encoder.config)print('\n\n AXIS CONFIG')print(odrv0.axis0.config)print('\n')print(odrv0.axis1.config)def full_calibration(app_shutdown_token):"""Reset the odrive and calibrate everything:1) Gains and limits2) Motor3) Encoder index4) Encoder offset5) Set startup"""# Find a connected ODrive (this will block until you connect one)odrv0 = get_odrive(app_shutdown_token)reset_odrive(odrv0)odrv0 = get_odrive(app_shutdown_token)init_odrive(odrv0)set_odrive_gains(odrv0)calibrate_odrive(odrv0)odrv0.save_configuration()#run_state(axis=odrv0.axis0, requested_state=AXIS_STATE_CLOSED_LOOP_CONTROL, wait=False);#run_state(axis=odrv0.axis1, requested_state=AXIS_STATE_CLOSED_LOOP_CONTROL, wait=False);#odrv0.axis0.controller.pos_setpoint = 0;#odrv0.axis1.controller.pos_setpoint = 0;print_configs(odrv0)def bare_bones_calibration(app_shutdown_token, reset=True):"""Just calibrate motors and basic gains"""print("Run doghome\n ")odrv0 = get_odrive(app_shutdown_token)# print(odrv0)# if reset:# reset_odrive(odrv0)# odrv0 = get_odrive(app_shutdown_token)# init_odrive(odrv0)# set_odrive_gains(odrv0)# calibrate_motor(odrv0, odrv0.axis0)# calibrate_motor(odrv0, odrv0.axis1)# odrv0.axis0.config.startup_encoder_offset_calibration = True# odrv0.axis1.config.startup_encoder_offset_calibration = True# odrv0.axis0.config.startup_closed_loop_control = True# odrv0.axis1.config.startup_closed_loop_control = True# odrv0.axis0.encoder.config.use_index = False# odrv0.axis1.encoder.config.use_index = False# odrv0.axis0.config.startup_encoder_index_search =False# odrv0.axis1.config.startup_encoder_index_search =False# odrv0.save_configuration()# print_configs(odrv0)def main(app_shutdown_token):"""!! Main program !!Looks for odrive, then calibrates, then sets gains, then tests motorsWARNING: Saving more than twice per boot will cause a reversion of all changes"""bare_bones_calibration(app_shutdown_token, reset=True)#bare_bones_calibration(app_shutdown_token, reset=False)import odrive
from fibre import Logger, Event
from odrive.utils import OperationAbortedException
from fibre.protocol import ChannelBrokenExceptionapp_shutdown_token = Event()
try:main(app_shutdown_token)# init_odrive(odrv0)
except OperationAbortedException:logger.info("Operation aborted.")
finally:app_shutdown_token.set()# encoder calibration
# set use_index ot true
# request encoder_index_search
# pre_calibrated to True
# then change startup to encoder_index_search true机器人控制
#!/usr/bin/env python3
import odrive
from odrive.enums import *
from fibre import Logger, Event# 赋值速度
class Car:odrv0 = Noneodrv1 = NonemaxSpeed = 60.isInverse = [1., 1., -1.] # 电机是否需要正反向参数def __init__(self): print('Looking for ODrive...')self.odrv0 = odrive.find_any(serial_number='385B38543439')self.odrv1 = odrive.find_any(serial_number='346933773433')print('Found.')# 进入闭环def requestStatus(self):self.odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROLself.odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROLself.odrv1.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL# odrv1.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL# 释放电机def releaseMoto(self):self.odrv0.axis0.requested_state = AXIS_STATE_IDLEself.odrv0.axis1.requested_state = AXIS_STATE_IDLEself.odrv1.axis0.requested_state = AXIS_STATE_IDLE# 设置三个电机的速度def setVelocity(self, m0, m1, m2):if(m0 > self.maxSpeed):m0 = self.maxSpeedif (m0 < -self.maxSpeed):m0 = -self.maxSpeedif(m1 > self.maxSpeed):m1 = self.maxSpeedif(m1 < -self.maxSpeed):m1 = -self.maxSpeedif(m2 > self.maxSpeed):m2 = self.maxSpeedif(m2 < -self.maxSpeed):m2 = -self.maxSpeedself.odrv0.axis0.controller.input_vel = m0 * self.isInverse[0]self.odrv0.axis1.controller.input_vel = m1 * self.isInverse[1]self.odrv1.axis0.controller.input_vel = m2 * self.isInverse[2]# 根据小车模型计算速度def carModel(self, vx, vy, vw):d = 0.2# m1 = 0. + vy + vw*d# m2 = -1.732050808/2 * vx - 0.5 * vy + vw*d# m3 = 1.732050808/2 * vx - 0.5 * vy + vw*dm1 = - 1.732050808 / 2 * vx + 0.5 * vy + vw * dm2 = 0 -vy + vw * dm3 = 1.732050808 / 2 * vx + 0.5 * vy + vw * dprint("speed = ",m1,m2,m3)self.setVelocity(m1, m2, m3)
)
)