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)