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()