def test_get_pwm_frequency(self):
# Arrange
dev = Device(0, 0, FakeSMBus)
# Act
dev.set_pwm_frequency(197)
# Assert
self.assertEqual(dev.get_pwm_frequency(), 197)
def test_set_pwm_frequency_wrong_input(self):
# Arrange
dev = Device(0, 0, FakeSMBus)
# Act & Assert
with self.assertRaises(DeviceException):
dev.set_pwm_frequency(2000)
with self.assertRaises(DeviceException):
dev.set_pwm_frequency(10)
def test_set_pwm_frequency_wrong_input(self):
# Arrange
dev = Device(0, 0, FakeSMBus)
# Act & Assert
with self.assertRaises(DeviceException):
dev.set_pwm_frequency(2000)
with self.assertRaises(DeviceException):
dev.set_pwm_frequency(10)
def test_get_pwm_frequency(self):
# Arrange
dev = Device(0, 0, FakeSMBus)
# Act
dev.set_pwm_frequency(197)
# Assert
self.assertEqual(dev.get_pwm_frequency(), 197)
def test_set_pwm_frequency(self):
# Arrange
dev = Device(0, 0, FakeSMBus)
# Act
dev.set_pwm_frequency(200)
# Assert
self.assertEqual(dev.bus.wrote_values[0], (0, 17))
self.assertEqual(dev.bus.wrote_values[1], (254, 30))
self.assertEqual(dev.bus.wrote_values[2], (0, 1))
def test_set_pwm_frequency(self):
# Arrange
dev = Device(0, 0, FakeSMBus)
# Act
dev.set_pwm_frequency(200)
# Assert
self.assertEqual(dev.bus.wrote_values[0], (0, 17))
self.assertEqual(dev.bus.wrote_values[1], (254, 30))
self.assertEqual(dev.bus.wrote_values[2], (0, 1))
def initPCA9685(self):
"""
@description configure Jetson Nano GPIO for communication with PCA9685
@return a PCA9685 Device object to communicate with PCA9685 chip
"""
GPIO.setmode(GPIO.BOARD)
mode = GPIO.getmode()
print("Jetson Nano GPIO Mode: {}".format(mode))
# discover I2C devices
i2c_devs = Device.get_i2c_bus_numbers()
print("The following /dev/i2c-* devices were found:\n{}".format(
i2c_devs))
# Create I2C device
"""
working_devs = list()
print("Looking out which /dev/i2c-* devices is connected to PCA9685")
for dev in i2c_devs:
try:
pca9685 = Device(0x40,dev)
# Set duty cycle
pca9685.set_pwm(5, 2047)
# set pwm freq
pca9685.set_pwm_frequency(1000)
print("Device {} works!".format(dev))
working_devs.append(dev)
except:
print("Device {} does not work.".format(dev))
# Select any working device, for example, the first one
print("Configuring PCA9685 connected to /dev/i2c-{} device.".format(working_devs[0]))
pca9685 = Device(0x40, working_devs[0])
"""
pca9685 = Device(
0x40, 1
) # Immediately set a working device, this assumes PCA9685 is connected to I2C channel 1
# ESC work at 50Hz
pca9685.set_pwm_frequency(50)
# Set slow speed duty cycles
self.set_channel_duty_cycle(pca9685, 0, 5.0)
self.set_channel_duty_cycle(pca9685, 1, 5.0)
self.set_channel_duty_cycle(pca9685, 2, 5.0)
self.set_channel_duty_cycle(pca9685, 3, 5.0)
# configure rest of channels to 0 duty cycle
rest = np.arange(4, 16, 1)
for channel in rest:
self.set_channel_duty_cycle(pca9685, channel, 0.0)
return pca9685
class Drive:
"""Control motors of RC car"""
def __init__(self, thread_id, thread_name):
"""Initialize PCA9685 servo driver and set angles for servo motors
Args:
thread_id (int): id of thread
thread_name (str): name of thread
"""
self.thread_id = thread_id
self.thread_name = thread_name
self.pwm = Device(0x40) # setup PCA9685 servo driver device
self.pwm.set_pwm_frequency(60) # setup PCA9685 servo driver frequency
self.steering_angle = 90 # set initial angle of servo for steering
self.motor_angle = 133 # set initial angle of servo for motor
self.set_angle(14, self.steering_angle) # set angle for motors
self.set_angle(15, self.motor_angle) # set angle for motors
def set_angle(self, channel, angle):
"""Calculate pulse width and set angle of servo motor
Args:
channel (int): channel of servo motor which is to be changed
angle (int): angle to set servo motor to
"""
pulse = (int(angle) * 2.5) + 150
self.pwm.set_pwm(channel, int(pulse))
def car_stopped(self):
self.steering_angle = 90
self.motor_angle = 133
self.set_angle(14, self.steering_angle) # set angle for motors
self.set_angle(15, self.motor_angle) # set angle for motors
def drive_forward(self):
self.steering_angle = 90
self.motor_angle = 128
self.set_angle(14, self.steering_angle) # set angle for motors
self.set_angle(15, self.motor_angle) # set angle for motors
def drive_left(self):
self.steering_angle = 120
self.motor_angle = 128
self.set_angle(14, self.steering_angle) # set angle for motors
self.set_angle(15, self.motor_angle) # set angle for motors
def drive_right(self):
self.steering_angle = 60
self.motor_angle = 128
self.set_angle(14, self.steering_angle) # set angle for motors
self.set_angle(15, self.motor_angle) # set angle for motors
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=0).tag(config=True)
left_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
right_motor_channel = traitlets.Integer(default_value=1).tag(config=True)
right_motor_alpha = traitlets.Float(default_value=1.0).tag(config=True)
def __init__(self, *args, **kwargs):
super(Robot, self).__init__(*args, **kwargs)
self.motor_driver = Device(0x40, 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.motor_driver.set_pwm_frequency(50)
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
from pca9685_driver import Device
def set_duty_cycle(pwmdev, channel, dt):
"""
@channel Channel or PIN number in PCA9685 to configure 0-15
@dt desired duty cycle
"""
val = (dt * 4095) // 100
pwmdev.set_pwm(channel, val)
pca9685 = Device(0x40, 1)
# Set duty cycle
#pca9685.set_pwm(0, 2047)
# set pwm freq
pca9685.set_pwm_frequency(60)
set_duty_cycle(pca9685, 0, 7)
class Motors:
def __init__(self):
#importing needed data
self.CWL = clawWidthList.clawWidthList()
self.UsSensor = UsSensor.UsSensor()
# setting channel names
self.StepY = 22
self.DirectionY = 19
self.ResetY = 21
self.clockZ = 5
self.dataZ = 3
self.clawChannel = 0 #unchanged
self.emChannel = 1 #unchanged
self.StepX = 38
self.DirectionX = 40
self.ResetX = 36
self.PWMZ = 14 #,dutycycle is speed
self.DirectionZ = 15
self.ResetZ = 11
self.dutycyclevalue = 0
self.deltaerror = 1
self.error0 = 1
self.error1 = 1
self.kp = .34
self.kd = .01
self.ki = 20
self.readings = 50
# Assuming X is direction in which trays open/close
# These are the ports for the motors which move things in the x,y,or z axis.
self.PORTX = "X"
self.PORTY = "Y"
self.PORTZ = "Z"
# These are locations for the robot when they trays are closed
self.CLOSETRAYY = 0
# these are locations for when the part is being dropped from serializer
self.DEFAULTX = 100
self.DEFAULTY = 1000
# Variable becomes false if it is reset
# Code to set up GPIO stuff
GPIO.setmode(GPIO.BOARD)
GPIO.setup(self.DirectionX, GPIO.OUT, initial=GPIO.LOW) # directionX
GPIO.setup(self.StepX, GPIO.OUT, initial=GPIO.LOW) # stepX
GPIO.setup(self.ResetX, GPIO.IN)
GPIO.setup(self.DirectionY, GPIO.OUT, initial=GPIO.LOW) # directionX
GPIO.setup(self.StepY, GPIO.OUT, initial=GPIO.LOW) # stepX
GPIO.setup(self.ResetY, GPIO.IN)
GPIO.setup(self.ResetZ, GPIO.IN)
self.xpos = 0.0
self.ypos = 0.0
self.zpos = 0.0
# Set state: GPIO.output(channel,state)
# set state: GPIO.output(channel,state, intital = GPIO.HIGH OR GPIO.LOW)
# Read value (high or low): GPIO.input(channel)
self.move = True
# working_devs = [1,2,3,4,5]
self.pca9685 = Device(0x40, 1)
# Set duty cycle
self.pca9685.set_pwm(0, 2047)
# set pwm freq
self.pca9685.set_pwm_frequency(50)
self.stepFrac = 1
self.set_duty_cycle(self.pca9685, self.PWMZ, 0)
self.zerror = 1
def set_duty_cycle(self, pwmdev, channel, dt):
"""
@pwmdev a Device class object already configured
@channel Channel or PIN number in PCA9685 to configure 0-15
@dt desired duty cycle
"""
val = (int((dt * 4095) // 100))
pwmdev.set_pwm(channel, val)
def goTo(self, locationList, pocketNumber):
goalx = int(locationList[0])
goaly = int(locationList[1])
goalz = float(locationList[2])
#First turn everything on
#make sure the claw is closed
#Move to pos to get part
#first y plus to get out of the way of the trays
#next x since it can now move properly
#z does not need to move
#next move to the position to place the part
#first move z since it is out of the way
#Next move x since it is away from the trays
#Next move y to 0 to grab the tray
#Now move y to the part position
#Place part (open claw and then sleep to let it be open for a little while)
#close tray
#Close claw
#Move y to 0
#turn off magnet
#Move y 100
#First turn everything on
#make sure the claw is closed
self.closeClaw()
time.sleep(.25)
self.MagnetOff()
#Move to pos to get part
#first y plus to get out of the way of the trays
self.MotorGoTo("Y", self.DEFAULTY)
#next x since it can now move properly
self.MotorGoTo("X", self.DEFAULTX)
#z does not need to move
#next move to the position to place the part
#first move z since it is out of the way
self.MotorGoTo("Z", goalz)
#Next move x since it is away from the trays
self.MotorGoTo("X", goalx)
#Next move y to 0 to grab the tray
self.MotorGoTo("Y", self.CLOSETRAYY)
time.sleep(.1)
self.MagnetOn()
time.sleep(.1)
#Now move y to the part position
self.MotorGoTo("Y", goaly)
#Place part (open claw and then sleep to let it be open for a little while)
self.MagnetOff()
time.sleep(.1)
self.openClaw()
time.sleep(.25)
#close tray
#Close claw
self.closeClaw()
#Move y to 0 (close tray)
self.MotorGoTo("Y", -1000)
self.MotorGoTo("X", -1000)
#turn off magnet
self.MagnetOff()
#Move y 100
self.MotorGoTo("Y", 100)
def MotorGoTo(self, name, goal):
# TODO Add low level motor stuff
self.move = True
print("Motor " + str(name) + " is moving to " + str(goal))
if name == "X":
# assuming at 0
# step angle = 1.2 deg
# OD = 15 mm
# Circ = 47.23 mm
# 300 steps per circumference
# .157 mm / step
if self.xpos < goal and goal < 201:
GPIO.output(self.DirectionX, GPIO.HIGH)
while self.xpos < goal and self.move == True:
GPIO.output(self.StepX, GPIO.HIGH)
time.sleep(0.002 / self.stepFrac)
GPIO.output(self.StepX, GPIO.LOW)
time.sleep(0.002 / self.stepFrac)
self.xpos = self.xpos + .192 / self.stepFrac
#if GPIO.input(self.ResetX) == GPIO.LOW:
#self.xpos = 0
#self.move = False
print(self.xpos)
self.move = True
elif self.xpos > goal:
GPIO.output(self.DirectionX, GPIO.LOW)
while self.xpos > goal and self.move == True:
GPIO.output(self.StepX, GPIO.HIGH)
time.sleep(0.002 / self.stepFrac)
GPIO.output(self.StepX, GPIO.LOW)
time.sleep(0.002 / self.stepFrac)
self.xpos = self.xpos - .192 / self.stepFrac
if GPIO.input(self.ResetX) == GPIO.LOW:
self.xpos = 0
self.move = False
print(self.xpos)
self.move = True
else:
pass
elif name == "Y":
# assuming at 0
# step angle = 1.2 deg
# OD = 15 mm
# Circ = 47.23 mm
# 300 steps per circumference
# .157 mm / step
if self.ypos < goal and goal < 400:
GPIO.output(self.DirectionY, GPIO.HIGH)
while self.ypos < goal and self.move == True:
GPIO.output(self.StepY, GPIO.HIGH)
time.sleep(0.001 / self.stepFrac)
GPIO.output(self.StepY, GPIO.LOW)
time.sleep(0.001 / self.stepFrac)
self.ypos = self.ypos + .195 / self.stepFrac
#if GPIO.input(self.ResetY) == GPIO.LOW:
#self.ypos = 0
#self.move = False
print(self.ypos)
self.move = True
elif self.ypos > goal:
GPIO.output(self.DirectionY, GPIO.LOW)
while self.ypos > goal and self.move == True:
GPIO.output(self.StepY, GPIO.HIGH)
time.sleep(0.001 / self.stepFrac)
GPIO.output(self.StepY, GPIO.LOW)
time.sleep(0.001 / self.stepFrac)
self.ypos = self.ypos - .195 / self.stepFrac
if GPIO.input(self.ResetY) == GPIO.LOW:
self.ypos = 0
self.move = False
print(self.ypos)
self.move = True
else:
pass
elif name == "Z":
self.error0 = 0
# iterator = 0
# self.zpos = self.UsSensor.USMeasure()
# if self.zpos < goal and goal < 100:
# # ztime = 0.0
# # while iterator<100 and self.move == True:
# # self.set_duty_cycle(self.pca9685, self.DirectionZ, 100)
# # self.set_duty_cycle(self.pca9685, self.PWMZ, 20)
# # time.sleep(abs(goal)/100)
# # #if GPIO.input(self.ResetZ) == GPIO.LOW:
# # #self.ypos = 0
# # #self.move = False
# # print(self.zpos)
# # iterator += 1
# # self.mov+e = True
# # self.set_duty_cycle(self.pca9685, self.PWMZ, 0)
# self.zerror = goal - self.zpos
# self.error0 = 0
# self.error1 = 0
self.zerror = 20
while not (self.zerror < 5 and self.zerror > -5):
reading = 0
for n in range(self.readings):
reading += self.UsSensor.USMeasure()
time.sleep(.0001)
self.zpos = reading / self.readings
self.zerror = goal - self.zpos
self.error0 = self.error1 + self.error0
self.error1 = self.zerror
self.deltaerror = self.error0 - self.error1
self.dutycyclevalue = int(self.kp * self.zerror +
self.kd * self.deltaerror)
if self.dutycyclevalue > 0:
if self.dutycyclevalue > 100:
self.dutycyclevalue = 100
self.set_duty_cycle(self.pca9685, self.DirectionZ, 100)
self.set_duty_cycle(self.pca9685, self.PWMZ,
self.dutycyclevalue)
else:
self.set_duty_cycle(self.pca9685, self.DirectionZ, 100)
self.set_duty_cycle(self.pca9685, self.PWMZ,
self.dutycyclevalue)
elif self.dutycyclevalue < 0:
if self.dutycyclevalue < -100:
self.dutycyclevalue = -100
self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
self.set_duty_cycle(self.pca9685, self.PWMZ,
-1 * self.dutycyclevalue)
else:
self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
self.set_duty_cycle(self.pca9685, self.PWMZ,
-1 * self.dutycyclevalue)
time.sleep(.1)
print(str(self.zpos) + " z pos")
print(str(goal) + " goal")
print(str(self.zerror) + " z error")
print(str(self.dutycyclevalue) + "DCV")
# elif self.zpos > goal and abs(float(goal)) < 100:
# ztime = 0.0
# while iterator < 100 and self.move == True:
# self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
# self.set_duty_cycle(self.pca9685, self.PWMZ, 40)
# time.sleep(abs(goal)/100)
# if GPIO.input(self.ResetZ) == GPIO.LOW:
# self.ypos = 0
# self.move = False
# print(self.zpos)
# iterator += 1
# self.move = True
# self.set_duty_cycle(self.pca9685, self.PWMZ, 0)
# else:
# pass
self.set_duty_cycle(self.pca9685, self.PWMZ, 0)
def MagnetOn(self):
print("Magnet turning on")
self.set_duty_cycle(self.pca9685, self.emChannel, 100)
def MagnetOff(self):
print("Magnet turning off")
self.set_duty_cycle(self.pca9685, self.emChannel, 0)
def dropPart(self):
print("Dropping part")
# Flaps are 2 inches wide
def openClaw(self):
print("Opening claw")
self.set_duty_cycle(self.pca9685, self.clawChannel, 4)
time.sleep(1)
def neutralClaw(self):
print("Opening claw neutral")
self.set_duty_cycle(self.pca9685, self.clawChannel, 6.85)
def closeClaw(self):
print("Closing claw")
self.set_duty_cycle(self.pca9685, self.clawChannel, 8.5)
def openClawPercent(self, percent):
print("opening claw " + percent + " %")
DCValue = percent * 5 + 5
self.set_duty_cycle(self.pca9685, self.clawChannel, DCValue)
def openClawWidth(self, width):
print("opening claw to a width of " + str(width) + " mm")
widthPow = (((8.6 / math.pi) * (math.acos((-1 * width / 89) +
(45 / 44.5)) + 3.7))) - 7
print("duty cycle is " + str(widthPow))
self.set_duty_cycle(self.pca9685, self.clawChannel, widthPow)
def MotorGoToXZ(self, goalx, goalz):
done = False
xdir = 2
zdir = 2
error0 = 0
error1 = 0
if self.xpos > goalx:
xdir = -1
GPIO.output(self.DirectionX, GPIO.LOW)
elif self.xpos < goalx:
xdir = 1
GPIO.output(self.DirectionX, GPIO.HIGH)
else:
xdir = 0
if self.zpos > goalz:
zdir = -1
elif self.zpos < goalz:
zdir = 1
else:
zdir = 0
while done == False:
if (self.xpos + .1 < goalx
or self.xpos - .1 > goalx) and (self.xpos + 1 < goalx
or self.xpos - 1 > goalx):
done == True
if not (self.xpos + .1 < goalx or self.xpos - .1 > goalx):
self.xpos = self.xpos + xdir * .157 / self.stepFrac
GPIO.output(self.StepX, GPIO.HIGH)
time.sleep(0.002 / self.stepFrac)
GPIO.output(self.StepX, GPIO.LOW)
time.sleep(0.002 / self.stepFrac)
if (not (self.zpos + .1 < goalz or self.zpos - .1 > goalz)):
#zpos = USReading
#set direction pin to the correct direction
if self.zpos - 1 > goalz:
self.set_duty_cycle(self.pca9685, self.DirectionZ, 100)
else:
self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
self.set_duty_cycle(self.pca9685, self.DirectionZ, 0)
#Set dutycycle to value deterined by controller
error0 = error1
error1 = goalx - self.zpos
deltaerror = error1 - error0
DCContVal = kp * error1 + kd * (deltaerror)
self.set_duty_cycle(self.pca9685, self.PWMZ, DCContVal)
# discover I2C devices
i2c_devs = Device.get_i2c_bus_numbers()
print("The following /dev/i2c-* devices were found:\n{}".format(i2c_devs))
# Create I2C device
working_devs = list()
print("Looking out which /dev/i2c-* devices is connected to PCA9685")
for dev in i2c_devs:
try:
pca9685 = Device(0x40, dev)
# Set duty cycle
pca9685.set_pwm(5, 2047)
# set pwm freq
pca9685.set_pwm_frequency(1000)
print("Device {} works!".format(dev))
working_devs.append(dev)
except:
print("Device {} does not work.".format(dev))
# Select any working device, for example, the first one
print("Configuring PCA9685 connected to /dev/i2c-{} device.".format(
working_devs[0]))
pca9685 = Device(0x40, working_devs[0])
# Set duty cycle
pca9685.set_pwm(0, 2047)
# set pwm freq
pca9685.set_pwm_frequency(1000)
import sys
import time
import datetime
import random
import state
sys.path.append('./PCA9685Driver')
# Import the PCA9685 module.
from pca9685_driver import Device
# launch servo
pwm = Device(0x41)
# Set frequency to 60hz, good for pwm.
pwm.set_pwm_frequency(60)
#PC9685 - TB6612FNG
# PWM8 - PWMA
# PWM9 - AIN1
# PWM10 - AIN2
#
# PWM11 - BIN1
# PWM12 - BIN2
# PWM13 - PWMB
pwmA = 8
ain1 = 9
ain2 = 10
pwmB = 11
# drive rc car with xbox one controller, only used for testing
from picamera.array import PiRGBArray # for pi camera
from picamera import PiCamera # for pi camera
from approxeng.input.selectbinder import ControllerResource # for xbox controller
from pca9685_driver import Device # for PCA9685 servo driver
from time import sleep
pwm = Device(0x40) # setup PCA9685 servo driver
pwm.set_pwm_frequency(60) # setup PCA9685 servo driver
steering_angle = 90 # initial angle of servo for steering
motor_angle = 133 # initial angle of servo for motor
def set_angle(channel, angle):
pulse = (int(angle) * 2.5) + 150
pwm.set_pwm(channel, int(pulse))
print('Ready, drive only')
while 1:
try:
with ControllerResource(print_events=False, controller_class=None, hot_zone=0.1,
dead_zone=0.1) as controller:
print('Found a controller')
while controller.connected:
set_angle(14, steering_angle)
set_angle(15, motor_angle)
# right trigger moved (motor forward)
class DataCapture(object):
"""Collect image and controller input data for neural network to .npy file
collected data consists of:
processed image with edge detection
input data from controller, array of [left_val, motor_val, right_val] (0 or 1) e.g. [0,1,0] is forward
"""
def __init__(self, thread_id, name):
self.thread_id = thread_id
self.name = name
self.pwm = Device(0x40) # setup PCA9685 servo driver device
self.pwm.set_pwm_frequency(60) # setup PCA9685 servo driver frequency
self.steering_angle = 90 # set initial angle of servo for steering
self.motor_angle = 133 # set initial angle of servo for motor
# numpy data setup
self.npy_file = 'datasets/dataset.npy' # numpy file for storing training data
self.left_val = 0 # [0,*,*]
self.forward_val = 0 # [*,0,*]
self.right_val = 0 # [*,*,0]
self.training_data = [
] # array for controller input data [left_val, motor_val, right_val]
self.printed_length = False # used to print length of training data
self.stream = frame.Frame(1, 'SaveFrame') # setup camera stream
self.stream.start() # start camera stream
self.start() # start data collection
def start(self):
Thread(target=self.gather_data(), args=()).start()
return self
def set_angle(self, channel, angle):
"""Calculate pulse width and set angle of servo motor
Args:
channel: channel of servo motor which is to be changed
angle: angle to set servo motor to
"""
pulse = (int(angle) * 2.5) + 150
self.pwm.set_pwm(channel, int(pulse))
def gather_data(self):
"""Loop to gather data using image data and controller input data"""
if os.path.isfile(self.npy_file):
print('DataSet file exists.. loading previous data')
self.training_data = list(np.load(self.npy_file))
else:
print('DataSet file does not exist.. starting new file')
self.training_data = []
print('Training data samples: {}'.format(len(self.training_data)))
print(
'Ready, connect controller and drive around to collect data ((X) controller button to stop collection)'
)
while 1:
try:
with ControllerResource(print_events=False,
controller_class=None,
hot_zone=0.1,
dead_zone=0.1) as controller:
print('Found a controller')
while controller.connected:
self.set_angle(
14, self.steering_angle) # set angle for motors
self.set_angle(
15, self.motor_angle) # set angle for motors
# right trigger moved (motor forward)
if controller['rt'] == 1:
self.forward_val = int(
controller['rt']
) # update forward val for input data array [*,0,*]
self.motor_angle = 125 # update servo angle for motor
self.printed_length = False
img = self.stream.read(
) # read latest frame from camera stream
self.save_sample(img) # save sample to dataset
# left stick moved (steering)
if controller['lx'] is not 0:
stick_val = int(controller['lx'])
if stick_val > 0: # stick moved right
self.right_val = stick_val # update value for input data array [*,*,1]
self.steering_angle = 60 # update servo angle for steering
if stick_val < 0: # stick moved left
self.left_val = stick_val * -1 # update value for input data array [1,*,*]
self.steering_angle = 120 # update servo angle for steering
# left stick released (steering centered)
if (controller['lx'] is 0) and (self.steering_angle
is not 90):
self.steering_angle = 90 # update servo angle for steering
self.left_val = 0 # update value for input data array [0,*,*]
self.right_val = 0 # update value for input data array [*,*,0]
# right trigger released (motor stopped)
if controller['rt'] is 0:
self.motor_angle = 133 # update servo angle for motor
self.forward_val = 0 # update value for input data array [*,0,*]
# save dataset to npy file
if controller[
'square']: # (X) button on Xbox One controller
self.stop_collection()
if controller[
'triangle']: # (Y) button on Xbox One controller
if not self.printed_length: # to print length of data only once in loop
self.printed_length = True
print("Length of data: {}".format(
len(self.training_data))
) # print current data length
if (len(self.training_data) % 100 == 0) and (len(
self.training_data) is not 0):
print("100 more samples")
except IOError:
print('Waiting for controller...')
sleep(1)
def save_sample(self, img):
"""Save individual data sample
sample consists of (processed image, input array)
print input data array and show new processed image frame
Args:
img: image which is to be processed with edge detection
"""
edges = self.stream.process(img) # edge detection
output = [
self.left_val, self.forward_val, self.right_val
] # [left, forward, right] update array of controller input data
self.training_data.append([edges, output])
print(output)
cv2.imshow('Data Collection Frame Preview', edges)
cv2.waitKey(1) & 0xFF
def stop_collection(self):
"""Stop data collection, save dataset, close camera preview, close stream"""
print('-----Stopping Data Capture-----')
self.save_dataset()
cv2.destroyAllWindows()
self.stream.stop_stream()
print("-----END-----")
exit()
def save_dataset(self):
"""Save complete dataset to npy file"""
print("Saving training data to file: ", self.npy_file)
np.save(self.npy_file, self.training_data)
print('Training data samples: {}'.format(len(self.training_data)))
class Plugin(plugin.PluginProto):
PLUGIN_ID = 22
PLUGIN_NAME = "Extra IO - PCA9685"
PLUGIN_VALUENAME1 = "PWM"
MAX_PINS = 15
MAX_PWM = 4095
MIN_FREQUENCY = 23.0 # Min possible PWM cycle frequency
MAX_FREQUENCY = 1500.0 # Max possible PWM cycle frequency
def __init__(self, taskindex): # general init
plugin.PluginProto.__init__(self, taskindex)
self.dtype = rpieGlobals.DEVICE_TYPE_I2C
self.vtype = rpieGlobals.SENSOR_TYPE_NONE
self.ports = 0
self.valuecount = 0
self.senddataoption = False
self.timeroption = False
self.pca = None
def plugin_init(self, enableplugin=None):
plugin.PluginProto.plugin_init(self, enableplugin)
self.initialized = False
self.pca = None
if self.enabled:
i2cport = -1
try:
for i in range(0, 2):
if gpios.HWPorts.is_i2c_usable(
i) and gpios.HWPorts.is_i2c_enabled(i):
i2cport = i
break
except:
i2cport = -1
if i2cport > -1 and int(self.taskdevicepluginconfig[0]) > 0:
try:
freq = int(self.taskdevicepluginconfig[1])
if freq < self.MIN_FREQUENCY or freq > self.MAX_FREQUENCY:
freq = self.MAX_FREQUENCY
except:
freq = self.MAX_FREQUENCY
try:
self.pca = Device(address=int(
self.taskdevicepluginconfig[0]),
bus_number=int(i2cport))
if self.pca is not None:
self.initialized = True
self.pca.set_pwm_frequency(freq)
except Exception as e:
misc.addLog(rpieGlobals.LOG_LEVEL_ERROR,
"PCA9685 device init failed: " + str(e))
self.pca = None
if self.pca is not None:
pass
def webform_load(self): # create html page for settings
choice1 = int(float(
self.taskdevicepluginconfig[0])) # store i2c address
optionvalues = []
for i in range(0x40, 0x78):
optionvalues.append(i)
options = []
for i in range(len(optionvalues)):
options.append(str(hex(optionvalues[i])))
webserver.addFormSelector("Address", "p022_adr", len(options), options,
optionvalues, None, choice1)
webserver.addFormNote(
"Enable <a href='pinout'>I2C bus</a> first, than <a href='i2cscanner'>search for the used address</a>!"
)
webserver.addFormNumericBox("Frequency", "p022_freq",
self.taskdevicepluginconfig[2],
self.MIN_FREQUENCY, self.MAX_FREQUENCY)
webserver.addUnit("Hz")
return True
def webform_save(self, params): # process settings post reply
cha = False
par = webserver.arg("p022_adr", params)
if par == "":
par = 0x40
if self.taskdevicepluginconfig[0] != int(par):
cha = True
self.taskdevicepluginconfig[0] = int(par)
par = webserver.arg("p022_freq", params)
if par == "":
par = self.MAX_FREQUENCY
if self.taskdevicepluginconfig[2] != int(par):
cha = True
self.taskdevicepluginconfig[2] = int(par)
if cha:
self.plugin_init()
return True
def plugin_write(self, cmd): # handle incoming commands
res = False
cmdarr = cmd.split(",")
cmdarr[0] = cmdarr[0].strip().lower()
if self.pca is not None:
if cmdarr[0] == "pcapwm":
pin = -1
val = -1
try:
pin = int(cmdarr[1].strip())
val = int(cmdarr[2].strip())
except:
pin = -1
if pin > -1 and val >= 0 and val <= self.MAX_PWM:
misc.addLog(rpieGlobals.LOG_LEVEL_DEBUG,
"PCAPWM" + str(pin) + " set to " + str(val))
try:
self.pca.set_pwm(pin, val)
except Exception as e:
misc.addLog(rpieGlobals.LOG_LEVEL_ERROR,
"PCAPWM" + str(pin) + ": " + str(e))
return True
if cmdarr[0] == "pcafrq":
freq = -1
try:
freq = int(cmdarr[1].strip())
except:
freq = -1
if (freq > -1) and (freq >= self.MIN_FREQUENCY) and (
freq <= self.MAX_FREQUENCY):
misc.addLog(rpieGlobals.LOG_LEVEL_DEBUG,
"PCAFRQ" + str(freq))
try:
self.pca.set_pwm_frequency(freq)
except Exception as e:
misc.addLog(rpieGlobals.LOG_LEVEL_ERROR,
"PCAFRQ" + str(e))
return True
return res
else:
print('\nBoard addr {} NOT in center file, using default.'.format(
board_addr))
else:
print('\nFile {} does not exist, using default pwm list.'.format(
center_pwm_file))
servo_center_dict = {}
# 0x40 from i2cdetect -y 1 (1 if Raspberry pi 2)
print('i2c hex board_addr:', args.board_addr, type(args.board_addr))
if args.board_addr == 40:
dev = Device(0x40)
if args.board_addr == 41:
dev = Device(0x41)
dev.set_pwm_frequency(50)
min_pwm = 110
max_pwm = 515
mid_pwm = int((min_pwm + max_pwm) / 2)
delta_pwm = 10
cur_servo = 0
#[dev.set_pwm(k, v) for k,v in pwm_dict.items()]
def moveCursorRefresh(stdscr):
stdscr.move(curses.LINES - 1, curses.COLS - 1)
stdscr.refresh() #Do this after addstr
#!/usr/bin/env python3
import time
from pyPS4Controller.controller import Controller
import board
import busio
import Jetson.GPIO as GPIO
import sys
sys.path.append('/opt/nvidia/jetson-gpio/lib/python')
sys.path.append('/opt/nvidia/jetson-gpio/lib/python/Jetson/GPIO')
from pca9685_driver import Device
servo = Device(0x40,1)
servo.set_pwm_frequency(50)
time.sleep(0.5)
servo.set_pwm(0,330)#range 250x350
def connect():
print("connected")
pass
class MyController(Controller):
def __init__(self, **kwargs):
Controller.__init__(self, **kwargs)
def on_x_press(self):