class Robot:
"""
Class to setup a Robot. Takes a dictionary of "commands" and
their associated output pins (BOARD number)
"""
def __init__(self, drive_map, sensor_map, servo_map, control_map):
"""
Initialize Output (and Input) pins. Assign objects to groups
"""
self.drive = drive_map
self.sensor = sensor_map
self.servo = servo_map
self.control = control_map
# Initialize the PWM Device
self.pwm = PWM(0x40)
self.pwm.setPWMFreq(FREQ)
for key in self.drive:
self.pwm.setPWM(self.drive[key],0,0)
for key in self.servo:
self.servo[key]["POS"] = self.servo[key]["MID"]
self.servo[key]["WIDTH"] = self.servo[key]["MAX"] - self.servo[key]["MIN"]
self.pwm.setPWM(self.servo[key]["PIN"],0,self.servo[key]["POS"])
def __repr__(self):
"""
String Representation of Robot instance
"""
rep = "Robot: " + "\n\n"
rep += "Drive Map: " + str(self.drive) + "\n\n"
rep += "Sensor Map: " + str(self.sensor) + "\n\n"
rep += "Servo Map: " + str(self.servo) + "\n\n"
rep += "Control Map: " + str(self.control) + "\n\n"
return rep
def halt(self):
"""
Function to reset Robot to Initial Values and Stop execution
"""
print "HALT()"
for key in self.drive:
self.pwm.setPWM(self.drive[key],0,0)
for key in self.servo:
self.pwm.setPWM(self.servo[key]["PIN"],0,self.servo[key]["MID"])
def update(self, event):
"""
Update Control States and Update PWM for each Motor or Servo when appropriate
"""
## update control states
self.control[event.key] = event.value
## Drive Motors
if event.key in ["X1","Y1"]:
throttle = float(self.control["Y1"]) / MAXJOYSTICK
direction = float(self.control["X1"]) / MAXJOYSTICK
L = MAXPULSE * (throttle + direction)
R = MAXPULSE * (throttle - direction)
if L >= 0:
self.pwm.setPWM(self.drive["LR"],0,0)
self.pwm.setPWM(self.drive["LF"],0,int(min(abs(L),MAXPULSE)))
else:
self.pwm.setPWM(self.drive["LF"],0,0)
self.pwm.setPWM(self.drive["LR"],0,int(min(abs(L),MAXPULSE)))
if R >= 0:
self.pwm.setPWM(self.drive["RR"],0,0)
self.pwm.setPWM(self.drive["RF"],0,int(min(abs(R),MAXPULSE)))
else:
self.pwm.setPWM(self.drive["RF"],0,0)
self.pwm.setPWM(self.drive["RR"],0,int(min(abs(R),MAXPULSE)))
## Pivot Arm
if event.key == "X2":
pivot = float(self.control["X2"]) / MAXJOYSTICK
pivot = -(pivot)
pivot = pivot * self.servo["PVT"]["WIDTH"] / 2
pivot = int(pivot) + self.servo["PVT"]["MID"]
self.servo["PVT"]["POS"] = pivot
self.pwm.setPWM(self.servo["PVT"]["PIN"],0,self.servo["PVT"]["POS"])
## Reach Arm
if event.key == "Y2":
reach = float(self.control["Y2"]) / MAXJOYSTICK
reach = reach * self.servo["RCH"]["WIDTH"] / 2
reach = int(reach) + self.servo["PVT"]["MID"]
self.servo["RCH"]["POS"] = reach
self.pwm.setPWM(self.servo["RCH"]["PIN"],0,self.servo["RCH"]["POS"])
## Lift Arm
if event.key in ["LT","RT"]:
lift = float(self.control["RT"] - self.control["LT"]) / MAXTRIGGER
lift = lift * self.servo["LFT"]["WIDTH"] / 2
lift = int(lift) + self.servo["LFT"]["MID"]
self.servo["LFT"]["POS"] = lift
self.pwm.setPWM(self.servo["LFT"]["PIN"],0,self.servo["LFT"]["POS"])
## CLOSE GRIP
if event.key == "RB" and event.value == 1:
self.servo["GRP"]["POS"] = self.servo["GRP"]["MAX"]
self.pwm.setPWM(self.servo["GRP"]["PIN"],0,self.servo["GRP"]["POS"])
## OPEN GRIP
if event.key == "LB" and event.value == 1:
self.servo["GRP"]["POS"] = self.servo["GRP"]["MIN"]
self.pwm.setPWM(self.servo["GRP"]["PIN"],0,self.servo["GRP"]["POS"])
def read_sensors(self, command):
pass
pwm.setPWMFreq(60) # Set frequency to 60 Hz
rt_intensity = 0
lt_intensity = 0
steer = servoMid
for event in xbox_read.event_stream(deadzone=12000):
# Triggers control speed
if event.key=='RT' or event.key=='LT':
if event.key=='RT':
rt_intensity = event.value
else:
lt_intensity = event.value
# Change direction outputs when one trigger is pulled harder than the other
new_direction = FORWARD if rt_intensity>=lt_intensity else BACKWARD
if not direction==new_direction:
print 'set direction: %s' % {FORWARD:'FORWARD',BACKWARD:'BACKWARD'}[new_direction]
setDirection(new_direction)
intensity = max(rt_intensity,lt_intensity) * 16
pwm.setPWM(0, 0, intensity)
print 'set speed: %d' % intensity
# Left thumbstick controls the steering
if event.key=='X1':
steer = int( servoMid + (servoWidth*-event.value)/32768 )
pwm.setPWM(1, 0, steer)
if event.key=='Y' and event.is_press():
servoMid = steer
print 'Calibrate steering around %d' % steer
servoMove = 150 #used for min value for camera servos
servoMoveBasket = 400 #used for min value for basket servos
servoMoveArm = 400 #used for min values for arm servos
menu = 1 #used to toggle between camera servos and capture carry servos
pwm.setPWMFreq(50)
pwm2.setPWMFreq(2000) # Set frequency to 50 Hz
#main loop
while (True):
for event in xbox_read.event_stream(
deadzone=6000
): #looks for a button to be pressed sets deadzone (sensitivity) to 6000
if (event.key == 'Y1' and event.value > 0
): #moves drive servos based on degree of analog stick movement
move = 4003 - (3500 * event.value) / 32768
pwm2.setPWM(0, servoMin, move)
if (event.key == 'Y1' and event.value < 0):
move = 4004 - (3500 * -event.value) / 32768
pwm2.setPWM(2, servoMin, move)
if (event.key == 'Y1' and event.value == 0):
servoMin = 500
pwm2.setPWM(0, servoMin, move)
pwm2.setPWM(2, servoMin, move)
if (event.key == 'Y2' and event.value > 0):
move = 4003 - (3500 * event.value) / 32768
pwm2.setPWM(1, servoMin, move)
if (event.key == 'Y2' and event.value < 0):
move = 4004 - (3500 * -event.value) / 32768
pwm2.setPWM(3, servoMin, move)
if (event.key == 'Y2' and event.value == 0):
servoMin = 500
import RPi.GPIO as GPIO
from lib.Adafruit_PWM_Servo_Driver import PWM
from lib import xbox_read
# Initialise the PWM servo driver using the default address, there are pins you can solder shut, for when you have several contollers.
pwm = PWM(0x40, debug=True)
# Default calibration values
servoDrive = 425 #max turn speed of the servo
servoDriveWidth = 360 #kind of servo, 360 is continuous rotation servo
servoSteer = 425 #max turn speed of the servo
servoSteerWidth = 180 #kind of servo, 180 is 180 degrees rotation servo
pwm.setPWMFreq(60) # Set frequency to 60 Hz
# set the names and values of the servos
drive = servoDrive
steer = servoSteer
for event in xbox_read.event_stream(deadzone=12000):
# left thumbstick controls the speed
if event.key=='Y1':
# if your servo turns the wrong way, add a minus in front of: 32768 so it would be */-32768
drive = int( servoDrive + (servoDriveWidth*-event.value)/32768 )
pwm.setPWM(0, 0, drive)
# Right thumbstick controls the steering
if event.key=='X2':
# if your servo turns the wrong way, add a minus in front of: 32768 so it would be */-32768
steer = int( servoSteer + (servoSteerWidth*-event.value)/32768 )
pwm.setPWM(1, 0, steer)
