def initialize(self):
self.finished = False
# The boom is currently at the intake
if self.robot.boomState == BOOM_STATE.Intake:
# Double-check the pot to ensure the boom is at the switch
startPotError = abs(self.robot.boom.getPotPositionInDegrees() -
self.robot.boom.POT_INTAKE_POSITION_DEG)
if startPotError < self.robot.boom.POT_ERROR_LIMIT:
# Create the motion profile controller object
startingPostion = self.robot.boom.getPotPosition()
self.motionProfileController = MotionProfileController(self.robot.boom.talon,
self.intakeToScalePath,
False,
startingPostion,
1,
0)
# The start method will signal the motion profile controller to start
self.motionProfileController.start()
else:
logger.warning("Boom to Scale Command not started - StartPotError: %3.1f" %
(startPotError))
self.finished = True
# The boom is currently at the scale
elif self.robot.boomState == BOOM_STATE.Switch:
# Double-check the pot to ensure the boom is at the switch
startPotError = abs(self.robot.boom.getPotPositionInDegrees() -
self.robot.boom.POT_SWITCH_POSITION_DEG)
if startPotError < self.robot.boom.POT_ERROR_LIMIT:
# Create the motion profile controller object
startingPostion = self.robot.boom.getPotPosition()
self.motionProfileController = MotionProfileController(self.robot.boom.talon,
self.switchToScalePath,
False,
startingPostion,
0,
0)
# The start method will signal the motion profile controller to start
self.motionProfileController.start()
else:
logger.warning("Boom to Scale Command not started - StartPotError: %3.1f" %
(startPotError))
self.finished = True
# The boom will be at unknown position
else:
logger.warning("Boom to Switch Command not started - BoomState: %s" %
(self.robot.boomState))
self.finished = True
class BoomToScale(Command):
"""
This command will move the boom to the intake position.
"""
def __init__(self, robot):
super().__init__()
self.requires(robot.boom)
self.robot = robot
self.finished = True
# Read up the pickled path file
with open(
os.path.join(os.path.dirname(__file__),
'boom_switch_to_scale.pickle'), "rb") as fp:
self.switchToScalePath = pickle.load(fp)
with open(
os.path.join(os.path.dirname(__file__),
'boom_intake_to_scale.pickle'), "rb") as fp:
self.intakeToScalePath = pickle.load(fp)
def initialize(self):
self.finished = False
# The boom is currently at the intake
if self.robot.boomState == BOOM_STATE.Intake:
# Double-check the pot to ensure the boom is at the switch
startPotError = abs(self.robot.boom.getPotPositionInDegrees() -
self.robot.boom.POT_INTAKE_POSITION_DEG)
if startPotError < self.robot.boom.POT_ERROR_LIMIT:
# Create the motion profile controller object
startingPostion = self.robot.boom.getPotPosition()
self.motionProfileController = MotionProfileController(
self.robot.boom.talon, self.intakeToScalePath, False, 1, 0)
# The start method will signal the motion profile controller to start
self.motionProfileController.start()
else:
logger.warning(
"Boom to Scale Command not started - StartPotError: %3.1f"
% (startPotError))
self.finished = True
# The boom is currently at the scale
elif self.robot.boomState == BOOM_STATE.Switch:
# Double-check the pot to ensure the boom is at the switch
startPotError = abs(self.robot.boom.getPotPositionInDegrees() -
self.robot.boom.POT_SWITCH_POSITION_DEG)
if startPotError < self.robot.boom.POT_ERROR_LIMIT:
# Create the motion profile controller object
startingPostion = self.robot.boom.getPotPosition()
self.motionProfileController = MotionProfileController(
self.robot.boom.talon, self.switchToScalePath, False, 0, 0)
# The start method will signal the motion profile controller to start
self.motionProfileController.start()
else:
logger.warning(
"Boom to Scale Command not started - StartPotError: %3.1f"
% (startPotError))
self.finished = True
# The boom will be at unknown position
else:
logger.warning(
"Boom to Switch Command not started - BoomState: %s" %
(self.robot.boomState))
self.finished = True
def execute(self):
if not self.finished:
if self.motionProfileController.isFinished():
self.finished = True
else:
self.motionProfileController.control()
# Output debug data to the smartdashboard
if LOGGER_LEVEL == logging.DEBUG:
self.robot.smartDashboard.putNumber(
"EncPos",
self.robot.boom.talon.getSensorCollection().getAnalogInRaw(
))
self.robot.smartDashboard.putNumber(
"ActPos",
self.robot.boom.talon.getActiveTrajectoryPosition())
self.robot.smartDashboard.putNumber(
"EncVel", self.robot.boom.talon.getAnalogInVel())
self.robot.smartDashboard.putNumber(
"ActVel",
self.robot.boom.talon.getActiveTrajectoryVelocity())
self.robot.smartDashboard.putNumber(
"PrimaryTarget",
self.robot.boom.talon.getClosedLoopTarget(0))
self.robot.smartDashboard.putNumber(
"PrimaryError",
self.robot.boom.talon.getClosedLoopError(0))
self.robot.smartDashboard.putNumber("TimeStamp",
self.robot.timer.get())
def isFinished(self):
return self.finished
def end(self):
endPotError = (self.robot.boom.getPotPositionInDegrees() -
self.robot.boom.POT_SCALE_POSITION_DEG)
if abs(endPotError) < self.robot.boom.POT_ERROR_LIMIT:
self.robot.boomState = BOOM_STATE.Scale
else:
self.robot.boomState = BOOM_STATE.Unknown
logger.warning(
"Boom to Scale Command finish poorly - endPotError: %3.1f" %
(endPotError))
self.robot.boom.initOpenLoop()