class ArmRobot(object):
def __init__(self,
addr=0x60,
left_id=3,
right_id=4,
left_trim=0,
right_trim=0,
armClaw=0,
armWaist=1,
armLeft=14,
armRight=15,
stop_at_exit=True):
"""Create an instance of the robot. Can specify the following optional
parameters:
- addr: The I2C address of the motor HAT, default is 0x60.
- left_id: The ID of the left motor, default is 1.
- right_id: The ID of the right motor, default is 2.
- left_trim: Amount to offset the speed of the left motor, can be positive
or negative and use useful for matching the speed of both
motors. Default is 0.
- right_trim: Amount to offset the speed of the right motor (see above).
- stop_at_exit: Boolean to indicate if the motors should stop on program
exit. Default is True (highly recommended to keep this
value to prevent damage to the bot on program crash!).
"""
# Initialize motor HAT and left, right motor.
self._mh = Adafruit_MotorHAT(addr, freq=60)
self._left = self._mh.getMotor(left_id)
self._right = self._mh.getMotor(right_id)
self._left_trim = left_trim
self._right_trim = right_trim
self._armLeft = self._mh.getServo(armLeft)
self._armRight = self._mh.getServo(armRight)
self._armClaw = self._mh.getServo(armClaw)
self._armWaist = self._mh.getServo(armWaist)
# Start with motors turned off.
self._left.run(Adafruit_MotorHAT.RELEASE)
self._right.run(Adafruit_MotorHAT.RELEASE)
# Configure all motors to stop at program exit if desired.
if stop_at_exit:
atexit.register(self.stop)
def _left_speed(self, speed):
"""Set the speed of the left motor, taking into account its trim offset.
"""
assert 0 <= speed <= 255, 'Speed must be a value between 0 to 255 inclusive!'
speed += self._left_trim
speed = max(0, min(255,
speed)) # Constrain speed to 0-255 after trimming.
self._left.setSpeed(speed)
def _right_speed(self, speed):
"""Set the speed of the right motor, taking into account its trim offset.
"""
assert 0 <= speed <= 255, 'Speed must be a value between 0 to 255 inclusive!'
speed += self._right_trim
speed = max(0, min(255,
speed)) # Constrain speed to 0-255 after trimming.
self._right.setSpeed(speed)
def stop(self):
"""Stop all movement."""
self._left.run(Adafruit_MotorHAT.RELEASE)
self._right.run(Adafruit_MotorHAT.RELEASE)
def forward(self, speed, seconds=None):
"""Move forward at the specified speed (0-255). Will start moving
forward and return unless a seconds value is specified, in which
case the robot will move forward for that amount of time and then stop.
"""
# Set motor speed and move both forward.
self._left_speed(speed)
self._right_speed(speed)
self._left.run(Adafruit_MotorHAT.FORWARD)
self._right.run(Adafruit_MotorHAT.FORWARD)
# If an amount of time is specified, move for that time and then stop.
if seconds is not None:
time.sleep(seconds)
self.stop()
def backward(self, speed, seconds=None):
"""Move backward at the specified speed (0-255). Will start moving
backward and return unless a seconds value is specified, in which
case the robot will move backward for that amount of time and then stop.
"""
# Set motor speed and move both backward.
self._left_speed(speed)
self._right_speed(speed)
self._left.run(Adafruit_MotorHAT.BACKWARD)
self._right.run(Adafruit_MotorHAT.BACKWARD)
# If an amount of time is specified, move for that time and then stop.
if seconds is not None:
time.sleep(seconds)
self.stop()
def right(self, speed, seconds=None):
"""Spin to the right at the specified speed. Will start spinning and
return unless a seconds value is specified, in which case the robot will
spin for that amount of time and then stop.
"""
# Set motor speed and move both forward.
self._left_speed(speed)
self._right_speed(speed)
self._left.run(Adafruit_MotorHAT.FORWARD)
self._right.run(Adafruit_MotorHAT.BACKWARD)
# If an amount of time is specified, move for that time and then stop.
if seconds is not None:
time.sleep(seconds)
self.stop()
def left(self, speed, seconds=None):
"""Spin to the left at the specified speed. Will start spinning and
return unless a seconds value is specified, in which case the robot will
spin for that amount of time and then stop.
"""
# Set motor speed and move both forward.
self._left_speed(speed)
self._right_speed(speed)
self._left.run(Adafruit_MotorHAT.BACKWARD)
self._right.run(Adafruit_MotorHAT.FORWARD)
# If an amount of time is specified, move for that time and then stop.
if seconds is not None:
time.sleep(seconds)
self.stop()
def armLeft(self, ang):
self._armLeft.write(ang)
def armRight(self, ang):
self._armRight.write(ang)
def armClaw(self, ang):
self._armClaw.write(ang)
def armWaist(self, ang):
self._armWaist.write(ang)
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor,Adafruit_Servo import time import atexit # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT(addr=0x6F,freq=60) servo0 = mh.getServo(14) #servo1 = mh.getServo(1) #servo14 = mh.getServo(14) #servo15 = mh.getServo(15) servo0.write(0.8) time.sleep(3) servo0.write(1.5) time.sleep(3) #servo14.write(2.0)
