import cv2
import numpy as np
import time
from Adafruit_MotorHAT import Adafruit_MotorHAT as amhat from Adafruit_MotorHAT import Adafruit_DCMotor as adamo
# create motor objects
motHAT = amhat(addr=0x60)
mot1 = motHAT.getMotor(1)
mot2 = motHAT.getMotor(2)
mot3 = motHAT.getMotor(3)
mot4 = motHAT.getMotor(4)
motors = [mot1, mot2, mot3, mot4]
# motor multipliers
motorMultiplier = [1.0, 1.0, 1.0, 1.0, 1.0]
# motor speeds
motorSpeed = [0,0,0,0]
# speeds
speedDef = 100
leftSpeed = speedDef
rightSpeed = speedDef
diff= 0
maxDiff = 50
turnTime = 0.5
# create camera object
cap = cv2.VideoCapture(0)
time.sleep(1)
# PID
kp = 1.0
ki = 1.0
kd = 1.0
ballX = 0.0
ballY = 0.0
motorNames = config.motorNames
print 'Motor Names: {}'.format(motorNames)
motorNumbers = config.motorPositions
print 'Motor Numbers: {}'.format(motorNumbers)
leftMotorNumbers = [1, 2]
rightMotorNumbers = [3, 4]
# Set Speeds
highSpeed = 500
lowSpeed = 50
# Drive Time (in seconds)
driveTime = 0.01
# Initialize Motor Hat
motHAT = amhat(addr=config.motorHATaddress)
# Setup motors
motors = [motHAT.getMotor(m) for m in motorNumbers]
leftMotors = [motHAT.getMotor(lm) for lm in leftMotorNumbers]
rightMotors = [motHAT.getMotor(rm) for rm in rightMotorNumbers]
def destroy():
# Kill all motor action
for m in motors:
m.run(amhat.RELEASE)
def driveForward(driveTime=driveTime):
for m in motors:
from Adafruit_MotorHAT import Adafruit_MotorHAT as amhat, Adafruit_DCMotor as adcm
import time
# create 2 motor objects
mh = amhat(addr=0x60)
motor1 = mh.getMotor(1)
motor2 = mh.getMotor(2)
motor3 = mh.getMotor(3)
motor4 = mh.getMotor(4)
# set start speed
motor1.setSpeed(0)
motor2.setSpeed(0)
motor3.setSpeed(0)
motor4.setSpeed(0)
# direction variable
direction = 0
# wrap actions in try loop
try:
while True:
# if direction = 1 then motor1 forward and motor2 backward
# else motor1 backward and motor2 forward
if direction == 0:
motor1.run(amhat.FORWARD)
motor2.run(amhat.FORWARD)
