class Auto3():
CLAW_DEPLOY = 1
DRIVE_OFF_PLATFORM = 2
LIFT_DEPLOY = 3
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
def init(self):
self.logger.info("Initializing auto 3")
self.state = 0
self.timer.start()
def iterate(self):
if self.state == 0:
self.state = 1
elif self.state == 1:
self.state = 2
elif self.state == 2:
self.state = 3
else:
pass
class AutoSetLiftPosition(Command):
def __init__(self, robot, d):
super().__init__()
self.robot = robot
self.requires(robot.lift)
self.m_setpoint = d #inches
self.total_timer = Timer()
# Called just before this Command runs the first time
def initialize(self):
self.total_timer.reset()
self.total_timer.start()
self.robot.lift.setMotionMagicSetpoint(self.m_setpoint)
# Make this return true when this Command no longer needs to run execute()
def isFinished(self):
returnval = False
if self.total_timer.get() > 1.0:
if self.robot.lift.isMotionMagicNearTarget():
returnval = True
elif self.total_timer > 9.0:
returnval = True
return returnval
# Called when another command which requires one or more of the same
# subsystems is scheduled to run
def interrupted(self):
self.robot.lift.hold()
self.robot.debug.Stop()
class PreciseDelay(object):
"""
Used to synchronize a timing loop.
Usage:
delay = PreciseDelay(time_to_delay)
while something:
# do things here
delay.wait()
"""
def __init__(self, delay_period):
""" :param delay_period: The amount of time to do a delay """
self.timer = Timer()
self.delay_period = delay_period
self.timer.start()
def wait(self):
""" Waits until the delay period has passed """
# we must *always* yield here, so other things can run
Timer.delay(0.001)
while not self.timer.hasPeriodPassed(self.delay_period):
Timer.delay(0.001)
class AcquireCube(Command):
def __init__(self, drive_speed, timeout, drive: Drivetrain,
intake: Intake):
super().__init__("AcquireCube", timeout)
self.drive_speed = drive_speed
self.drive = drive
self.intake = intake
self.requires(intake)
self.requires(drive)
self.state = 0
self.timer = Timer()
def initialize(self):
self.state = 0
self.intake.set_arm_state(ArmState.DOWN)
self.intake.set_grab_state(GrabState.OUT)
self.timer.start()
def execute(self):
if self.state == 0:
self.drive.arcade_drive(0, 0)
if self.timer.get() > 0.2:
self.state = 1
elif self.state == 1:
self.intake.intake()
self.drive.arcade_drive(self.drive_speed, 0)
def isFinished(self):
return self.intake.has_acquired_cube() or self.isTimedOut()
def end(self):
self.intake.set_grab_state(GrabState.IN)
self.intake.set_arm_state(ArmState.UP)
self.intake.run_intake(0)
self.drive.arcade_drive(0, 0)
class MoveElevatorToInitialPosition(Command):
def __init__(self):
super().__init__('MoveElevatorToInitialPosition')
self._elevator = self.getRobot().elevator
self.requires(self._elevator)
self._logger = self.getRobot().logger
self._speed = -robotmap.ElevatorSpeed #Negated
self._smartDashboard = NetworkTables.getTable('SmartDashboard')
self._timer = Timer()
def initialize(self):
self._elevator.stop()
self._smartDashboard.putString("ElevatorPosition",
str(self._elevator.currentPosition()))
self._timer.reset()
self._timer.start()
def execute(self):
self._elevator.move(self._speed)
self._smartDashboard.putString("ElevatorPosition",
str(self._elevator.currentPosition()))
def isFinished(self):
if self._timer.get() > 1.0:
return True
else:
return False
def interrupted(self):
self.end()
def end(self):
self._elevator.stop()
class MoveLiftOneBoxHeight(Command):
def __init__(self, robot, d):
super().__init__()
self.robot = robot
self.requires(robot.lift)
self.total_timer = Timer()
self.direction = d
# Called just before this Command runs the first time
def initialize(self):
self.total_timer.reset()
self.total_timer.start()
# Called repeatedly when this Command is scheduled to run
def execute(self):
setpoint = self.robot.lift.getCurrentSetpoint()
if self.direction == Direction.UP:
setpoint = setpoint + robotmap.BOX_HEIGHT_INCHES
elif self.direction == Direction.DOWN:
setpoint = setpoint - robotmap.BOX_HEIGHT_INCHES
self.robot.lift.setMotionMagicSetpoint(setpoint)
# Make this return true when this Command no longer needs to run execute()
def isFinished(self):
return True
class TrajectoryTracker:
chassis: chassis.Chassis
BETA = 2.0
ZETA = 0.7
def __init__(self):
self.trajectories: trajectories.Trajectories = trajectories.Trajectories(
)
self.ramsete = RamseteController(self.BETA, self.ZETA)
self.desired_trajectory: Trajectory = None
self.timer = Timer()
self.is_tracking = False
def on_disable(self):
self.is_tracking = False
def setTrajectory(self, trajectory):
self.timer.start()
self.timer.reset()
self.desired_trajectory = trajectory
def track(self):
self.is_tracking = True
def stop(self):
self.is_tracking = False
def isFinished(self):
if self.desired_trajectory == None:
return True
return self.timer.get() >= self.desired_trajectory.totalTime()
def execute(self):
if self.isFinished():
self.is_tracking = False
if self.is_tracking:
cur_state = self.chassis.getPose()
desired_state = self.desired_trajectory.sample(self.timer.get())
desired_chassis = self.ramsete.calculate(cur_state, desired_state)
self.chassis.setChassisVelocity(desired_chassis.vx,
desired_chassis.vy,
desired_chassis.omega)
class MoveLiftMinHeight(Command):
def __init__(self, robot):
super().__init__()
self.robot = robot
self.requires(robot.lift)
self.total_timer = Timer()
# Called just before this Command runs the first time
def initialize(self):
self.robot.lift.setMotionMagicSetpoint(0)
self.total_timer.reset()
self.total_timer.start()
# Make this return true when this Command no longer needs to run execute()
def isFinished(self):
return self.robot.lift.isMotionMagicNearTarget(
) or self.total_timer.get() > 5.0
class MoveElevatorToPosition(Command):
def __init__(self, position):
super().__init__('MoveElevatorDown')
self._elevator = self.getRobot().elevator
self.requires(self._elevator)
self._logger = self.getRobot().logger
self._speed = robotmap.ElevatorSpeed
self._smartDashboard = NetworkTables.getTable('SmartDashboard')
self._targetPosition = position
self._timer = Timer()
def initialize(self):
self._elevator.stop()
self._timer.reset()
self._timer.start()
self._smartDashboard.putString("ElevatorPosition",
str(self._elevator.currentPosition()))
if self._elevator.currentPosition() > self._targetPosition.value:
self._speed = -self._speed
if self._targetPosition == ElevatorPosition.INITIAL_DEPLOY:
while not self._timer.hasPeriodPassed(robotmap.Deploy_Delay):
continue
def execute(self):
self._elevator.move(self._speed)
self._smartDashboard.putString("ElevatorPosition",
str(self._elevator.currentPosition()))
def isFinished(self):
if self._elevator.currentPosition() == self._targetPosition.value:
return True
elif self._elevator.atLimit(
ElevatorLimit.LOWER) or self._elevator.atLimit(
ElevatorLimit.UPPER):
return True
else:
return False
def interrupted(self):
self.end()
def end(self):
self._elevator.stop()
class FollowJoystick(Command):
"""
This command will read the joysticks' y-axes and uses tank drive.
"""
def __init__(self):
super().__init__('Follow Joystick')
self.requires(subsystems.motors)
self.logger = logging.getLogger("robot")
self.drive = RectifiedDrive(25, 0.05, squared_inputs=True)
self.sd = NetworkTables.getTable("SmartDashboard")
self.timer = Timer()
self.timer.start()
def execute(self):
if self.timer.hasPeriodPassed(0.05): # period of 0.05 seconds
# tank drive
# subsystems.motors.robot_drive.tankDrive(oi.joystick, robotmap.joystick.left_port, oi.joystick,
# robotmap.joystick.right_port, True)
# arcade drive
# subsystems.motors.robot_drive.arcadeDrive(oi.joystick)
# rectified arcade drive
power = -oi.joystick.getRawAxis(
robotmap.joystick.forwardAxis) * 0.75
angular_vel = oi.joystick.getRawAxis(
robotmap.joystick.steeringAxis)
if self.sd.containsKey("slowmode") and self.sd.getBoolean(
"slowmode"):
power *= 0.75
angular_vel *= 0.75
if abs(angular_vel) < 0.1: # tolerance
angular_vel = 0
self.drive.rectified_drive(power, angular_vel)
def end(self):
subsystems.motors.robot_drive.setLeftRightMotorOutputs(0, 0)
class ToggleIntakeCommand(Command):
def __init__(self, intake: Intake):
super().__init__()
self.last_arm_state = intake.get_arm_state()
self.new_arm_state = ArmState.DOWN
self.requires(intake)
self.intake = intake
self.arm_timer = Timer()
self.arm_timer_latch = False
def initialize(self):
self.last_arm_state = self.intake.get_arm_state()
self.new_arm_state = ArmState.DOWN if self.last_arm_state == ArmState.UP else ArmState.UP
if self.last_arm_state != ArmState.UP:
self.arm_timer.start()
self.arm_timer.reset()
self.arm_timer_latch = True
def execute(self):
intake = self.intake
intake.set_arm_state(self.new_arm_state)
self.last_arm_state = self.intake.get_arm_state()
if self.arm_timer.get() > 0.5:
self.arm_timer_latch = False
self.arm_timer.stop()
if self.arm_timer_latch:
intake.run_intake(0.1)
else:
intake.run_intake(0)
def isFinished(self):
return not self.arm_timer_latch
def end(self):
self.arm_timer.reset()
self.arm_timer.stop()
self.arm_timer_latch = False
self.intake.run_intake(0)
class AutoShootBox(Command):
def __init__(self, robot):
super().__init__()
self.robot = robot
self.requires(robot.claw)
self.total_timer = Timer()
# Called just before this Command runs the first time
def initialize(self):
self.total_timer.reset()
self.total_timer.start()
# Called repeatedly when this Command is scheduled to run
def execute(self):
self.robot.claw.setSpeed(robotmap.BOX_OUT_SPEED)
self.robot.claw.close()
# Make this return true when this Command no longer needs to run execute()
def isFinished(self):
return self.total_timer.get() > 0.25
class Expel(Command):
def __init__(self):
super().__init__('Expel')
self._scooper = self.getRobot().scooper
self.requires(self._scooper)
self._logger = self.getRobot().logger
self._timer = Timer()
def initialize(self):
self._scooper.stop()
self._timer.reset()
self._timer.start()
def execute(self):
self._scooper.expel()
def isFinished(self):
return True if self._timer.get() > 0.5 else False
def end(self):
self._scooper.stop()
class CMDJustDriveForward(Command):
def __init__(self, robot):
super().__init__()
self.requires(robot.drivetrain)
self.robot = robot
self.pMotionProfiler = PFLJustDriveForward(robot)
self.pTimer = Timer()
def initialize(self):
print("[CMDJustDriveForward] initialized")
self.robot.drivetrain.initMotionProfilingMode()
self.pTimer.reset()
self.pTimer.start()
def execute(self):
self.pMotionProfiler.control()
self.pMotionProfiler.periodicTask()
setOutput = self.pMotionProfiler.getSetValue()
self.robot.drivetrain.left_front_motor.set(WPI_TalonSRX.ControlMode.MotionProfile, setOutput)
self.robot.drivetrain.right_front_motor.set(WPI_TalonSRX.ControlMode.MotionProfile, setOutput)
self.pMotionProfiler.start()
def isFinished(self):
if self.pMotionProfiler.isFinished():
print("[CMDJustDriveForward] motion profile finished")
return True
return False
def end(self):
self.robot.drivetrain.resetDrive()
def interrupted(self):
self.end()
class MoveIntakeCommand(Command):
def __init__(self, intake: Intake, new_state: ArmState):
super().__init__("MoveIntakeCommand")
self.intake = intake
self.new_state = new_state
self.old_state = None
self.timer = Timer()
self.requires(intake)
def initialize(self):
self.old_state = self.intake.get_arm_state()
self.intake.set_arm_state(self.new_state)
self.timer.reset()
self.timer.start()
def isFinished(self):
return self.new_state == self.old_state or \
(self.new_state == ArmState.DOWN and self.timer.get() > 0.2) or \
(self.new_state == ArmState.UP and self.timer.get() > 0.5)
def end(self):
self.timer.stop()
class TurnByGyro(Command):
def __init__(self, robot, angle):
super().__init__()
self.requires(robot.drivetrain)
self.robot = robot
self.angle = angle
self.timer = Timer()
def initialize(self):
print("[TurnByGyro] initializing")
self.robot.drivetrain.resetGyro()
self.robot.drivetrain.turnController.setSetpoint(self.angle)
self.timer.reset()
self.timer.start()
def execute(self):
self.robot.drivetrain.turn()
def isFinished(self):
if self.timer.get() > 5.0:
print("[TurnByGyro] timed out")
return True
# if self.robot.drivetrain.reachedAngle(self.angle):
# print("[TurnByGyro] reached target")
# return True
return False
def end(self):
print("[TurnByGyro] ending")
self.robot.drivetrain.resetDrive()
def interrupted(self):
print("[ClimbByGyro] interrupted")
self.end()
class MyRobot(IterativeRobot):
def robotInit(self):
print('Starting gyro init...')
self.myGyro = ADIS16448_IMU(ADIS16448_IMU.IMUAxis.kZ, SPI.Port.kMXP, 4)
print('Gyro init completed.')
self.statusTimer = Timer()
self.statusTimer.start()
def robotPeriodic(self):
if (self.statusTimer.get() >= 0.25):
print('X:', str(round(self.myGyro.getGyroAngleX(), 5)).ljust(25), \
'Y:', str(round(self.myGyro.getGyroAngleY(), 5)).ljust(25), \
'Z:', str(round(self.myGyro.getGyroAngleZ(), 5)).ljust(25))
self.statusTimer.reset()
def teleopInit(self):
"""Executed at the start of teleop mode"""
pass
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
pass
class Auto1():
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
if not self.robot.isSimulation():
with open("/home/lvuser/traj", "rb") as fp:
self.trajectory = pickle.load(fp)
else:
with open("/home/ubuntu/traj", "rb") as fp:
self.trajectory = pickle.load(fp)
self.target_chooser = sendablechooser.SendableChooser()
self.target_chooser.setDefaultOption(TargetHeight.LOW.name,
TargetHeight.LOW)
self.target_chooser.addOption(TargetHeight.MIDDLE.name,
TargetHeight.MIDDLE)
self.target_chooser.addOption(TargetHeight.HIGH.name,
TargetHeight.HIGH)
self.left_right_chooser = sendablechooser.SendableChooser()
self.left_right_chooser.setDefaultOption(RightLeft.RIGHT.name,
RightLeft.RIGHT)
self.left_right_chooser.addOption(RightLeft.LEFT.name, RightLeft.LEFT)
self.hab_level_chooser = sendablechooser.SendableChooser()
self.hab_level_chooser.setDefaultOption(HabLevel.LEVEL1.name,
HabLevel.LEVEL1)
self.hab_level_chooser.addOption(HabLevel.LEVEL2.name, HabLevel.LEVEL2)
SmartDashboard.putData("TargetChooser", self.target_chooser)
SmartDashboard.putData("LeftRightChooser", self.left_right_chooser)
SmartDashboard.putData("HabLevelChooser", self.hab_level_chooser)
self.chosen_target = TargetHeight.LOW
self.left_right = RightLeft.RIGHT
self.hab_level = HabLevel.LEVEL1
def init(self):
self.logger.info("Initializing auto 1")
self.chosen_target = self.target_chooser.getSelected()
self.left_right = self.left_right_chooser.getSelected()
self.hab_level = self.hab_level_chooser.getSelected()
self.logger.info("<Auto1> " + self.hab_level.name + ", " +
self.left_right.name + ", " + self.chosen_target.name)
self.state = 0
self.timer.start()
def iterate(self):
SmartDashboard.putNumber("Auto1 State", self.state)
if self.state == 0:
self.robot.harpoon.deploy_harpoon()
self.timer.reset()
if self.hab_level == HabLevel.LEVEL2:
self.state = 1
else:
self.state = 2
elif self.state == 1:
self.robot.drive.drive_straight(0.4)
if self.timer.get() > 1.0:
self.state = 2
elif self.state == 2:
self.robot.drive.drive_straight(0.0)
self.robot.lift.deploy_lift()
self.state = 3
elif self.state == 3:
if self.robot.lift.current_state == LiftState.LIFT_DEPLOYED:
self.state = 4
elif self.state == 4:
self.robot.drive.follow_a_path(self.trajectory)
self.state = 5
elif self.state == 5:
if self.robot.drive.leftDone and self.robot.drive.rightDone:
self.state = 6
elif self.state == 6:
if self.chosen_target == TargetHeight.LOW:
self.robot.lift.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_LOW)
elif self.chosen_target == TargetHeight.MIDDLE:
self.robot.lift.go_to_preset(
LiftPreset.LIFT_PRESET_HATCH_MIDDLE)
if self.chosen_target == TargetHeight.HIGH:
self.robot.lift.go_to_preset(LiftPreset.LIFT_PRESET_HATCH_HIGH)
self.state = 7
elif self.state == 7:
if self.robot.lift.on_target:
self.state = 8
elif self.state == 8:
self.robot.harpoon.stow_harpoon()
self.state = 9
elif self.state == 9:
if self.robot.harpoon.current_state == HarpoonState.HARPOON_STOWED:
self.timer.reset()
self.state = 10
elif self.state == 10:
self.robot.drive.drive_straight(-0.4)
if self.timer.get() > 1.0:
self.state = 11
elif self.state == 11:
self.robot.drive.drive_straight(0.0)
self.state = 12
elif self.state == 12:
self.robot.lift.go_to_preset(LiftPreset.LIFT_PRESET_STOW)
self.state = 13
elif self.state == 13:
if self.robot.lift.on_target:
self.state = 14
elif self.state == 14:
pass
class Robot(TimedRobot):
def robotInit(self):
self.driver = DriverControl(0)
self.operator = OperatorControl(1)
self.dashboard = NetworkTables.getTable("SmartDashboard")
self.right1 = DriveMotor(3, True)
self.right2 = DriveMotor(4, True)
self.right = DriveMotorGroup([self.right1, self.right2])
self.left1 = DriveMotor(1, False)
self.left2 = DriveMotor(2, False)
self.left = DriveMotorGroup([self.left1, self.left2])
self.drive = DriveBase(self.right, self.left, self.driver)
self.driveMotors = [self.right1, self.right2, self.left1, self.left2]
self.shifter = Shifter(0, 1, self.driver)
self.indexer = Indexer(self.operator)
self.arm = Arm(self.operator)
self.lifter = Lifter(self.operator)
self.intake = Intake(self.operator)
def disabledInit(self):
pass
def autonomousInit(self):
self.autonomousTimer = Timer()
self.autonomousTimer.start()
def teleopInit(self):
pass
def testInit(self):
pass
def robotPeriodic(self):
pass
def disabledPeriodic(self):
pass
def autonomousPeriodic(self):
if (self.autonomousTimer.get() < 2):
self.drive.setRaw(.5, .5)
else:
self.drive.setRaw(0, 0)
def teleopPeriodic(self):
self.indexer.update()
self.shifter.update()
self.drive.update()
self.arm.update()
self.lifter.update()
self.intake.update()
def testPeriodic(self):
pass
class Pitchfork(TimedRobot):
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.scoringElementChooser = SendableChooser()
self.scoreScale = ScoreScale()
self.scoreSwitch = ScoreSwitch()
self.scoringElementChooser.addObject("Score Scale", self.scoreScale)
self.scoringElementChooser.addObject("Score Switch", self.scoreSwitch)
self.scoringElementChooser.addDefault("Score Scale", self.scoreScale)
self.smartDashboard.putData("Score Field Element",
self.scoringElementChooser)
self.crossFieldChooser = SendableChooser()
self.crossFieldEnable = CrossFieldEnable()
self.crossFieldDisable = CrossFieldDisable()
self.crossFieldChooser.addObject("Cross Field Enable",
self.crossFieldEnable)
self.crossFieldChooser.addObject("Cross Field Disable",
self.crossFieldDisable)
self.crossFieldChooser.addDefault("Cross Field Disable",
self.crossFieldDisable)
self.smartDashboard.putData("Cross Field Enable",
self.crossFieldChooser)
self.positionChooser = SendableChooser()
self.startingLeft = StartingLeft()
self.startingRight = StartingRight()
self.startingMiddle = StartingMiddle()
self.positionChooser.addObject("Start Left", self.startingLeft)
self.positionChooser.addObject("Start Right", self.startingRight)
self.positionChooser.addObject("Start Middle", self.startingMiddle)
self.positionChooser.addDefault("Start Middle", self.startingMiddle)
self.smartDashboard.putData("Starting Position", self.positionChooser)
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
self.autonForward = AutonForward(self)
self.autonMiddleStartLeftSwitch = AutonMiddleStartLeftSwitch(self)
self.autonLeftStartLeftScale = AutonLeftStartLeftScale(self)
# Output debug data to the smartdashboard
if LOGGER_LEVEL == logging.DEBUG:
#=======================================================================================
# self.smartDashboard.putNumber("RightEncPos", 0.0)
# self.smartDashboard.putNumber("RightActPos", 0.0)
# self.smartDashboard.putNumber("RightEncVel", 0.0)
# self.smartDashboard.putNumber("RightActVel", 0.0)
# self.smartDashboard.putNumber("RightPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("RightPrimaryError", 0.0)
# self.smartDashboard.putNumber("TimeStamp", 0.0)
# self.smartDashboard.putNumber("LeftEncPos", 0.0)
# self.smartDashboard.putNumber("LeftActPos", 0.0)
# self.smartDashboard.putNumber("LeftEncVel", 0.0)
# self.smartDashboard.putNumber("LeftActVel", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryError", 0.0)
# self.smartDashboard.putNumber("RightTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftBottomBufferCount", 0.0)
# self.smartDashboard.putNumber("RightBottomBufferCount", 0.0)
#=======================================================================================
self.smartDashboard.putNumber("EncPos", 0.0)
self.smartDashboard.putNumber("ActPos", 0.0)
self.smartDashboard.putNumber("EncVel", 0.0)
self.smartDashboard.putNumber("ActVel", 0.0)
self.smartDashboard.putNumber("PrimaryTarget", 0.0)
self.smartDashboard.putNumber("PrimaryError", 0.0)
self.smartDashboard.putNumber("TimeStamp", 0.0)
def disabledInit(self):
"""
Initialization code for disabled mode should go here. This method will be called each
time the robot enters disabled mode.
"""
self.timer.stop()
self.timer.reset()
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here. This method will be called every 20ms.
"""
if self.timer.running:
self.timer.stop()
self.timer.reset()
def robotPeriodic(self):
pass
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# Get the prioritized scoring element, robot starting posion, and the alliance
# scale/switch data.
self.selectedCrossFieldEnable = self.crossFieldChooser.getSelected()
self.selectedScoringElement = self.scoringElementChooser.getSelected()
self.selectedStartingPosition = self.positionChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
self.crossFieldEnable = self.selectedCrossFieldEnable.getCrossFieldEnable(
)
self.scoringElement = self.selectedScoringElement.getScoringElement()
self.startingPosition = self.selectedStartingPosition.getStartingPosition(
)
def autonomousPeriodic(self):
"""
Periodic code for autonomous mode should go here. This method will be called every 20ms.
"""
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
if self.scheduleAutonomous:
self.scheduleAutonomous = False
logger.info("Game Data: %s" % (self.gameData))
logger.info("Cross Field Enable: %s" % (self.crossFieldEnable))
logger.info("Scoring Element %s" % (self.scoringElement))
logger.info("Starting Position %s" % (self.startingPosition))
self.autonMiddleStartLeftSwitch.start()
#self.autonLeftStartLeftScale.start()
#self.autonForward.start()
Scheduler.getInstance().run()
def teleopInit(self):
"""
Initialization code for teleop mode should go here. This method will be called each time
the robot enters teleop mode.
"""
if not self.timer.running:
self.timer.start()
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
class Auto2():
def __init__(self, robot, logger):
self.logger = logger
self.robot = robot
self.timer = Timer()
self.target_chooser = sendablechooser.SendableChooser()
self.target_chooser.setDefaultOption(TargetHeight.LOW.name,
TargetHeight.LOW)
self.target_chooser.addOption(TargetHeight.MIDDLE.name,
TargetHeight.MIDDLE)
self.target_chooser.addOption(TargetHeight.HIGH.name,
TargetHeight.HIGH)
self.left_right_chooser = sendablechooser.SendableChooser()
self.left_right_chooser.setDefaultOption(RightLeft.RIGHT.name,
RightLeft.RIGHT)
self.left_right_chooser.addOption(RightLeft.LEFT.name, RightLeft.LEFT)
self.hab_level_chooser = sendablechooser.SendableChooser()
self.hab_level_chooser.setDefaultOption(HabLevel.LEVEL1.name,
HabLevel.LEVEL1)
self.hab_level_chooser.addOption(HabLevel.LEVEL2.name, HabLevel.LEVEL2)
SmartDashboard.putData("TargetChooser", self.target_chooser)
SmartDashboard.putData("LeftRightChooser", self.left_right_chooser)
SmartDashboard.putData("HabLevelChooser", self.hab_level_chooser)
self.chosen_target = TargetHeight.LOW
self.left_right = RightLeft.RIGHT
self.hab_level = HabLevel.LEVEL1
def init(self):
self.logger.info("Initializing auto 2")
self.chosen_target = self.target_chooser.getSelected()
self.left_right = self.left_right_chooser.getSelected()
self.hab_level = self.hab_level_chooser.getSelected()
self.logger.info("<Auto2> " + self.hab_level.name + ", " +
self.left_right.name + ", " + self.chosen_target.name)
self.state = 0
self.timer.start()
def iterate(self):
SmartDashboard.putNumber("Auto2 State", self.state)
if self.state == 0:
self.robot.harpoon.deploy_harpoon()
self.timer.reset()
if self.hab_level == HabLevel.LEVEL2:
self.state = 1
else:
self.state = 2
elif self.state == 1:
self.robot.drive.drive_straight(0.4)
if self.timer.get() > 1.0:
self.state = 2
elif self.state == 2:
self.robot.drive.drive_straight(0.0)
self.robot.lift.deploy_lift()
self.state = 3
elif self.state == 3:
if self.robot.lift.current_state == LiftState.LIFT_DEPLOYED:
self.state = 4
elif self.state == 4:
pass
class Intake(Component):
def __init__(self):
super().__init__()
self._l_motor = Talon(hardware.intake_left)
self._r_motor = Talon(hardware.intake_right)
self._intake_piston = Solenoid(hardware.intake_solenoid)
self._left_pwm = 0
self._right_pwm = 0
self._open = False
self._left_intake_amp = CircularBuffer(25)
self._right_intake_amp = CircularBuffer(25)
self._pulse_timer = Timer()
def update(self):
self._l_motor.set(self._left_pwm)
self._r_motor.set(-self._right_pwm)
self._intake_piston.set(not self._open)
self._left_intake_amp.append(hardware.left_intake_current())
self._right_intake_amp.append(hardware.right_intake_current())
def intake_tote(self):
if hardware.game_piece_in_intake(): # anti-bounce & slowdown
power = .8
else:
power = .3 if not hardware.back_photosensor.get() else 1
self.set(power)
if self.intake_jammed() and not self._pulse_timer.running:
self._pulse_timer.start()
if self._pulse_timer.running:
self.set(-self._pulse_timer.get() / 2)
if self._pulse_timer.get() > .05:
self._pulse_timer.stop()
self._pulse_timer.reset()
def intake_bin(self):
power = .3 if not hardware.back_photosensor.get() else .65
self.set(power, power)
def pause(self):
self.set(0)
def open(self):
self._open = True
def close(self):
self._open = False
def stop(self):
self._l_motor.set(0)
self._r_motor.set(0)
def set(self, l, r=None):
""" Spins the intakes at a given power. Pass either a power or left and right power. """
self._left_pwm = l
if r is None:
self._right_pwm = l
else:
self._right_pwm = r
def intake_jammed(self):
if not hardware.back_photosensor.get():
return False
return self._left_intake_amp.average > AMP_THRESHOLD or self._right_intake_amp.average > AMP_THRESHOLD
def update_nt(self):
log.info("left current: %s" % self._left_intake_amp.average)
log.info("right current: %s" % self._right_intake_amp.average)
class ProfiledForward(wpilib.command.Command):
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_ft = distance_ft
self.dist_enc = distance_ft * self.drivetrain.ratio
self.timer = Timer()
self.period = self.getRobot().getPeriod()
self.ctrl_heading = PIDController(
Kp=0,
Ki=0,
Kd=0,
Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity_fps = 3 # ft/sec
self.max_v_encps = self.drivetrain.fps_to_encp100ms(
self.max_velocity_fps)
self.max_acceleration = 3 # ft/sec^2
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=-self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.ctrl_l = DriveController(
Kp=0,
Kd=0,
Ks=1.293985,
Kv=0.014172,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_v_encps, self.max_v_encps)
self.ctrl_r = DriveController(
Kp=0,
Kd=0,
Ks=1.320812,
Kv=0.013736,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_v_encps, self.max_v_encps)
self.target_v_l = 0
self.target_v_r = 0
self.target_a_l = 0
self.target_a_r = 0
self.pos_ft_l = 0
self.pos_ft_r = 0
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
self.logger = DataLogger("profiled_forward.csv")
self.drivetrain.init_logger(self.logger)
self.logger.add("profile_vel_r", lambda: self.target_v_r)
self.logger.add("profile_vel_l", lambda: self.target_v_l)
self.logger.add("pos_ft_l", lambda: self.pos_ft_l)
self.logger.add("target_pos_l", lambda: self.profiler_l.target_pos)
self.logger.add("adist_l", lambda: self.profiler_l.adist)
self.logger.add("err_l", lambda: self.profiler_l.err)
self.logger.add("choice_l", lambda: self.profiler_l.choice)
self.logger.add("adist_r", lambda: self.profiler_l.adist)
self.logger.add("err_r", lambda: self.profiler_l.err)
self.logger.add("choice_r", lambda: self.profiler_l.choice)
self.timer.start()
#print ('pdf init')
def execute(self):
self.pos_ft_l = self.drivetrain.getLeftEncoder(
) / self.drivetrain.ratio
self.pos_ft_r = self.drivetrain.getRightEncoder(
) / self.drivetrain.ratio
#print ('pdf exec ', self.timer.get())
self.profiler_l.calculate_new_velocity(self.pos_ft_l, self.period)
self.profiler_r.calculate_new_velocity(self.pos_ft_r, self.period)
self.target_v_l = self.drivetrain.fps_to_encp100ms(
self.profiler_l.current_target_v)
self.target_v_r = self.drivetrain.fps_to_encp100ms(
self.profiler_r.current_target_v)
self.target_a_l = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_l.current_a)
self.target_a_r = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_r.current_a)
self.ctrl_l.setSetpoint(self.target_v_l)
self.ctrl_l.setAccelerationSetpoint(self.target_a_l)
self.ctrl_r.setSetpoint(self.target_v_r)
self.ctrl_r.setAccelerationSetpoint(self.target_a_r)
#self.drivetrain.setLeftMotor(self.ctrl_l.calculateFeedForward())
#self.drivetrain.setRightMotor(self.ctrl_r.calculateFeedForward())
self.logger.log()
self.drivetrain.feed()
def isFinished(self):
return (abs(self.pos_ft_l - self.dist_ft) < 1 / .3
and self.profiler_l.current_target_v == 0
and self.profiler_l.current_a == 0
and self.profiler_r.current_target_v == 0
and self.profiler_r.current_a == 0)
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
self.logger.flush()
#print ('pdf end')
def correct_heading(self, correction):
self.profiler_l.setCruiseVelocityScale(1 + correction)
self.profiler_r.setCruiseVelocityScale(1 - correction)
class Pitchfork(TimedRobot):
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.startSpotChooser = SendableChooser()
self.startSpotChooser.addObject("Start Left", 'Left')
self.startSpotChooser.addObject("Start Right", 'Right')
self.startSpotChooser.addDefault("Start Middle", 'Middle')
self.smartDashboard.putData("Starting Spot", self.startSpotChooser)
self.scaleDisableChooser = SendableChooser()
self.scaleDisableChooser.addObject("Enable Scale", 'Scale')
self.scaleDisableChooser.addDefault("Disable Scale", 'No Scale')
self.smartDashboard.putData("Scale Enable", self.scaleDisableChooser)
# Build up the autonomous dictionary. Fist key is the starting position. The second key is the switch. The third key is the scale.
self.chooserOptions = {"Left": {"R": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
"L": {"R": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonLeftStartLeftSwitch},
"Scale": {'command': AutonLeftStartLeftSwitch}
},
},
},
"Middle": {"R": {"R": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartRightSwitch},
"Scale": {'command': AutonMiddleStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
"L": {"No Scale": {'command': AutonMiddleStartLeftSwitch},
"Scale": {'command': AutonMiddleStartLeftSwitch}
},
},
},
"Right": {"R": {"R": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
"L": {"No Scale": {'command': AutonRightStartRightSwitch},
"Scale": {'command': AutonRightStartRightSwitch}
},
},
"L": {"R": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
"L": {"No Scale": {'command': AutonForward},
"Scale": {'command': AutonForward}
},
},
},
}
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
#===========================================================================================
# if LOGGER_LEVEL == logging.INFO:
# self.smartDashboard.putNumber("rVelocity",
# self.driveTrain.getRightVelocity())
# self.smartDashboard.putNumber("lVelocity",
# self.driveTrain.getLeftVelocity())
# self.smartDashboard.putNumber("rVoltage",
# self.driveTrain.getRightVoltage())
# self.smartDashboard.putNumber("lVoltage",
# self.driveTrain.getLeftVoltage())
# self.smartDashboard.putNumber("TimeStamp",
# self.timer.get())
#===========================================================================================
def disabledInit(self):
"""
Initialization code for disabled mode should go here. This method will be called each
time the robot enters disabled mode.
"""
self.timer.stop()
self.timer.reset()
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here. This method will be called every 20ms.
"""
if self.timer.running:
self.timer.stop()
self.timer.reset()
def robotPeriodic(self):
pass
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
self.startingPosition = self.startSpotChooser.getSelected()
self.scaleDisable = self.scaleDisableChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
logger.info("Game Data: %s" % (self.gameData))
logger.info("Starting Position %s" % (self.startingPosition))
logger.info("Scale Enable %s" % (self.scaleDisable))
self.autonCommand = self.chooserOptions[self.startingPosition][self.gameData[0]][self.gameData[1]][self.scaleDisable]['command'](self)
self.autonCommand.start()
def autonomousPeriodic(self):
"""
Periodic code for autonomous mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
def teleopInit(self):
"""
Initialization code for teleop mode should go here. This method will be called each time
the robot enters teleop mode.
"""
if not self.timer.running:
self.timer.start()
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
class ADXRS453Z(object):
def __init__(self, port):
spi = SPI(port)
spi.setClockRate(4000000) # 4 MHz (rRIO max, gyro can go high)
spi.setClockActiveHigh()
spi.setChipSelectActiveLow()
spi.setMSBFirst()
self._spi = spi
self._command = [0x00] * DATA_SIZE
self._data = [0x00] * DATA_SIZE
self._command[0] = READ_COMMAND
self._accumulated_angle = 0.0
self._current_rate = 0.0
self._accumulated_offset = 0.0
self._rate_offset = 0.0
self._last_time = 0
self._current_time = 0
self._calibration_timer = Timer()
self._calibration_timer.start()
self._update_timer = Timer()
self._update_timer.start()
#
# self._update_thread = Thread(self.update, daemon=True)
# self._update_thread.start()
self.update()
def update(self):
# Check parity
num_bits = sum([bits(c) for c in self._command])
if num_bits % 2 == 0:
self._command[3] |= PARITY_BIT
self._data = self._spi.transaction(self._command)
if self._calibration_timer.get() < WARM_UP_PERIOD:
self._last_time = self._current_time = self._update_timer.get()
return
elif self._calibration_timer.get() < CALIBRATE_PERIOD:
self.calibrate()
else:
self.update_data()
def update_data(self):
sensor_data = self._assemble_sensor_data(self._data)
rate = sensor_data / 80.0
self._current_rate = rate
self._current_rate -= self._rate_offset
self._current_time = self._update_timer.get()
dt = self._current_time - self._last_time
self._accumulated_offset += rate * dt
self._accumulated_angle += self._current_rate * dt
self._last_time = self._current_time
def calibrate(self):
sensor_data = self._assemble_sensor_data(self._data)
rate = sensor_data / 80.0
self._current_rate = rate
self._current_rate -= self._rate_offset
self._current_time = self._update_timer.get()
dt = self._current_time - self._last_time
self._accumulated_offset += rate * dt
self._rate_offset = self._accumulated_offset / (
self._calibration_timer.get() - WARM_UP_PERIOD)
self._last_time = self._current_time
@staticmethod
def _assemble_sensor_data(data):
#cast to short to make space for shifts
#the 16 bits from the gyro are a 2's complement short
#so we just cast it too a C++ short
#the data is split across the output like this (MSB first): (D = data bit, X = not data)
# X X X X X X D D | D D D D D D D D | D D D D D D X X | X X X X X X X X X
return (data[0] & FIRST_BYTE_DATA) << 14 | (data[1]) << 6 | (
data[2] & THIRD_BYTE_DATA) >> 2
def getRate(self):
return self._current_rate
def getAngle(self):
return self._accumulated_angle
def getOffset(self):
return self._accumulated_offset
def reset(self):
self._data = [0x00] * 4
self._current_rate = 0
self._accumulated_angle = 0
self._rate_offset = 0
self._accumulated_offset = 0
self._calibration_timer.reset()
self._update_timer.reset()
class Tachyon(SampleRobot):
# because robotInit is called straight from __init__
def robotInit(self):
hardware.init() # this makes everything not break
self.drive = drive.Drive()
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer(
) # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous',
self.components)
self.state = States.STACKING
def autonomous(self):
self.autonomous_modes.run(CONTROL_LOOP_WAIT_TIME,
iter_fn=self.update_all)
Timer.delay(CONTROL_LOOP_WAIT_TIME)
def update_all(self):
self.update()
def disabled(self):
while self.isDisabled():
Timer.delay(0.01)
def operatorControl(self):
precise_delay = delay.PreciseDelay(CONTROL_LOOP_WAIT_TIME)
while self.isOperatorControl() and self.isEnabled():
# States!
if hardware.driver.left_trigger():
self.state = States.DROPPING
elif hardware.operator.right_trigger():
self.state = States.CAPPING
else:
self.state = States.STACKING
if self.elevator.is_full():
self.intake.pause()
elif not self.elevator.has_bin and not self.elevator.tote_first:
self.intake.intake_bin()
else:
self.intake.intake_tote()
if hardware.driver.right_bumper():
self.intake.open()
else:
self.intake.close()
if self.state == States.STACKING:
if hardware.operator.left_bumper() or True:
self.elevator.stack_tote_first()
if hardware.driver.a():
self.elevator.manual_stack()
elif self.state == States.DROPPING:
self.elevator.drop_stack()
self.elevator.reset_stack()
self.intake.pause()
self.intake.open()
if hardware.driver.right_bumper():
self.intake.close()
elif self.state == States.CAPPING:
self.elevator.cap()
wheel = deadband(hardware.driver.right_x(), .2)
throttle = -deadband(hardware.driver.left_y(), .2)
quickturn = hardware.driver.left_bumper()
low_gear = hardware.driver.right_trigger()
self.drive.cheesy_drive(wheel, throttle, quickturn, low_gear)
driver_dpad = hardware.driver.dpad()
if driver_dpad == 180: # down on the dpad
self.drive.set_distance_goal(-2)
elif driver_dpad == 0:
self.drive.set_distance_goal(2)
elif driver_dpad == 90:
self.drive.set_distance_goal(-18)
operator_dpad = hardware.operator.dpad(
) # You can only call it once per loop, bcus dbouncing
if operator_dpad == 0 and self.elevator.tote_count < 6:
self.elevator.tote_count += 1
elif operator_dpad == 180 and self.elevator.tote_count > 0:
self.elevator.tote_count -= 1
elif operator_dpad == 90:
self.elevator.has_bin = not self.elevator.has_bin
if hardware.operator.start():
self.elevator.reset_stack()
if hardware.operator.b():
self.intake.set(0) # Pause?!
if hardware.operator.a() or hardware.driver.b():
self.intake.set(-1)
# Deadman switch. very important for safety (at competitions).
if not self.ds.isFMSAttached(
) and not hardware.operator.left_trigger(
) and False: # TODO re-enable at competitions
for component in self.components.values():
component.stop()
else:
self.update()
precise_delay.wait()
def test(self):
while self.isTest() and self.isEnabled():
LiveWindow.run()
for component in self.components.values():
component.stop()
if self.nt_timer.hasPeriodPassed(.5):
component.update_nt()
def update(self):
""" Calls the update functions for every component """
for component in self.components.values():
try:
component.update()
if self.nt_timer.hasPeriodPassed(.5):
component.update_nt()
except Exception as e:
if self.ds.isFMSAttached():
log.error("In subsystem %s: %s" % (component, e))
else:
raise
class Tachyon(SampleRobot):
# noinspection PyAttributeOutsideInit
# because robotInit is called straight from __init__
def robotInit(self):
self.chandler = XboxController(0)
self.meet = XboxController(1)
self.drive = drive.Drive() # So redundant omg
self.pneumatics = pneumatics.Pneumatics()
self.intake = intake.Intake()
self.elevator = elevator.Elevator()
self.components = {
'drive': self.drive,
'pneumatics': self.pneumatics,
'intake': self.intake,
'elevator': self.elevator,
}
self.nt_timer = Timer() # timer for SmartDashboard update so we don't use all our bandwidth
self.nt_timer.start()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
quickdebug.add_printables(self, ('match_time', Timer.getMatchTime))
quickdebug.init()
def autonomous(self):
self.autonomous_modes.run(CONTROL_LOOP_WAIT_TIME, iter_fn=self.update_all)
Timer.delay(CONTROL_LOOP_WAIT_TIME)
def update_all(self):
self.update()
self.update_networktables()
def disabled(self):
while self.isDisabled():
self.update_networktables()
Timer.delay(0.01)
def operatorControl(self):
precise_delay = delay.PreciseDelay(CONTROL_LOOP_WAIT_TIME)
while self.isOperatorControl() and self.isEnabled():
if self.meet.left_bumper() or self.elevator.has_bin:
self.elevator.stack_tote_first()
if self.elevator.full():
self.intake.spin(0)
else:
self.intake.intake_tote()
else:
self.intake.intake_bin()
self.elevator.force_stack = self.chandler.a()
if self.chandler.right_bumper():
self.intake.open()
else:
self.intake.close()
if self.chandler.left_trigger(): # If we're trying to drop the stack
self.intake.spin(0)
self.intake.open()
self.elevator.drop_stack()
wheel = deadband(self.chandler.right_x(), .2)
throttle = -deadband(self.chandler.left_y(), .2) * .8
if self.chandler.right_trigger():
wheel *= 0.3
throttle *= 0.3
self.drive.cheesy_drive(wheel, throttle, self.chandler.left_bumper())
self.drive.auto_drive() # encoders n shit
ticks = self.chandler.dpad()
if ticks == 180: # dowdn on the dpad
self.drive.set_distance_goal(-2)
elif ticks == 0:
self.drive.set_distance_goal(2)
elif ticks == 90:
self.drive.set_distance_goal(-18)
dpad = self.meet.dpad() # You can only call it once per loop, bcus dbouncing
if dpad == 0 and self.elevator.tote_count < 6:
self.elevator.add_tote()
elif dpad == 180 and self.elevator.tote_count > 0:
self.elevator.remove_tote()
elif dpad == 90:
self.elevator.set_bin(not self.elevator.has_bin)
if self.meet.start():
self.elevator._new_stack = True
if self.meet.b():
self.intake.spin(0)
if self.meet.a() or self.chandler.b():
self.intake.spin(-1)
self.elevator.auto_stacking = not self.meet.right_bumper() # Disable automatic stacking if bumper pressed
# Deadman's switch! very important for safety.
if not self.ds.isFMSAttached() and not self.meet.left_trigger():
for component in self.components.values():
component.stop()
else:
self.update()
self.update_networktables()
precise_delay.wait()
def test(self):
for component in self.components.values():
component.stop()
while self.isTest() and self.isEnabled():
LiveWindow.run()
self.update_networktables()
def update(self):
""" Calls the update functions for every component """
for component in self.components.values():
try:
component.update()
except Exception as e:
if self.ds.isFMSAttached():
log.error("In subsystem %s: %s" % (component, e))
else:
raise e
def update_networktables(self):
if not self.nt_timer.hasPeriodPassed(0.2): # we don't need to update every cycle
return
quickdebug.sync()
class PIDController(SendableBase):
"""Can be used to control devices via a PID Control Loop.
Creates a separate thread which reads the given :class:`.PIDSource` and takes
care of the integral calculations, as well as writing the given
:class:`.PIDOutput`.
This feedback controller runs in discrete time, so time deltas are not used
in the integral and derivative calculations. Therefore, the sample rate affects
the controller's behavior for a given set of PID constants.
"""
kDefaultPeriod = .05
instances = 0
PIDSourceType = PIDSource.PIDSourceType
# Tolerance is the type of tolerance used to specify if the PID controller
# is on target. The various implementations of this such as
# PercentageTolerance and AbsoluteTolerance specify types of tolerance
# specifications to use.
def PercentageTolerance_onTarget(self, percentage):
return abs(self.getError()) < percentage / 100.0 * self.inputRange
def AbsoluteTolerance_onTarget(self, value):
return abs(self.getError()) < value
def __init__(self, Kp, Ki, Kd, *args, **kwargs):
"""Allocate a PID object with the given constants for P, I, D, and F
Arguments can be structured as follows:
- Kp, Ki, Kd, Kf, source, output, period
- Kp, Ki, Kd, source, output, period
- Kp, Ki, Kd, source, output
- Kp, Ki, Kd, Kf, source, output
:param Kp: the proportional coefficient
:type Kp: float or int
:param Ki: the integral coefficient
:type Ki: float or int
:param Kd: the derivative coefficient
:type Kd: float or int
:param Kf: the feed forward term
:type Kf: float or int
:param source: Called to get values
:type source: A function, or an object that implements :class:`.PIDSource`
:param output: Receives the output percentage
:type output: A function, or an object that implements :class:`.PIDOutput`
:param period: the loop time for doing calculations. This particularly
effects calculations of the integral and differential terms.
The default is 50ms.
:type period: float or int
"""
super().__init__(False)
f_arg = ("Kf", [float, int])
source_arg = ("source",
[HasAttribute("pidGet"),
HasAttribute("__call__")])
output_arg = ("output",
[HasAttribute("pidWrite"),
HasAttribute("__call__")])
period_arg = ("period", [float, int])
templates = [[f_arg, source_arg, output_arg, period_arg],
[source_arg, output_arg, period_arg],
[source_arg, output_arg], [f_arg, source_arg, output_arg]]
_, results = match_arglist('PIDController.__init__', args, kwargs,
templates)
self.P = Kp # factor for "proportional" control
self.I = Ki # factor for "integral" control
self.D = Kd # factor for "derivative" control
self.F = results.pop("Kf", 0.0) # factor for feedforward term
self.pidOutput = results.pop("output")
self.origSource = results.pop("source")
self.period = results.pop("period", self.kDefaultPeriod)
self.filter = LinearDigitalFilter.movingAverage(self.origSource, 1)
self.pidInput = self.filter
self.pidInput = PIDSource.from_obj_or_callable(self.pidInput)
if hasattr(self.pidOutput, 'pidWrite'):
self.pidOutput = self.pidOutput.pidWrite
self.maximumOutput = 1.0 # |maximum output|
self.minimumOutput = -1.0 # |minimum output|
self.maximumInput = 0.0 # maximum input - limit setpoint to this
self.minimumInput = 0.0 # minimum input - limit setpoint to this
self.inputRange = 0.0 # input range - difference between maximum and minimum
self.continuous = False # do the endpoints wrap around? eg. Absolute encoder
self.enabled = False # is the pid controller enabled
self.prevError = 0.0 # the prior error (used to compute velocity)
self.totalError = 0.0 #the sum of the errors for use in the integral calc
self.setpoint = 0.0
self.prevSetpoint = 0.0
self.error = 0.0
self.result = 0.0
self.mutex = threading.RLock()
self.pidWriteMutex = threading.RLock()
self.pid_task = Notifier(self._calculate)
self.pid_task.startPeriodic(self.period)
self.setpointTimer = Timer()
self.setpointTimer.start()
# Need this to free on unit test wpilib reset
Resource._add_global_resource(self)
PIDController.instances += 1
hal.report(hal.UsageReporting.kResourceType_PIDController,
PIDController.instances)
self.setName("PIDController", PIDController.instances)
def free(self):
"""Free the PID object"""
super().free()
# TODO: is this useful in Python? Should make TableListener weakref.
self.pid_task.stop()
with self.mutex:
self.pidInput = None
self.pidOutput = None
def _calculate(self):
"""Read the input, calculate the output accordingly, and write to the
output. This should only be called by the PIDTask and is created
during initialization."""
if self.origSource is None or self.pidOutput is None:
return
with self.mutex:
enabled = self.enabled # take snapshot of these values...
if enabled:
feedForward = self.calculateFeedForward()
with self.mutex:
input = self.pidInput.pidGet()
pidSourceType = self.pidInput.getPIDSourceType()
P = self.P
I = self.I
D = self.D
minimumOutput = self.minimumOutput
maximumOutput = self.maximumOutput
prevError = self.prevError
error = self.getContinuousError(self.setpoint - input)
totalError = self.totalError
# start
if pidSourceType == self.PIDSourceType.kRate:
if P != 0:
totalError = self.clamp(totalError + error,
minimumOutput / P,
maximumOutput / P)
result = P * totalError + D * error + feedForward
else:
if I != 0:
totalError = self.clamp(totalError + error,
minimumOutput / I,
maximumOutput / I)
result = P * error + I * totalError + D * (error - prevError) + \
feedForward
result = self.clamp(result, minimumOutput, maximumOutput)
with self.pidWriteMutex:
with self.mutex:
enabled = self.enabled
if enabled:
self.pidOutput(result)
with self.mutex:
self.prevError = error
self.error = error
self.totalError = totalError
self.result = result
def calculateFeedForward(self):
"""Calculate the feed forward term
Both of the provided feed forward calculations are velocity feed forwards.
If a different feed forward calculation is desired, the user can override
this function and provide his or her own. This function does no
synchronization because the PIDController class only calls it in
synchronized code, so be careful if calling it oneself.
If a velocity PID controller is being used, the F term should be set to 1
over the maximum setpoint for the output. If a position PID controller is
being used, the F term should be set to 1 over the maximum speed for the
output measured in setpoint units per this controller's update period (see
the default period in this class's constructor).
"""
if self.pidInput.getPIDSourceType() == self.PIDSourceType.kRate:
return self.F * self.getSetpoint()
else:
temp = self.F * self.getDeltaSetpoint()
self.prevSetpoint = self.setpoint
self.setpointTimer.reset()
return temp
def setPID(self, p, i, d, f=0.0):
"""Set the PID Controller gain parameters.
Set the proportional, integral, and differential coefficients.
:param p: Proportional coefficient
:param i: Integral coefficient
:param d: Differential coefficient
:param f: Feed forward coefficient (optional, default is 0.0)
"""
with self.mutex:
self.P = p
self.I = i
self.D = d
self.F = f
def getP(self):
"""Get the Proportional coefficient.
:returns: proportional coefficient
"""
with self.mutex:
return self.P
def getI(self):
"""Get the Integral coefficient
:returns: integral coefficient
"""
with self.mutex:
return self.I
def getD(self):
"""Get the Differential coefficient.
:returns: differential coefficient
"""
with self.mutex:
return self.D
def getF(self):
"""Get the Feed forward coefficient.
:returns: feed forward coefficient
"""
with self.mutex:
return self.F
def get(self):
"""Return the current PID result.
This is always centered on zero and constrained the the max and min
outs.
:returns: the latest calculated output
"""
with self.mutex:
return self.result
def setContinuous(self, continuous=True):
"""Set the PID controller to consider the input to be continuous.
Rather then using the max and min input range as constraints, it considers them
to be the same point and automatically calculates the shortest route
to the setpoint.
:param continuous: Set to True turns on continuous, False turns off
continuous
"""
if continuous and self.inputRange <= 0:
raise ValueError(
"No input range set when calling setContinuous().")
with self.mutex:
self.continuous = continuous
def setInputRange(self, minimumInput, maximumInput):
"""Sets the maximum and minimum values expected from the input.
:param minimumInput: the minimum percentage expected from the input
:param maximumInput: the maximum percentage expected from the output
"""
with self.mutex:
if minimumInput > maximumInput:
raise ValueError("Lower bound is greater than upper bound")
self.minimumInput = minimumInput
self.maximumInput = maximumInput
self.inputRange = self.maximumInput - self.minimumInput
self.setSetpoint(self.setpoint)
def setOutputRange(self, minimumOutput, maximumOutput):
"""Sets the minimum and maximum values to write.
:param minimumOutput: the minimum percentage to write to the output
:param maximumOutput: the maximum percentage to write to the output
"""
with self.mutex:
if minimumOutput > maximumOutput:
raise ValueError("Lower bound is greater than upper bound")
self.minimumOutput = minimumOutput
self.maximumOutput = maximumOutput
def setSetpoint(self, setpoint):
"""Set the setpoint for the PIDController.
:param setpoint: the desired setpoint
"""
with self.mutex:
if self.maximumInput > self.minimumInput:
if setpoint > self.maximumInput:
newsetpoint = self.maximumInput
elif setpoint < self.minimumInput:
newsetpoint = self.minimumInput
else:
newsetpoint = setpoint
else:
newsetpoint = setpoint
self.setpoint = newsetpoint
def getSetpoint(self):
"""Returns the current setpoint of the PIDController.
:returns: the current setpoint
"""
with self.mutex:
return self.setpoint
def getDeltaSetpoint(self):
"""Returns the change in setpoint over time of the PIDController
:returns: the change in setpoint over time
"""
with self.mutex:
t = self.setpointTimer.get()
# During testing/simulation it is possible to get a divide by zero because
# the threads' calls aren't strictly aligned with the master clock.
if t:
return (self.setpoint - self.prevSetpoint) / t
else:
return 0.0
def getError(self):
"""Returns the current difference of the input from the setpoint.
:return: the current error
"""
with self.mutex:
return self.getContinuousError(self.getSetpoint() -
self.pidInput.pidGet())
def getAvgError(self):
"""
Returns the current difference of the error over the past few iterations. You can specify the
number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
used for the onTarget() function.
.. deprecated :: 2018.0.0
Use getError(), which is now already filtered.
:returns: the current average of the error
"""
warnings.warn("use getRate instead", DeprecationWarning)
with self.mutex:
return self.getError()
def setPIDSourceType(self, pidSourceType):
"""Sets what type of input the PID controller will use
:param pidSourceType: the type of input
"""
self.pidInput.setPIDSourceType(pidSourceType)
def getPIDSourceType(self):
"""Returns the type of input the PID controller is using
:returns: the PID controller input type
"""
return self.pidInput.getPIDSourceType()
def setAbsoluteTolerance(self, absvalue):
"""Set the absolute error which is considered tolerable for use with
:func:`onTarget`.
:param absvalue: absolute error which is tolerable in the units of the
input object
"""
with self.mutex:
self.onTarget = lambda: \
self.AbsoluteTolerance_onTarget(absvalue)
def setPercentTolerance(self, percentage):
"""Set the percentage error which is considered tolerable for use with
:func:`onTarget`. (Input of 15.0 = 15 percent)
:param percentage: percent error which is tolerable
"""
with self.mutex:
self.onTarget = lambda: \
self.PercentageTolerance_onTarget(percentage)
def setToleranceBuffer(self, bufLength):
"""Set the number of previous error samples to average for tolerancing. When
determining whether a mechanism is on target, the user may want to use a
rolling average of previous measurements instead of a precise position or
velocity. This is useful for noisy sensors which return a few erroneous
measurements when the mechanism is on target. However, the mechanism will
not register as on target for at least the specified bufLength cycles.
:param bufLength: Number of previous cycles to average.
:type bufLength: int
.. deprecated:: 2018.0.0
Use a LinearDigitalFilter as the input
"""
with self.mutex:
self.filter = LinearDigitalFilter.movingAverage(
self.origSource, bufLength)
self.pidInput = self.filter
def onTarget(self):
"""Return True if the error is within the percentage of the total input
range, determined by setTolerance. This assumes that the maximum and
minimum input were set using :func:`setInput`.
:returns: True if the error is less than the tolerance
"""
with self.mutex:
return self.tolerance.onTarget()
def enable(self):
"""Begin running the PIDController."""
with self.mutex:
self.enabled = True
def disable(self):
"""Stop running the PIDController, this sets the output to zero before
stopping."""
with self.pidWriteMutex:
with self.mutex:
self.enabled = False
self.pidOutput(0)
def isEnabled(self):
"""Return True if PIDController is enabled."""
with self.mutex:
return self.enabled
def reset(self):
"""Reset the previous error, the integral term, and disable the
controller."""
with self.mutex:
self.disable()
self.prevError = 0
self.totalError = 0
self.result = 0
def setP(self, p):
"""
Set the Proportional coefficient of the PID controller gain.
:param p: Proportional coefficient
"""
with self.mutex:
self.P = p
def setI(self, i):
"""
Set the Integral coefficient of the PID controller gain.
:param i: Integral coefficient
"""
with self.mutex:
self.I = i
def setD(self, d):
"""
Set the Differential coefficient of the PID controller gain.
:param d: differential coefficient
"""
with self.mutex:
self.D = d
def setF(self, f):
"""
Set the Feed forward coefficient of the PID controller gain.
:param f: feed forward coefficient
"""
with self.mutex:
self.F = f
def setEnabled(self, enable):
"""Set the enabled state of the PIDController."""
if enable:
self.enable()
else:
self.disable()
def initSendable(self, builder):
builder.setSmartDashboardType("PIDController")
builder.setSafeState(self.reset)
builder.addDoubleProperty("p", self.getP, self.setP)
builder.addDoubleProperty("i", self.getI, self.setI)
builder.addDoubleProperty("d", self.getD, self.setD)
builder.addDoubleProperty("f", self.getF, self.setF)
builder.addDoubleProperty("setpoint", self.getSetpoint,
self.setSetpoint)
builder.addBooleanProperty("enabled", self.isEnabled, self.setEnabled)
def getContinuousError(self, error):
"""
Wraps error around for continuous inputs. The original error is
returned if continuous mode is disabled. This is an unsynchronized
function.
:param error: The current error of the PID controller.
:return: Error for continuous inputs.
"""
if self.continuous and self.inputRange > 0:
error %= self.inputRange
if abs(error) > self.inputRange / 2:
if error > 0:
return error - self.inputRange
else:
return error + self.inputRange
return error
def clamp(self, value, low, high):
return max(low, min(value, high))
class driveTrain(Component) :
def __init__(self, robot):
super().__init__()
self.robot = robot
# Constants
WHEEL_DIAMETER = 8
PI = 3.1415
ENCODER_TICK_COUNT_250 = 250
ENCODER_TICK_COUNT_360 = 360
ENCODER_GOAL = 0 # default
ENCODER_TOLERANCE = 1 # inch0
self.RPM = 4320/10.7
self.INCHES_PER_REV = WHEEL_DIAMETER * 3.1415
self.CONTROL_TYPE = False # False = disable PID components
self.LEFTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.RIGHTBACKCUMULATIVE = 0
self.rfmotor = CANTalon(0)
self.rbmotor = CANTalon(1)
self.lfmotor = CANTalon(2)
self.lbmotor = CANTalon(3)
self.lfmotor.reverseOutput(True)
self.lbmotor.reverseOutput(True)
#self.rfmotor.reverseOutput(True)
#self.rbmotor.reverseOutput(True)#practice bot only
self.rfmotor.enableBrakeMode(True)
self.rbmotor.enableBrakeMode(True)
self.lfmotor.enableBrakeMode(True)
self.lbmotor.enableBrakeMode(True)
absolutePosition = self.lbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lbmotor.setEncPosition(absolutePosition)
absolutePosition = self.lfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.lfmotor.setEncPosition(absolutePosition)
absolutePosition = self.rbmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rbmotor.setEncPosition(absolutePosition)
absolutePosition = self.rfmotor.getPulseWidthPosition() & 0xFFF; # mask out the bottom12 bits, we don't care about the wrap arounds use the low level API to set the quad encoder signal
self.rfmotor.setEncPosition(absolutePosition)
self.rfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.rbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lfmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
self.lbmotor.setFeedbackDevice(CANTalon.FeedbackDevice.CtreMagEncoder_Relative)
#setting up the distances per rotation
self.lfmotor.configEncoderCodesPerRev(4096)
self.rfmotor.configEncoderCodesPerRev(4096)
self.lbmotor.configEncoderCodesPerRev(4096)
self.rbmotor.configEncoderCodesPerRev(4096)
self.lfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rfmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.rbmotor.setPID(0.0005, 0, 0.0, profile=0)
self.lbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.lfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.lfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.lfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rbmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rbmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rbmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.configNominalOutputVoltage(+0.0, -0.0)
self.rfmotor.configPeakOutputVoltage(+12.0, -12.0)
self.rfmotor.setControlMode(CANTalon.ControlMode.Speed)
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
self.lfmotor.reverseSensor(True)
self.lbmotor.reverseSensor(True)
'''
# changing the encoder output from DISTANCE to RATE (we're dumb)
self.lfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.lbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rfencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
self.rbencoder.setPIDSourceType(wpilib.PIDController.PIDSourceType.kRate)
# LiveWindow settings (Encoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Front Encoder", self.lfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Front Encoder", self.rfencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Left Back Encoder", self.lbencoder)
wpilib.LiveWindow.addSensor("Drive Train", "Right Back Encoder", self.rbencoder)
'''
'''
# Checking the state of the encoders on the Smart Dashboard
wpilib.SmartDashboard.putBoolean("Right Front Encoder Enabled?", self.rfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Right Back Encoder Enabled?", self.rbmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Front Encoder Enabled?", self.lfmotor.isSensorPresent)
wpilib.SmartDashboard.putBoolean("Left Back Encoder Enabled?", self.lbmotor.isSensorPresent)
'''
if self.CONTROL_TYPE:
# Initializing PID Controls
self.pidRightFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rfmotor.feedbackDevice, self.rfmotor, 0.02)
self.pidLeftFront = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lfmotor.feedbackDevice, self.lfmotor, 0.02)
self.pidRightBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.rbmotor.feedbackDevice, self.rbmotor, 0.02)
self.pidLeftBack = wpilib.PIDController(0.002, 0.8, 0.005, 0, self.lbmotor.feedbackDevice, self.lbmotor, 0.02)
# PID Absolute Tolerance Settings
self.pidRightFront.setAbsoluteTolerance(0.05)
self.pidLeftFront.setAbsoluteTolerance(0.05)
self.pidRightBack.setAbsoluteTolerance(0.05)
self.pidLeftBack.setAbsoluteTolerance(0.05)
# PID Output Range Settings
self.pidRightFront.setOutputRange(-1, 1)
self.pidLeftFront.setOutputRange(-1, 1)
self.pidRightBack.setOutputRange(-1, 1)
self.pidLeftBack.setOutputRange(-1, 1)
# Enable PID
#self.enablePIDs()
'''
# LiveWindow settings (PID)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front PID", self.pidRightFront)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front PID", self.pidLeftFront)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back PID", self.pidRightBack)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back PID", self.pidLeftBack)
'''
self.dashTimer = Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
'''
# Adding components to the LiveWindow (testing)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Front Motor", self.lfmotor)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Front Motor", self.rfmotor)
wpilib.LiveWindow.addActuator("Drive Train Left", "Left Back Motor", self.lbmotor)
wpilib.LiveWindow.addActuator("Drive Train Right", "Right Back Motor", self.rbmotor)
'''
def log(self):
#The log method puts interesting information to the SmartDashboard. (like velocity information)
'''
#no longer implemented because of change of hardware
wpilib.SmartDashboard.putNumber("Left Front Speed", self.lfmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Right Front Speed", self.rfmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Left Back Speed", self.lbmotor.getEncVelocity())
wpilib.SmartDashboard.putNumber("Right Back Speed", self.rbmotor.getEncVelocity())
'''
wpilib.SmartDashboard.putNumber("RF Mag Enc Position", self.rfmotor.getPosition())
wpilib.SmartDashboard.putNumber("RB Mag Enc Position", self.rbmotor.getPosition())
wpilib.SmartDashboard.putNumber("LF Mag Enc Position", self.lfmotor.getPosition())
wpilib.SmartDashboard.putNumber("LB Mag Enc Position", self.lbmotor.getPosition())
'''
wpilib.SmartDashboard.putNumber("Right Front Mag Distance(inches)", self.convertEncoderRaw(self.rfmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Right Back Mag Distance(inches)", self.convertEncoderRaw(self.rbmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Left Front Mag Distance(inches)", self.convertEncoderRaw(self.lfmotor.getPosition()*0.57))
wpilib.SmartDashboard.putNumber("Left Back Mag Distance(inches)", self.convertEncoderRaw(self.lbmotor.getPosition()*0.57))
'''
# drive forward function
def drive_forward(self, speed) :
self.drive.tankDrive(speed, speed, True)
# manual drive function for Tank Drive
def xboxTankDrive(self, leftSpeed, rightSpeed, leftB, rightB, leftT, rightT):
#self.lfmotor.setCloseLoopRampRate(1)
#self.lbmotor.setCloseLoopRampRate(1)
#self.rfmotor.setCloseLoopRampRate(1)
#self.rbmotor.setCloseLoopRampRate(1)
if (leftB == True): #Straight Button
rightSpeed = leftSpeed
if (rightB == True): #Slow Button
#leftSpeed = leftSpeed/1.75
#rightSpeed = rightSpeed/1.75
if(not(leftSpeed < -0.5 and rightSpeed > 0.5 or leftSpeed > -0.5 and rightSpeed < 0.5)): #only do t if not turning
leftSpeed = leftSpeed/1.75
rightSpeed = rightSpeed/1.75
# Fast button
if(rightT == True):
#self.lfmotor.setCloseLoopRampRate(24)
#self.lbmotor.setCloseLoopRampRate(24)
#self.rfmotor.setCloseLoopRampRate(24)
#self.rbmotor.setCloseLoopRampRate(24)
leftSpeed = leftSpeed*(1.75)
rightSpeed = rightSpeed*(1.75)
if(leftT == True):
leftSpeed = 0.1
rightSpeed = 0.1
# Creating margin for error when using the joysticks, as they're quite sensitive
if abs(rightSpeed) < 0.04 :
rightSpeed = 0
if abs(leftSpeed) < 0.04 :
leftSpeed = 0
if self.CONTROL_TYPE:
self.pidRightFront.setSetpoint(rightSpeed)
self.pidRightBack.setSetpoint(rightSpeed)
self.pidLeftFront.setSetpoint(leftSpeed)
self.pidLeftBack.setSetpoint(leftSpeed)
else:
self.lfmotor.set(leftSpeed*512)
self.rfmotor.set(rightSpeed*512)
self.lbmotor.set(leftSpeed*512)
self.rbmotor.set(rightSpeed*512)
#autononmous tank drive (to remove a need for a slow, striaght, or fast button)
def autonTankDrive(self, leftSpeed, rightSpeed):
self.log()
#self.drive.tankDrive(leftSpeed, rightSpeed, True)
self.rfmotor.set(rightSpeed)
self.rbmotor.set(rightSpeed*(-1))
self.lfmotor.set(leftSpeed)
self.lbmotor.set(leftSpeed*(-1))
# stop function
def drive_stop(self) :
self.drive.tankDrive(0,0)
# fucntion to reset the PID's and encoder values
def reset(self):
self.rfmotor.setPosition(0)
self.rbmotor.setPosition(0)
self.lfmotor.setPosition(0)
self.lbmotor.setPosition(0)
if self.CONTROL_TYPE:
self.LEFTFRONTCUMULATIVE = 0
self.RIGHTFRONTCUMULATIVE = 0
self.LEFTBACKCUMULATIVE= 0
self.RIGHTBACKCUMULATIVE = 0
self.pidLeftBack.setSetpoint(0)
self.pidLeftFront.setSetpoint(0)
self.pidRightBack.setSetpoint(0)
self.pidRightFront.setSetpoint(0)
# def getDistance(self)
# return (abs(self.convertEncoderRaw(LEFTFRONTCUMULATIVE) + abs(self.convertEncoderRaw(LEFTBACKCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTFRONTCUMULATIVE)) + abs(self.convertEncoderRaw(RIGHTBACKCUMULATIVE)))
def turn_angle(self, degrees):
desired_inches = self.INCHES_PER_DEGREE * degrees
if degrees < 0:
while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches :
self.autonTankDrive(0.4, -0.4)
elif degrees > 0:
while (abs(self.lfencoder.getDistance()) + abs(self.rfencoder.getDistance())) <= desired_inches :
self.autonTankDrive(-0.4, 0.4)
# Enable PID Controllers
def enablePIDs(self):
'''
#No longer required because we swapped from analog encoders to magnetic encoders
self.pidLeftFront.enable()
self.pidLeftBack.enable()
self.pidRightFront.enable()
self.pidRightBack.enable()
'''
# Disable PID Controllers
def disablePIDs(self):
'''
#see explaination above
self.pidLeftFront.disable()
self.pidLeftBack.disable()
self.pidRightFront.disable()
self.pidRightBack.disable()
'''
def getAutonDistance(self):
return (self.convertEncoderRaw(abs(self.rfmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.rbmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.lfmotor.getPosition()*0.57))
+ self.convertEncoderRaw(abs(self.lbmotor.getPosition()*0.57)))/4
#detirmines how many ticks the encoder has processed
def getMotorDistance(self, motor, cumulativeDistance):
currentRollovers = 0 #number of times the encoder has gone from 1023 to 0
previousValue = cumulativeDistance #variable for comparison
currentValue = motor.getEncPosition() #variable for comparison
if(previousValue > currentValue): #checks to see if the encoder reset itself from 1023 to 0
currentRollovers += 1 #notes the rollover
return currentValue + (currentRollovers * 1024) #adds current value to the number of rollovers, each rollover == 1024 ticks
#converts ticks from getMotorDistance into inches
def convertEncoderRaw(self, selectedEncoderValue):
return selectedEncoderValue * self.INCHES_PER_REV
class OpIntakeCommand(Command):
def __init__ (self, intake: Intake):
super().__init__ ()
self.requires(intake)
self.intake = intake
self.last_arm_state = ArmState.UP
self.arm_timer = Timer()
self.arm_timer_latch = False
def initialize(self):
self.last_arm_state = self.intake.get_arm_state()
self.arm_timer.reset()
self.arm_timer.stop()
self.arm_timer_latch = False
def execute(self):
oi = OI.get()
intake = self.intake
if oi.arm_is_down():
intake.set_arm_state(ArmState.DOWN)
else:
intake.set_arm_state(ArmState.UP)
if self.last_arm_state != ArmState.UP:
self.arm_timer.start()
self.arm_timer.reset()
self.arm_timer_latch = True
self.last_arm_state = self.intake.get_arm_state()
if self.arm_timer.get() > 0.5:
self.arm_timer_latch = False
self.arm_timer.stop()
if self.arm_timer_latch:
intake.run_intake(0.1)
else:
self.arm_timer.stop()
if oi.intake_is_active():
# intake.set_grab_state(GrabState.OUT)
pwr = oi.get_outtake_command()
if abs(pwr) < 0.05:
pwr = 0
max_intake_power = 0.5
if pwr > max_intake_power:
pwr = max_intake_power
intake.run_intake(pwr)
if intake.has_acquired_cube() and pwr > 0:
oi.rumble_op()
else:
oi.unrumble_op()
else:
intake.run_intake(0)
oi.unrumble_op()
if oi.arm_is_open():
intake.set_grab_state(GrabState.OUT)
else:
intake.set_grab_state(GrabState.IN)
def end(self):
OI.get().unrumble_op()
def isFinished(self):
return False
