def __init__(self, mode):
super().__init__('Finish Autonomous')
self.mode = mode
self.logger = logging.getLogger("robot")
# def initialize(self):
# if switches.gear_mech_switch.get(): # don't do anything if switch not pressed
# return
self.addSequential(WaitCommand(0.5))
if self.mode == "left":
self.addSequential(SetSpeed(-0.1, 1.0))
self.addSequential(SetSpeed(-0.3, 0.5))
self.addSequential(Rotate(-60))
self.addSequential(SetSpeed(0.3, 2.0))
self.addSequential(Rotate(90))
elif self.mode == "right":
self.addSequential(SetSpeed(-0.1, 1.0))
self.addSequential(SetSpeed(-0.3, 0.5))
self.addSequential(Rotate(60))
self.addSequential(SetSpeed(0.3, 2.0))
self.addSequential(Rotate(-90))
else:
self.addSequential(SetSpeed(-0.1, 1.0))
self.addSequential(SetSpeed(-0.3, 0.5))
self.addSequential(WaitCommand(5.0))
self.addSequential(SetSpeed(0.3, 0.8))
def createSideCommands(self):
#move robot
self.addSequential(LifterCommandTimed(.4, timeoutInSeconds=.5))
self.addSequential(WaitCommand(1))
self.addSequential(LoaderToggle(self.loaderState.kClose))
self.addSequential(MoveRobot(-.5, .5, timeoutInSeconds=1))
self.addSequential(MoveRobot(.5, -.5, timeoutInSeconds=1))
self.addSequential(MoveRobot(-.5, .5, timeoutInSeconds=1.2))
self.addParallel(LoaderToggle(self.loaderState.kOpen))
self.addSequential(WaitCommand(0.5))
self.addSequential(LoaderToggle(self.loaderState.kClose))
self.addSequential(MoveRobot(.5, -.5, timeoutInSeconds=1.2))
self.addSequential(WaitCommand(2))
self.addSequential(LifterCommandTimed(0.4, timeoutInSeconds=3))
self.addParallel(MoveRobot(-.5, .5, timeoutInSeconds=2.6))
self.addSequential(WaitCommand(3))
#drop box if needed
self.createDropBoxCommands()
#self.addSequential(LoaderToggle())
self.addSequential(LifterCommandTimed(0.3, timeoutInSeconds=1))
self.addSequential(LifterCommandTimed(0.27, timeoutInSeconds=2))
#self.addSequential(WaitCommand(1))
self.addParallel(MoveRobot(.5, -.5, timeoutInSeconds=1))
def __init__(self):
super().__init__("Autonomous Program")
self.addSequential(MoveRobot(0.5,0,0,timeoutInSeconds = 1))
self.addSequential(WaitCommand(timeoutInSeconds = 1))
self.addSequential(MoveRobot(-0.5,0,0,timeoutInSeconds = 1))
self.addSequential(WaitCommand(timeoutInSeconds = 1))
self.addSequential(MoveRobot(0.5,0,1,timeoutInSeconds = 1))
def __init__(self):
super().__init__('Autonomous Program')
self.addSequential(MoveRobot(.5, 0, 0, timeoutInSeconds=1))
self.addSequential(WaitCommand(timeout=1))
self.addSequential(MoveRobot(-.5, 0, 0, timeoutInSeconds=1))
self.addSequential(WaitCommand(timeout=1))
self.addSequential(MoveRobot(.5, 0, 1.0, timeoutInSeconds=1))
def __init__(self):
super().__init__('Auto Test')
self.addSequential(SetDistance(413))
self.addSequential(WaitCommand(timeout=1))
self.addSequential(SetGyroAngle(90))
self.addSequential(WaitCommand(timeout=1))
self.addSequential(SetDistance(717))
def __init__(self, mode):
super().__init__('Autonomous Program')
self.logger = logging.getLogger("robot")
if mode == "borkleft": # just cross the base line
self.addSequential(SetSpeed(0.2, 0.5))
self.addSequential(Rotate(-20))
self.addSequential(SetSpeed(0.2, 2.0))
elif mode == "borkright":
self.addSequential(SetSpeed(0.2, 0.5))
self.addSequential(Rotate(20))
self.addSequential(SetSpeed(0.2, 2.0))
elif mode == "left":
self.addSequential(SetSpeed(0.4, 0.3))
self.addSequential(Rotate(-30))
self.addSequential(SetSpeed(0.3, 0.7))
self.addSequential(Rotate(85))
self.addSequential(DriveToRod(timeout=4.0))
self.addSequential(FinishAutonomous("left"))
elif mode == "right":
self.addSequential(SetSpeed(0.4, 0.3))
self.addSequential(Rotate(30))
self.addSequential(SetSpeed(0.3, 0.7))
self.addSequential(Rotate(-85))
self.addSequential(DriveToRod(timeout=4.0))
self.addSequential(FinishAutonomous("right"))
elif mode == "boilerleft":
self.addSequential(SetSpeed(0.3, 0.35))
self.addSequential(ControlDumper(False))
self.addSequential(WaitCommand(2.0))
self.addParallel(ControlDumper(True))
self.addSequential(Rotate(45))
self.addSequential(SetSpeed(0.3, 0.3))
self.addSequential(Rotate(80))
self.addSequential(DriveToRod(timeout=4.0))
self.addSequential(FinishAutonomous("left"))
elif mode == "boilerright":
self.addSequential(SetSpeed(-0.3, 0.4))
self.addParallel(ControlDumper(False))
self.addSequential(WaitCommand(2.0))
self.addParallel(ControlDumper(True))
self.addSequential(Rotate(-45))
self.addSequential(SetSpeed(-0.3, 0.3))
self.addSequential(Rotate(140))
self.addSequential(DriveToRod(timeout=4.0))
self.addSequential(FinishAutonomous("right"))
else: # center mode
# self.addSequential(SetSpeed(0.3, 0.3))
self.logger.info("Drive to rod now!")
self.addSequential(DriveToRod(timeout=4.0))
self.addSequential(FinishAutonomous("center"))
def __init__(self, desiredPos):
super().__init__()
self.addSequential(MoveElevator(desiredPos))
self.addSequential(InhaleExhale(IntakeOutput.CargeGiveOrTake.GIVE))
self.addSequential(WaitCommand(1))
self.addSequential(InhaleExhale(IntakeOutput.CargeGiveOrTake.GIVE))
self.addSequential(MoveElevator(Constants.LOWEST_ENABLED_ELEVATOR_POS))
def __init__(self):
super().__init__(name=self.__class__.__name__)
self.addSequential(TakePanel(), 0.25)
self.addSequential(SetIntakeMode(IntakeOutput.IntakeInOutNeutral.IN))
self.addSequential(InhaleExhale(IntakeOutput.CargeGiveOrTake.TAKE))
self.addSequential(WaitCommand(0.25))
self.addSequential(PutPanel(), 0.25)
def __init__(self, robot):
super().__init__()
self.robot = robot
self.addSequential(WaitCommand(2.0))
self.addSequential(Close_Container(self.robot))
def __init__(self, robot):
super().__init__()
self.robot = robot
self.addSequential(Open_Gear_Claws(self.robot))
self.addSequential(WaitCommand(0.04))
self.addSequential(Close_Gear_Claws(self.robot))
def __init__(self):
super().__init__('RightToRight')
# Go to switch
self.addSequential(SetDistance(399.4))
self.addParallel(ClimbForTime(3))
self.addParallel(SuckForTime(12))
# Turn left
self.addSequential(WaitCommand(timeout=0.1))
self.addSequential(SetGyroAngle(-90))
# Drive right up to the switch
self.addSequential(WaitCommand(timeout=0.1))
self.addSequential(SetDistance(8.39))
# Drop off the cube
self.addSequential(RetractCubeForTime(3))
def __init__(self, desiredPos):
super().__init__()
self.addSequential(MoveElevator(desiredPos))
self.addSequential(PutPanel(), 0.4)
self.addSequential(MoveElevator(desiredPos))
self.addSequential(WaitCommand(0.35))
self.addSequential(TakePanel(), 0)
self.addSequential(MoveElevator(Constants.LOWEST_ENABLED_ELEVATOR_POS))
def __init__(self):
super().__init__('Autonomous Program')
self.addSequential(Move(robotmap.auto.initialDrive))
self.addSequential(Rotate(-60))
self.addSequential(Move(robotmap.auto.stageTwoDrive))
self.addSequential(OpenGear())
self.addSequential(WaitCommand(1))
self.addSequential(Move(robotmap.auto.stageThreeDrive))
def createDropBoxCommands(self):
#wait for robot to stablize
#opend loader
if (self.getRobot().ourSwitchSide == self.robotLocation):
print("Adding box drop command")
self.addSequential(LoaderToggle())
else:
print("Robot on wrong side. Not dropping box")
self.addSequential(WaitCommand(1.0))
def __init__(self, liftPos=-27, elevatorPos=-19.5):
super().__init__(name="OneClimbG {}: {}".format(liftPos, elevatorPos))
self.addSequential(RaiseLiftElevator(liftPos, elevatorPos), 3)
self.addSequential(LiftDrive(-1), 4)
self.addSequential(MoveElevator(7))
self.addSequential(ActivateArms(IntakeOutput.Position.DOWN))
self.addParallel(LiftDrive(-1), 3.5)
self.addSequential(Move(3.5), 0.3)
self.addSequential(WaitCommand(1.0))
self.addParallel(Move(1), 0.1)
self.addParallel(MoveLift(2.5))
self.addSequential(LiftDrive(-1), 1)
def __init__(self):
super().__init__('CenterToLeft')
# Go to bussing lane
self.addSequential(SetDistance(137.05))
self.addParallel(ClimbForTime(3))
self.addParallel(SuckForTime(12))
# Turn left
self.addSequential(WaitCommand(timeout=0.1))
self.addSequential(SetGyroAngle(-90))
# Set up for going to switch
self.addSequential(WaitCommand(timeout=0.1))
self.addSequential(SetDistance(309.34))
# Turn right
self.addSequential(WaitCommand(timeout=0.1))
self.addSequential(SetGyroAngle(90))
# Drive up to switch
self.addSequential(WaitCommand(timeout=0.1))
self.addSequential(SetDistance(262.35))
# Face the switch
self.addSequential(WaitCommand(timeout=0.1))
self.addSequential(SetGyroAngle(90))
# Drive right up to the switch
self.addSequential(WaitCommand(timeout=0.1))
self.addSequential(SetDistance(8.39))
# Drop off cube
self.addSequential(RetractCubeForTime(3))
def __init__(self, robot):
super().__init__()
self.robot = robot
self.requires(self.robot.gear_claw)
self.requires(self.robot.feeder)
self.addParallel(
commands.feeder.Set_Feeder_State(self.robot,
const.ID_FEEDER_DISENGAGED))
self.addSequential(Open_Gear_Claws(self.robot))
self.addSequential(WaitCommand(0.06))
self.addSequential(Open_Gear_Punch(self.robot))
def __init__(self, robot):
super().__init__()
self.robot = robot
#self.addSequential( Set_State_Tank( self.robot ), timeout=0.0 )
#self.addSequential( Rotate_To_Gear( self.robot ), timeout=5 )
#self.addSequential( Drive_Distance_To_Gear(robot, 4, 2.5, .60 ), 4.0)
#self.addSequential( Rotate_To_Gear_And_Forward( self.robot ), 3.0)
#self.addSequential( Drive_With_Tank_Values(self.robot, 0, 0, 0), .25 )
#self.addSequential( Drive_With_Tank_Values(self.robot, 0, -.7, 0), timeout=0.75 ) # usual timeout 0.75
#self.addSequential( Set_State_Tank( self.robot ), timeout=0.0 )
self.addSequential(Rotate_To_Gear(self.robot), timeout=5)
self.addSequential(Drive_Distance_To_Gear(self.robot, 5.0, 3.0, 0.65),
5.0)
self.addSequential(WaitCommand(0.25))
self.addSequential(Rotate_To_Gear(self.robot), timeout=5)
self.addSequential(WaitCommand(0.50))
self.addSequential(Drive_Distance_To_Gear(self.robot, 5.0, 0.0, 0.4),
3.0)
self.addSequential(WaitCommand(0.50))
self.addSequential(Drive_With_Tank_Values(self.robot, 0, -.7, 0),
timeout=0.75) # usual timeout 0.75
def __init__(self):
super().__init__("Autonomous Program")
self.addSequential(SetSpeed(power=0.7, timeoutInSeconds=2))
self.addSequential(WaitCommand(timeout=1))
self.addSequential(SetSpeed(power=-0.7, timeoutInSeconds=2))
def __init__(self):
super().__init__('No Auto')
print('[AUTO] NoAuto running')
self.addSequential(WaitCommand(timeout=15))
def __init__(self):
super().__init__()
self.addSequential(AutoDriveStraight(1300))
self.addSequential(WaitCommand(2.5))
self.addSequential(AutoDriveStraight(350))
if start_position == 'right' OR start_position == 'left':
self.addSequential(Move(robotmap.auto.initialDriveSide))
if start_position == 'right':
self.addSequential(Rotate(-60))
else:
self.addSequential(Rotate(60))
else:
self.addSequential(Move(robotmap.auto.initialDriveCenter))
'''Now add to the sequential command group the command to rotate
while moving. This moves the robot forward while images are taken
with the camera to determine a turning direction. The ccommand finishes
when the forward motion of the robot stops due to hitting an obstacle'''
self.addSequential(RotateWhileMoving())
'''If we have stopped then it's time to let go of the gear'''
self.addSequential(OpenGear())
self.addSequential(WaitCommand(1))
'''Now it's time to back away from the peg'''
self.addSequential(Move(robotmap.auto.stageThreeDrive))
def __init__(self):
super().__init__('Autonomous Program')
self.addSequential(SetSpeed(power=0.7, time=2))
self.addSequential(WaitCommand(timeout=1))
self.addSequential(SetSpeed(power=0.7, time=2))
def __init__(self):
super().__init__("Toggle")
self.addSequential(Toggle())
self.addSequential(WaitCommand(timeout=2))
self.addSequential(Toggle())
def __init__(self):
super().__init__('Autonomous Program')
self.addSequential(TankDriveTimed(1, .5, .5))
self.addSequential(WaitCommand(timeout=1))
self.addSequential(TankDriveTimed(1, -.5, .5))
