def teleopPeriodic(self):
"""
Process inputs from the driver station here.
This is run each iteration of the control loop before magicbot
components are executed.
"""
if self.joystick.getRawButtonPressed(10):
self.chassis.odometry_x = 0.0
self.chassis.odometry_y = 0.0
self.motion.set_waypoints(
np.array([[0, 0, 0, 1], [1, 0, 0, 1], [1, 1, 0, 1],
[2, 1, 0, 1], [2, 1, 0, 0]]))
if self.joystick.getRawButtonPressed(9):
self.motion.disable()
self.chassis.field_oriented = True
if self.joystick.getRawButtonPressed(8):
print("Heading sp set")
self.chassis.set_heading_sp(self.bno055.getAngle() + math.pi)
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants for whatever robot they are on
vx = -rescale_js(
self.joystick.getY(), deadzone=0.05, exponential=1.2, rate=4)
vy = -rescale_js(
self.joystick.getX(), deadzone=0.05, exponential=1.2, rate=4)
vz = -rescale_js(self.joystick.getZ(),
deadzone=0.2,
exponential=15.0,
rate=self.spin_rate)
self.chassis.set_inputs(vx, vy, vz)
def teleopPeriodic(self):
"""This is run each iteration of the control loop before magicbot components are executed."""
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants forvwhatever robot they are on
vx = -rescale_js(
self.joystick.getY(), deadzone=0.05, exponential=1.2, rate=4)
vy = -rescale_js(
self.joystick.getX(), deadzone=0.05, exponential=1.2, rate=4)
vz = -rescale_js(self.joystick.getZ(),
deadzone=0.2,
exponential=15.0,
rate=self.spin_rate)
self.chassis.set_inputs(vx, vy, vz)
def testPeriodic(self):
self.vision.execute() # Keep the time offset calcs running
joystick_vx = -rescale_js(
self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=0.5
)
for button, module in zip((5, 3, 4, 6), self.chassis.modules):
if self.joystick.getRawButton(button):
module.store_steer_offsets()
module.steer_motor.set(ctre.ControlMode.PercentOutput, joystick_vx)
if self.joystick.getTriggerPressed():
module.steer_motor.set(
ctre.ControlMode.Position,
module.steer_motor.getSelectedSensorPosition(0)
+ self.offset_rotation_rate,
)
if self.joystick.getRawButtonPressed(2):
module.steer_motor.set(
ctre.ControlMode.Position,
module.steer_motor.getSelectedSensorPosition(0)
- self.offset_rotation_rate,
)
if self.joystick.getRawButtonPressed(8):
for module in self.chassis.modules:
module.drive_motor.set(ctre.ControlMode.PercentOutput, 0.3)
if self.joystick.getRawButtonPressed(12):
for module in self.chassis.modules:
module.steer_motor.set(
ctre.ControlMode.Position, module.steer_enc_offset
)
if self.gamepad.getStartButton():
self.climber.retract_all()
self.climber.execute()
if self.gamepad.getPOV() != -1:
speed = 0.1
azimuth = math.radians(-self.gamepad.getPOV())
for module in self.chassis.modules:
module.set_velocity(
speed * math.cos(azimuth),
speed * math.sin(azimuth),
absolute_rotation=True,
)
if self.gamepad.getTriggerAxis(self.gamepad.Hand.kLeft) > 0.5:
self.hatch_enable_piston.set(wpilib.DoubleSolenoid.Value.kReverse)
def testPeriodic(self):
self.vision.execute() # Keep the time offset calcs running
joystick_vx = -rescale_js(
self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=0.5)
self.sd.putNumber("joy_vx", joystick_vx)
for button, module in zip((5, 3, 4, 6), self.chassis.modules):
if self.joystick.getRawButton(button):
module.store_steer_offsets()
module.steer_motor.set(ctre.ControlMode.PercentOutput,
joystick_vx)
if self.joystick.getTriggerPressed():
module.steer_motor.set(
ctre.ControlMode.Position,
module.steer_motor.getSelectedSensorPosition(0) +
self.offset_rotation_rate,
)
if self.joystick.getRawButtonPressed(2):
module.steer_motor.set(
ctre.ControlMode.Position,
module.steer_motor.getSelectedSensorPosition(0) -
self.offset_rotation_rate,
)
if self.joystick.getRawButtonPressed(8):
for module in self.chassis.modules:
module.drive_motor.set(ctre.ControlMode.PercentOutput, 0.3)
if self.joystick.getRawButtonPressed(12):
for module in self.chassis.modules:
module.steer_motor.set(ctre.ControlMode.Position,
module.steer_enc_offset)
if self.gamepad.getStartButtonPressed():
self.climber.retract_all()
if self.gamepad.getBackButtonPressed():
self.climber.stop_all()
def teleopPeriodic(self):
"""
Process inputs from the driver station here.
This is run each iteration of the control loop before magicbot components are executed.
"""
if self.gamepad.getBumperPressed(
wpilib.interfaces.GenericHID.Hand.kLeft):
self.intake.push(not self.intake.push_on)
if self.gamepad.getBumperPressed(
wpilib.interfaces.GenericHID.Hand.kRight):
self.intake.toggle_arms()
if self.gamepad.getBButtonPressed():
self.intake.toggle_clamp()
if self.joystick.getTrigger():
self.cubeman.start_intake(force=True)
if self.joystick.getRawButtonPressed(4) or self.gamepad.getTriggerAxis(
wpilib.interfaces.GenericHID.Hand.kRight) > 0.5:
self.cubeman.eject(force=True)
# if self.joystick.getRawButtonPressed(2) or self.gamepad.getStartButtonPressed():
if self.gamepad.getStartButtonPressed():
self.cubeman.panic()
if self.joystick.getRawButtonPressed(3) or self.gamepad.getTriggerAxis(
wpilib.interfaces.GenericHID.Hand.kLeft) > 0.5:
self.cubeman.deposit_exchange(force=True)
if self.gamepad.getAButtonPressed():
self.cubeman.lift_to_scale(force=True)
if self.gamepad.getXButtonPressed():
self.cubeman.lift_to_switch(force=True)
if self.gamepad.getBackButtonPressed():
self.cubeman.reset(force=True)
if self.joystick.getRawButtonPressed(10):
self.imu.resetHeading()
self.chassis.set_heading_sp(0)
throttle = (1 - self.joystick.getThrottle()) / 2
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants for whatever robot they are on
joystick_vx = -rescale_js(self.joystick.getY(),
deadzone=0.1,
exponential=1.5,
rate=4 * throttle)
joystick_vy = -rescale_js(self.joystick.getX(),
deadzone=0.1,
exponential=1.5,
rate=4 * throttle)
joystick_vz = -rescale_js(self.joystick.getZ(),
deadzone=0.2,
exponential=20.0,
rate=self.spin_rate)
joystick_hat = self.joystick.getPOV()
if joystick_vx or joystick_vy or joystick_vz:
self.chassis.set_inputs(
joystick_vx,
joystick_vy,
joystick_vz,
field_oriented=not self.joystick.getRawButton(6))
elif self.gamepad.getStickButton(self.gamepad.Hand.kLeft):
# TODO Tune these constants for the gamepad.
gamepad_vx = -rescale_js(self.gamepad.getY(
self.gamepad.Hand.kRight),
deadzone=0.1,
exponential=1.5,
rate=0.5)
gamepad_vy = -rescale_js(self.gamepad.getX(
self.gamepad.Hand.kRight),
deadzone=0.1,
exponential=1.5,
rate=0.5)
self.chassis.set_inputs(gamepad_vx,
gamepad_vy,
0,
field_oriented=True)
else:
self.chassis.set_inputs(0, 0, 0)
if joystick_hat != -1:
constrained_angle = -constrain_angle(math.radians(joystick_hat))
self.chassis.set_heading_sp(constrained_angle)
def teleopPeriodic(self):
"""Allow the drivers to control the robot."""
# self.chassis.heading_hold_off()
throttle = max(0.1, (1 - self.joystick.getThrottle()) / 2) # min 10%
self.sd.putNumber("joy_throttle", throttle)
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants for whatever robot they are on
joystick_vx = -rescale_js(self.joystick.getY(),
deadzone=0.1,
exponential=1.5,
rate=4 * throttle)
joystick_vy = -rescale_js(self.joystick.getX(),
deadzone=0.1,
exponential=1.5,
rate=4 * throttle)
joystick_vz = -rescale_js(self.joystick.getZ(),
deadzone=0.2,
exponential=20.0,
rate=self.spin_rate)
joystick_hat = self.joystick.getPOV()
self.sd.putNumber("joy_vx", joystick_vx)
self.sd.putNumber("joy_vy", joystick_vy)
self.sd.putNumber("joy_vz", joystick_vz)
# Allow big stick movements from the driver to break out of an automation
if abs(joystick_vx) > 0.5 or abs(joystick_vy) > 0.5:
self.hatch_intake.done()
self.hatch_deposit.done()
self.cargo_deposit.done()
if not self.chassis.automation_running:
if joystick_vx or joystick_vy or joystick_vz:
self.chassis.set_inputs(
joystick_vx,
joystick_vy,
joystick_vz,
field_oriented=not self.joystick.getRawButton(6),
)
else:
self.chassis.set_inputs(0, 0, 0)
if joystick_hat != -1:
if self.intake.has_cargo:
constrained_angle = -constrain_angle(
math.radians(joystick_hat) + math.pi)
else:
constrained_angle = -constrain_angle(
math.radians(joystick_hat))
self.chassis.set_heading_sp(constrained_angle)
if self.joystick.getRawButtonPressed(4):
self.hatch.punch()
if self.joystick.getTrigger():
angle = FieldAngle.closest(self.imu.getAngle())
self.logger.info("closest field angle: %s", angle)
if angle is FieldAngle.LOADING_STATION:
self.hatch_intake.engage()
else:
self.hatch_deposit.engage()
self.chassis.set_heading_sp(angle.value)
if self.joystick.getRawButton(2):
self.chassis.set_heading_sp(FieldAngle.LOADING_STATION.value)
self.hatch_intake.engage()
if self.joystick.getRawButtonPressed(5):
self.hatch.clear_to_retract = True
if self.joystick.getRawButtonPressed(3):
if self.chassis.hold_heading:
self.chassis.heading_hold_off()
else:
self.chassis.heading_hold_on()
if self.gamepad.getStartButtonPressed():
self.climb_automation.start_climb_lv3()
if self.gamepad.getBackButtonPressed():
self.climb_automation.done()
def teleopPeriodic(self):
"""Allow the drivers to control the robot."""
throttle = max(0.1, (1 - self.joystick.getThrottle()) / 2) # min 10%
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants for whatever robot they are on
joystick_vx = -rescale_js(
self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=4 * throttle
)
joystick_vy = -rescale_js(
self.joystick.getX(), deadzone=0.1, exponential=1.5, rate=4 * throttle
)
joystick_vz = -rescale_js(
self.joystick.getZ(), deadzone=0.2, exponential=20.0, rate=self.spin_rate
)
joystick_hat = self.joystick.getPOV()
# Allow big stick movements from the driver to break out of an automation
if abs(joystick_vx) > 0.5 or abs(joystick_vy) > 0.5:
self.hatch_intake.done()
self.hatch_deposit.done()
self.cargo_deposit.done()
if not self.chassis.automation_running:
if joystick_vx or joystick_vy or joystick_vz:
self.chassis.set_inputs(
joystick_vx,
joystick_vy,
joystick_vz,
field_oriented=not self.joystick.getRawButton(6),
)
else:
self.chassis.set_inputs(0, 0, 0)
if joystick_hat != -1:
if self.cargo_component.has_cargo or self.cargo.is_executing:
constrained_angle = -constrain_angle(
math.radians(joystick_hat) + math.pi
)
else:
constrained_angle = -constrain_angle(math.radians(joystick_hat))
self.chassis.set_heading_sp(constrained_angle)
# Starts Hatch Alignment and Cargo State Machines
if (
self.joystick.getTrigger()
or self.gamepad.getTriggerAxis(self.gamepad.Hand.kLeft) > 0.5
or self.gamepad.getTriggerAxis(self.gamepad.Hand.kRight) > 0.5
):
angle = FieldAngle.closest(self.imu.getAngle())
self.logger.info("closest field angle: %s", angle)
if self.cargo_component.has_cargo:
self.cargo_deposit.engage()
else:
if angle is FieldAngle.LOADING_STATION:
self.hatch_intake.engage()
else:
self.hatch_deposit.engage()
self.chassis.set_heading_sp(angle.value)
# Hatch Manual Outake/Intake
if self.joystick.getRawButtonPressed(5) or self.gamepad.getBumperPressed(6):
angle = FieldAngle.closest(self.imu.getAngle())
self.logger.info("closest field angle: %s", angle)
if angle is not FieldAngle.LOADING_STATION:
self.hatch_automation.outake()
else:
self.hatch_automation.grab()
if self.gamepad.getXButtonPressed():
self.hatch.retract_fingers()
self.hatch.retract()
# Stops Cargo Intake Motor
if self.gamepad.getBButtonPressed():
self.cargo.outake_cargo_ship(force=True)
# Toggles the Heading Hold
if self.joystick.getRawButtonPressed(8):
if self.chassis.hold_heading:
self.chassis.heading_hold_off()
else:
self.chassis.heading_hold_on()
# Resets the IMU's Heading
if self.joystick.getRawButtonPressed(7):
self.imu.resetHeading()
self.chassis.set_heading_sp(0)
# Start Button starts Climb State Machine
if self.gamepad.getStartButtonPressed() and self.gamepad.getRawButtonPressed(5):
self.climb_automation.start_climb_lv3()
# Back Button Ends Climb State Machine
if self.gamepad.getBackButtonPressed():
if self.gamepad.getRawButtonPressed(5):
self.climb_automation.abort()
else:
self.climb_automation.done()
# Cargo Floor Intake
if self.gamepad.getAButtonPressed():
self.cargo.intake_floor(force=True)
# Cargo Loading Station Intake
if self.gamepad.getYButtonPressed():
self.cargo.intake_loading(force=True)
self.chassis.set_heading_sp(
FieldAngle.CARGO_FRONT.value
) # Reversed side of robot
if self.gamepad.getPOV() != -1:
speed = 0.65
azimuth = math.radians(-self.gamepad.getPOV())
if self.cargo_component.has_cargo:
azimuth += math.pi
self.chassis.set_inputs(
speed * math.cos(azimuth),
speed * math.sin(azimuth),
0,
field_oriented=False,
)
def teleopPeriodic(self):
"""
Process inputs from the driver station here.
This is run each iteration of the control loop before magicbot components are executed.
"""
if self.joystick.getRawButtonPressed(12):
self.intake.clamp_on = not self.intake.clamp_on
if self.joystick.getRawButtonPressed(11):
self.intake.push_on = not self.intake.push_on
if self.joystick.getTrigger():
self.intake_automation.engage(initial_state="intake_cube")
if self.joystick.getRawButtonPressed(4) or self.gamepad.getTriggerAxis(
wpilib.interfaces.GenericHID.Hand.kRight) > 0.5:
self.intake_automation.engage(initial_state="eject_cube")
if self.joystick.getRawButtonPressed(
2) or self.gamepad.getStartButtonPressed():
self.intake_automation.engage(initial_state="stop", force=True)
if self.joystick.getRawButtonPressed(3) or self.gamepad.getTriggerAxis(
wpilib.interfaces.GenericHID.Hand.kLeft) > 0.5:
self.intake_automation.engage(initial_state="exchange")
if self.gamepad.getAButtonPressed():
self.lifter_automation.engage(initial_state="move_upper_scale",
force=True)
if self.gamepad.getBButtonPressed():
self.lifter_automation.engage(initial_state="move_balanced_scale",
force=True)
if self.gamepad.getYButtonPressed():
self.lifter_automation.engage(initial_state="move_lower_scale",
force=True)
if self.gamepad.getXButtonPressed():
self.lifter_automation.engage(initial_state="move_switch",
force=True)
if self.gamepad.getBackButtonPressed():
self.lifter_automation.engage(initial_state="reset", force=True)
if self.joystick.getRawButtonPressed(10):
self.imu.resetHeading()
self.chassis.set_heading_sp_current()
if self.joystick.getRawButtonPressed(8):
print("Heading sp set")
self.chassis.set_heading_sp(self.imu.getAngle() + math.pi)
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants for whatever robot they are on
vx = -rescale_js(
self.joystick.getY(), deadzone=0.1, exponential=1.5, rate=4)
vy = -rescale_js(
self.joystick.getX(), deadzone=0.1, exponential=1.5, rate=4)
vz = -rescale_js(self.joystick.getZ(),
deadzone=0.4,
exponential=10.0,
rate=self.spin_rate)
self.chassis.set_inputs(vx, vy, vz)
self.loop_timer.measure()
def teleopPeriodic(self):
"""
Process inputs from the driver station here.
This is run each iteration of the control loop before magicbot components are executed.
"""
if self.joystick.getRawButtonPressed(10):
self.imu.resetHeading()
self.chassis.set_heading_sp(0)
throttle = (1 - self.joystick.getThrottle()) / 2
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants for whatever robot they are on
joystick_vx = -rescale_js(self.joystick.getY(),
deadzone=0.1,
exponential=1.5,
rate=4 * throttle)
joystick_vy = -rescale_js(self.joystick.getX(),
deadzone=0.1,
exponential=1.5,
rate=4 * throttle)
joystick_vz = -rescale_js(self.joystick.getZ(),
deadzone=0.2,
exponential=20.0,
rate=self.spin_rate)
joystick_hat = self.joystick.getPOV()
if joystick_vx or joystick_vy or joystick_vz:
self.chassis.set_inputs(
joystick_vx,
joystick_vy,
joystick_vz,
field_oriented=not self.joystick.getRawButton(6))
elif self.gamepad.getStickButton(self.gamepad.Hand.kLeft):
# TODO Tune these constants for the gamepad.
gamepad_vx = -rescale_js(self.gamepad.getY(
self.gamepad.Hand.kRight),
deadzone=0.1,
exponential=1.5,
rate=0.5)
gamepad_vy = -rescale_js(self.gamepad.getX(
self.gamepad.Hand.kRight),
deadzone=0.1,
exponential=1.5,
rate=0.5)
self.chassis.set_inputs(gamepad_vx,
gamepad_vy,
0,
field_oriented=True)
else:
self.chassis.set_inputs(0, 0, 0)
if joystick_hat != -1:
constrained_angle = -constrain_angle(math.radians(joystick_hat))
self.chassis.set_heading_sp(constrained_angle)
def teleopPeriodic(self):
"""
Process inputs from the driver station here.
This is run each iteration of the control loop before magicbot components are executed.
"""
if self.joystick.getRawButtonPressed(3):
self.intake_automation.engage(initial_state="intake_cube")
if self.joystick.getRawButtonPressed(4):
self.intake_automation.engage(initial_state='eject_cube')
if self.joystick.getRawButtonPressed(5):
self.intake_automation.engage(initial_state="stop", force=True)
if self.joystick.getRawButtonPressed(6):
self.intake_automation.engage(initial_state="deposit")
if self.gamepad.getAButtonPressed():
self.lifter_automation.engage(initial_state="move_upper_scale",
force=True)
if self.gamepad.getBButtonPressed():
self.lifter_automation.engage(initial_state="move_balanced_scale",
force=True)
if self.gamepad.getXButtonPressed():
self.lifter_automation.engage(initial_state="move_lower_scale",
force=True)
if self.gamepad.getYButtonPressed():
self.lifter_automation.engage(initial_state="move_switch",
force=True)
if self.joystick.getRawButtonPressed(10):
self.chassis.odometry_x = 0.0
self.chassis.odometry_y = 0.0
self.motion.set_waypoints(
np.array([[0, 0, 0, 1], [1, 0, 0, 1], [1, 1, 0, 1],
[2, 1, 0, 1], [2, 1, 0, 0]]))
if self.joystick.getRawButtonPressed(9):
self.motion.disable()
self.chassis.field_oriented = True
if self.joystick.getRawButtonPressed(8):
print("Heading sp set")
self.chassis.set_heading_sp(self.imu.getAngle() + math.pi)
# this is where the joystick inputs get converted to numbers that are sent
# to the chassis component. we rescale them using the rescale_js function,
# in order to make their response exponential, and to set a dead zone -
# which just means if it is under a certain value a 0 will be sent
# TODO: Tune these constants for whatever robot they are on
vx = -rescale_js(
self.joystick.getY(), deadzone=0.05, exponential=1.2, rate=4)
vy = -rescale_js(
self.joystick.getX(), deadzone=0.05, exponential=1.2, rate=4)
vz = -rescale_js(self.joystick.getZ(),
deadzone=0.4,
exponential=15.0,
rate=self.spin_rate)
self.chassis.set_inputs(vx, vy, vz)