def setup(self): # Servo object representing the door. self.openValue = 100 # Value in degrees the servo should be when the door is open. self.closedValue = 172 # Value in degrees the servo should be when the door is closed. self.servo = Servo(self.tamp, 25) self.start()
def setup(self): self.servo = Servo(self.tamp, self.SERVO_PIN) self.servo.write(0) self.servoval = 0 self.delta = 1 self.timer = Timer() self.end = False
def setup(self): self.LEFT_TOWER = 85 self.RIGHT_TOWER = 24 self.CENTER = 53 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.servo = Servo(self.tamp, 23) #pin TBD self.red_detected = False self.green_detected = False self.servo.write(self.CENTER) self.counter = 0 self.servo_bottom = Servo(self.tamp,22) self.servo_bottom.write(34) self.motor = Motor(self.tamp, 24, 25) self.motor.write(True, 250) self.jammed = False self.conveyor_encoder = Encoder(self.tamp, *self.pins, continuous=True) self.prev_encoder_value = 0 self.conveyor_encoder.update() self.timer = Timer() self.sorter_delta = 50
def setup(self): # ROS Setup rospy.init_node('driver') rospy.Subscriber("driver_commands", String, callback) # self.tof_pub = rospy.Publisher('tof_readings', String, queue_size=10) # Device Setup self.motor1 = Motor(self.tamp, MOTOR1_DIR, MOTOR1_PWM) self.motor2 = Motor(self.tamp, MOTOR2_DIR, MOTOR2_PWM) self.encoder1 = Encoder(self.tamp, *ENCODER1, continuous=True) self.encoder2 = Encoder(self.tamp, *ENCODER2, continuous=True) # self.encoder1_value = 0 # self.encoder2_value = 0 # TimeOfFlight Sensors # self.tof1 = TimeOfFlight(self.tamp, TOF1, 1, continuous=False) # self.tof2 = TimeOfFlight(self.tamp, TOF2, 2, continuous=False) # self.tof1.enable() # self.tof2.enable() # self.tof1_value = 0 # self.tof2_value = 0 # Servo for the Release self.servo = Servo(self.tamp, SERVO) self.servo.write(SERVO_CLOSE) # Controller Params self.pid = PID(p=P, i=I, d=D) self.target1 = 0 self.target2 = 0 self.servo_commands = Queue()
class Arm: def __init__(self,tamp): self.tamp=tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 10 #5 for metal self.servo.top = 150 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState="None" self.nextArmState="None" def pickUpBlock(self): self.armState="Up" self.nextArmState="Down" def moveArmUp(self): if self.servoval <= self.servo.top: self.delta = self.servo.speed self.armState="Up" else: self.setPause(.25) self.armState="Pause" #self.armState=self.nextArmState #self.nextArmState="None" def moveArmDown(self): if self.servoval > self.servo.bottom: self.delta = -self.servo.speed self.armState="Down" else: self.armState=self.nextArmState self.nextArmState="None" def setPause(self,pauseLength): self.startPauseTime=time.time() self.pauseLength=pauseLength def pauseArm(self): if(time.time()-self.startPauseTime>self.pauseLength): self.armState=self.nextArmState self.nextArmState="None" self.delta=0 def update(self): if self.armState!="None": if self.armState=="Up": self.moveArmUp() elif self.armState=="Down": self.moveArmDown() elif(self.armState=="Pause"): self.pauseArm() self.servoval += self.delta if(self.servoval<0): self.servoval=0 self.servo.write(abs(self.servoval))
class Arm: def __init__(self, tamp): self.tamp = tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 10 #5 for metal self.servo.top = 150 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState = "None" self.nextArmState = "None" def pickUpBlock(self): self.armState = "Up" self.nextArmState = "Down" def moveArmUp(self): if self.servoval <= self.servo.top: self.delta = self.servo.speed self.armState = "Up" else: self.setPause(.25) self.armState = "Pause" #self.armState=self.nextArmState #self.nextArmState="None" def moveArmDown(self): if self.servoval > self.servo.bottom: self.delta = -self.servo.speed self.armState = "Down" else: self.armState = self.nextArmState self.nextArmState = "None" def setPause(self, pauseLength): self.startPauseTime = time.time() self.pauseLength = pauseLength def pauseArm(self): if (time.time() - self.startPauseTime > self.pauseLength): self.armState = self.nextArmState self.nextArmState = "None" self.delta = 0 def update(self): if self.armState != "None": if self.armState == "Up": self.moveArmUp() elif self.armState == "Down": self.moveArmDown() elif (self.armState == "Pause"): self.pauseArm() self.servoval += self.delta if (self.servoval < 0): self.servoval = 0 self.servo.write(abs(self.servoval))
def __init__(self, tamp): self.tamp = tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 5 self.servo.top = 180 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState = "None" self.nextArmState = "None"
def setup(self): pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.servo = Servo(self.tamp, self.SERVO_PIN) self.servo.write(0) self.servoval = 0 self.delta = 1 self.timer = Timer() self.end = False self.wait = False
def __init__(self, tamp, servo_pin, lower, upper, up_traj, down_traj): """ lower: servo us corresponding to 0 angle upper: servo us corresponding to 180 angle up_traj: list of (angle, dt) tuples for up trajectory down_traj: list of (angle, dt) tuples for down trajectory """ self.servo = Servo(tamp, servo_pin, lower, upper) self.servo.write(0) self.up_traj = up_traj self.down_traj = down_traj
class ServoWrite(Sketch): """Cycles a servo back and forth between 1050us and 1950us pulse widths (most servos are 1000-2000)""" def setup(self): self.servo = Servo(self.tamp, 10) self.servo.write(2200) self.timer = Timer() self.val = 2200 def loop(self): raw_input() self.val += 10 print self.val self.servo.write(self.val)
class moveDoor(SyncedSketch): def setup(self): # Servo object representing the door. self.openValue = 100 # Value in degrees the servo should be when the door is open. self.closedValue = 172 # Value in degrees the servo should be when the door is closed. self.servo = Servo(self.tamp, 25) self.start() def loop(self): # COMMENT 1 OF THESE #self.servo.write(self.openValue) # Use to open door self.servo.write(self.closedValue) # Use to close door
def setup(self): self.LEFT_TOWER = 83.4 self.RIGHT_TOWER = 27 self.CENTER = 54 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.servo = Servo(self.tamp, 23) #pin TBD self.red_detected = False self.green_detected = False self.servo.write(self.CENTER) self.counter = 0 self.timer = Timer()
def __init__(self, tamp): self.tamp = tamp self.servo = Servo(self.tamp, 5) self.servo.center = 90 self.servo.right = 25 self.servo.left = 165 self.servo.speed = 15 self.servo.rightJostle = 60 self.servo.leftJostle = 120 self.servo.write(self.servo.center) self.sorterval = self.servo.center self.dsorter = 0 self.sorterState = "None" self.startPauseTime = 0 self.pauseLength = 0
class Arm: def __init__(self, tamp): self.tamp = tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 5 self.servo.top = 180 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState = "None" self.nextArmState = "None" def pickUpBlock(self): self.armState = "Up" self.nextArmState = "Down" def moveArmUp(self): if self.servoval <= self.servo.top: self.delta = self.servo.speed self.armState = "Up" else: self.armState = self.nextArmState self.nextArmState = "None" def moveArmDown(self): if self.servoval > self.servo.bottom: self.delta = -self.servo.speed self.armState = "Down" else: self.armState = self.nextArmState self.nextArmState = "None" def update(self): if self.armState != "None": if self.armState == "Up": self.moveArmUp() elif self.armState == "Down": self.moveArmDown() self.servoval += self.delta if (self.servoval < 0): self.servoval = 0 self.servo.write(abs(self.servoval))
class Arm: def __init__(self,tamp): self.tamp=tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 5 self.servo.top = 180 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState="None" self.nextArmState="None" def pickUpBlock(self): self.armState="Up" self.nextArmState="Down" def moveArmUp(self): if self.servoval <= self.servo.top: self.delta = self.servo.speed self.armState="Up" else: self.armState=self.nextArmState self.nextArmState="None" def moveArmDown(self): if self.servoval > self.servo.bottom: self.delta = -self.servo.speed self.armState="Down" else: self.armState=self.nextArmState self.nextArmState="None" def update(self): if self.armState!="None": if self.armState=="Up": self.moveArmUp() elif self.armState=="Down": self.moveArmDown() self.servoval += self.delta if(self.servoval<0): self.servoval=0 self.servo.write(abs(self.servoval))
class ServoWrite(Sketch): """Cycles a servo back and forth between 0 and 180 degrees. However, these degrees are not guaranteed accurate, and each servo's range of valid microsecond pulses is different""" SERVO_PIN = 9 def setup(self): pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.servo = Servo(self.tamp, self.SERVO_PIN) self.servo.write(0) self.servoval = 0 self.delta = 1 self.timer = Timer() self.end = False self.wait = False def loop(self): for event in pygame.event.get(): if event.type == pygame.KEYDOWN: if event.key == pygame.K_UP: self.servoval = 180 if event.key == pygame.K_DOWN: self.servoval = 0 if event.key == pygame.K_SPACE: self.servoval = 90 if (self.timer.millis() > 10): self.timer.reset() if self.servoval >= 180: self.delta = -1 # down elif self.servoval <= 0: self.delta = 1 # up elif self.wait == True: self.delta = 0 # self.servoval += self.delta print self.servoval self.servo.write(abs(self.servoval))
def __init__(self,tamp): self.tamp=tamp self.servo = Servo(self.tamp, 9) self.servo.bottom = 5 self.servo.top = 180 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.armState="None" self.nextArmState="None"
class ServoWrite(Sketch): """Cycles a servo back and forth between 0 and 180 degrees. However, these degrees are not guaranteed accurate, and each servo's range of valid microsecond pulses is different""" SERVO_PIN = 25 def setup(self): self.servo = Servo(self.tamp, self.SERVO_PIN) self.servo.write(0) self.servoval = 0 self.delta = 1 self.timer = Timer() self.end = False def loop(self): self.servo.write(180) if self.timer.millis() > 1000: self.stop() '''
class Arm(HardwareDevice): """ A loose wrapper of a servo device, that deals with the up and down trajectories we need """ def __init__(self, tamp, servo_pin, lower, upper, up_traj, down_traj): """ lower: servo us corresponding to 0 angle upper: servo us corresponding to 180 angle up_traj: list of (angle, dt) tuples for up trajectory down_traj: list of (angle, dt) tuples for down trajectory """ self.servo = Servo(tamp, servo_pin, lower, upper) self.servo.write(0) self.up_traj = up_traj self.down_traj = down_traj def up(self): for angle, dt in self.up_traj: self.servo.write(angle) time.sleep(dt) def down(self): for angle, dt in self.down_traj: self.servo.write(angle) time.sleep(dt)
class ServoWrite(Sketch): """Cycles a servo back and forth between 0 and 180 degrees. However, these degrees are not guaranteed accurate, and each servo's range of valid microsecond pulses is different""" SERVO_PIN = 9 def setup(self): self.servo = Servo(self.tamp, self.SERVO_PIN) self.servo.write(0) self.servoval = 0 self.delta = 1 self.timer = Timer() self.end = False def loop(self): if (self.timer.millis() > 10): self.timer.reset() if self.servoval >= 180: self.delta = -1 elif self.servoval <= 0: self.delta = 1 self.servoval += self.delta print self.servoval self.servo.write(abs(self.servoval))
def setup(self): self.ttt = Timer() self.timer = Timer() self.loopTimer = Timer() self.servo = Servo(self.tamp, SERVO_PIN) self.motorRight = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) self.motorLeft = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) self.encoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE) self.dropoff = DropoffModule(self.timer, self.servo, self.motorRight, self.motorLeft, self.encoder) self.dropoff.start()
def __init__(self, tamp): self.tamp=tamp self.servo = Servo(self.tamp, 5) self.servo.center = 90 self.servo.right = 25 self.servo.left = 165 self.servo.speed = 15 self.servo.rightJostle = 60 self.servo.leftJostle = 120 self.servo.write(self.servo.center) self.sorterval = self.servo.center self.dsorter = 0 self.sorterState = "None" self.startPauseTime=0 self.pauseLength=0
class Silo(HardwareDevice): def __init__(self, tamp): self.latch = Servo(tamp, pins.stack_latch, 1830, 1130) self.latch.write(0) def open(self): self.latch.write(180) time.sleep(0.5) def close(self): self.latch.write(0) time.sleep(0.5)
class SortBlocks(SyncedSketch): def setup(self): self.LEFT_TOWER = 83.4 self.RIGHT_TOWER = 27 self.CENTER = 54 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.servo = Servo(self.tamp, 23) #pin TBD self.red_detected = False self.green_detected = False self.servo.write(self.CENTER) self.counter = 0 self.timer = Timer() def loop(self): if self.timer.millis() > 200: self.timer.reset() # print self.color.r, self.color.g, self.color.b, self.color.c # print self.color.colorTemp, self.color.lux #if not self.red_detected and not self.green_detected and self.color.r > 1.3 * self.color.g and self.color.r > 1.3 * self.color.b: detect_red = self.color.r > 1.3*self.color.g detect_green = self.color.g > 1.3*self.color.r sum_val = self.color.r+self.color.g+self.color.b if detect_red and sum_val > 300: self.servo.write(self.LEFT_TOWER) self.counter = 0 #elif not self.red_detected and not self.green_detected and self.color.g > 1.3 * self.color.r and self.color.g > 1.3 * self.color.b: elif detect_green and sum_val > 300: self.servo.write(self.RIGHT_TOWER) self.counter = 0 elif self.counter > 400: self.servo.write(self.CENTER) self.counter += 100
class ServoWrite(Sketch): """Cycles a servo back and forth between 1050us and 1950us pulse widths (most servos are 1000-2000)""" def setup(self): self.servo = Servo(self.tamp, 9) self.servo.write(1050) self.timer = Timer() self.end = False def loop(self): if (self.timer.millis() > 2000): self.timer.reset() if self.end: self.servo.write(1050) else: self.servo.write(1950) self.end = not self.end
def setup(self): # initialize sensors, settings, start timers, etc. self.gameTimer = Timer() self.motorGripper = Motor(self.tamp, 23, 3) self.motorLeft = Motor(self.tamp, 7, 22) self.motorRight = Motor(self.tamp, 0, 21) self.motorval = 0 self.motorLeft.write(1,0) self.motorRight.write(1,0) self.motorGripper.write(1,0) self.currentGripperLevel = 2 print "Motors connected." self.servoDoor = Servo(self.tamp, 20) self.servovalDoor = self.DOOR_CLOSE_POS self.servoDoor.write(self.DOOR_CLOSE_POS) self.timerDoor = Timer() self.servoGripper = Servo(self.tamp, 10) self.servovalGripper = self.GRIPPER_CLOSE_POS self.servoGripper.write(self.servovalGripper) self.timerGripper = Timer() print "Servos connected." self.timer = Timer() self.gyro = Gyro(self.tamp, 9) print "Gyro connected." self.theta = self.gyro.val self.dT = .01 self.cam = cv2.VideoCapture(0) print "Camera connected." self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) frontLeftIR_pin = 14 self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin) frontRightIR_pin = 15 self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin) leftIR_pin = 16 self.leftIR = AnalogInput(self.tamp, leftIR_pin) rightIR_pin = 17 self.rightIR = AnalogInput(self.tamp, rightIR_pin) # Initialize PID Values self.P = 10 self.I = 0 #5 self.D = 0 self.last_diff = 0 self.integral = 0 self.desiredAngle = self.theta self.finishedCollectingBlock = False self.finishedDiscardingBlock = False self.timer = Timer() self.state = ExploreState() # self.state = CollectBlockState() self.blocksCollected = 0 self.leftIRVals = deque([]) self.rightIRVals = deque([]) self.frontRightIRVals = deque([]) self.frontLeftIRVals = deque([]) # Starts the robot print "Robot setup complete."
def setup(self): # initialize sensors, settings, start timers, etc. self.gameTimer = Timer() # Pins (7,22); (3,23); (0,21) work. # Problem was with the Double Motor controller. self.motorGripper = Motor(self.tamp, 23, 3) self.motorLeft = Motor(self.tamp, 7, 22) self.motorRight = Motor(self.tamp, 0, 21) # good self.motorval = 0 self.motorLeft.write(1,0) self.motorRight.write(1,0) self.motorGripper.write(1,0) self.currentGripperLevel = 2 print "Motors connected." self.servoDoor = Servo(self.tamp, 20) self.servovalDoor = self.DOOR_CLOSE_POS self.servoDoor.write(self.DOOR_CLOSE_POS) self.timerDoor = Timer() self.servoGripper = Servo(self.tamp, 10) self.servovalGripper = self.GRIPPER_CLOSE_POS self.servoGripper.write(self.servovalGripper) self.timerGripper = Timer() print "Servos connected." left_pins = 6,5 right_pins = 3,4 # Encoder doesn't work when after gyro self.encoderLeft = Encoder(self.tamp, 6,5, continuous=False) self.encoderRight = Encoder(self.tamp, 3,4, continuous=True) print "Encoders connected." # TODO: set encoder to 0 self.timer = Timer() self.gyro = Gyro(self.tamp, 9) print "Gyro connected." self.theta = self.gyro.val self.dT = .01 self.cam = cv2.VideoCapture(0) print "Camera connected." # self.color = Color(self.tamp, # integrationTime=Color.INTEGRATION_TIME_101MS, # gain=Color.GAIN_1X) self.encoderLeft.start_continuous() self.encoderRight.start_continuous() frontLeftIR_pin = 14 self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin) frontRightIR_pin = 15 self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin) leftIR_pin = 16 self.leftIR = AnalogInput(self.tamp, leftIR_pin) rightIR_pin = 17 self.rightIR = AnalogInput(self.tamp, rightIR_pin) # Initialize PID Values self.P = 10 self.I = 5 self.D = 5 self.last_diff = 0 self.integral = 0 self.desiredAngle = self.theta self.finishedCollectingBlock = False self.finishedDiscardingBlock = False self.timer = Timer() self.state = ExploreState() # self.state = CollectBlockState() self.blocksCollected = 0 self.leftIRVals = deque([]) self.rightIRVals = deque([]) self.frontRightIRVals = deque([]) self.frontLeftIRVals = deque([]) self.goIntoTimeoutState = False self.timeoutCounter = 0 # Starts the robot print "Robot setup complete."
class SortBlocks(SyncedSketch): pins = 31, 32 def setup(self): self.LEFT_TOWER = 85 self.RIGHT_TOWER = 24 self.CENTER = 53 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.servo = Servo(self.tamp, 23) #pin TBD self.red_detected = False self.green_detected = False self.servo.write(self.CENTER) self.counter = 0 self.servo_bottom = Servo(self.tamp,22) self.servo_bottom.write(34) self.motor = Motor(self.tamp, 24, 25) self.motor.write(True, 250) self.jammed = False self.conveyor_encoder = Encoder(self.tamp, *self.pins, continuous=True) self.prev_encoder_value = 0 self.conveyor_encoder.update() self.timer = Timer() self.sorter_delta = 50 def loop(self): if self.timer.millis() > 300: self.timer.reset() #self.conveyor_encoder.update() encoder_value = self.conveyor_encoder.val #print self.prev_encoder_value #print encoder_value if(encoder_value == self.prev_encoder_value): self.jammed = True else: self.jammed = False self.prev_encoder_value = encoder_value self.motor.write(not self.jammed,250) # print self.color.r, self.color.g, self.color.b, self.color.c # print self.color.colorTemp, self.color.lux #if not self.red_detected and not self.green_detected and self.color.r > 1.3 * self.color.g and self.color.r > 1.3 * self.color.b: detect_red = self.color.r > 1.3*self.color.g detect_green = self.color.g > 1.3*self.color.r sum_val = self.color.r+self.color.g+self.color.b if detect_red and sum_val > 300: self.servo.write(self.LEFT_TOWER) self.counter = 0 #elif not self.red_detected and not self.green_detected and self.color.g > 1.3 * self.color.r and self.color.g > 1.3 * self.color.b: elif detect_green and sum_val > 300: self.servo.write(self.RIGHT_TOWER) self.counter = 0 elif self.counter > 400: self.servo.write(self.CENTER) # else: # self.servo.write(self.CENTER + self.sorter_delta) # self.sorter_delta *= -1 self.counter += 100
class Robot(SyncedSketch): def setup(self): #################### #### EE SETUP #### #################### # Motor object representing the left motor. self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) # Encoder object for the left motor. self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW, LEFT_DRIVE_ENCODER_WHITE) # Motor object representing the right motor. self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) # Encoder object for the right motor. self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE) # Motor object representing the intake mechanism motors. self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM) # Encoder object for the intake motor. self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW, HUGS_MOTOR_ENCODER_WHITE) # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE) # Long range IR sensors self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) # Color sensor self.color = Color(self.tamp) # Limit switches self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH) self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) # Servo controlling the door of the collection chamber. self.backDoorServo = Servo(self.tamp, SERVO_PIN) # Make sure the door is closed self.backDoorServo.write(SERVO_CLOSE) # The switch that tells the program that the competition has started self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE) ################################# #### INTERNAL MODULE SETUP #### ################################# # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors reversing? True if so, False if going forwards. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) # Wall Follow object self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR) # A timer to make sure timesteps are only 10 times/second self.timestepTimer = Timer() # Timer object describing how long the current module has been running. self.moduleTimer = Timer() # Timer for the whole game self.gameTimer = Timer() # Runs the PICKUP process self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder) # Runs the DROPOFF process self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder) # Runs the FOLLOW process TODO: Fix forward to actually mean forward. self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch) # Runs the CHECK process. TODO: pass in proper timers. self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color) # Waits for the game to start self.off = OffModule(self.gameTimer, self.competitionModeSwitch) # Describes which stage of the program is running. self.module = MODULE_OFF def loop(self): if self.timestepTimer.millis() > 100: self.timestepTimer.reset() #print "Module Number", self.module state = -1 if self.module == MODULE_OFF: state = self.off.run() elif self.module == MODULE_CHECK: state = self.check.run() elif self.module == MODULE_PICKUP: state = self.pickup.run() elif self.module == MODULE_DROPOFF: self.module = MODULE_FOLLOW state = self.follow.run() elif self.module == MODULE_FOLLOW: state = self.follow.run() else: print "Attempting to run nonexistent module" self.stop() self.updateState(state) if self.gameTimer.millis() % 10000 == 0: print "Game Time: ", self.gameTimer.millis() if self.gameTimer.millis() > GAME_LENGTH - 500: self.backDoorServo.write(SERVO_OPEN) self.leftMotor.write(0,0) self.rightMotor.write(0,0) if self.gameTimer.millis() > GAME_LENGTH: self.module = MODULE_END self.stop() return ## Switch module if necessary. def updateState(self, module): if self.module == module: return elif module == MODULE_OFF: self.off.start() self.module = MODULE_OFF return elif module == MODULE_CHECK: self.check.start() self.module = MODULE_CHECK return elif module == MODULE_PICKUP: self.pickup.start() self.module = MODULE_PICKUP return elif module == MODULE_DROPOFF: self.dropoff.start() self.module = MODULE_DROPOFF return elif module == MODULE_FOLLOW: self.follow.start() self.module = MODULE_FOLLOW return else: print "Attempting to switch to nonexistent module" self.stop() ## Make sure that the intake motor does not stall. # If so, reverse the intake motors. # # @param checkTime Time in ms between checking stalls. # @param reverseTime Time in ms that the intake motors will reverse if needed. def checkForIntakeErrors(self, checkTime = 100, reverseTime = 800): if self.intakeDirection: # We are moving forward. if self.intakeTimer.millis() > checkTime: self.intakeTimer.reset() if self.intakeEncoder.val < INTAKE_ENCODER_LIMIT: # if we're stalled self.intakeDirection = True self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) else: # if we're not stalled self.intakeEncoder.write(0) else: # We are reversing the motors. if self.intakeTimer.millis() > reverseTime: self.intakeTimer.reset() self.intakeDirection = False self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) self.intakeEncoder.write(0) self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
class PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin # image_pipe = './image' # if not os.path.exists(image_pipe): # os.mkfifo(image_pipe) # image_fd = os.open(image_pipe, os.O_RDONLY) def setup(self): self.TicpIn = 4480/2.875 self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10) self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 # print "initial angle:"+str(self.initAngle) self.Follow = 'Right' self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]} self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.IRPID = PID(10, 5, .15) self.fwdVel = 30 self.turnVel = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer() def loop(self): #state 0 - wait for first no block, do nothing #transition: no block -> state 1 #state 1 - look for block #transition: found block -> state 2 #sate 2 - drive over block #transition: ir triggered -> state 3 #sate 3 - pick up block #transition: color sensor done -> sate 1 if self.timer.millis() > 100: # print 'Color' # print self.color.c # print self.color.r, self.color.g if self.color.c > 800 and self.Sort == 0: if self.color.r > self.color.g: self.Sort = 1 else: self.Sort = 2 # print self.uIR.val if self.uIR.val == 0: # self.MoveArm = True self.State = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = self.servo.bottom self.servo.write(self.servo.bottom) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= self.servo.top: time.sleep(1) self.delta = -self.servo.speed self.Top = 1 elif self.servoval <= self.servo.bottom: self.delta = self.servo.speed self.Bottom = 1 if self.Top == 1: self.Bottom = 2 if self.Sort == 1: if self.sorterval < self.sorter.left: self.dsorter = self.sorter.speed elif self.sorterval >= self.sorter.left: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 2: if self.sorterval > self.sorter.right: self.dsorter = -self.sorter.speed elif self.sorterval <= self.sorter.right: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 3: if time.time() - self.SortPause > 1: self.sorterval = self.sorter.center self.Sort = 0 self.sorterval += self.dsorter self.sorter.write(abs(self.sorterval)) self.servoval += self.delta self.servo.write(abs(self.servoval)) self.initAngle += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle=self.gyro.val - self.totalDrift #corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro=self.gyro.val self.totalDrift+=self.drift # message = os.read(self.image_fd, 20) # if ParseMessage(message): # self.blockDistance, self.blockAngle = ParseMessage(message) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.ir_array.update() ir = self.ir_array.ir_value ir90 = self.IRs[self.Follow][0] ir30 = self.IRs[self.Follow][1] avg = (ir[self.IRs[self.Follow][0]]+ir[self.IRs[self.Follow][1]]/2)/2 min_val = min(ir[self.IRs[self.Follow][0]], ir[self.IRs[self.Follow][1]]) if avg != float('inf'): pidResult= -self.IRPID.valuePID(4, avg) elif min_val != float('inf'): pidResult= -self.IRPID.valuePID(4, min_val) else: pidResult = -20 if ir[self.IRs[self.Follow][2]] < 4: pidResult = 20 # self.fwdVel = 0 else: self.fwdVel = 30 pidResult = self.PID.valuePID(self.initAngle, cAngle) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult print 'Distance Traveled : ' + str(self.getDistanceTraveled()) self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) ''' print "ok" #response = self.rp.read() #print "Got response %s" % response ''' def ParseMessage(message): if message: if message[:2] == 'no': blockDistance, blockAngle = 0, 0 return None else: try: blockDistance, blockAngle = [number[:6] for number in message.split(',')] except: blockDistance, blockAngle = 0, 0 return blockDistance, blockAngle else: return None def getDistanceTraveled(self): """returns Distance traveled in inches. Call resetEncoders, drive forward until you've reached your destination""" print 'Left: ' + str(self.encoderL.val) print 'Right: ' + str(self.encoderR.val) avg = (self.encoderL.val + self.encoderR.val)/2 return avg/360.
def setup(self): #################### #### EE SETUP #### #################### # Motor object representing the left motor. self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) # Encoder object for the left motor. self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW, LEFT_DRIVE_ENCODER_WHITE) # Motor object representing the right motor. self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) # Encoder object for the right motor. self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE) # Motor object representing the intake mechanism motors. self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM) # Encoder object for the intake motor. self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW, HUGS_MOTOR_ENCODER_WHITE) # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE) # Long range IR sensors self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) # Color sensor self.color = Color(self.tamp) # Limit switches self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH) self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) # Servo controlling the door of the collection chamber. self.backDoorServo = Servo(self.tamp, SERVO_PIN) # Make sure the door is closed self.backDoorServo.write(SERVO_CLOSE) # The switch that tells the program that the competition has started self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE) ################################# #### INTERNAL MODULE SETUP #### ################################# # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors reversing? True if so, False if going forwards. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) # Wall Follow object self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR) # A timer to make sure timesteps are only 10 times/second self.timestepTimer = Timer() # Timer object describing how long the current module has been running. self.moduleTimer = Timer() # Timer for the whole game self.gameTimer = Timer() # Runs the PICKUP process self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder) # Runs the DROPOFF process self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder) # Runs the FOLLOW process TODO: Fix forward to actually mean forward. self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch) # Runs the CHECK process. TODO: pass in proper timers. self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color) # Waits for the game to start self.off = OffModule(self.gameTimer, self.competitionModeSwitch) # Describes which stage of the program is running. self.module = MODULE_OFF
def setup(self): #Pygame stuff pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.TicpIn = 4480/2.875 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 print "initial angle:"+str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.fwdVel = 0 self.turnVel = 0 self.turn = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer()
def setup(self): """One-time method that sets up the robot, like in Arduino""" ##--------------------------------------------## ##--------- DRIVING MOTORS (+encoder) --------## ##--------------------------------------------## # Create the motor objects self.lmotor = Motor(self.tamp, *LMOTOR_PINS) self.rmotor = Motor(self.tamp, *RMOTOR_PINS) # Create a subscriber to listen for drive motor commands rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD, self.drive_callback) ## --------- ## if ENCODER: # encoder objects self.lmotor_encoder = Encoder(self.tamp, *LMOTOR_ENCODER_PINS, continuous=True) self.rmotor_encoder = Encoder(self.tamp, *RMOTOR_ENCODER_PINS, continuous=True) # Publishers for encoder values self.pub_lmotor_encoder_val = rospy.Publisher('lmotor_encoder_val', Int64, queue_size=10) self.pub_rmotor_encoder_val = rospy.Publisher('rmotor_encoder_val', Int64, queue_size=10) ##--------------------------------------------## ##--------------- INTAKE MOTOR ---------------## ##--------------------------------------------## # Create the motor object self.intake_motor = Motor(self.tamp, *INTAKE_MOTOR_PINS) # Create a subscriber to listen for intake motor commands (passively continuous) rospy.Subscriber('intake_cmd', kitware.msg.DriveCMD, self.intake_callback) ##--------------------------------------------## ##-------------- DISPENSER SERVO -------------## ##--------------------------------------------## # Setup servo self.servo = Servo(self.tamp, SERVO_PIN) self.servo.write(0) self.servo_val = 0 self.delta = DISPENSOR_SPEED self.end = False # Subscriber to listen for drive motor commands rospy.Subscriber('dispenser_servo_speed', Int64, self.dispenser_servo_callback) # Publisher for current servo value self.pub_dispenser_servo_val = rospy.Publisher('dispenser_servo_val', Int64, queue_size=10) ##--------------------------------------------## ##--------- DISPENSER MOTOR (+encoder) -------## ##--------------------------------------------## # Create the motor object self.dispenser_motor = Motor(self.tamp, *DISPENSER_MOTOR_PINS) # Create a subscriber to listen for dispenser motor commands rospy.Subscriber('dispenser_cmd', kitware.msg.DriveCMD, self.dispenser_motor_callback) ## --------- ## if ENCODER: # encoder objects self.dispenser_motor_encoder = Encoder( self.tamp, *DISPENSER_MOTOR_ENCODER_PINS, continuous=True) # Publishers for encoder values self.pub_dispenser_motor_encoder_val = rospy.Publisher( 'dispenser_motor_encoder_val', Int64, queue_size=10) ##--------------------------------------------## ##---------- General ---------## ##--------------------------------------------## # timer for loop() self.timer = Timer() # reset all motor encoders self.lmotor_encoder.val = 0 self.rmotor_encoder.val = 0 self.dispenser_motor_encoder.val = 0
class SemiBot(ROSSketch): """ROS Node that controls the SemiBot via the Teensy and tamproxy""" def setup(self): """One-time method that sets up the robot, like in Arduino""" ##--------------------------------------------## ##--------- DRIVING MOTORS (+encoder) --------## ##--------------------------------------------## # Create the motor objects self.lmotor = Motor(self.tamp, *LMOTOR_PINS) self.rmotor = Motor(self.tamp, *RMOTOR_PINS) # Create a subscriber to listen for drive motor commands rospy.Subscriber('drive_cmd', kitware.msg.DriveCMD, self.drive_callback) ## --------- ## if ENCODER: # encoder objects self.lmotor_encoder = Encoder(self.tamp, *LMOTOR_ENCODER_PINS, continuous=True) self.rmotor_encoder = Encoder(self.tamp, *RMOTOR_ENCODER_PINS, continuous=True) # Publishers for encoder values self.pub_lmotor_encoder_val = rospy.Publisher('lmotor_encoder_val', Int64, queue_size=10) self.pub_rmotor_encoder_val = rospy.Publisher('rmotor_encoder_val', Int64, queue_size=10) ##--------------------------------------------## ##--------------- INTAKE MOTOR ---------------## ##--------------------------------------------## # Create the motor object self.intake_motor = Motor(self.tamp, *INTAKE_MOTOR_PINS) # Create a subscriber to listen for intake motor commands (passively continuous) rospy.Subscriber('intake_cmd', kitware.msg.DriveCMD, self.intake_callback) ##--------------------------------------------## ##-------------- DISPENSER SERVO -------------## ##--------------------------------------------## # Setup servo self.servo = Servo(self.tamp, SERVO_PIN) self.servo.write(0) self.servo_val = 0 self.delta = DISPENSOR_SPEED self.end = False # Subscriber to listen for drive motor commands rospy.Subscriber('dispenser_servo_speed', Int64, self.dispenser_servo_callback) # Publisher for current servo value self.pub_dispenser_servo_val = rospy.Publisher('dispenser_servo_val', Int64, queue_size=10) ##--------------------------------------------## ##--------- DISPENSER MOTOR (+encoder) -------## ##--------------------------------------------## # Create the motor object self.dispenser_motor = Motor(self.tamp, *DISPENSER_MOTOR_PINS) # Create a subscriber to listen for dispenser motor commands rospy.Subscriber('dispenser_cmd', kitware.msg.DriveCMD, self.dispenser_motor_callback) ## --------- ## if ENCODER: # encoder objects self.dispenser_motor_encoder = Encoder( self.tamp, *DISPENSER_MOTOR_ENCODER_PINS, continuous=True) # Publishers for encoder values self.pub_dispenser_motor_encoder_val = rospy.Publisher( 'dispenser_motor_encoder_val', Int64, queue_size=10) ##--------------------------------------------## ##---------- General ---------## ##--------------------------------------------## # timer for loop() self.timer = Timer() # reset all motor encoders self.lmotor_encoder.val = 0 self.rmotor_encoder.val = 0 self.dispenser_motor_encoder.val = 0 ##--------- GENERAL ---------## def loop(self): """Method that loops at a fast rate, like in Arduino (isn't looping???)""" #rospy.loginfo("looping!") if self.timer.millis() > 100: self.timer.reset() self.publish_servo_val() self.publish_encoder_val() pass def speed_to_dir_pwm(self, speed): """Converts floating point speed (-1.0 to 1.0) to dir and pwm values""" speed = max(min(speed, 1), -1) return speed > 0, int(abs(speed * 255)) ##--------- ENCODERS ---------## def publish_encoder_val(self): if ENCODER: self.pub_lmotor_encoder_val.publish(self.lmotor_encoder.val) self.pub_rmotor_encoder_val.publish(self.rmotor_encoder.val) self.pub_dispenser_motor_encoder_val.publish( self.dispenser_motor_encoder.val) ##--------- DRIVING MOTORS ---------## def drive_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.lmotor.write(*self.speed_to_dir_pwm(msg.l_speed)) self.rmotor.write(*self.speed_to_dir_pwm(msg.r_speed)) ##--------- INTAKE MOTOR ---------## def intake_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.intake_motor.write(*self.speed_to_dir_pwm( msg.l_speed)) # use lspeed ##--------- DISPENSER SERVO ---------## def publish_servo_val(self): self.pub_dispenser_servo_val.publish(self.servo_val) def dispenser_servo_callback(self, msg): # write to servo self.delta = msg.data self.servo_val += self.delta self.servo.write(abs(self.servo_val)) ##--------- DISPENSER MOTOR ---------## def dispenser_motor_callback(self, msg): """Processes a new drive command and controls motors appropriately""" self.dispenser_motor.write(*self.speed_to_dir_pwm( msg.l_speed)) # use LEFT motor: l_speed
class PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin image_pipe = './image' if not os.path.exists(image_pipe): os.mkfifo(image_pipe) image_fd = os.open(image_pipe, os.O_RDONLY) def setup(self): #Pygame stuff pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.TicpIn = 4480/2.875 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 print "initial angle:"+str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.fwdVel = 0 self.turnVel = 0 self.turn = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer() def loop(self): #state 0 - wait for first no block, do nothing #transition: no block -> state 1 #state 1 - look for block #transition: found block -> state 2 #sate 2 - drive over block #transition: ir triggered -> state 3 #sate 3 - pick up block #transition: color sensor done -> sate 1 message = os.read(self.image_fd, 20) if message: # print("Recieved: '%s'" % message) if message[:2] == 'no': self.blockAngle = 0 if self.State == 0: self.State = 1 if self.State == 2: self.State = 3 else: if self.State != 0: self.State = 2 try: self.blockDistance, self.blockAngle = [number[:6] for number in message.split(',')] except: self.blockAngle = 0 self.blockAngle = float(self.blockAngle) if self.timer.millis() > 100: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.display.quit() if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.turnVel = -10 if event.key == pygame.K_RIGHT: self.turnVel = 10 if event.key == pygame.K_UP: self.fwdVel = 100 if event.key == pygame.K_DOWN: self.fwdVel = -100 if event.key == pygame.K_1: self.Sort = 1 if event.key == pygame.K_2: self.Sort = 2 if event.key == pygame.K_SPACE: self.MoveArm = True if event.type == pygame.KEYUP: if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT: self.turnVel = 0 self.turn = 0 if event.key == pygame.K_UP or event.key == pygame.K_DOWN: self.fwdVel = 0 print 'Color' print self.color.c print self.color.r, self.color.g print if self.color.c > 800 and self.Sort == 0: if self.color.r > self.color.g: self.Sort = 1 else: self.Sort = 2 # print self.uIR.val if self.uIR.val == 0: self.MoveArm = True self.State = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = self.servo.bottom self.servo.write(self.servo.bottom) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= self.servo.top: time.sleep(1) self.delta = -self.servo.speed self.Top = 1 elif self.servoval <= self.servo.bottom: self.delta = self.servo.speed self.Bottom = 1 if self.Top == 1: self.Bottom = 2 if self.Sort == 1: if self.sorterval < self.sorter.left: self.dsorter = self.sorter.speed elif self.sorterval >= self.sorter.left: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 2: if self.sorterval > self.sorter.right: self.dsorter = -self.sorter.speed elif self.sorterval <= self.sorter.right: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 3: if time.time() - self.SortPause > 1: self.sorterval = self.sorter.center self.Sort = 0 self.sorterval += self.dsorter self.sorter.write(abs(self.sorterval)) self.servoval += self.delta self.servo.write(abs(self.servoval)) self.turn += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle=self.gyro.val - self.totalDrift #corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro=self.gyro.val self.totalDrift+=self.drift # print 'State: ' + str(self.Searching) self.newAngle = cAngle + self.blockAngle print self.blockAngle pidResult=self.PID.valuePID(cAngle, cAngle+self.blockAngle+self.turn) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult self.motorL.write(self.motorLdrive < 0,abs(self.motorLdrive)) self.motorR.write(self.motorRdrive < 0,abs(self.motorRdrive)) '''
def setup(self): self.servo = Servo(self.tamp, 10) self.servo.write(2200) self.timer = Timer() self.val = 2200
def setup(self): self.TicpIn = 4480/2.875 self.ir_array = Ir_array(self.tamp, 16, 15, 14, 40, 11, 10) self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp,1, 3) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 # print "initial angle:"+str(self.initAngle) self.Follow = 'Right' self.IRs = {'Left': [0, 1, 2], 'Right': [5, 4, 3]} self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(.5, 1, 0.15) self.IRPID = PID(10, 5, .15) self.fwdVel = 30 self.turnVel = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer()
class MyRobot(SyncedSketch): runtime = 180000 #ms DOOR_OPEN_POS = 40 DOOR_CLOSE_POS = 144 GRIPPER_OPEN_POS = 0 GRIPPER_CLOSE_POS = 180 GRIPPER_DOWN = 1 GRIPPER_UP = 0 def setup(self): # initialize sensors, settings, start timers, etc. self.gameTimer = Timer() self.motorGripper = Motor(self.tamp, 23, 3) self.motorLeft = Motor(self.tamp, 7, 22) self.motorRight = Motor(self.tamp, 0, 21) self.motorval = 0 self.motorLeft.write(1,0) self.motorRight.write(1,0) self.motorGripper.write(1,0) self.currentGripperLevel = 2 print "Motors connected." self.servoDoor = Servo(self.tamp, 20) self.servovalDoor = self.DOOR_CLOSE_POS self.servoDoor.write(self.DOOR_CLOSE_POS) self.timerDoor = Timer() self.servoGripper = Servo(self.tamp, 10) self.servovalGripper = self.GRIPPER_CLOSE_POS self.servoGripper.write(self.servovalGripper) self.timerGripper = Timer() print "Servos connected." self.timer = Timer() self.gyro = Gyro(self.tamp, 9) print "Gyro connected." self.theta = self.gyro.val self.dT = .01 self.cam = cv2.VideoCapture(0) print "Camera connected." self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) frontLeftIR_pin = 14 self.frontLeftIR = AnalogInput(self.tamp, frontLeftIR_pin) frontRightIR_pin = 15 self.frontRightIR = AnalogInput(self.tamp, frontRightIR_pin) leftIR_pin = 16 self.leftIR = AnalogInput(self.tamp, leftIR_pin) rightIR_pin = 17 self.rightIR = AnalogInput(self.tamp, rightIR_pin) # Initialize PID Values self.P = 10 self.I = 0 #5 self.D = 0 self.last_diff = 0 self.integral = 0 self.desiredAngle = self.theta self.finishedCollectingBlock = False self.finishedDiscardingBlock = False self.timer = Timer() self.state = ExploreState() # self.state = CollectBlockState() self.blocksCollected = 0 self.leftIRVals = deque([]) self.rightIRVals = deque([]) self.frontRightIRVals = deque([]) self.frontLeftIRVals = deque([]) # Starts the robot print "Robot setup complete." def loop(self): if self.timer.millis() > self.dT*1000: # print("self.gameTimer", self.gameTimer) # print(self.gameTimer.millis()) if (self.gameTimer.millis() > self.runtime - 5000): # 5 seconds left in the game self.openDoorAndBuildTower() inputs = self.readSensors() process = self.state.process(inputs) print "Process: " + process.__class__.__name__ # print(self.gyro.val) self.state = process.get_next_state() self.processOutputs(process.get_outputs()) self.timer.reset() def readSensors(self): # Calculate the distance traveled, change in theta, and then reset sensors distance_traveled = 0 #(self.encoderLeft.val + self.encoderRight.val) / 2.0 #encoder_omega = self.encoderLeft.val - self.encoderRight.val # print('frontRightIR: ', self.frontRightIR.val) # print("frontLeftIR: ", self.frontLeftIR.val) # print("leftIR: ", self.leftIR.val) # print("rightIR: ", self.rightIR.val) # Camera ret, frame = self.cam.read() img = cv2.resize(frame,None,fx=0.25, fy=0.25, interpolation = cv2.INTER_AREA) blocks = CalculateBlocks(img); #what should CalculateBlocks return? leftIR = self.leftIR.val rightIR = self.rightIR.val frontLeftIR = self.frontLeftIR.val frontRightIR = self.frontRightIR.val return Inputs(distance_traveled, self.gyro.val, frontRightIR, frontLeftIR, leftIR, rightIR, self.finishedCollectingBlock, blocks, self.color.r, self.color.g, self.color.b) def processOutputs(self, Outputs): # TODO Missing servo outputs if (Outputs.driving == True): self.motorval = 50 #25? else: self.motorval = 0 if (Outputs.turning == True): # if we turn, then update self.desiredAngle self.desiredAngle = self.gyro.val if (Outputs.turn_clockwise == True): self.PID(self.desiredAngle + 3) else: self.PID(self.desiredAngle - 3) else: # self.PID(self.gyro.val) self.PID(self.desiredAngle) if Outputs.isCollectingBlock and not self.finishedCollectingBlock: # Gripper is at level 2 and closed if self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_CLOSE_POS: self.closeOrOpenGripper() # Gripper is at level 2 and open elif self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_OPEN_POS: self.moveGripper() # Gripper is at level 1 and open elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_OPEN_POS: self.closeOrOpenGripper() # Gripper is at level 1 and closed elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_CLOSE_POS: self.moveGripper() # self.finishedCollectingBlock = True time.sleep(2) self.blocksCollected += 1 if (self.blocksCollected == 4): self.finishedCollectingBlock = True if Outputs.isDiscardingBlock and not self.finishedDiscardingBlock: # Gripper level 2, closed if self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_CLOSE_POS: self.moveGripper() # Gripper level 1, closed elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_CLOSE_POS: self.closeOrOpenGripper() # Gripper level 1, open elif self.currentGripperLevel == 1 and self.servovalGripper == self.GRIPPER_OPEN_POS: self.moveGripper() # Gripper level 2, open elif self.currentGripperLevel == 2 and self.servovalGripper == self.GRIPPER_OPEN_POS: self.closeOrOpenGripper() self.finishedDiscardingBlock = True def PID(self, desired_theta): # Set encoder to 0 after turning. # To turn in place, set bias (i.e. motorval to 0) estimated = self.gyro.val # TODO: calculate estimated with encoder # print(self.gyro.val) diff = desired_theta - estimated # print diff self.integral += diff * self.dT derivative = (diff - self.last_diff)/self.dT power = self.P*diff + self.I*self.integral + self.D*derivative # NOTE: Cap self.D*derivative, use as timeout # print("motorLeft: ", min(75, abs(self.motorval + power))) # print("motorRight: ", min(75, abs(self.motorval - power))) self.motorLeft.write((self.motorval + power)>0, min(75, abs(self.motorval + power))) self.motorRight.write((self.motorval - power)>0, min(75, abs(self.motorval - power))) # print "EncoderLeft: " + str(self.encoderLeft.val) # print "EncoderRight: " + str(self.encoderRight.val) def moveGripper(self): if self.currentGripperLevel == 1: self.motorGripper.write(self.GRIPPER_UP, 100) timeToSleep = 1.3 if (self.blocksCollected == 1): timeToSleep = 1.4 elif (self.blocksCollected == 2): timeToSleep = 1.55 elif (self.blocksCollected == 3): timeToSleep = 1.7 elif self.currentGripperLevel == 2: self.motorGripper.write(self.GRIPPER_DOWN, 100) timeToSleep = 0.9 time.sleep(timeToSleep) self.motorGripper.write(1,0) if self.currentGripperLevel == 1: self.currentGripperLevel = 2 else: self.currentGripperLevel = 1 def closeOrOpenGripper(self): if (self.servovalGripper > self.GRIPPER_OPEN_POS): while(self.servovalGripper > self.GRIPPER_OPEN_POS): if (self.timerGripper.millis() > 1): self.timerGripper.reset() self.servovalGripper -= 1 self.servoGripper.write(abs(self.servovalGripper)) elif (self.servovalGripper < self.GRIPPER_CLOSE_POS): while(self.servovalGripper < self.GRIPPER_CLOSE_POS): if (self.timerGripper.millis() > 1): self.timerGripper.reset() self.servovalGripper += 1 self.servoGripper.write(abs(self.servovalGripper)) time.sleep(.1) def openDoorAndBuildTower(self): self.moveGripper() # should be blocking self.closeOrOpenGripper() while(self.servovalDoor > self.DOOR_OPEN_POS): if (self.timerDoor.millis() > 10): self.timerDoor.reset() self.servovalDoor -= 1 # print self.servovalDoor self.servoDoor.write(abs(self.servovalDoor))
class pythonDriver(SyncedSketch): ss_pin = 10 #gyro select pin # also 11, 12, and 13 color_pins = 18, 19 uIR_pin = 17 Lencoder_pins = 22, 23 Rencoder_pins = 21, 20 arm_pin = 9 sorter_pin = 5 Rmoter_pins = 1, 3 Lmotor_pins = 2, 4 pipes = './gyro', './IR' for path in pipes: if not os.path.exists(path): os.mkfifo(path) def setup(self): self.init_time = time.time() self.functions = {} self.servo = Servo(self.tamp,self.arm_pin) # self.servo.write(20) # self.servoval = 20 # self.delta = 0 self.functions['servoARM'] = lambda angle: self.servo.write(angle) self.sorter = Servo(self.tamp, self.sorter_pin) # self.sorter.center = 98 # self.sorter.right = 20 # self.sorter.left = 170 # self.sorter.speed = 30 self.functions['servoSORT'] = lambda angle: self.sorter.write(angle) self.encoderL = Encoder(self.tamp, *self.Lencoder_pins) self.encoderR = Encoder(self.tamp, *self.Rencoder_pins) #motor left self.motorL = Motor(self.tamp, *self.Lmotor_pins) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 self.functions['left'] = lambda speed: self.motorL.write(speed<0, abs(speed)) #motor right self.motorR = Motor(self.tamp, *self.Rmoter_pins) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 self.functions['right'] = lambda speed: self.motorR.write(speed<0, abs(speed)) #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val print "initial angle:"+str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(2,1,0.15) self.fwdVel = 0 self.turnVel = 0 self.maxVel = 40 self.Distance = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.timer = Timer() #connect to pipe pipe_path = "./robot" try: os.mkfifo(pipe_path) except OSError: print "error" self.pipe_fd = os.open(pipe_path, os.O_RDONLY | os.O_NONBLOCK) def loop(self): if self.timer.millis() > 100: #if get message #read message '''Alternative Command Names: left right servoARM servoSORT ''' response = os.read(self.pipe_fd,100) print "Got response %s" % response #rp.close() print "no message" value, function = response.split(' ') functions[function](int(value)) ''' if response == "LEFT": self.turnVel = -5 print 'Got Key' if response == "RIGHT": self.turnVel = 5 if response == "UP": self.fwdVel = 40 if response == "DOWN": self.fwdVel = -40 if response == "SPACE": self.MoveArm = True #stop message if response =="STOP": self.turVel=0 self.fwdVel=0 if event.key == 'SORT': if self.Sort == 0: self.Sort = 1 if event.key == pygame.K_2: if self.Sort == 0: self.Sort = 2 if response == "LEFT" or response == "RIGHT": self.turnVel = 0 if response == "UP" or response == "DOWN": self.fwdVel = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = 20 self.servo.write(20) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= 170: self.delta = -30 self.Top = 1 elif self.servoval <= 30: self.delta = 30 self.Bottom = 1 if self.Top == 1: self.Bottom = 2 self.servoval += self.delta self.servo.write(abs(self.servoval)) print 'delta: ' + str(self.delta) print 'servoval: ' + str(self.servoval) print 'MoveArm: ' + str(self.MoveArm) self.initAngle += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bit''' cAngle=self.gyro.val - self.totalDrift #corrected angle #print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift '''
class Driver(Sketch): def setup(self): # ROS Setup rospy.init_node('driver') rospy.Subscriber("driver_commands", String, callback) # self.tof_pub = rospy.Publisher('tof_readings', String, queue_size=10) # Device Setup self.motor1 = Motor(self.tamp, MOTOR1_DIR, MOTOR1_PWM) self.motor2 = Motor(self.tamp, MOTOR2_DIR, MOTOR2_PWM) self.encoder1 = Encoder(self.tamp, *ENCODER1, continuous=True) self.encoder2 = Encoder(self.tamp, *ENCODER2, continuous=True) # self.encoder1_value = 0 # self.encoder2_value = 0 # TimeOfFlight Sensors # self.tof1 = TimeOfFlight(self.tamp, TOF1, 1, continuous=False) # self.tof2 = TimeOfFlight(self.tamp, TOF2, 2, continuous=False) # self.tof1.enable() # self.tof2.enable() # self.tof1_value = 0 # self.tof2_value = 0 # Servo for the Release self.servo = Servo(self.tamp, SERVO) self.servo.write(SERVO_CLOSE) # Controller Params self.pid = PID(p=P, i=I, d=D) self.target1 = 0 self.target2 = 0 self.servo_commands = Queue() def rotate(self, angle): target_rotation = (angle / 360.0) * ROBOT_RADIUS / WHEEL_RADIUS target_ticks = FULL_TICKS * TURN_COEFFICIENT * target_rotation self._rotate(target_ticks) def _rotate(self, ticks): self.target1 = self.encoder1.val + MOTOR1_DIRECTION * ticks self.target2 = self.encoder2.val + MOTOR2_DIRECTION * ticks def move(self, distance): target_rotation = distance / (2 * np.pi * WHEEL_RADIUS) target_ticks = target_rotation * FULL_TICKS self.target1 = self.encoder1.val - MOTOR1_DIRECTION * target_ticks self.target2 = self.encoder2.val + MOTOR2_DIRECTION * target_ticks def release(self): self.servo_commands.put(SERVO_OPEN) sleep(SERVO_WAIT) self.servo_commands.put(SERVO_CLOSE) def loop(self): sleep(SLEEP_TIME) # Send Sensor Requests # self.tamp.send_request(self.encoder1.id, # self.encoder1.READ_CODE, # self._handle_encoder1_update, # continuous=False, # weight=1) # self.tamp.send_request(self.encoder2.id, # self.encoder2.READ_CODE, # self._handle_encoder2_update, # continuous=False, # weight=1) # self.tamp.send_request(self.tof1.id, # self.tof1.READ_CODE, # self._handle_tof1_update, # continuous=False, # weight=1) # self.tamp.send_request(self.tof2.id, # self.tof2.READ_CODE, # self._handle_tof2_update, # continuous=False, # weight=1) if self.servo_commands.qsize() != 0: self.servo.write(self.servo_commands.get()) # Main Control Logic diff1 = self.target1 - self.encoder1.val diff2 = self.target2 - self.encoder2.val if abs(diff1) < CONTROLLER_THRESHOLD: diff1 = 0 if abs(diff2) < CONTROLLER_THRESHOLD: diff2 = 0 write(self.motor1, MOTOR1_DIRECTION, self.pid(feedback=diff1)) write(self.motor2, MOTOR2_DIRECTION, self.pid(feedback=diff2))
def setup(self): self.init_time = time.time() self.functions = {} self.servo = Servo(self.tamp,self.arm_pin) # self.servo.write(20) # self.servoval = 20 # self.delta = 0 self.functions['servoARM'] = lambda angle: self.servo.write(angle) self.sorter = Servo(self.tamp, self.sorter_pin) # self.sorter.center = 98 # self.sorter.right = 20 # self.sorter.left = 170 # self.sorter.speed = 30 self.functions['servoSORT'] = lambda angle: self.sorter.write(angle) self.encoderL = Encoder(self.tamp, *self.Lencoder_pins) self.encoderR = Encoder(self.tamp, *self.Rencoder_pins) #motor left self.motorL = Motor(self.tamp, *self.Lmotor_pins) self.motorLdrive = 0 self.motorL.write(1,0) #self.deltaL = 1 #self.motorvalL = 0 self.functions['left'] = lambda speed: self.motorL.write(speed<0, abs(speed)) #motor right self.motorR = Motor(self.tamp, *self.Rmoter_pins) self.motorRdrive = 0 self.motorR.write(1,0) #self.deltaR = 1 #self.motorvalR = 0 self.functions['right'] = lambda speed: self.motorR.write(speed<0, abs(speed)) #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val print "initial angle:"+str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID=PID(2,1,0.15) self.fwdVel = 0 self.turnVel = 0 self.maxVel = 40 self.Distance = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.timer = Timer() #connect to pipe pipe_path = "./robot" try: os.mkfifo(pipe_path) except OSError: print "error" self.pipe_fd = os.open(pipe_path, os.O_RDONLY | os.O_NONBLOCK)
class Robot(SyncedSketch): def setup(self): #################### #### EE SETUP #### #################### # Motor object representing the left motor. self.leftMotor = Motor(self.tamp, LEFT_DRIVE_CONTROLLER_DIRECTION, LEFT_DRIVE_CONTROLLER_PWM) # Encoder object for the left motor. self.leftEncoder = Encoder(self.tamp, LEFT_DRIVE_ENCODER_YELLOW, LEFT_DRIVE_ENCODER_WHITE) # Motor object representing the right motor. self.rightMotor = Motor(self.tamp, RIGHT_DRIVE_CONTROLLER_DIRECTION, RIGHT_DRIVE_CONTROLLER_PWM) # Encoder object for the right motor. self.rightEncoder = Encoder(self.tamp, RIGHT_DRIVE_ENCODER_YELLOW, RIGHT_DRIVE_ENCODER_WHITE) # Motor object representing the intake mechanism motors. self.intakeMotor = Motor(self.tamp, HUGS_MOTOR_CONTROLLER_DIRECTION, HUGS_MOTOR_CONTROLLER_PWM) # Encoder object for the intake motor. self.intakeEncoder = Encoder(self.tamp, HUGS_MOTOR_ENCODER_YELLOW, HUGS_MOTOR_ENCODER_WHITE) # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, BELT_MOTOR_CONTROLLER_DIRECTION, BELT_MOTOR_CONTROLLER_PWM) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, BELT_MOTOR_ENCODER_YELLOW, BELT_MOTOR_ENCODER_WHITE) # Long range IR sensors self.irBL = LRIR(self.tamp, LONG_DISTANCE_IR_BL) self.irBR = LRIR(self.tamp, LONG_DISTANCE_IR_BR) self.irFL = LRIR(self.tamp, LONG_DISTANCE_IR_FL) self.irFR = LRIR(self.tamp, LONG_DISTANCE_IR_FR) # Color sensor self.color = Color(self.tamp) # Limit switches self.conveyorLimSwitch = DigitalInput(self.tamp, CONVEYOR_LIMIT_SWITCH) self.blockLimSwitch = DigitalInput(self.tamp, BLOCK_LIMIT_SWITCH) # Servo controlling the door of the collection chamber. self.backDoorServo = Servo(self.tamp, SERVO_PIN) # Make sure the door is closed self.backDoorServo.write(SERVO_CLOSE) # The switch that tells the program that the competition has started self.competitionModeSwitch = DigitalInput(self.tamp, COMPETITION_MODE) ################################# #### INTERNAL MODULE SETUP #### ################################# # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors reversing? True if so, False if going forwards. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) # Wall Follow object self.wallFollow = WallFollow(self.leftMotor, self.rightMotor, Timer(), self.irFL, self.irFR, self.irBL, self.irBR) # A timer to make sure timesteps are only 10 times/second self.timestepTimer = Timer() # Timer object describing how long the current module has been running. self.moduleTimer = Timer() # Timer for the whole game self.gameTimer = Timer() # Runs the PICKUP process self.pickup = PickupModule(self.moduleTimer, self.conveyorLimSwitch, self.conveyorMotor, self.conveyorEncoder) # Runs the DROPOFF process self.dropoff = DropoffModule(self.moduleTimer, self.backDoorServo, self.rightMotor, self.leftMotor, self.rightEncoder) # Runs the FOLLOW process TODO: Fix forward to actually mean forward. self.follow = FollowModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.wallFollow, FORWARD_SPEED, self.blockLimSwitch) # Runs the CHECK process. TODO: pass in proper timers. self.check = CheckModule(self.moduleTimer, self.leftMotor, self.rightMotor, self.intakeMotor, self.color) # Waits for the game to start self.off = OffModule(self.gameTimer, self.competitionModeSwitch) # Describes which stage of the program is running. self.module = MODULE_OFF def loop(self): if self.timestepTimer.millis() > 100: self.timestepTimer.reset() #print "Module Number", self.module state = -1 if self.module == MODULE_OFF: state = self.off.run() elif self.module == MODULE_CHECK: state = self.check.run() elif self.module == MODULE_PICKUP: state = self.pickup.run() elif self.module == MODULE_DROPOFF: self.module = MODULE_FOLLOW state = self.follow.run() elif self.module == MODULE_FOLLOW: state = self.follow.run() else: print "Attempting to run nonexistent module" self.stop() self.updateState(state) if self.gameTimer.millis() % 10000 == 0: print "Game Time: ", self.gameTimer.millis() if self.gameTimer.millis() > GAME_LENGTH - 500: self.backDoorServo.write(SERVO_OPEN) self.leftMotor.write(0, 0) self.rightMotor.write(0, 0) if self.gameTimer.millis() > GAME_LENGTH: self.module = MODULE_END self.stop() return ## Switch module if necessary. def updateState(self, module): if self.module == module: return elif module == MODULE_OFF: self.off.start() self.module = MODULE_OFF return elif module == MODULE_CHECK: self.check.start() self.module = MODULE_CHECK return elif module == MODULE_PICKUP: self.pickup.start() self.module = MODULE_PICKUP return elif module == MODULE_DROPOFF: self.dropoff.start() self.module = MODULE_DROPOFF return elif module == MODULE_FOLLOW: self.follow.start() self.module = MODULE_FOLLOW return else: print "Attempting to switch to nonexistent module" self.stop() ## Make sure that the intake motor does not stall. # If so, reverse the intake motors. # # @param checkTime Time in ms between checking stalls. # @param reverseTime Time in ms that the intake motors will reverse if needed. def checkForIntakeErrors(self, checkTime=100, reverseTime=800): if self.intakeDirection: # We are moving forward. if self.intakeTimer.millis() > checkTime: self.intakeTimer.reset() if self.intakeEncoder.val < INTAKE_ENCODER_LIMIT: # if we're stalled self.intakeDirection = True self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) else: # if we're not stalled self.intakeEncoder.write(0) else: # We are reversing the motors. if self.intakeTimer.millis() > reverseTime: self.intakeTimer.reset() self.intakeDirection = False self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) self.intakeEncoder.write(0) self.intakeMotor.write(self.intakeDirection, INTAKE_POWER)
class PIDDrive(SyncedSketch): ss_pin = 10 #gyro select pin image_pipe = './image' if not os.path.exists(image_pipe): os.mkfifo(image_pipe) image_fd = os.open(image_pipe, os.O_RDONLY) def setup(self): #Pygame stuff pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.TicpIn = 4480 / 2.875 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1, 0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp, 1, 3) self.motorRdrive = 0 self.motorR.write(1, 0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 print "initial angle:" + str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID = PID(.5, 1, 0.15) self.fwdVel = 0 self.turnVel = 0 self.turn = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer() def loop(self): #state 0 - wait for first no block, do nothing #transition: no block -> state 1 #state 1 - look for block #transition: found block -> state 2 #sate 2 - drive over block #transition: ir triggered -> state 3 #sate 3 - pick up block #transition: color sensor done -> sate 1 message = os.read(self.image_fd, 20) if message: # print("Recieved: '%s'" % message) if message[:2] == 'no': self.blockAngle = 0 if self.State == 0: self.State = 1 if self.State == 2: self.State = 3 else: if self.State != 0: self.State = 2 try: self.blockDistance, self.blockAngle = [ number[:6] for number in message.split(',') ] except: self.blockAngle = 0 self.blockAngle = float(self.blockAngle) if self.timer.millis() > 100: for event in pygame.event.get(): if event.type == pygame.QUIT: pygame.display.quit() if event.type == pygame.KEYDOWN: if event.key == pygame.K_LEFT: self.turnVel = -10 if event.key == pygame.K_RIGHT: self.turnVel = 10 if event.key == pygame.K_UP: self.fwdVel = 100 if event.key == pygame.K_DOWN: self.fwdVel = -100 if event.key == pygame.K_1: self.Sort = 1 if event.key == pygame.K_2: self.Sort = 2 if event.key == pygame.K_SPACE: self.MoveArm = True if event.type == pygame.KEYUP: if event.key == pygame.K_LEFT or event.key == pygame.K_RIGHT: self.turnVel = 0 self.turn = 0 if event.key == pygame.K_UP or event.key == pygame.K_DOWN: self.fwdVel = 0 print 'Color' print self.color.c print self.color.r, self.color.g print if self.color.c > 800 and self.Sort == 0: if self.color.r > self.color.g: self.Sort = 1 else: self.Sort = 2 # print self.uIR.val if self.uIR.val == 0: self.MoveArm = True self.State = 0 if self.MoveArm: if self.Bottom == 2 and self.Top == 1: self.delta = 0 self.servoval = self.servo.bottom self.servo.write(self.servo.bottom) self.Bottom, self.Top = 0, 0 self.MoveArm = False elif self.servoval >= self.servo.top: time.sleep(1) self.delta = -self.servo.speed self.Top = 1 elif self.servoval <= self.servo.bottom: self.delta = self.servo.speed self.Bottom = 1 if self.Top == 1: self.Bottom = 2 if self.Sort == 1: if self.sorterval < self.sorter.left: self.dsorter = self.sorter.speed elif self.sorterval >= self.sorter.left: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 2: if self.sorterval > self.sorter.right: self.dsorter = -self.sorter.speed elif self.sorterval <= self.sorter.right: self.dsorter = 0 self.Sort = 3 self.SortPause = time.time() if self.Sort == 3: if time.time() - self.SortPause > 1: self.sorterval = self.sorter.center self.Sort = 0 self.sorterval += self.dsorter self.sorter.write(abs(self.sorterval)) self.servoval += self.delta self.servo.write(abs(self.servoval)) self.turn += self.turnVel self.timer.reset() #response = self.rp.read() #print "Got response %s" % response # Valid gyro status is [0,1], see datasheet on ST1:ST0 bits cAngle = self.gyro.val - self.totalDrift #corrected angle # print (self.gyro.val-self.prevGyro), self.gyro.val, self.gyro.status, cAngle, self.totalDrift self.prevGyro = self.gyro.val self.totalDrift += self.drift # print 'State: ' + str(self.Searching) self.newAngle = cAngle + self.blockAngle print self.blockAngle pidResult = self.PID.valuePID(cAngle, cAngle + self.blockAngle + self.turn) # print 'Angle Dif: ' + str(cAngle-self.initAngle) + '\tPID RESULT: '+ str(pidResult) # print 'Encoders:\tR: ' + str(self.encoderR.val) + '\tL: ' + str(self.encoderL.val) # print 'AVG: ' + str((self.encoderR.val + self.encoderL.val)/2.) self.motorLdrive = self.fwdVel - pidResult self.motorRdrive = self.fwdVel + pidResult self.motorL.write(self.motorLdrive < 0, abs(self.motorLdrive)) self.motorR.write(self.motorRdrive < 0, abs(self.motorRdrive)) '''
def setup(self): self.servo = Servo(self.tamp, 9) self.servo.write(1050) self.timer = Timer() self.end = False
class Sorter: def __init__(self, tamp): self.tamp=tamp self.servo = Servo(self.tamp, 5) self.servo.center = 90 self.servo.right = 25 self.servo.left = 165 self.servo.speed = 15 self.servo.jostleSpeed = 40 self.servo.rightJostle = 60 self.servo.leftJostle = 120 self.servo.write(self.servo.center) self.sorterval = self.servo.center self.dsorter = 0 self.sorterState = "None" self.startPauseTime=0 self.pauseLength=0 def moveSorterLeft(self): if self.sorterval < self.servo.left: self.sorterState="Left" self.dsorter = self.servo.speed elif self.sorterval >= self.servo.left: self.setPause(1) self.sorterState="Pause" self.nextSorterState="Center" self.dsorter = 0 def moveSorterRight(self): if self.sorterval > self.servo.right: self.sorterState="Right" self.dsorter = -self.servo.speed elif self.sorterval <= self.servo.right: self.setPause(1) self.sorterState="Pause" self.nextSorterState="Center" self.dsorter = 0 def moveSorterCenter(self): self.sorterState="None" self.sorterval = self.servo.center def jostleServoLeft(self): if self.sorterval < self.servo.leftJostle: self.sorterState="LeftJostle" self.dsorter = self.servo.jostleSpeed elif self.sorterval >= self.servo.leftJostle: self.sorterState="RightJostle" self.dsorter = 0 def jostleServoRight(self): if self.sorterval > self.servo.rightJostle: self.sorterState="RightJostle" self.dsorter = -self.servo.jostleSpeed elif self.sorterval <= self.servo.rightJostle: self.sorterState="Center" self.nextSorterState="None" self.dsorter = 0 #jostle moves the servo back and forth in a way that does not allow the block to fall into either tower #it is meant to "de-choke" the funnel if a block has become stuck def jostle(self): self.sorterState ="LeftJostle" def setPause(self,pauseLength): self.startPauseTime=time.time() self.pauseLength=pauseLength def pauseSorter(self): if(time.time()-self.startPauseTime>self.pauseLength): self.sorterState=self.nextSorterState self.nextSorterState="None" def update(self): if(self.sorterState!="None"): if(self.sorterState=="Left"): self.moveSorterLeft() elif(self.sorterState=="Right"): self.moveSorterRight() elif(self.sorterState=="Center"): self.moveSorterCenter() elif(self.sorterState=="Pause"): self.pauseSorter() elif self.sorterState=="LeftJostle": self.jostleServoLeft() elif self.sorterState =="RightJostle": self.jostleServoRight() self.sorterval += self.dsorter self.servo.write(abs(self.sorterval))
def setup(self): #Pygame stuff pygame.init() self.screen = pygame.display.set_mode((300, 300)) self.TicpIn = 4480 / 2.875 self.color = Color(self.tamp, integrationTime=Color.INTEGRATION_TIME_101MS, gain=Color.GAIN_1X) self.uIR = DigitalInput(self.tamp, 17) self.uIR.val = 1 self.servo = Servo(self.tamp, 9) self.servo.bottom = 0 self.servo.top = 200 self.servo.speed = 30 self.servo.write(self.servo.bottom) self.servoval = self.servo.bottom self.delta = 0 self.sorter = Servo(self.tamp, 5) self.sorter.center = 90 self.sorter.right = 25 self.sorter.left = 165 self.sorter.speed = 15 self.sorter.write(self.sorter.center) self.sorterval = self.sorter.center self.dsorter = 0 self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.init_time = time.time() # self.encoderL = Encoder(self.tamp, 23, 22) # self.encoderR = Encoder(self.tamp, 21, 20) #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorLdrive = 0 self.motorL.write(1, 0) #self.deltaL = 1 #self.motorvalL = 0 #motor right self.motorR = Motor(self.tamp, 1, 3) self.motorRdrive = 0 self.motorR.write(1, 0) #self.deltaR = 1 #self.motorvalR = 0 #gyro self.gyro = Gyro(self.tamp, self.ss_pin, integrate=True) self.initAngle = self.gyro.val self.newAngle = 0 self.blockAngle = 0 self.blockDistance = 0 print "initial angle:" + str(self.initAngle) self.prevGyro = 0 self.drift = -.02875 self.totalDrift = 0 self.drifts = [] self.PID = PID(.5, 1, 0.15) self.fwdVel = 0 self.turnVel = 0 self.turn = 0 self.MoveArm, self.Top, self.Bottom = False, 0, 0 self.Sort = 0 self.SortPause = 0 self.State = 1 self.timer = Timer()