class EncoderWrap: def __init__(self, tamp): self.tamp = tamp self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.isRobotMoving = False self.prevEncoderL = 0 self.pervEncoderR = 0 self.NOT_MOVING_EPSILON = 62 #5 degrees of change def resetEncoders(self): self.encoderL.write(0) self.encoderR.write(0) def getDistanceTraveled(self): avg = (self.encoderL.val + self.encoderR.val) / 2 return avg / 360. def update(self): if (abs(self.encoderL.val - self.prevEncoderL) < self.NOT_MOVING_EPSILON and abs(self.encoderL.val - self.prevEncoderL) < self.NOT_MOVING_EPSILON): self.isRobotMoving = False else: self.isRobotMoving = True self.prevEncoderL = self.encoderL.val self.prevEncoderR = self.encoderR.val
class EncoderWrap: def __init__(self,tamp): self.tamp=tamp self.encoderL = Encoder(self.tamp, 22, 23) self.encoderR = Encoder(self.tamp, 21, 20) self.isRobotMoving=False self.prevEncoderL=0 self.pervEncoderR=0 self.NOT_MOVING_EPSILON=62 #5 degrees of change def resetEncoders(self): self.encoderL.write(0) self.encoderR.write(0) def getDistanceTraveled(self): avg = (self.encoderL.val+self.encoderR.val)/2 return avg/360. def update(self): if(abs(self.encoderL.val-self.prevEncoderL)<self.NOT_MOVING_EPSILON and abs(self.encoderL.val-self.prevEncoderL)<self.NOT_MOVING_EPSILON): self.isRobotMoving=False else: self.isRobotMoving=True self.prevEncoderL=self.encoderL.val self.prevEncoderR=self.encoderR.val
class HugTest(SyncedSketch): def setup(self): # 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) # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors going forward? True if so, False if reversing. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) def loop(self): self.checkForIntakeErrors() def checkForIntakeErrors(self, checkTime=1000, reverseTime=3000): 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 HugTest(SyncedSketch): def setup(self): # 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) # Timer object to moderate checking for intake errors. self.intakeTimer = Timer() # Are the intake motors going forward? True if so, False if reversing. self.intakeDirection = False # Start the intake motor. self.intakeMotor.write(self.intakeDirection, INTAKE_POWER) def loop(self): self.checkForIntakeErrors() def checkForIntakeErrors(self, checkTime = 1000, reverseTime = 3000): 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 TestBelt(SyncedSketch): def setup(self): self.timer = Timer() self.ttt = Timer() self.timeout = 5000 # Motor object representing the conveyor belt motor. self.conveyorMotor = Motor(self.tamp, 7, 6) # Encoder object for the conveyor belt motor. self.conveyorEncoder = Encoder(self.tamp, 28, 27) self.blocksPickedUp = 0 self.startThing() def loop(self): if self.ttt.millis() > 100: print self.conveyorEncoder.val self.ttt.reset() self.runThing() ## Set up the beginning of the pickup process. def startThing(self): self.conveyorEncoder.write(0) self.conveyorMotor.write(UP, CONVEYOR_POWER) self.timer.reset() ## Pick up a block from the block capture mechanism. # # Move the conveyor belt upwards until the encoders indicate that # the block has moved far enough. Then move the conveyor belt back. # # @return The value of the next module to return to. def runThing(self): # Allow timeout. if self.timer.millis() > self.timeout: print "Timed out from PICKUP to FIND" self.stop() encval = self.conveyorEncoder.val # Move up the conveyor belt until it hits the encoder limit. if encval > CONVEYOR_ENCODER_LIMIT: self.conveyorMotor.write(DOWN, CONVEYOR_POWER) else: self.conveyorMotor.write(UP, CONVEYOR_POWER) # Stop the motor when it gets to the bottom. if encval < 0 and self.timer.millis() > 1000: self.conveyorMotor.write(False, 0) self.blocksPickedUp += 1 # Switch modules if self.blocksPickedUp >= 4: print "Going from PICKUP to DROPOFF" self.stop() else: print "Going from PICKUP to FIND" self.stop() return
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 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)