def __init__(self):
super().__init__('Sequence')
#self.addSequential(DriveToDistance(1, 1))
#for i in range(4): self.addSequential(TurnDrive(90))
for i in range(4):
self.addSequential(DriveToDistance(1, 1))
self.addSequential(TurnDrive(90.0), 1.2)
def __init__(self, direction):
super().__init__("SameSide")
self.direction = direction
# import it here
from commands.auto import Direction
print("__init__ sameside")
#if _direction:
# Left
# direction = 1
#else:
# Right
# direction = -1
if direction == Direction.MIDDLE:
print(
"warning, passed Direction.MIDDLE to a side command, doing nothing"
)
return
# Drive Forward 10 ft
end_point = auto_measures.to_scale - inches(30)
start_turning_point = end_point - inches(46)
#Raise Arm if not raised
self.addSequential(AutoArmExtender(True))
self.addSequential(
DriveToDistance(start_turning_point, start_turning_point), 4.5)
# drive then turn absed on direction
print("DIRECTION: " + str(direction))
# Turn Slightly towards goal. NOTE: Look into raising arm before you get to the scale.
if direction == Direction.LEFT:
print('running same side left')
self.addSequential(DoNothing(waits.turn))
# turn towards the scale and then drive towards it
self.addSequential(TurnDrive(-auto_measures.angle_switch), 2.0)
#dist = end_point - start_turning_point, auto_measures.robot_starting_offset)
#dist = math.hypot(end_point - start_turning_point, auto_measures.robot_starting_offset)
self.addSequential(AutoArmExtender(True))
self.addSequential(DoNothing(3))
#self.addSequential(RunIntake(1.0), 1.0)
elif direction == Direction.RIGHT:
print('running same side right')
self.addSequential(DoNothing(waits.turn))
# turn towards the scale and then drive towards it
self.addSequential(TurnDrive(auto_measures.angle_switch), 2.0)
#dist = end_point - start_turning_point, auto_measures.robot_starting_offset)
#dist = math.hypot(end_point - start_turning_point, auto_measures.robot_starting_offset)
self.addSequential(AutoArmExtender(True))
self.addSequential(DoNothing(3))
def __init__(self, direction):
super().__init__("InvSameSide")
self.direction = direction
from commands.auto import Direction
if direction == Direction.MIDDLE:
print(
"warning, passed Direction.MIDDLE to a side command, doing nothing"
)
return
# Drive Forward 10 ft
end_point = auto_measures.to_switch
self.addSequential(DriveToDistance(end_point, end_point), 3.5)
# drive then turn absed on direction
# Turn Slightly towards goal. NOTE: Look into raising arm before you get to the scale.
if direction == Direction.LEFT:
self.addSequential(DoNothing(waits.turn))
self.addSequential(TurnDrive(90), 1.2)
elif direction == Direction.RIGHT:
self.addSequential(DoNothing(waits.turn))
self.addSequential(TurnDrive(-90), 1.2)
# self.addParallel(LiftToProportion(measures.ROBOT_ARM_SWITCH_DROP), 1.5)
self.addSequential(
DriveToDistance(
inches(20) + auto_measures.robot_starting_offset,
inches(20) + auto_measures.robot_starting_offset), 3.5)
def __init__(self, whereisgoal):
super().__init__("Middle")
from commands.auto import Direction
# Drive Forward to the Scale, pass the Auto Line
start_turning = auto_measures.robot_middle_turn_dist
end_point = auto_measures.to_switch
self.addSequential(DriveToDistance(start_turning, start_turning), 2.5)
turn_angle = 45
self.addSequential(DoNothing(waits.turn))
if whereisgoal == Direction.LEFT:
self.addSequential(TurnDrive(-turn_angle), 1.6)
elif whereisgoal == Direction.RIGHT:
self.addSequential(TurnDrive(turn_angle), 1.6)
# self.addParallel(LiftToProportion(measures.ROBOT_ARM_SWITCH_DROP), 1.5)
# should be (end_point - start_turning) * math.sec()
hypot_dist = math.hypot((end_point - start_turning), math.tan(turn_angle * math.pi / 180.0) * (end_point - start_turning))
self.addSequential(DriveToDistance(hypot_dist, hypot_dist), 2.4)
#self.addSequential(RunIntake(1.0), 1.0)
# Turn to the scale of based on the game data. TODO: Find the angle to turn to.
# self.addSequential(TurnDrive(45*direction), 2.0)
# Deliver the cube
# self.addSequential( [Insert Deliver Cube method here.] )
def generate_auto_program(self):
choice = self.chooser.getSelected()
data = None
try:
data = str(DriverStation.getInstance().getGameSpecificMessage())
except Exception as e:
print("!!! exception: " + str(e))
if data is not None and len(data) == 3:
if choice == "l":
self.autonomousProgram = commands.auto.get_left_command(data)
elif choice == "m":
self.autonomousProgram = commands.auto.get_middle_command(data)
elif choice == "r":
self.autonomousProgram = commands.auto.get_right_command(data)
else:
self.autonomousProgram = choice
else:
self.autonomousProgram = DriveToDistance(3.048, 3.048)
#self.autonomousProgram = DoNothing(15)
#self.autonomousProgram = wpilib.command.CommandGroup()
#self.autonomousProgram.addParallel(self.chooser.getSelected())
self.auto_data = data
def __init__(self, side1):
super().__init__('DriveTriangle')
for i in range(0, 4):
self.addSequential(DriveToDistance(side1, side1))
self.addSequential(TurnDrive(90))
def robotInit(self):
"""
WPILIB method on initialization
"""
# instansiate a getter method so you can do 'import robot;
# robot.get_robot()'
global get_robot
try:
wpilib.CameraServer.launch('cameraservant.py:main')
except:
print("Could not find module cscore")
get_robot = self.get_self
self.num_loops = 0
subsystems.init()
self.autonomousProgram = PulseMotor()
self.chooser = wpilib.SendableChooser()
#self.chooser.addDefault("SEQUENCE", Sequence())
self.chooser.addObject("Go 1 meter forward", DriveToDistance(1, 1))
self.chooser.addObject("Turn 90 Degrees Clockwise", TurnDrive(90))
# self.chooser.addObject("Turn Arm Horizontal", LiftToProportion(robotmap.measures.ROBOT_ARM_HORIZONTAL))
self.chooser.addObject("Do Nothing Auto", DoNothing(15))
self.chooser.addObject("Grabber(True)", Grabber(True))
self.chooser.addObject("Grabber(False)", Grabber(False))
#self.chooser.addObject("ParametricLine", ParametricDrive(lambda t: .1 * t, lambda t: .4 * t, 5))
# The Auto Line is 10 ft (~3.048 meters) from the start point. May have to be tweaked a bit.
self.chooser.addObject("Drive Forward", DriveToDistance(3.048, 3.048))
self.chooser.addObject("Switch Back (inplace)", SwitchBack_inplace())
self.chooser.addObject("COMP: Left", "l")
self.chooser.addObject("COMP: Middle", "m")
self.chooser.addObject("COMP: Right", "r")
self.chooser.addDefault("COMP: Minimal Auto",
DriveToDistance(3.048, 3.048))
self.chooser.addObject("COMP: Do Nothing", DoNothing(15))
self.auto_data = None
#self.chooser.addObject('PulseMotor', PulseMotor())
wpilib.SmartDashboard.putData('Autonomous Program', self.chooser)
self.teleopProgram = wpilib.command.CommandGroup()
self.teleopProgram.addParallel(PIDTankDriveJoystick())
#self.teleopProgram.addParallel(TankDriveJoystick())
# self.teleopProgram.addParallel(ArmExtender())
self.teleopProgram.addParallel(ArmRotate())
#self.teleopProgram.addParallel(NavXCommand())
#self.teleopProgram.addParallel(CorrectTip())
oi.init()
class The2018Thing(CommandBasedRobot):
def get_self(self):
"""
simple helper to get itself
"""
return self
def robotInit(self):
"""
WPILIB method on initialization
"""
# instansiate a getter method so you can do 'import robot;
# robot.get_robot()'
global get_robot
try:
wpilib.CameraServer.launch('cameraservant.py:main')
except:
print("Could not find module cscore")
get_robot = self.get_self
self.num_loops = 0
subsystems.init()
self.autonomousProgram = PulseMotor()
self.chooser = wpilib.SendableChooser()
#self.chooser.addDefault("SEQUENCE", Sequence())
self.chooser.addObject("Go 1 meter forward", DriveToDistance(1, 1))
self.chooser.addObject("Turn 90 Degrees Clockwise", TurnDrive(90))
# self.chooser.addObject("Turn Arm Horizontal", LiftToProportion(robotmap.measures.ROBOT_ARM_HORIZONTAL))
self.chooser.addObject("Do Nothing Auto", DoNothing(15))
self.chooser.addObject("Grabber(True)", Grabber(True))
self.chooser.addObject("Grabber(False)", Grabber(False))
#self.chooser.addObject("ParametricLine", ParametricDrive(lambda t: .1 * t, lambda t: .4 * t, 5))
# The Auto Line is 10 ft (~3.048 meters) from the start point. May have to be tweaked a bit.
self.chooser.addObject("Drive Forward", DriveToDistance(3.048, 3.048))
self.chooser.addObject("Switch Back (inplace)", SwitchBack_inplace())
self.chooser.addObject("COMP: Left", "l")
self.chooser.addObject("COMP: Middle", "m")
self.chooser.addObject("COMP: Right", "r")
self.chooser.addDefault("COMP: Minimal Auto",
DriveToDistance(3.048, 3.048))
self.chooser.addObject("COMP: Do Nothing", DoNothing(15))
self.auto_data = None
#self.chooser.addObject('PulseMotor', PulseMotor())
wpilib.SmartDashboard.putData('Autonomous Program', self.chooser)
self.teleopProgram = wpilib.command.CommandGroup()
self.teleopProgram.addParallel(PIDTankDriveJoystick())
#self.teleopProgram.addParallel(TankDriveJoystick())
# self.teleopProgram.addParallel(ArmExtender())
self.teleopProgram.addParallel(ArmRotate())
#self.teleopProgram.addParallel(NavXCommand())
#self.teleopProgram.addParallel(CorrectTip())
oi.init()
def generic_loop(self):
if self.num_loops % log_every_n_loops == 0:
subsystems.dump_info()
self.num_loops += 1
def generate_auto_program(self):
choice = self.chooser.getSelected()
data = None
try:
data = str(DriverStation.getInstance().getGameSpecificMessage())
except Exception as e:
print("!!! exception: " + str(e))
if data is not None and len(data) == 3:
if choice == "l":
self.autonomousProgram = commands.auto.get_left_command(data)
elif choice == "m":
self.autonomousProgram = commands.auto.get_middle_command(data)
elif choice == "r":
self.autonomousProgram = commands.auto.get_right_command(data)
else:
self.autonomousProgram = choice
else:
self.autonomousProgram = DriveToDistance(3.048, 3.048)
#self.autonomousProgram = DoNothing(15)
#self.autonomousProgram = wpilib.command.CommandGroup()
#self.autonomousProgram.addParallel(self.chooser.getSelected())
self.auto_data = data
def autonomousInit(self):
if self.autonomousProgram is not None:
self.autonomousProgram.cancel()
self.generate_auto_program()
if self.autonomousProgram is not None:
subsystems.smartdashboard.putString(
"auto_program_type",
type(self.autonomousProgram).__name__)
subsystems.smartdashboard.putString("auto_program",
str(self.autonomousProgram))
self.autonomousProgram.start()
def teleopInit(self):
self.teleopProgram.start()
def teleopPeriodic(self):
self.generic_loop()
super().teleopPeriodic()
def autonomousPeriodic(self):
self.generic_loop()
cur_data = str(DriverStation.getInstance().getGameSpecificMessage())
if cur_data != self.auto_data:
print("WARNING: Game data changed, changing autonomous program")
if self.autonomousProgram is not None:
self.autonomousProgram.cancel()
self.generate_auto_program()
if self.autonomousProgram is not None:
subsystems.smartdashboard.putString(
"auto_program_type",
type(self.autonomousProgram).__name__)
subsystems.smartdashboard.putString(
"auto_program", str(self.autonomousProgram))
self.autonomousProgram.start()
super().autonomousPeriodic()
def disabledPeriodic(self):
self.generic_loop()
super().disabledPeriodic()
def testPeriodic(self):
self.generic_loop()
super().testPeriodic()
def __init__(self, direction, whereisgoal=None):
super().__init__("SwitchBack")
if whereisgoal is None:
whereisgoal = direction
self.direction = direction
from commands.auto import Direction
if direction == Direction.MIDDLE:
start_turning = auto_measures.robot_middle_turn_dist + 0.15
end_point = auto_measures.to_switch
self.addSequential(DriveToDistance(start_turning, start_turning), 2.5)
turn_angle = 23
self.addSequential(DoNothing(waits.turn))
if whereisgoal == Direction.LEFT:
self.addSequential(TurnDrive(auto_measures.angle_switch), 2.5)
elif whereisgoal == Direction.RIGHT:
self.addSequential(TurnDrive(-(auto_measures.angle_switch)), 2.5)
#self.addParallel(LiftToProportion(measures.ROBOT_ARM_SWITCH_DROP), 1.5)
# should be (end_point - start_turning) * math.sec()
#hypot_dist = math.hypot((end_point - start_turning), math.tan(turn_angle * math.pi / 180.0) * (end_point - start_turning))
#self.addSequential(DriveToDistance(-hypot_dist, -hypot_dist), 2.6)
#self.addSequential(Grabber(True))
# self.addSequential(LiftToProportion(1.0), 5)
# self.addSequential(LiftToProportion(-1.0))
else:
# Drive Forward 10 ft
end_point = auto_measures.to_switch
self.addSequential(DriveToDistance(
end_point,
end_point
), 3.5)
# drive then turn absed on direction
# Turn Slightly towards goal. NOTE: Look into raising arm before you get to the scale.
if direction == Direction.LEFT:
print('running switchback left')
self.addSequential(DoNothing(waits.turn))
self.addSequential(TurnDrive(90), 1.2)
elif direction == Direction.RIGHT:
print('running switchback right')
self.addSequential(DoNothing(waits.turn))
self.addSequential(TurnDrive(-90), 1.2)
#self.addParallel(LiftToProportion(measures.ROBOT_ARM_SWITCH_DROP), 1.5)
dist = inches(5) + auto_measures.robot_starting_offset
self.addSequential(DriveToDistance(dist,dist), 2.5)
self.addSequential(AutoArmExtender(False))
def pass_green_tape():
return DriveToDistance(auto_measures.green_tape + 0.25,
auto_measures.green_tape + 0.25)
def __init__(self, direction):
super().__init__("SameSide")
self.direction = direction
# import it here
from commands.auto import Direction
print ("__init__ sameside")
#if _direction:
# Left
# direction = 1
#else:
# Right
# direction = -1
if direction == Direction.MIDDLE:
print ("warning, passed Direction.MIDDLE to a side command, doing nothing")
return
# Drive Forward 10 ft
end_point = auto_measures.to_scale - inches(30)
start_turning_point = end_point - inches(46)
self.addSequential(DriveToDistance(
start_turning_point,
start_turning_point
), 4.5)
# drive then turn absed on direction
print ("DIRECTION: " + str(direction))
# Turn Slightly towards goal. NOTE: Look into raising arm before you get to the scale.
if direction == Direction.LEFT:
self.addSequential(DoNothing(waits.turn))
# turn towards the scale and then drive towards it
self.addSequential(TurnDrive(auto_measures.angle_scale), 2.0)
#dist = end_point - start_turning_point, auto_measures.robot_starting_offset)
#dist = math.hypot(end_point - start_turning_point, auto_measures.robot_starting_offset)
dist = math.hypot(auto_measures.robot_starting_offset, (end_point - start_turning_point) * math.tan(auto_measures.angle_scale * math.pi / 180.0))
self.addSequential(DriveToDistance(dist, dist), 2.0)
#self.addSequential(DoNothing(.5))
# turn back to face it
self.addSequential(DoNothing(waits.turn))
self.addSequential(TurnDrive(-auto_measures.angle_scale), 2.0)
elif direction == Direction.RIGHT:
self.addSequential(DoNothing(waits.turn))
# turn towards the scale and then drive towards it
self.addSequential(TurnDrive(-auto_measures.angle_scale), 2.0)
#dist = end_point - start_turning_point, auto_measures.robot_starting_offset)
#dist = math.hypot(end_point - start_turning_point, auto_measures.robot_starting_offset)
dist = math.hypot(auto_measures.robot_starting_offset, (end_point - start_turning_point) * math.tan(auto_measures.angle_scale * math.pi / 180.0))
self.addSequential(DriveToDistance(dist, dist), 2.0)
#self.addSequential(DoNothing(.5))
# turn back to face it
self.addSequential(DoNothing(waits.turn))
self.addSequential(TurnDrive(auto_measures.angle_scale), 2.0)
# TODO: deliver first cube
# self.addSequential(LiftToProportion(measures.ROBOT_ARM_SCALE_DROP), 5.0)
#self.addSequential(DoNothing(1.2))
self.addSequential(AutoArmExtender(True))
self.addParallel(TankDriveTimed(.34, .34, .4))
self.addSequential(DoNothing(1.2))
self.addSequential(RunIntake(1.0), 1.0)