def __init__(self):
super().__init__()
# open Adafruit MotorHAT driver
self.driver = Adafruit_MotorHAT(i2c_bus=1)
# get motor objects from driver
self.motors = {
self.MOTOR_LEFT : self.driver.getMotor(self.MOTOR_LEFT),
self.MOTOR_RIGHT : self.driver.getMotor(self.MOTOR_RIGHT)
}
def initializeHat():
mh = Adafruit_MotorHAT(addr=0x60)
# recommended for auto-disabling motors on shutdown!
def turnOffMotors():
for i in range(4):
mh.getMotor(i + 1).run(Adafruit_MotorHAT.RELEASE)
atexit.register(turnOffMotors)
return mh
def handleConnected(self):
self.pi = pigpio.pi()
print(self.address, 'connected')
# motor hat
self.mh = Adafruit_MotorHAT(addr=0x60)
print("motorHAT connected")
# 2개의 stepper 모터의 설정정보를 멤버변수로 기억해둠
self.steps_per_turn = [None, None]
self.stepper = [None, None]
self.stepstyle = [None, None]
self.rpm = [None, None]
def __init__(self):
"""
setup the GPIO and motor hat for the Pi
:return: BoxController
"""
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.SENSOR_OUT_PIN, GPIO.OUT)
GPIO.setup(self.SENSOR_IN_PIN, GPIO.IN, pull_up_down=GPIO.PUD_DOWN)
mh = Adafruit_MotorHAT(addr=0x60)
self.stepper = mh.getStepper(200, 1)
self.stepper.setSpeed(1000)
def __init__(self, addr=0x60):
self.motor_hat = Adafruit_MotorHAT(addr=addr)
self.left_x = self.motor_hat.getMotor(4)
self.right_x = self.motor_hat.getMotor(3)
self.left_up = self.motor_hat.getMotor(2)
self.right_up = self.motor_hat.getMotor(1)
self.left_x_speed = 0
self.left_up_speed = 0
self.right_x_speed = 0
self.right_up_speed = 0
def __init__(self):
self.motorhat = Adafruit_MotorHAT(addr= 0x60)
self.leftMotor = self.motorhat.getMotor(1)
self.rightMotor = self.motorhat.getMotor(2)
self.posx=0
self.posy=0
self.turned=0
self.old_posx=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
self.old_posy=[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
self.index=0
self.turn_index=0
self.communication()
def __init__(self):
self.mh = Adafruit_MotorHAT(addr=0x60)
self.fl = self.mh.getMotor(1)
self.fr = self.mh.getMotor(3)
self.bl = self.mh.getMotor(2)
self.br = self.mh.getMotor(4)
# Start with the module disabled
self.set_enabled(False)
# Turn off the motors if the module is killed
atexit.register(self._turnOffMotors)
def __init__(self, addr, motorlist, verbose=False):
self.name = addr
self.verbose = verbose
self.hat = Adafruit_MotorHAT(int(addr, 16))
self.motors = dict([(m, self.hat.getMotor(m)) for m in motorlist])
self.until = dict([(m, None) for m in motorlist])
for motorname, motor in self.motors.items():
motor.setSpeed(150)
motor.run(Adafruit_MotorHAT.FORWARD)
motor.run(Adafruit_MotorHAT.RELEASE)
if self.verbose:
print("init hat %s motor %s" % (self.name, motorname))
def executor():
""" Executes commands on the queue with specified time delay.
"""
# Get time delay
timedelay = int(config['output']['timedelay'])
# Loop to get messages
message = ''
while not message.strip().lower() in ['x', 'exit', 'q', 'quit']:
# Get message from queue and split into time and message
msg = queue.get()
timesec, message = msg.split(' ', 1)
timesec = float(timesec)
commsocket.send_log('Running: %s' % msg, logport, 'output.execute', 'DEBUG')
# Wait until it's timesec+timedelay
while time.time() - timesec < timedelay :
time.sleep(0.5)
### EXECUTE MESSAGES HERE
# Motor:
if message[0].lower() == 'm':
# Make a motor hat with correct number
mh =Adafruit_MotorHAT(addr=motorHatAddr)
motor=mh.getMotor(int(message[1]))
# Get speed, direction and duration
msplit = message.split()
speed = int(msplit[1])
if speed < 0:
direction = Adafruit_MotorHAT.BACKWARD
speed = -speed
elif speed > 0:
direction = Adafruit_MotorHAT.FORWARD
else:
direction = Adafruit_MotorHAT.RELEASE
duration = float(msplit[2])
# Set motor movement
motor.run(direction)
motor.setSpeed(speed)
# Wait for duration
if duration!=0:
time.sleep(duration)
motor.run(Adafruit_MotorHAT.RELEASE)
# Servo
if message[0] == 's':
pwm = Adafruit_PCA9685.PCA9685(address=servoHatAddr)
pwm.set_pwm_freq(60)
pwm.set_pwm(int(message[1]),0,int(message[3:]))
time.sleep(1)
if message[0:5]=='color':
print("Color Sent")
incamport=int(config['ports']['flask_incam'])
commsocket.send_msg(message,incamport)
commsocket.send_log('Executed: %s' % message, logport, 'output.execute', 'INFO')
# Sleep a bit to make sure all messages are out and threads can close
time.sleep(0.2)
def __init__(self, metaclass=Singleton):
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
import atexit
self.FORWARD = Adafruit_MotorHAT.FORWARD
self.BACKWARD = Adafruit_MotorHAT.BACKWARD
self.RELEASE = Adafruit_MotorHAT.RELEASE
self.mh = Adafruit_MotorHAT(addr=0x60)
#self.mh = Adafruit_MotorHAT(addr=0x6f)
self.mhM1 = self.mh.getMotor(1)
self.mhM2 = self.mh.getMotor(2)
def __init__(self):
self.initialized = False
self.hat = Adafruit_MotorHAT(addr=0x60)
self.motorOne = None
self.motorTwo = None
self.motorThree = None
self.motorFour = None
self.moving = False
self.pwm = GPIO.PWM(18, 50) # channel=12 frequency=50Hz
self.pwm.start(100)
def test_motor(hat, motor):
print hat
print motor
global TYPE
hat = int(hat, 16)
mh = Adafruit_MotorHAT(addr = hat)
motor_number = motor
myStepper = mh.getStepper(200, motor_number) # 200 steps/rev, motor port #1
myStepper.setSpeed(120) # 30 RPM
myStepper.step(STEPS, Adafruit_MotorHAT.FORWARD, TYPE)
myStepper.step(STEPS, Adafruit_MotorHAT.BACKWARD, TYPE)
def __init__(self):
self.initialized = False
self.hat = Adafruit_MotorHAT(addr=0x60)
self.motorOne = None
self.motorTwo = None
self.motorThree = None
self.motorFour = None
self.moving = False
self.currentSpeed = 0
self.leftCurrentDirection = None
self.rightCurrentDirection = None
def __init__(self):
self.m_initialized = False
self.mh = Adafruit_MotorHAT(addr=0x60)
self.m_frontLeftMotor = None
self.m_frontRightMotor = None
self.m_backLeftMotor = None
self.m_backRightMotor = None
self.m_moving = False
self.m_currentSpeed = 0
self.m_leftCurrentDirection = None
self.m_rightCurrentDirection = None
def __init__(self):
self.motorhat = Adafruit_MotorHAT(0x60)
self.fl = self.motorhat.getMotor(1) # Front left
self.fr = self.motorhat.getMotor(2) # Front right
self.rl = self.motorhat.getMotor(3) # Rear left
self.rr = self.motorhat.getMotor(4) # Rear right
self.pid_fr = PID(30.0, 20.0, 0.0, sample_time = 0.028)
self.pid_fl = PID(30.0, 20.0, 0.0, sample_time = 0.028)
self.pid_rr = PID(30.0, 20.0, 0.0, sample_time = 0.028)
self.pid_rl = PID(30.0, 20.0, 0.0, sample_time = 0.028)
self.pid_fr.output_limits = (-255, 255)
self.pid_fl.putput_limits = (-255, 255)
self.pid_rr.output_limits = (-255, 255)
self.pid_rl.output_limits = (-255, 255)
self.port = rospy.get_param('~port', '/dev/ttyACM0')
self.pub_tf = rospy.get_param('~pub_tf', False)
self.ard = serial.Serial(self.port, 57600)
# Flush serial data
for i in range(0, 20):
_ = self.ard.readline()
# Subscriber and publisher
self.pub_odom = rospy.Publisher('/wheel_odom', Odometry, queue_size = 10)
self.sub_cmd = rospy.Subscriber('/cmd_vel', Twist, self.cmd_cb, queue_size = 1)
# Service
self.reset_odom_srv = rospy.Service('reset_wheel_odom', Empty, self.reset_odom)
if self.pub_tf:
self.tf_br = tf.TransformBroadcaster()
self.time = rospy.Time.now()
rospy.Timer(rospy.Duration(1/35.0), self.read_data) # 35Hz
# Four wheel angular velocities
self.w_fr = None
self.w_fl = None
self.w_rr = None
self.w_rl = None
# Position variables
self.x = 0
self.y = 0
self.heading = 0
# Car velocity
self.v_x = 0
self.v_y = 0
self.omega = 0
# Desired velocity
self.vx_d = 0
self.vy_d = 0
self.w_d = 0
# Odom msg
self.my_odom = Odometry()
self.my_odom.header.frame_id = 'odom'
self.velocity_change = True
self.shutdown_ = False
rospy.loginfo("[%s] Initialized" %(rospy.get_name()))
def __init__(self, device, software_only=False):
''' device is ignored, it is required for bartendro hardware'''
self.device = device
self.__doc__ = 'foo'
self.dispenser_cnt = 8
self.software_only = software_only
self.dispenser_version = DISPENSER_DEFAULT_VERSION
self.startup_log = ""
# I need a hellodrinkbot switch
#if not software_only:
if 1:
try:
self.mh1 = Adafruit_MotorHAT(addr=0x60)
self.ports = [self.mh1.getMotor(foo+1) for foo in range(4)]
for motor in range(4):
self.ports[motor].setSpeed(255)
except:
# no motor hat, but that might be fine
log.info("No Motor Hat?")
#self.mh1 = Adafruit_MotorHAT(addr=0x60)
#self.ports = [self.mh1.getMotor(foo) for foo in range(1, 5)]
#for motor in range(8):
#self.ports[motor].setSpeed(255)
pass
# Add a second motor hat, with a second address. Comment the
# above lines, replace with something like this:
# self.mh1 = Adafruit_MotorHAT(addr=0x60)
# self.mh2 = Adafruit_MotorHAT(addr=0xXX)
# self.ports = [self.mh1.getMotor(range(1,9))]
# for motor in range(8):
# self.ports[motor].setSpeed(255)
else:
#self.ports = [i for i in range(1, 5)]
pass
self.num_dispensers = MAX_DISPENSERS
self.dispensers = [
#{'port': None, 'direction': MOTOR_DIRECTION_FORWARD},
{'port': 0, 'direction': MOTOR_DIRECTION_FORWARD},
{'port': 0, 'direction': MOTOR_DIRECTION_BACKWARD},
{'port': 1, 'direction': MOTOR_DIRECTION_FORWARD},
{'port': 1, 'direction': MOTOR_DIRECTION_BACKWARD},
{'port': 2, 'direction': MOTOR_DIRECTION_FORWARD},
{'port': 2, 'direction': MOTOR_DIRECTION_BACKWARD},
{'port': 3, 'direction': MOTOR_DIRECTION_FORWARD},
{'port': 3, 'direction': MOTOR_DIRECTION_BACKWARD},
]
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)
self.brush_motor = Motor(self.motor_driver,
channel=self.brush_motor_channel,
alpha=self.brush_motor_alpha) #
def initMotors(self):
# new Motor HAT
self.mh = Adafruit_MotorHAT(addr=0x60)
atexit.register(self.disableTurret)
#create and set stepper motor objects
self.verticalStepper = self.mh.getStepper(200, 2)
self.verticalStepper.setSpeed(5)
self.horizontalStepper = self.mh.getStepper(200, 1)
self.horizontalStepper.setSpeed(5)
return self
def __init__(self,
hat_addr=constants.HAT_ADDRESS,
fwd_speed=_FWD_SPEED,
bwd_speed=_BWD_SPEED,
spool_speed=_SPOOL_SPEED):
print_info()
self._motor_hat = Adafruit_MotorHAT(addr=hat_addr)
self.fwd_speed = fwd_speed
self.bwd_speed = bwd_speed
self.spool_speed = spool_speed
self.motors = {1: None, 2: None, 3: None, 4: None}
self.drive_motors = {1: None, 2: None, 3: None, 4: None}
self.spool_motors = {1: None, 2: None, 3: None, 4: None}
def __init__(self, channel, alpha=0.5, offset=0.2, i2c_bus=1, debug=False):
self.i2c_bus, self.alpha, self.offset = i2c_bus, alpha, offset
self.driver = Adafruit_MotorHAT(i2c_bus=self.i2c_bus)
self.motor = self.driver.getMotor(channel)
self._debug = debug
self._channel = channel
self._old_speed = -1
if (channel == 1):
self.ina, self.inb = 1, 0
else:
self.ina, self.inb = 2, 3
atexit.register(self.stop)
def __init__(self,
hat_addr=0x60,
flow_meter_pin=26,
calibration_button_pin=22,
tick_range=500):
self.name = "Wash Input Valve"
self._tick_range = tick_range
self.flow_meter = FlowMeter(flow_meter_pin)
self.calibration_button = Button(calibration_button_pin,
self.max_range_callback)
motor_hat = Adafruit_MotorHAT(addr=hat_addr)
self.stepper = Stepper(motor_hat)
self._has_calib = False
def __init__(self):
self.node_name = "gazebo_car_control_node"
self.active = True
self.i = 0
self.motorhat = Adafruit_MotorHAT(addr=0x60)
self.gazebo_car_control_L = self.motorhat.getMotor(1)
self.gazebo_car_control_R = self.motorhat.getMotor(2)
self.threshold = 0
self.sub_control_value = rospy.Subscriber(
"/gazebo_sub_jointstate/control_value",
Point,
self.cbControl_value,
queue_size=1)
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " %(self.node_name))
self.mh = Adafruit_MotorHAT(addr=0x60)
self.myMotor = self.mh.getMotor(4)
# set the speed to start, from 0 (off) to 255 (max speed)
self.myMotor.setSpeed(150)
# Setup subscribers
self.sub_grip = rospy.Subscriber("~gripper_cmd", Pixel, self.gripCmd, queue_size=1)
def __init__(self):
self.motor_controller = Adafruit_MotorHAT(i2c_address)
self.FL = Wheel(self.motor_controller, FL_id, trim[0], invert=True)
self.FR = Wheel(self.motor_controller, FR_id, trim[1], invert=True)
self.RL = Wheel(self.motor_controller, RL_id, trim[2])
self.RR = Wheel(self.motor_controller, RR_id, trim[3])
self.all_wheels = [self.FL, self.FR, self.RL, self.RR]
self.left_wheels = [self.FL, self.RL]
self.right_wheels = [self.FR, self.RR]
self.left_diag = [self.FL, self.RR]
self.right_diag = [self.FR, self.RL]
atexit.register(self.stop)
self.stop()
def __init__(self):
GPIO.setmode(GPIO.BCM)
self.drivingSpeed = 200
self.mh = Adafruit_MotorHAT(addr=0x60)
atexit.register(self.turnOffMotors)
self.motorA = self.mh.getMotor(1)
self.motorB = self.mh.getMotor(2)
self.forward = [-1, 1, 1, -1]
self.backward = [1, -1, -1, 1]
self.left = [1, -1, 1, -1]
self.right = [-1, 1, -1, 1]
self.turnDelay = 0.1
self.straightDelay = 0.5
self.motorSpeedCorrection = [0, 0, 10, 10]
def __init__(self):
Car.__init__(self)
self._calibration = {}
self._calibration["offset_left"] = 0.2
self._calibration["gain_left"] = 0.8
self._calibration["offset_right"] = 0.2
self._calibration["gain_right"] = 0.8
self._mh = Adafruit_MotorHAT(addr=0x60)
atexit.register(self._turn_off_motors)
self._left_motor = self._mh.getMotor(1)
self._right_motor = self._mh.getMotor(2)
def __init__(self):
self.LOGGER = Logger(self)
self.controller = Adafruit_MotorHAT(addr=0x60, i2c_bus=1)
self.motors = [0, 0, 0, 0]
# Initialize motor subscribers
rospy.init_node('interface')
rospy.Subscriber("/motor", String, self.on_motor_callback, queue_size=1)
# Turn off motors when the script exits.
atexit.register(self.turn_off_motors)
# Motor tracking
r = rospy.Rate(2)
while not rospy.is_shutdown():
self.LOGGER.info(str(self.motors))
r.sleep()
def __init__(self, motor_num):
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
import atexit
self.FORWARD = Adafruit_MotorHAT.FORWARD
self.BACKWARD = Adafruit_MotorHAT.BACKWARD
self.mh = Adafruit_MotorHAT(addr=0x60)
self.motor = self.mh.getMotor(motor_num)
self.motor_num = motor_num
atexit.register(self.turn_off_motors)
self.speed = 0
self.throttle = 0
def reverse(axesVT, axesHZ):
if (axesVT > 0):
try:
myMotorRT.run(Adafruit_MotorHAT.BACKWARD)
myMotorLT.run(Adafruit_MotorHAT.BACKWARD)
except OSError:
mh = Adafruit_MotorHAT(addr=0x60)
print("error error error error error")
axesMotorRT = axesVT
axesMotorLT = axesVT
if (axesHZ > 0):
axesMotorRT = (axesVT * (1 - axesHZ))
print('reverse right')
elif (axesHZ < 0):
axesMotorLT = (axesVT * (1 - (axesHZ * (-1))))
print('reverse left')
try:
myMotorLT.setSpeed(int(axesMotorLT))
myMotorRT.setSpeed(int(axesMotorRT))
except OSError:
mh = Adafruit_MotorHAT(addr=0x60)
print("error error error error error")
def main():
rospy.init_node('me212bot', anonymous=True)
if DEMO == 1:
serialComm = serial.Serial('/dev/ttyACM0', 115200, timeout=5)
Motorhat = Adafruit_MotorHAT(addr=0x60)
leftMotor = Motorhat.getMotor(1)
rightMotor = Motorhat.getMotor(2)
odometry_thread = threading.Thread(target=read_odometry_loop)
odometry_thread.start()
## 1. Initialize a subscriber (subscribe ROS topic)
cmdvel_sub = rospy.Subscriber('/cmdvel', WheelCmdVel, cmdvel_callback)
rospy.spin()
