class CircleRobot:
motor_left_ID = 1
motor_right_ID = 2
max_pwm = 255
def __init__(self):
self.setup_motor_controller()
self.last_known_command_time = rospy.Time.now()
def setup_motor_controller(self):
rospy.loginfo("Starting with controller %s", CONTROLLER)
if CONTROLLER == 'adafruit':
# setup motor controller
self.motor_driver = Adafruit_MotorHAT(i2c_bus=1)
self.motor_left = self.motor_driver.getMotor(
CircleRobot.motor_left_ID)
self.motor_right = self.motor_driver.getMotor(
CircleRobot.motor_right_ID)
# stop the motors as precaution
self.all_stop()
elif CONTROLLER == 'qwiic':
self.motor_driver = qwiic_scmd.QwiicScmd()
self.motor_driver.disable()
def circle(self, throttle_pct=0):
speed = int(throttle_pct * self.max_pwm)
rospy.loginfo("speed: %d", speed)
self.motor_driver.set_drive(0, 0, speed)
self.motor_driver.set_drive(1, 1, speed)
self.motor_driver.enable()
def main(self):
self.circle(0.25)
rospy.sleep(10)
self.motor_driver.disable()
rospy.spin()
class Move():
def __init__(self):
rospy.loginfo("Setup Move...")
# setup motor controller
if rospy.get_param("motor_controller").lower() == 'adafruit':
self.motor_driver = Adafruit_MotorHAT(
i2c_bus=int(rospy.get_param("i2c_bus")))
self.motor_left_ID = 1
self.motor_right_ID = 2
self.motor_left = self.motor_driver.getMotor(self.motor_left_ID)
self.motor_right = self.motor_driver.getMotor(self.motor_right_ID)
self.all_stop()
elif rospy.get_param("motor_controller").lower() == 'qwiic':
self.motor_driver = qwiic_scmd.QwiicScmd()
self.motor_driver.disable()
rospy.loginfo("Setup Move Complete")
def set_speed(self, motor_ID, value):
max_pwm = float(rospy.get_param("max_pwm"))
speed = int(min(max(abs(value * max_pwm), 0), max_pwm))
if motor_ID == 1:
motor = self.motor_left
elif motor_ID == 2:
motor = self.motor_right
else:
rospy.logerror('set_speed(%d, %f) -> invalid motor_ID=%d',
motor_ID, value, motor_ID)
return
if rospy.get_param("motor_controller").lower() == 'adafruit':
motor.setSpeed(speed)
elif rospy.get_param("motor_controller").lower() == 'qwiic':
if (speed > 0):
dir = 1
else:
dir = -1
self.motor_driver.set_drive(motor_ID, dir, abs(speed))
self.motor_driver.set_drive(motor_ID, dir, abs(speed))
self.motor_driver.enable()
if value > 0:
motor.run(Adafruit_MotorHAT.FORWARD)
else:
motor.run(Adafruit_MotorHAT.BACKWARD)
def start(self):
rospy.init_node('move')
rospy.Subscriber('~cmd_dir', String, self.on_cmd_dir)
rospy.Subscriber('~cmd_vel', Twist, self.on_cmd_vel)
rospy.Subscriber('~cmd_raw', String, self.on_cmd_raw)
rospy.Subscriber('~cmd_str', String, self.on_cmd_str)
rospy.spin()
# stops all motors
def all_stop(self):
if rospy.get_param("motor_controller").lower() == 'adafruit':
self.motor_left.setSpeed(0)
self.motor_right.setSpeed(0)
self.motor_left.run(Adafruit_MotorHAT.RELEASE)
self.motor_right.run(Adafruit_MotorHAT.RELEASE)
elif rospy.get_param("motor_controller").lower() == 'qwiic':
self.motor_driver.disable()
def move_dir(self, val):
if val == "left":
self.set_speed(self.motor_left_ID, float(rospy.get_param("speed")))
self.set_speed(self.motor_right_ID,
(-1 * float(rospy.get_param("speed"))))
elif val == "right":
self.set_speed(self.motor_left_ID, float(rospy.get_param("speed")))
self.set_speed(self.motor_right_ID,
float(rospy.get_param("speed")))
elif val == "backward":
self.set_speed(self.motor_left_ID, float(rospy.get_param("speed")))
self.set_speed(self.motor_right_ID,
float(rospy.get_param("speed")))
elif val == "forward":
self.set_speed(self.motor_left_ID,
(-1 * float(rospy.get_param("speed"))))
self.set_speed(self.motor_right_ID,
(-1 * float(rospy.get_param("speed"))))
elif val == "stop":
self.all_stop()
else:
rospy.logerror('Direction not supported.')
# directional commands (degree, speed)
def on_cmd_dir(self, msg):
rospy.loginfo(rospy.get_caller_id() + ' cmd_dir=%s', msg.data)
# velocity, twist commands (Twist)
def on_cmd_vel(self, msg):
x = msg.linear.x
y = msg.angular.z / 10
if x > 0 and y < 0: #backward right
rospy.loginfo(rospy.get_caller_id() +
', backward right (left, right)=(%s,%s)' %
((abs(y) + 0.1), (0.2 + y + 0.1)))
self.set_speed(self.motor_left_ID, (abs(y) + 0.1))
self.set_speed(self.motor_right_ID, (0.2 + y + 0.1))
elif x > 0 and y > 0: #backward left
rospy.loginfo(rospy.get_caller_id() +
', backward left (left, right)=(%s,%s)' %
((0.2 - y + 0.1), (y + 0.1)))
self.set_speed(self.motor_left_ID, (0.2 - y + 0.1))
self.set_speed(self.motor_right_ID, (y + 0.1))
elif x < 0 and y > 0: #forward left
rospy.loginfo(rospy.get_caller_id() +
', forward left (left, right)=(%s,%s)' %
((-(0.2 - y) - 0.1), -(y + 0.1)))
self.set_speed(self.motor_left_ID, (-(0.2 - y) - 0.1))
self.set_speed(self.motor_right_ID, -(y + 0.1))
elif x < 0 and y < 0: #forward right
rospy.loginfo(rospy.get_caller_id() +
', forward right (left, right)=(%s,%s)' %
(y - 0.1, (-(0.2 + y) - 0.1)))
self.set_speed(self.motor_left_ID, y - 0.1)
self.set_speed(self.motor_right_ID, (-(0.2 + y) - 0.1))
else:
self.all_stop()
# raw L/R motor commands (speed, speed)
def on_cmd_raw(self, msg):
rospy.loginfo(rospy.get_caller_id() + ' cmd_raw=%s', msg.data)
move_data_recv = json.loads(msg.data)
self.set_speed(self.motor_left_ID, float(move_data_recv['left']))
self.set_speed(self.motor_right_ID, float(move_data_recv['right']))
# simple string commands (left/right/forward/backward/stop)
def on_cmd_str(self, msg):
rospy.loginfo(rospy.get_caller_id() + ' cmd_str=%s', msg.data)
self.move_dir(msg.data.lower())
class Robot(SingletonConfigurable):
left_motor = traitlets.Instance(Motor)
right_motor = traitlets.Instance(Motor)
# config
i2c_bus = traitlets.Integer(default_value=1).tag(config=True)
left_motor_channel = traitlets.Integer(default_value=1).tag(config=True)
left_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
right_motor_channel = traitlets.Integer(default_value=2).tag(config=True)
right_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
# Adafruit Hardware
if 96 in addresses:
def __init__(self, *args, **kwargs):
super(Robot, self).__init__(*args, **kwargs)
self.motor_driver = Adafruit_MotorHAT(i2c_bus=self.i2c_bus)
self.left_motor = Motor(self.motor_driver,
channel=self.left_motor_channel,
alpha=self.left_motor_alpha)
self.right_motor = Motor(self.motor_driver,
channel=self.right_motor_channel,
alpha=self.right_motor_alpha)
def set_motors(self, left_speed, right_speed):
self.left_motor.value = left_speed
self.right_motor.value = right_speed
def forward(self, speed=1.0, duration=None):
self.left_motor.value = speed
self.right_motor.value = speed
def backward(self, speed=1.0):
self.left_motor.value = -speed
self.right_motor.value = -speed
def left(self, speed=1.0):
self.left_motor.value = -speed
self.right_motor.value = speed
def right(self, speed=1.0):
self.left_motor.value = speed
self.right_motor.value = -speed
def stop(self):
self.left_motor.value = 0
self.right_motor.value = 0
# SparkFun Hardware
elif 93 in addresses:
def __init__(self, *args, **kwargs):
super(Robot, self).__init__(*args, **kwargs)
self.motor_driver = qwiic.QwiicScmd()
self.left_motor = Motor(self.motor_driver,
channel=self.left_motor_channel,
alpha=self.left_motor_alpha)
self.right_motor = Motor(self.motor_driver,
channel=self.right_motor_channel,
alpha=self.right_motor_alpha)
self.motor_driver.enable()
def set_motors(self, left_speed, right_speed):
self.left_motor.value = left_speed
self.right_motor.value = right_speed
self.motor_driver.enable()
# Set Motor Controls: .set_drive( motor number, direction, speed)
# Motor Number: A = 0, B = 1
# Direction: FWD = 0, BACK = 1
# Speed: (-255) - 255 (neg. values reverse direction of motor)
def forward(self, speed=1.0, duration=None):
speed = int(speed * 255)
self.motor_driver.set_drive(0, 0, speed)
self.motor_driver.set_drive(1, 0, speed)
self.motor_driver.enable()
def backward(self, speed=1.0):
speed = int(speed * 255)
self.motor_driver.set_drive(0, 1, speed)
self.motor_driver.set_drive(1, 1, speed)
self.motor_driver.enable()
def left(self, speed=1.0):
speed = int(speed * 255)
self.motor_driver.set_drive(0, 1, speed)
self.motor_driver.set_drive(1, 0, speed)
self.motor_driver.enable()
def right(self, speed=1.0):
speed = int(speed * 255)
self.motor_driver.set_drive(0, 0, speed)
self.motor_driver.set_drive(1, 1, speed)
self.motor_driver.enable()
def stop(self):
self.motor_driver.set_drive(0, 0, 0)
self.motor_driver.set_drive(1, 1, 0)
self.motor_driver.disable()
