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
# Initialise the PWM device using the default address pwm = PWM(0x40, debug=True) #connects to breakout board address 0x40 in debug mode pwm2 = PWM(0x41, debug=True) # Default calibration values #32768 max range in xbox controller analog stick / 255 for triggers move = 0 move2 = 500 #used for pwm signal max value servoMin = 500 #used for min value for drive servos 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):
#!/usr/bin/python from lib.Adafruit_PWM_Servo_Driver import PWM from lib import xbox_read import time # Define Constants MAXPULSE = 4095 MAXJOYSTICK = 32700 MAXTRIGGER = 255 FREQ = 60 # Initialize the PWM Device pwm = PWM(0x40) pwm.setPWMFreq(FREQ) 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)
# Setup direction of travel def setDirection(new_direction): global direction # One global property, cut me some slack if direction: GPIO.output(direction,False) GPIO.output(new_direction,True) direction = new_direction setDirection(FORWARD) # Default calibration values servoMid = 425 servoWidth = 180 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: