def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"a",
steer_talon=ctre.TalonSRX(42),
drive_talon=ctre.TalonSRX(48),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"b",
steer_talon=ctre.TalonSRX(58),
drive_talon=ctre.TalonSRX(2),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
# create the imu object
self.imu = NavX()
self.sd = NetworkTables.getTable("SmartDashboard")
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
def __init__(self, mode):
self.mode = mode
if self.mode == 'bno055':
self.imu = BNO055()
elif self.mode == 'navx':
self.imu = NavX()
self.pidsource = self.PIDSourceType.kDisplacement
class IMU:
PIDSourceType = PIDSource.PIDSourceType
def __init__(self, mode):
self.mode = mode
if self.mode == 'bno055':
self.imu = BNO055()
elif self.mode == 'navx':
self.imu = NavX()
self.pidsource = self.PIDSourceType.kDisplacement
def getAngle(self):
return self.imu.getAngle()
def getHeadingRate(self):
return self.imu.getHeadingRate()
def resetHeading(self):
self.imu.resetHeading()
def pidGet(self):
if self.pidsource == self.PIDSourceType.kDisplacement:
return self.getAngle()
else:
return self.getHeadingRate()
def setPIDSourceType(self, pidsourcetype):
self.pidsource = pidsourcetype
def getPIDSourceType(self):
return self.pidsource
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"a",
steer_talon=ctre.TalonSRX(48),
drive_talon=ctre.TalonSRX(41),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.TalonSRX(58),
drive_talon=ctre.TalonSRX(51),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.TalonSRX(52),
drive_talon=ctre.TalonSRX(53),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.TalonSRX(42),
drive_talon=ctre.TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left_motor = ctre.TalonSRX(14)
self.intake_right_motor = ctre.TalonSRX(2)
self.clamp_arm = wpilib.Solenoid(2)
self.intake_kicker = wpilib.Solenoid(3)
self.left_extension = wpilib.Solenoid(6)
self.right_extension = wpilib.DoubleSolenoid(forwardChannel=5,
reverseChannel=4)
self.compressor = wpilib.Compressor()
self.lifter_motor = ctre.TalonSRX(3)
self.centre_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
self.intake_cube_switch = wpilib.DigitalInput(3)
# create the imu object
self.imu = NavX()
wpilib.SmartDashboard.putData('IMU', self.imu.ahrs)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
self.range_finder_counter = wpilib.Counter(
4, mode=wpilib.Counter.Mode.kPulseLength)
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
"a",
steer_talon=ctre.WPI_TalonSRX(48),
drive_talon=ctre.WPI_TalonSRX(49),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.WPI_TalonSRX(46),
drive_talon=ctre.WPI_TalonSRX(47),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.WPI_TalonSRX(44),
drive_talon=ctre.WPI_TalonSRX(45),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.WPI_TalonSRX(42),
drive_talon=ctre.WPI_TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left_motor = ctre.WPI_TalonSRX(14)
self.intake_right_motor = ctre.WPI_TalonSRX(2)
self.clamp_arm = wpilib.Solenoid(0)
self.intake_kicker = wpilib.Solenoid(1)
self.extension_arms = wpilib.Solenoid(3)
self.infrared = SharpIRGP2Y0A41SK0F(0)
self.lifter_motor = ctre.WPI_TalonSRX(3)
self.centre_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
# create the imu object
self.imu = NavX()
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 5
self.sd = NetworkTables.getTable("SmartDashboard")
class Robot(magicbot.MagicRobot):
# Add magicbot components here using variable annotations.
# Any components that directly actuate motors should be declared after
# any higher-level components (automations) that depend on them.
vision: Vision
range_finder: RangeFinder
# Automations
motion: ChassisMotion
cubeman: CubeManager
# Actuators
chassis: Chassis
intake: Intake
lifter: Lifter
module_drive_free_speed: float = 7800. # encoder ticks / 100 ms
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"a",
steer_talon=ctre.TalonSRX(48),
drive_talon=ctre.TalonSRX(41),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.TalonSRX(58),
drive_talon=ctre.TalonSRX(51),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.TalonSRX(52),
drive_talon=ctre.TalonSRX(53),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.TalonSRX(42),
drive_talon=ctre.TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left_motor = ctre.TalonSRX(14)
self.intake_right_motor = ctre.TalonSRX(2)
self.clamp_arm = wpilib.Solenoid(2)
self.intake_kicker = wpilib.Solenoid(3)
self.left_extension = wpilib.Solenoid(6)
self.right_extension = wpilib.DoubleSolenoid(forwardChannel=5,
reverseChannel=4)
self.compressor = wpilib.Compressor()
self.lifter_motor = ctre.TalonSRX(3)
self.centre_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
self.intake_cube_switch = wpilib.DigitalInput(3)
# create the imu object
self.imu = NavX()
wpilib.SmartDashboard.putData('IMU', self.imu.ahrs)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
self.range_finder_counter = wpilib.Counter(
4, mode=wpilib.Counter.Mode.kPulseLength)
# wpilib.CameraServer.launch("vision.py:main")
def teleopInit(self):
'''Called when teleop starts; optional'''
self.motion.enabled = False
self.chassis.set_inputs(0, 0, 0)
self.loop_timer = LoopTimer(self.logger)
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 testPeriodic(self):
if self.gamepad.getStartButtonPressed():
self.module_a.store_steer_offsets()
self.module_b.store_steer_offsets()
self.module_c.store_steer_offsets()
self.module_d.store_steer_offsets()
def robotPeriodic(self):
# super().robotPeriodic()
wpilib.SmartDashboard.updateValues()
def createObjects(self):
"""Create motors and stuff here."""
# a + + b - + c - - d + -
x_dist = 0.2625
y_dist = 0.2165
self.module_a = SwerveModule( # front right module
"a",
steer_talon=ctre.TalonSRX(3),
drive_talon=ctre.TalonSRX(4),
x_pos=x_dist,
y_pos=y_dist,
reverse_drive_encoder=True,
reverse_drive_direction=True,
)
self.module_b = SwerveModule( # front left module
"b",
steer_talon=ctre.TalonSRX(5),
drive_talon=ctre.TalonSRX(6),
x_pos=-x_dist,
y_pos=y_dist,
)
self.module_c = SwerveModule( # bottom left module
"c",
steer_talon=ctre.TalonSRX(1),
drive_talon=ctre.TalonSRX(2),
x_pos=-x_dist,
y_pos=-y_dist,
)
self.module_d = SwerveModule( # bottom right module
"d",
steer_talon=ctre.TalonSRX(7),
drive_talon=ctre.TalonSRX(8),
x_pos=x_dist,
y_pos=-y_dist,
)
self.imu = NavX()
self.sd = NetworkTables.getTable("SmartDashboard")
wpilib.SmartDashboard.putData("Gyro", self.imu.ahrs)
# hatch objects
self.hatch_bottom_puncher = wpilib.Solenoid(0)
self.hatch_left_puncher = wpilib.Solenoid(1)
self.hatch_right_puncher = wpilib.Solenoid(2)
self.hatch_left_limit_switch = wpilib.DigitalInput(8)
self.hatch_right_limit_switch = wpilib.DigitalInput(9)
self.climber_front_motor = rev.CANSparkMax(10,
rev.MotorType.kBrushless)
self.climber_back_motor = rev.CANSparkMax(11, rev.MotorType.kBrushless)
self.climber_front_podium_switch = wpilib.DigitalInput(4)
self.climber_back_podium_switch = wpilib.DigitalInput(5)
self.climber_drive_motor = ctre.TalonSRX(20)
self.climber_solenoid = wpilib.DoubleSolenoid(forwardChannel=4,
reverseChannel=5)
# cargo related objects
self.intake_motor = ctre.TalonSRX(9)
self.intake_switch = wpilib.DigitalInput(0)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 2.5
class Robot(magicbot.MagicRobot):
# Declare magicbot components here using variable annotations.
# NOTE: ORDER IS IMPORTANT.
# Any components that actuate objects should be declared after
# any higher-level components (automations) that depend on them.
# Automations
cargo: CargoManager
cargo_deposit: CargoDepositAligner
climb_automation: ClimbAutomation
hatch_deposit: HatchDepositAligner
hatch_intake: HatchIntakeAligner
# Actuators
arm: Arm
chassis: SwerveChassis
hatch: Hatch
intake: Intake
climber: Climber
vision: Vision
offset_rotation_rate = 20
def createObjects(self):
"""Create motors and stuff here."""
# a + + b - + c - - d + -
x_dist = 0.2625
y_dist = 0.2165
self.module_a = SwerveModule( # front right module
"a",
steer_talon=ctre.TalonSRX(3),
drive_talon=ctre.TalonSRX(4),
x_pos=x_dist,
y_pos=y_dist,
reverse_drive_encoder=True,
reverse_drive_direction=True,
)
self.module_b = SwerveModule( # front left module
"b",
steer_talon=ctre.TalonSRX(5),
drive_talon=ctre.TalonSRX(6),
x_pos=-x_dist,
y_pos=y_dist,
)
self.module_c = SwerveModule( # bottom left module
"c",
steer_talon=ctre.TalonSRX(1),
drive_talon=ctre.TalonSRX(2),
x_pos=-x_dist,
y_pos=-y_dist,
)
self.module_d = SwerveModule( # bottom right module
"d",
steer_talon=ctre.TalonSRX(7),
drive_talon=ctre.TalonSRX(8),
x_pos=x_dist,
y_pos=-y_dist,
)
self.imu = NavX()
self.sd = NetworkTables.getTable("SmartDashboard")
wpilib.SmartDashboard.putData("Gyro", self.imu.ahrs)
# hatch objects
self.hatch_bottom_puncher = wpilib.Solenoid(0)
self.hatch_left_puncher = wpilib.Solenoid(1)
self.hatch_right_puncher = wpilib.Solenoid(2)
self.hatch_left_limit_switch = wpilib.DigitalInput(8)
self.hatch_right_limit_switch = wpilib.DigitalInput(9)
self.climber_front_motor = rev.CANSparkMax(10,
rev.MotorType.kBrushless)
self.climber_back_motor = rev.CANSparkMax(11, rev.MotorType.kBrushless)
self.climber_front_podium_switch = wpilib.DigitalInput(4)
self.climber_back_podium_switch = wpilib.DigitalInput(5)
self.climber_drive_motor = ctre.TalonSRX(20)
self.climber_solenoid = wpilib.DoubleSolenoid(forwardChannel=4,
reverseChannel=5)
# cargo related objects
self.intake_motor = ctre.TalonSRX(9)
self.intake_switch = wpilib.DigitalInput(0)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 2.5
def disabledPeriodic(self):
self.chassis.set_inputs(0, 0, 0)
self.imu.resetHeading()
self.vision.execute() # Keep the time offset calcs running
def teleopInit(self):
"""Initialise driver control."""
self.chassis.set_inputs(0, 0, 0)
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 robotPeriodic(self):
super().robotPeriodic()
for module in self.chassis.modules:
self.sd.putNumber(
module.name + "_pos_steer",
module.steer_motor.getSelectedSensorPosition(0),
)
self.sd.putNumber(
module.name + "_pos_drive",
module.drive_motor.getSelectedSensorPosition(0),
)
self.sd.putNumber(
module.name + "_drive_motor_reading",
module.drive_motor.getSelectedSensorVelocity(0) *
10 # convert to seconds
/ module.drive_counts_per_metre,
)
self.sd.putBoolean("heading_hold", self.chassis.hold_heading)
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()
class Robot(magicbot.MagicRobot):
# Declare magicbot components here using variable annotations.
# NOTE: ORDER IS IMPORTANT.
# Any components that actuate objects should be declared after
# any higher-level components (automations) that depend on them.
# Automations
cargo: CargoManager
cargo_deposit: CargoDepositAligner
climb_automation: ClimbAutomation
hatch_deposit: HatchDepositAligner
hatch_intake: HatchIntakeAligner
hatch_automation: HatchAutomation
# Actuators
cargo_component: CargoManipulator
chassis: SwerveChassis
hatch: Hatch
climber: Climber
vision: Vision
offset_rotation_rate = 20
def createObjects(self):
"""Create motors and stuff here."""
# a + + b - + c - - d + -
x_dist = 0.2625
y_dist = 0.2665
self.module_a = SwerveModule( # front left module
"a",
steer_talon=ctre.TalonSRX(3),
drive_talon=ctre.TalonSRX(4),
x_pos=x_dist,
y_pos=y_dist,
)
self.module_b = SwerveModule( # front right module
"b",
steer_talon=ctre.TalonSRX(7),
drive_talon=ctre.TalonSRX(8),
x_pos=-x_dist,
y_pos=y_dist,
)
self.module_c = SwerveModule( # bottom left module
"c",
steer_talon=ctre.TalonSRX(1),
drive_talon=ctre.TalonSRX(6),
x_pos=-x_dist,
y_pos=-y_dist,
)
self.module_d = SwerveModule( # bottom right module
"d",
steer_talon=ctre.TalonSRX(23),
drive_talon=ctre.TalonSRX(24),
x_pos=x_dist,
y_pos=-y_dist,
)
self.imu = NavX()
wpilib.SmartDashboard.putData("Gyro", self.imu.ahrs)
# hatch objects
self.hatch_fingers = wpilib.DoubleSolenoid(7, 6)
self.hatch_punchers = wpilib.Solenoid(0)
self.hatch_enable_piston = wpilib.DoubleSolenoid(3, 2)
self.hatch_left_limit_switch = wpilib.DigitalInput(8)
self.hatch_right_limit_switch = wpilib.DigitalInput(9)
self.climber_front_motor = rev.CANSparkMax(10, rev.MotorType.kBrushless)
self.climber_back_motor = rev.CANSparkMax(11, rev.MotorType.kBrushless)
self.climber_front_podium_switch = wpilib.DigitalInput(4)
self.climber_back_podium_switch = wpilib.DigitalInput(5)
self.climber_drive_motor = ctre.TalonSRX(20)
self.climber_pistons = wpilib.DoubleSolenoid(forwardChannel=4, reverseChannel=5)
# cargo related objects
self.intake_motor = ctre.VictorSPX(9)
self.intake_switch = wpilib.DigitalInput(0)
self.arm_motor = rev.CANSparkMax(2, rev.MotorType.kBrushless)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
def autonomous(self):
self.imu.resetHeading()
self.chassis.set_heading_sp(0)
self.hatch.enable_hatch = True
self.hatch_intake.alignment_speed = 0.5
self.hatch_deposit.alignment_speed = 0.5
self.chassis.derate_drive_modules(6)
super().autonomous()
def disabledPeriodic(self):
self.chassis.set_inputs(0, 0, 0)
self.vision.execute() # Keep the time offset calcs running
def teleopInit(self):
"""Initialise driver control."""
self.chassis.set_inputs(0, 0, 0)
self.hatch_intake.alignment_speed = 0.5
self.hatch_deposit.alignment_speed = 0.5
self.chassis.derate_drive_modules(9)
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 robotPeriodic(self):
# super().robotPeriodic()
wpilib.SmartDashboard.updateValues()
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)
class Robot(magicbot.MagicRobot):
# Add magicbot components here using variable annotations.
# Any components that directly actuate motors should be declared after
# any higher-level components (automations) that depend on them.
vision: Vision
range_finder: RangeFinder
# Automations
motion: ChassisMotion
intake_automation: IntakeAutomation
lifter_automation: LifterAutomation
# Actuators
chassis: SwerveChassis
intake: Intake
lifter: Lifter
module_drive_free_speed: float = 7800. # encoder ticks / 100 ms
length: float = 0.88
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
"a",
steer_talon=ctre.TalonSRX(48),
drive_talon=ctre.TalonSRX(49),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.TalonSRX(46),
drive_talon=ctre.TalonSRX(47),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.TalonSRX(44),
drive_talon=ctre.TalonSRX(45),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.TalonSRX(42),
drive_talon=ctre.TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left_motor = ctre.TalonSRX(14)
self.intake_right_motor = ctre.TalonSRX(2)
self.clamp_arm = wpilib.Solenoid(2)
self.intake_kicker = wpilib.Solenoid(3)
self.left_extension = wpilib.Solenoid(6)
self.right_extension = wpilib.DoubleSolenoid(forwardChannel=5,
reverseChannel=4)
self.compressor = wpilib.Compressor()
self.lifter_motor = ctre.TalonSRX(3)
self.centre_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
# create the imu object
self.imu = NavX()
# wpilib.SmartDashboard.putData('gyro', self.imu.ahrs)
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 2
self.sd = NetworkTables.getTable("SmartDashboard")
self.range_finder_counter = wpilib.Counter(
4, mode=wpilib.Counter.Mode.kPulseLength)
def teleopInit(self):
'''Called when teleop starts; optional'''
self.motion.enabled = False
self.chassis.set_inputs(0, 0, 0)
self.loop_timer = LoopTimer(self.logger)
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 testPeriodic(self):
if self.gamepad.getStartButtonPressed():
self.module_a.store_steer_offsets()
self.module_b.store_steer_offsets()
self.module_c.store_steer_offsets()
self.module_d.store_steer_offsets()
def robotPeriodic(self):
# super().robotPeriodic()
if self.lifter.set_pos is not None:
self.sd.putNumber("lift/set_pos", self.lifter.set_pos)
class Robot(magicbot.MagicRobot):
# Add magicbot components here using variable annotations.
# Any components that directly actuate motors should be declared after
# any higher-level components (automations) that depend on them.
chassis: Chassis
module_drive_free_speed: float = 7800. # encoder ticks / 100 ms
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"a",
steer_talon=ctre.TalonSRX(42),
drive_talon=ctre.TalonSRX(48),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # top left module
# "a", steer_talon=ctre.TalonSRX(48), drive_talon=ctre.TalonSRX(49),
"b",
steer_talon=ctre.TalonSRX(58),
drive_talon=ctre.TalonSRX(2),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
# create the imu object
self.imu = NavX()
self.sd = NetworkTables.getTable("SmartDashboard")
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 1.5
def teleopInit(self):
'''Called when teleop starts; optional'''
self.chassis.set_inputs(0, 0, 0)
self.loop_timer = LoopTimer(self.logger)
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 testPeriodic(self):
if self.gamepad.getStartButtonPressed():
self.module_a.store_steer_offsets()
self.module_b.store_steer_offsets()
def robotPeriodic(self):
# super().robotPeriodic()
self.sd.putNumber("imu_heading", self.imu.getAngle())
class Robot(magicbot.MagicRobot):
# Add magicbot components here using variable annotations.
# Any components that directly actuate motors should be declared after
# any higher-level components (automations) that depend on them.
vision: Vision
# Automations
position_filter: PositionFilter
motion: ChassisMotion
intake_automation: IntakeAutomation
lifter_automation: LifterAutomation
# Actuators
chassis: SwerveChassis
intake: Intake
lifter: Lifter
module_drive_free_speed: float = 7800. # encoder ticks / 100 ms
length: float = 0.88
def createObjects(self):
"""Create non-components here."""
self.module_a = SwerveModule( # top left module
"a",
steer_talon=ctre.WPI_TalonSRX(48),
drive_talon=ctre.WPI_TalonSRX(49),
x_pos=0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_b = SwerveModule( # bottom left modulet
"b",
steer_talon=ctre.WPI_TalonSRX(46),
drive_talon=ctre.WPI_TalonSRX(47),
x_pos=-0.25,
y_pos=0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_c = SwerveModule( # bottom right modulet
"c",
steer_talon=ctre.WPI_TalonSRX(44),
drive_talon=ctre.WPI_TalonSRX(45),
x_pos=-0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.module_d = SwerveModule( # top right modulet
"d",
steer_talon=ctre.WPI_TalonSRX(42),
drive_talon=ctre.WPI_TalonSRX(43),
x_pos=0.25,
y_pos=-0.31,
drive_free_speed=Robot.module_drive_free_speed)
self.intake_left_motor = ctre.WPI_TalonSRX(14)
self.intake_right_motor = ctre.WPI_TalonSRX(2)
self.clamp_arm = wpilib.Solenoid(0)
self.intake_kicker = wpilib.Solenoid(1)
self.extension_arms = wpilib.Solenoid(3)
self.infrared = SharpIRGP2Y0A41SK0F(0)
self.lifter_motor = ctre.WPI_TalonSRX(3)
self.centre_switch = wpilib.DigitalInput(1)
self.top_switch = wpilib.DigitalInput(2)
# create the imu object
self.imu = NavX()
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = wpilib.XboxController(1)
self.spin_rate = 5
self.sd = NetworkTables.getTable("SmartDashboard")
def teleopInit(self):
'''Called when teleop starts; optional'''
self.motion.enabled = False
self.chassis.set_inputs(0, 0, 0)
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)
def robotPeriodic(self):
if self.lifter.set_pos is not None:
self.sd.putNumber("lift/set_pos", self.lifter.set_pos)
self.sd.putNumber("lift/pos", self.lifter.get_pos())
self.sd.putNumber(
"lift/velocity",
self.lifter.motor.getSelectedSensorVelocity(0) /
self.lifter.COUNTS_PER_METRE)
self.sd.putNumber("lift/current", self.lifter.motor.getOutputCurrent())