def initialize(self):
""" Read in and apply current drive choices. """
DriveHuman.setupDashboardControls()
global modeChooser
self.mode = modeChooser.getSelected()
self.quickTurn = SmartDashboard.getBoolean("Quick Turn", self.quickTurn)
self.squareInputs = SmartDashboard.getBoolean("Square Inputs", self.squareInputs)
self.fixedLeft = SmartDashboard.getNumber("Fixed Left", self.fixedLeft)
self.fixedRight = SmartDashboard.getNumber("Fixed Right", self.fixedRight)
self.rotationGain = SmartDashboard.getNumber("Rotation Gain", self.rotationGain)
self.slowGain = SmartDashboard.getNumber("Slow Gain", self.slowGain)
def initialize(self):
"""Called just before this Command runs the first time."""
strip_name = lambda x: str(x)[1 + str(x).rfind('.'):-2]
print(
"\n" +
f"** Started {self.name} with input {self.factor} at {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)} s **"
)
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
if self.from_dashboard == 'position':
dict_0 = {}
for key in keys:
value = float(SmartDashboard.getNumber(str(key) + '_0', 0))
dict_0.update({key: value})
self.robot.drivetrain.change_PIDs(factor=1, dict_0=dict_0)
if self.from_dashboard == 'velocity':
dict_1 = {}
for key in keys:
value = float(SmartDashboard.getNumber(str(key) + '_1', 0))
dict_1.update({key: value})
self.robot.drivetrain.change_PIDs(factor=1, dict_1=dict_1)
else:
self.robot.drivetrain.change_PIDs(self.factor)
def found_hook(self):
turning_amt = SmartDashboard.getNumber("Vision/Turn",
0) * self.turn_to_peg_P
self.drive.arcade_drive(turning_amt, 0.2)
def conditonAxis(self, axis, deadband, rate, expo, power, minimum, maximum):
if self.timer.hasPeriodPassed(5):
#Left Y
self.mlyRate = SmartDashboard.getNumber("left_y_rate", 0.85)
self.mlyExpo = SmartDashboard.getNumber("left_y_expo", 0.6)
self.mlyDeadband = SmartDashboard.getNumber("left_y_deadband", 0.02)
self.mlyPower = SmartDashboard.getNumber("left_y_power", 1.5)
self.mlyMin = SmartDashboard.getNumber("left_y_min", -1.0)
self.mlyMax = SmartDashboard.getNumber("left_y_max", 1.0)
#Left X
self.mlxRate = SmartDashboard.getNumber("left_x_rate", 0.85)
self.mlxExpo = SmartDashboard.getNumber("left_x_expo", 0.6)
self.mlxDeadband = SmartDashboard.getNumber("left_x_deadband", 0.02)
self.mlxPower = SmartDashboard.getNumber("left_x_power", 1.5)
self.mlxMin = SmartDashboard.getNumber("left_x_min", -1.0)
self.mlxMax = SmartDashboard.getNumber("left_x_max", 1.0)
#Right Y
self.mryRate = SmartDashboard.getNumber("right_y_rate", 1.0)
self.mryExpo = SmartDashboard.getNumber("right_y_expo", 0)
self.mryDeadband = SmartDashboard.getNumber("right_y_deadband", 0.1)
self.mryPower = SmartDashboard.getNumber("right_y_power", 1.0)
self.mryMin = SmartDashboard.getNumber("right_y_min", -1.0)
self.mryMax = SmartDashboard.getNumber("right_y_max", 1.0)
#Right X
self.mrxRate = SmartDashboard.getNumber("right_x_rate", 0.5)
self.mrxExpo = SmartDashboard.getNumber("right_x_expo", 1.0)
self.mrxDeadband = SmartDashboard.getNumber("right_x_deadband", 0.1)
self.mrxPower = SmartDashboard.getNumber("right_x_power", 1.5)
self.mrxMin = SmartDashboard.getNumber("right_x_min", -0.5)
self.mrxMax = SmartDashboard.getNumber("right_x_max", 0.5)
deadband = min(abs(deadband), 1)
rate = max(0.1, min(abs(rate), 10))
expo = max(0, min(abs(expo), 1))
power = max(1, min(abs(power), 10))
if axis > -deadband and axis < deadband:
axis = 0
else:
axis = rate * (math.copysign(1, axis) * ((abs(axis) - deadband) / (1 - deadband)))
if expo > 0.01:
axis = ((axis * (abs(axis) ** power) * expo) + (axis * (1 - expo)))
axis = max(min(axis, maximum), minimum)
return axis
def __init__(self, port):
super().__init__(port)
self.timer = wpilib.Timer()
self.timer.start()
self.btnX = JoystickButton(self, robotmap.XBOX_X)
self.btnY = JoystickButton(self, robotmap.XBOX_Y)
self.btnA = JoystickButton(self, robotmap.XBOX_A)
self.btnB = JoystickButton(self, robotmap.XBOX_B)
self.btnLB = JoystickButton(self, robotmap.XBOX_LEFT_BUMPER)
self.btnRB = JoystickButton(self, robotmap.XBOX_RIGHT_BUMPER)
self.btnStart = JoystickButton(self, robotmap.XBOX_START)
self.btnBack = JoystickButton(self, robotmap.XBOX_BACK)
self.btnDpadUp = JoystickButton(self, DPAD_BUTTON.DPAD_UP)
self.btnDpadRight = JoystickButton(self, DPAD_BUTTON.DPAD_RIGHT)
self.btnDpadDown = JoystickButton(self, DPAD_BUTTON.DPAD_DOWN)
self.btnDpadLeft = JoystickButton(self, DPAD_BUTTON.DPAD_LEFT)
#Left Y
self.mlyRate = SmartDashboard.getNumber("left_y_rate", 0.85)
self.mlyExpo = SmartDashboard.getNumber("left_y_expo", 0.6)
self.mlyDeadband = SmartDashboard.getNumber("left_y_deadband", 0.02)
self.mlyPower = SmartDashboard.getNumber("left_y_power", 1.5)
self.mlyMin = SmartDashboard.getNumber("left_y_min", -1.0)
self.mlyMax = SmartDashboard.getNumber("left_y_max", 1.0)
#Left X
self.mlxRate = SmartDashboard.getNumber("left_x_rate", 0.85)
self.mlxExpo = SmartDashboard.getNumber("left_x_expo", 0.6)
self.mlxDeadband = SmartDashboard.getNumber("left_x_deadband", 0.02)
self.mlxPower = SmartDashboard.getNumber("left_x_power", 1.5)
self.mlxMin = SmartDashboard.getNumber("left_x_min", -1.0)
self.mlxMax = SmartDashboard.getNumber("left_x_max", 1.0)
#Right Y
self.mryRate = SmartDashboard.getNumber("right_y_rate", 1.0)
self.mryExpo = SmartDashboard.getNumber("right_y_expo", 0)
self.mryDeadband = SmartDashboard.getNumber("right_y_deadband", 0.1)
self.mryPower = SmartDashboard.getNumber("right_y_power", 1.0)
self.mryMin = SmartDashboard.getNumber("right_y_min", -1.0)
self.mryMax = SmartDashboard.getNumber("right_y_max", 1.0)
#Right X
self.mrxRate = SmartDashboard.getNumber("right_x_rate", 0.5)
self.mrxExpo = SmartDashboard.getNumber("right_x_expo", 1.0)
self.mrxDeadband = SmartDashboard.getNumber("right_x_deadband", 0.1)
self.mrxPower = SmartDashboard.getNumber("right_x_power", 1.5)
self.mrxMin = SmartDashboard.getNumber("right_x_min", -0.5)
self.mrxMax = SmartDashboard.getNumber("right_x_max", 0.5)
def enable(self):
self.level = SmartDashboard.getNumber("Climber Multiplier", 1)
