class OI():
__instances = None
DEADZONE = 1 / 128
def __init__(self):
self.driverController = XboxController(0)
self.manipulatorController = XboxController(1)
self.aButtonDriver = JoystickButton(self.driverController, 0)
self.aButtonDriver.whenPressed(DriveStraightCommand(10))
@staticmethod
def getInstance() -> OI:
if (OI.__instances == None):
OI.__instances = OI()
return OI.__instances
def __deadzone(self, input, deadzone=DEADZONE) -> Number:
absValue = math.fabs(input)
# no signum ;-;
return 0 if abs < deadzone else ((absValue - deadzone) /
(1.0 - deadzone) *
math.copysign(1, input))
def getLeftY(self):
return self.__deadzone(
self.driverController.getY(GenericHID.Hand.kLeft))
def getRightY(self):
return self.__deadzone(
self.driverController.getY(GenericHID.Hand.kRight))
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
#self.lf_motor = wpilib.Victor(0b00) # => 0
#self.lr_motor = wpilib.Victor(0b01) # => 1
#self.rf_motor = wpilib.Victor(0b10) # => 2
#self.rr_motor = wpilib.Victor(0b11) # => 3
# TODO: This is not in any way an ideal numbering system.
# The PWM ports should be redone to use the old layout above.
self.lf_motor = wpilib.Victor(9)
self.lr_motor = wpilib.Victor(8)
self.rf_motor = wpilib.Victor(7)
self.rr_motor = wpilib.Victor(6)
self.drivetrain = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(
self.intake_wheel_left, self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_intake_in = JoystickButton(self.joystick_alt, 2)
self.btn_intake_out = JoystickButton(self.joystick_alt, 1)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
VictorSP(0),
VictorSP(1),
)
self.right_motors = wpilib.SpeedControllerGroup(
VictorSP(2),
VictorSP(3),
)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Intake
self.intake_motor = VictorSP(4)
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
self.lf_motor = wpilib.Victor(0b00) # =>0
self.lr_motor = wpilib.Victor(0b01) # =>1
self.rf_motor = wpilib.Victor(0b10) # =>2
self.rr_motor = wpilib.Victor(0b11) # =>3
self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(self.intake_wheel_left,
self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def __init__(self):
self.driverController = XboxController(0)
self.manipulatorController = XboxController(1)
self.aButtonDriver = JoystickButton(self.driverController, 0)
self.aButtonDriver.whenPressed(DriveStraightCommand(10))
class Bot(magicbot.MagicRobot):
drive: drive.Drive
intake: intake.Intake
def createObjects(self):
# Joysticks
self.joystick = wpilib.Joystick(0)
# Drive motor controllers
# Dig | 0/1
# 2^1 | Left/Right
# 2^0 | Front/Rear
self.lf_motor = wpilib.Victor(0b00) # =>0
self.lr_motor = wpilib.Victor(0b01) # =>1
self.rf_motor = wpilib.Victor(0b10) # =>2
self.rr_motor = wpilib.Victor(0b11) # =>3
self.drivetrain = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
self.btn_sarah = ButtonDebouncer(self.joystick, 2)
self.sarah = False
# Intake
self.intake_wheel_left = wpilib.Spark(5)
self.intake_wheel_right = wpilib.Spark(4)
self.intake_wheels = wpilib.SpeedControllerGroup(self.intake_wheel_left,
self.intake_wheel_right)
self.intake_wheels.setInverted(True)
self.btn_pull = JoystickButton(self.joystick, 3)
self.btn_push = JoystickButton(self.joystick, 1)
def autonomous(self):
super().autonomous()
def disabledPeriodic(self): pass
def disabledInit(self): pass
def teleopInit(self): pass
def teleopPeriodic(self):
# Normal joysticks
self.drive.move(-self.joystick.getY(),self.joystick.getX())
# Corrections for aviator joystick
#self.drive.move(-2*(self.joystick.getY()+.5),
# 2*(self.joystick.getX()+.5)+ROT_COR,
# sarah=self.sarah)
if self.btn_sarah:
self.sarah = not self.sarah
if self.btn_pull.get():
self.intake.pull()
elif self.btn_push.get():
self.intake.push()
def __init__(self, robot):
self.robot = robot
self.driver_joystick = wpilib.Joystick(0)
self.copilot_joystick = wpilib.Joystick(1)
self.driver_a_button = JoystickButton(self.driver_joystick, ctrlmap.BUTTON_A)
self.driver_b_button = JoystickButton(self.driver_joystick, ctrlmap.BUTTON_B)
self.driver_a_button.whenPressed(dcmds.SetDriveSpeedMult(robot, 1))
self.driver_b_button.whenPressed(dcmds.SetDriveSpeedMult(robot, 0.5))
def __init__(self, fl, rl, fr, rr, navx, stick):
self.fl = fl
self.rl = rl
self.fr = fr
self.rr = rr
self.drive = wpilib.drive.MecanumDrive(fl, rl, fr, rr)
self.navx = navx
self.stick = stick
self.foctoggle = JoystickButton(self.stick, 11)
self.focenabled = False
self.foctoggledown = False
def getJoystick():
"""
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
"""
joystick = Joystick(0)
trigger = JoystickButton(joystick, Joystick.ButtonType.kTriggerButton)
trigger.whenPressed(Crash())
return joystick
def init():
global joystick, A, B, X, Y, bumper_L, bumper_R, back, start, stick_L, stick_R
joystick = Joystick(robotmap.joystick)
A = JoystickButton(joystick, 1)
B = JoystickButton(joystick, 2)
X = JoystickButton(joystick, 3)
Y = JoystickButton(joystick, 4)
bumper_L = JoystickButton(joystick, 5)
bumper_R = JoystickButton(joystick, 6)
back = JoystickButton(joystick, 7)
start = JoystickButton(joystick, 8)
stick_L = JoystickButton(joystick, 9)
stick_R = JoystickButton(joystick, 10)
def init():
global logger, joystick, A, B, X, Y, bumper_L, bumper_R, back, start, stick_L, stick_R
logger = logging.getLogger("OI")
joystick = Joystick(robotmap.joystick)
A = JoystickButton(joystick, 1)
B = JoystickButton(joystick, 2)
X = JoystickButton(joystick, 3)
Y = JoystickButton(joystick, 4)
bumper_L = JoystickButton(joystick, 5)
bumper_R = JoystickButton(joystick, 6)
back = JoystickButton(joystick, 7)
start = JoystickButton(joystick, 8)
stick_L = JoystickButton(joystick, 9)
stick_R = JoystickButton(joystick, 10)
logger.debug("OI initialized")
def initPortcullis(self):
s = self.aimStick
self.pcUpButton = JoystickButton(s, OI.k_pcUpButton)
self.pcUpButton.whenPressed(PCmds.MoveUp(self.robot))
self.pcDownButton = JoystickButton(s, OI.k_pcDownButton)
self.pcDownButton.whenPressed(PCmds.MoveDown(self.robot))
self.pcBarInButton = JoystickButton(s, OI.k_pcBarInButton)
self.pcBarInButton.whenPressed(PCmds.BarIn(self.robot))
self.pcBarOutButton = JoystickButton(s, OI.k_pcBarOutButton)
self.pcBarOutButton.whenPressed(PCmds.BarOut(self.robot))
self.pcBarStopButton = JoystickButton(s, OI.k_pcBarStopButton)
self.pcBarStopButton.whenPressed(PCmds.BarStop(self.robot))
class Robot(MagicRobot):
def createObjects(self):
self.joystick = wpilib.Joystick(2)
self.winch_button = JoystickButton(self.joystick, 6)
self.cp_extend_button = JoystickButton(self.joystick, 5)
self.cp_solenoid = wpilib.DoubleSolenoid(4, 5)
self.cp_motor = CANSparkMax(10, MotorType.kBrushed)
self.solenoid = wpilib.Solenoid(0)
self.btn_solenoid = JoystickButton(self.joystick, 1)
self.intake = WPI_VictorSPX(1)
self.cp_motor_button = JoystickButton(self.joystick, 7)
self.winch_motor = CANSparkMax(8, MotorType.kBrushless)
self.six = JoystickButton(self.joystick, 12)
self.shooter_motor = CANSparkMax(7, MotorType.kBrushed)
self.shooter_motor.restoreFactoryDefaults()
self.shooter_motor.setOpenLoopRampRate(0)
self.intake_in = JoystickButton(self.joystick, 3)
self.intake_out = JoystickButton(self.joystick, 4)
def teleopPeriodic(self):
if self.winch_button.get():
self.winch_motor.set(1)
else:
self.winch_motor.set(0)
if self.six.get():
# 75% is good for initiation line
self.shooter_motor.set(0.75)
else:
self.shooter_motor.set(0)
self.solenoid.set(self.btn_solenoid.get())
if self.intake_in.get():
self.intake.set(-1)
elif self.intake_out.get():
self.intake.set(1)
else:
self.intake.set(0)
if self.cp_motor_button.get():
self.cp_motor.set(0.7)
else:
self.cp_motor.set(0)
if self.cp_extend_button.get(
) and self.cp_solenoid.get() != wpilib.DoubleSolenoid.Value.kReverse:
self.cp_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
if not self.cp_extend_button.get(
) and self.cp_solenoid.get() != wpilib.DoubleSolenoid.Value.kForward:
self.cp_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
def createObjects(self):
wpilib.CameraServer.launch()
wpilib.LiveWindow.disableAllTelemetry()
self.left_drive_motor = WPI_TalonSRX(0)
WPI_TalonSRX(1).set(WPI_TalonSRX.ControlMode.Follower,
self.left_drive_motor.getDeviceID())
self.right_drive_motor = WPI_TalonSRX(2)
WPI_TalonSRX(3).set(WPI_TalonSRX.ControlMode.Follower,
self.right_drive_motor.getDeviceID())
self.robot_drive = wpilib.drive.DifferentialDrive(
self.left_drive_motor, self.right_drive_motor)
self.r_intake_motor = WPI_VictorSPX(4)
self.l_intake_motor = WPI_VictorSPX(5)
self.elevator_winch = WPI_TalonSRX(6)
self.climber_motor = WPI_TalonSRX(7)
self.wrist_motor = WPI_TalonSRX(8)
self.intake_ir = wpilib.AnalogInput(0)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 3)
self.right_drive_joystick = wpilib.Joystick(0)
self.left_drive_joystick = wpilib.Joystick(1)
self.operator_joystick = wpilib.Joystick(2)
self.compressor = wpilib.Compressor()
self.elevator_limit_switch = wpilib.DigitalInput(0)
self.climber_motor = WPI_TalonSRX(7)
self.navx = AHRS.create_spi()
self.path_tracking_table = NetworkTables.getTable("path_tracking")
self.down_button = ButtonDebouncer(self.operator_joystick, 1)
self.right_button = ButtonDebouncer(self.operator_joystick, 2)
self.left_button = ButtonDebouncer(self.operator_joystick, 3)
self.has_cube_button = ButtonDebouncer(self.operator_joystick, 5)
self.up_button = ButtonDebouncer(self.operator_joystick, 4)
self.left_bumper_button = JoystickButton(self.operator_joystick, 5)
self.right_bumper_button = JoystickButton(self.operator_joystick, 6)
class OI:
"""
Operator interface of the robot.
Manages commands to run based on driver input.
"""
def __init__(self, robot):
self.robot = robot
self.driver_joystick = wpilib.Joystick(0)
self.copilot_joystick = wpilib.Joystick(1)
self.driver_a_button = JoystickButton(self.driver_joystick, ctrlmap.BUTTON_A)
self.driver_b_button = JoystickButton(self.driver_joystick, ctrlmap.BUTTON_B)
self.driver_a_button.whenPressed(dcmds.SetDriveSpeedMult(robot, 1))
self.driver_b_button.whenPressed(dcmds.SetDriveSpeedMult(robot, 0.5))
def createObjects(self):
"""
Initialize all wpilib motors & sensors
"""
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
self.btn_fine_movement = JoystickButton(self.joystick_right, 2)
# Drive motor controllers
# ID SCHEME:
# 10^1: 1 = left, 2 = right
# 10^0: 0 = front, 5 = rear
self.lf_motor = WPI_TalonSRX(10)
self.lr_motor = WPI_VictorSPX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_VictorSPX(25)
self.lf_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
self.rf_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
# Following masters
self.lr_motor.follow(self.lf_motor)
self.rr_motor.follow(self.rf_motor)
# Drive init
self.train = wpilib.drive.DifferentialDrive(
self.lf_motor, self.rf_motor
)
# Arm
self.arm_motor = WPI_TalonSRX(0)
self.arm_motor.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Absolute
)
self.arm_limit = wpilib.DigitalInput(4)
# Gyro
self.gyro = navx.AHRS.create_spi()
self.gyro.reset()
self.control_loop_wait_time = 0.02
def __init__(self, hs1, hs2, as1, as2, stick):
self.hatchsolenoid1 = hs1
self.hatchsolenoid2 = hs2
self.armsolenoid1 = as1
self.armsolenoid2 = as2
# the corresponding solenoids for each subsystem must be in opposite
# states.
self.hatchsolenoid1.set(False)
self.hatchsolenoid2.set(True)
self.armsolenoid1.set(False)
self.armsolenoid2.set(True)
# Currently trigger and front button.
# TODO separate controls out into separate config class.
self.hatchbutton = JoystickButton(stick, 2)
self.armbutton = JoystickButton(stick, 1)
self.hatchenabled = False
self.armenabled = False
class Robot(magicbot.MagicRobot):
drive: Drive
intake: Intake
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_intake_in = JoystickButton(self.joystick_alt, 2)
self.btn_intake_out = JoystickButton(self.joystick_alt, 1)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
VictorSP(0),
VictorSP(1),
)
self.right_motors = wpilib.SpeedControllerGroup(
VictorSP(2),
VictorSP(3),
)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Intake
self.intake_motor = VictorSP(4)
def teleopPeriodic(self):
self.drive.move(-self.joystick_left.getY(), self.joystick_right.getX())
# Intake
if self.btn_intake_out.get():
if self.joystick_right.getX() >= -0.1 and self.joystick_right.getX(
) <= 0.1:
self.intake.spin(1)
elif self.btn_intake_in.get():
self.intake.spin(-1)
def __init__(self, robot):
# driver mappings
self.driver_joystick = wpilib.XboxController(0)
JoystickButton(self.driver_joystick,
wpilib.XboxController.Button.kX).whenPressed(
TurnByGyro(robot, -90.0))
JoystickButton(self.driver_joystick,
wpilib.XboxController.Button.kY).whenPressed(
TurnByGyro(robot, 90.0))
JoystickButton(self.driver_joystick,
wpilib.XboxController.Button.kBack).whenPressed(
ClimbByGyro(robot))
JoystickButton(self.driver_joystick,
wpilib.XboxController.Button.kStart).whenPressed(
ToggleCompressor(robot))
# operator mappings
self.operator_joystick = wpilib.XboxController(1)
JoystickButton(self.operator_joystick,
wpilib.XboxController.Button.kA).whenPressed(
DeployHatch(robot))
JoystickButton(self.operator_joystick,
wpilib.XboxController.Button.kB).whenPressed(
DropCargo(robot))
JoystickButton(self.operator_joystick,
wpilib.XboxController.Button.kY).whenPressed(
ToggleLED(robot))
JoystickButton(self.operator_joystick,
wpilib.XboxController.Button.kY).whileHeld(
AlignByCamera(robot))
JoystickButton(self.operator_joystick,
wpilib.XboxController.Button.kY).whenReleased(
ToggleLED(robot))
JoystickButton(self.operator_joystick,
wpilib.XboxController.Button.kStart).whenPressed(
ToggleCompressor(robot))
class OI:
def __init__(self, robot):
self.robot = robot
self.xbox = wpilib.Joystick(0)
self.l_trigger = self.xbox.getRawAxis(2)
self.r_trigger = self.xbox.getRawAxis(3)
self.l_bumper = JoystickButton(self.xbox, 6)
self.r_bumper = JoystickButton(self.xbox, 5)
self.a_button = JoystickButton(self.xbox, 1)
self.b_button = JoystickButton(self.xbox, 2)
self.steer = self.xbox.getRawAxis(0)
self.speed = self.l_trigger - self.r_trigger
self.a_button.whileHeld(Pop(self.robot))
self.r_bumper.whileHeld(LiftFront(self.robot))
self.l_bumper.whileHeld(LiftRear(self.robot))
#self.a_button.toggleWhenPressed(DriveBot(self.robot))
#self.b_button.toggleWhenPressed(Circles(self.robot))
def getSteer(self):
return 0.8 * (self.xbox.getRawAxis(0)**3)
def getSpeed(self):
l_trigger = self.xbox.getRawAxis(2)
r_trigger = self.xbox.getRawAxis(3)
return 0.8 * ((l_trigger - r_trigger)**3)
def getIntakeSpeed(self):
return self.xbox.getRawAxis(5)
def robotInit(self):
try:
wpilib.CameraServer.launch()
except:
pass
self.stick = wpilib.Joystick(0)
self.base = Base(rev.CANSparkMax(4, rev.MotorType.kBrushless),
rev.CANSparkMax(2, rev.MotorType.kBrushless),
rev.CANSparkMax(3, rev.MotorType.kBrushless),
rev.CANSparkMax(1, rev.MotorType.kBrushless),
AHRS.create_spi(wpilib.SPI.Port.kMXP), self.stick)
self.lift = Lift(WPI_TalonSRX(3), wpilib.DigitalInput(0),
wpilib.DigitalInput(1), self.stick)
self.arm = Arm(wpilib.Solenoid(5, 2), wpilib.Solenoid(5, 4),
wpilib.Solenoid(5, 1), wpilib.Solenoid(5, 5),
self.stick)
self.testvar = 0
self.ballintake = BallIntake(WPI_TalonSRX(4))
try:
self.ledController = LEDController.getInstance()
except:
pass
# Various one-time initializations happen here.
self.lift.initSensor()
self.base.navx.reset()
try:
NetworkTables.initialize()
except:
pass
try:
self.visiontable = NetworkTables.getTable("visiontable")
except:
pass
self.lights = False
self.op = False
self.testButton = JoystickButton(self.stick, 16)
self.roller = Roller(WPI_TalonSRX(20), WPI_TalonSRX(10), self.stick,
"init")
def __init__(self, robot):
self.joystick = wpilib.Joystick(0)
JoystickButton(self.joystick, 12).whenPressed(LowGoal(robot))
JoystickButton(self.joystick, 10).whenPressed(Collect(robot))
JoystickButton(self.joystick,
11).whenPressed(SetPivotSetpoint(robot, Pivot.SHOOT))
JoystickButton(self.joystick,
9).whenPressed(SetPivotSetpoint(robot,
Pivot.SHOOT_NEAR))
DoubleButton(self.joystick, 2, 3).whenActive(Shoot(robot))
# SmartDashboard Buttons
SmartDashboard.putData("Drive Forward", DriveForward(robot, 2.25))
SmartDashboard.putData("Drive Backward", DriveForward(robot, -2.25))
SmartDashboard.putData("Start Rollers",
SetCollectionSpeed(robot, Collector.FORWARD))
SmartDashboard.putData("Stop Rollers",
SetCollectionSpeed(robot, Collector.STOP))
SmartDashboard.putData("Reverse Rollers",
SetCollectionSpeed(robot, Collector.REVERSE))
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
while self.isEnabled():
self.drive.driveCartesian(
-self.conditonAxis(self.lstick.getX(), 0.05, 0.85, 0.6, 1.5,
-1, 1),
self.conditonAxis(self.lstick.getY(), 0.05, 0.85, 0.6, 1.5, -1,
1),
-self.conditonAxis(self.rstick.getX(), 0.25, 0.85, 0.6, 0.5,
-1, 1), self.gyro.getYaw())
if self.timer.get() > 1:
if (JoystickButton(self.lstick, 3).get()):
self.timer.reset()
self.COMPRESSOR_STATE = not self.COMPRESSOR_STATE
if self.COMPRESSOR_STATE:
self.compressor.start()
else:
self.compressor.stop()
#pilotstick 1
if JoystickButton(self.lstick, 5).get():
self.BallDoorOpen()
if JoystickButton(self.lstick, 4).get():
self.BallDoorClose()
if JoystickButton(self.lstick, 6).get():
self.BallGrabStart()
if JoystickButton(self.lstick, 7).get():
self.BallGrabStop()
#pilotstick 2
if JoystickButton(self.rstick, 3).get():
self.activate(self.GearDoorRaise, self.GearDoorDrop,
self.GEAR_DOOR_STATE)
if JoystickButton(self.rstick, 5).get():
self.activate(self.GearAdjustExtend,
self.GearAdjustRetract,
self.GEAR_ADJUST_STATE)
if JoystickButton(self.rstick, 4).get():
self.activate(self.GearPusherExtend,
self.GearPusherRetract,
self.GEAR_PUSHER_STATE)
def teleopPeriodic(self):
"""Called when operation control mode is enabled"""
while self.isEnabled():
'''
self.logger.info("FrontLeft: " + str(self.frontLeftMotor.getMotorOutputVoltage()))
self.logger.info("RearLeft: " + str(self.rearLeftMotor.getMotorOutputVoltage()))
self.logger.info("FrontRight: " + str(self.frontRightMotor.getMotorOutputVoltage()))
self.logger.info("RearRight: " + str(self.rearRightMotor.getMotorOutputVoltage()))
'''
self.drive.driveCartesian(
-self.conditonAxis(self.lstick.getX(), 0.05, 0.85, 0.6, 1.5,
-1, 1),
self.conditonAxis(self.lstick.getY(), 0.05, 0.85, 0.6, 1.5, -1,
1),
-self.conditonAxis(self.rstick.getX(), 0.25, 0.85, 0.6, 0.5,
-1, 1)
#self.gyro.getYaw()
)
if self.solenoid_timer.hasPeriodPassed(0.5):
if JoystickButton(self.lstick, 3).get():
self.COMPRESSOR_STATE = not self.COMPRESSOR_STATE
self.activate(self.compressor.start, self.compressor.stop,
self.COMPRESSOR_STATE)
if JoystickButton(self.lstick, 2).get():
self.TICKLER_STATE = not self.TICKLER_STATE
self.activate(self.BallDoorOpen, self.BallDoorClose,
self.TICKLER_STATE)
if JoystickButton(self.lstick, 1).get():
self.GRABBER_STATE = not self.GRABBER_STATE
self.activate(self.BallGrabStart, self.BallGrabStop,
self.GRABBER_STATE)
#pilotstick 2
if JoystickButton(self.rstick, 3).get():
self.GEAR_DOOR_STATE = not self.GEAR_DOOR_STATE
self.activate(self.GearDoorRaise, self.GearDoorDrop,
self.GEAR_DOOR_STATE)
if JoystickButton(self.rstick, 5).get():
self.GEAR_ADJUST_STATE = not self.GEAR_ADJUST_STATE
self.activate(self.GearAdjustExtend,
self.GearAdjustRetract,
self.GEAR_ADJUST_STATE)
if JoystickButton(self.rstick, 4).get():
self.GEAR_PUSHER_STATE = not self.GEAR_PUSHER_STATE
self.activate(self.GearPusherExtend,
self.GearPusherRetract,
self.GEAR_PUSHER_STATE)
wpilib.Timer.delay(0.04)
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 = XboxDPadButton(self, DPAD_BUTTON.DPAD_UP)
self.btnDpadRight = XboxDPadButton(self, DPAD_BUTTON.DPAD_RIGHT)
self.btnDpadDown = XboxDPadButton(self, DPAD_BUTTON.DPAD_DOWN)
self.btnDpadLeft = XboxDPadButton(self, DPAD_BUTTON.DPAD_LEFT)
def robotInit(self):
# Robot initialization function
# VictorSPX = Motor Controllers
self.frontLeftMotor = ctre.WPI_VictorSPX(0)
self.rearLeftMotor = ctre.WPI_VictorSPX(1)
self.frontRightMotor = ctre.WPI_VictorSPX(4)
self.rearRightMotor = ctre.WPI_VictorSPX(5)
self.basketMotor = ctre.WPI_VictorSPX(3)
# Digital Inputs (Limit Switch)
self.basketLimitSwitch = wpilib.DigitalInput(0)
# Motor controller groups for each side of the robot
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor,
self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor,
self.rearRightMotor)
# Differential drive with left and right motor controller groups
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
self.direction = -1
# Declare gamepad
self.gamepad = wpilib.Joystick(0)
# Declare buttons
# Controller mapping (1-10): B (East), A (South), Y (North), X (West), Right Bumper, Left Bumper, ?, ?, ?, ?
self.toggleHatchButton = JoystickButton(self.gamepad, 1)
self.toggleBasketButton = JoystickButton(self.gamepad, 2)
self.toggleDirectionButton = JoystickButton(self.gamepad, 3)
self.speedUpButton = JoystickButton(self.gamepad, 4)
self.raiseBasketButton = JoystickButton(self.gamepad, 5)
self.lowerBasketButton = JoystickButton(self.gamepad, 6)
# Determine if already acted on
self.hatchActed = False
self.basketActed = False
self.directionActed = False
# Solenoids
self.hatchSolenoid = wpilib.DoubleSolenoid(0, 1)
self.basketSolenoid = wpilib.DoubleSolenoid(2, 3)
# Compressor
self.compressor = wpilib.Compressor(0)
# Camera Server
wpilib.CameraServer.launch()
def __init__(self, port: int = robotmap.joystick):
super().__init__(port)
buttons = {
'a': 1,
'b': 2,
'x': 3,
'y': 4,
'bumper_l': 5,
'bumper_r': 6,
'back': 7,
'start': 8,
'l_3': 9,
'r_3': 10
}
for button, number in buttons.items():
self.__dict__[str(button)] = JoystickButton(self, number)
def robotInit(self):
Command.getRobot = lambda x=0: self
self.driveSubsystem = DriveSubsystem()
self.controller = Joystick(0)
self.forward = JoystickButton(self.controller, PS4Button["TRIANGLE"])
self.backward = JoystickButton(self.controller, PS4Button["CROSS"])
self.right = JoystickButton(self.controller, PS4Button["CIRCLE"])
self.left = JoystickButton(self.controller, PS4Button["SQUARE"])
self.forward.whenPressed(MoveForward())
self.forward.whenReleased(Stop())
self.backward.whenPressed(MoveBackward())
self.backward.whenReleased(Stop())
self.right.whenPressed(TurnRight())
self.right.whenReleased(Stop())
self.left.whenPressed(TurnLeft())
self.left.whenReleased(Stop())
def __init__(self, robot):
self.robot = robot
self.xbox = wpilib.Joystick(0)
self.l_trigger = self.xbox.getRawAxis(2)
self.r_trigger = self.xbox.getRawAxis(3)
self.l_bumper = JoystickButton(self.xbox, 6)
self.r_bumper = JoystickButton(self.xbox, 5)
self.a_button = JoystickButton(self.xbox, 1)
self.b_button = JoystickButton(self.xbox, 2)
self.steer = self.xbox.getRawAxis(0)
self.speed = self.l_trigger - self.r_trigger
self.a_button.whileHeld(Pop(self.robot))
self.r_bumper.whileHeld(LiftFront(self.robot))
self.l_bumper.whileHeld(LiftRear(self.robot))
def initLauncher(self):
aS = self.aimStick
dS = self.driveStick
self.kickBallButton = JoystickButton(aS, OI.k_kickBallButton)
self.kickBallButton.whenPressed(ILCmds.LaunchBallCommandGroup(self.robot))
# TODO: Java code had two buttons for stop wheels
self.mechStopWheelsButton = JoystickButton(aS, OI.k_stopIntakeWheelsButton)
self.mechStopWheelsButton.whenPressed(ILCmds.StopWheelsCommand(self.robot))
self.grabBallButton = JoystickButton(dS, OI.k_intakeBallButton)
self.grabBallButton.whenPressed(ILCmds.IntakeBallCommandGroup(self.robot))
self.spinIntakeWheelsOutButton = JoystickButton(aS, OI.k_spinIntakeWheelsOutButton)
self.spinIntakeWheelsOutButton.whenPressed(ILCmds.SpinIntakeWheelsCommand(self.robot, "out"))
self.driveLauncherJumpToIntakeButton = JoystickButton(dS, OI.k_aimIntakeButton)
self.driveLauncherJumpToIntakeButton.whenPressed(ILCmds.LauncherGoToIntakeCommand(self.robot))
# TODO: Java code had two buttons for neutral
self.driveLauncherJumpToNeutralButton = JoystickButton(aS, OI.k_aimNeutralButton)
self.driveLauncherJumpToNeutralButton.whenPressed(ILCmds.LauncherGoToNeutralCommand(self.robot))
def __init__(self, robot):
self.driver = Joystick(0)
JoystickButton(self.driver, 3).whenPressed(OpenGripper(robot))
JoystickButton(self.driver, 4).whenPressed(CloseGripper(robot))
JoystickButton(self.driver, 1).whenPressed(ToggleGripper(robot))
def __init__(self, robot):
"""Double joysticks WOOT"""
print("Initializing joysticks...")
#Initialise the stick and the smart dashboard (in case we need stuff for auton):
self.stick = wpilib.Joystick(0)
self.setpointStick = wpilib.Joystick(1)
self.smart_dashboard = NetworkTable.getTable("SmartDashboard")
#Main stick POV.
#-----------------------------------------------------------------------
drive_north = POVButton(self.stick, 0)
drive_northeast = POVButton(self.stick, 45)
drive_east = POVButton(self.stick, 90)
drive_southeast = POVButton(self.stick, 135)
drive_south = POVButton(self.stick, 180)
drive_southwest = POVButton(self.stick, 225)
drive_west = POVButton(self.stick, 270)
drive_northwest = POVButton(self.stick, 315)
#Setpoint stick button mapping.
#-----------------------------------------------------------------------
drive_trigger = JoystickButton(self.stick, 1)
drive_thumb = JoystickButton(self.stick, 2)
drive_bottom_left = JoystickButton(self.stick, 3)
drive_bottom_right = JoystickButton(self.stick, 4)
drive_top_left = JoystickButton(self.stick, 5)
drive_top_right = JoystickButton(self.stick, 6)
#Goes from front to back. outer_base is the outer ring of buttons on
#the base, inner_base is the inner ring of buttons on the base.
#-----------------------------------------------------------------------
drive_outer_base_one = JoystickButton(self.stick, 7)
drive_inner_base_one = JoystickButton(self.stick, 8)
drive_outer_base_two = JoystickButton(self.stick, 9)
drive_inner_base_two = JoystickButton(self.stick, 10)
drive_outer_base_three = JoystickButton(self.stick, 11)
drive_inner_base_three = JoystickButton(self.stick, 12)
#Hat switch POV stuff.
#-----------------------------------------------------------------------
pov_north = POVButton(self.setpointStick, 0)
pov_northeast = POVButton(self.setpointStick, 45)
pov_east = POVButton(self.setpointStick, 90)
pov_southeast = POVButton(self.setpointStick, 135)
pov_south = POVButton(self.setpointStick, 180)
pov_southwest = POVButton(self.setpointStick, 225)
pov_west = POVButton(self.setpointStick, 270)
pov_northwest = POVButton(self.setpointStick, 315)
#Setpoint stick button mapping.
#-----------------------------------------------------------------------
bad_trigger = JoystickButton(self.setpointStick, 1)
thumb = JoystickButton(self.setpointStick, 2)
bottom_left = JoystickButton(self.setpointStick, 3)
bottom_right = JoystickButton(self.setpointStick, 4)
top_left = JoystickButton(self.setpointStick, 5)
top_right = JoystickButton(self.setpointStick, 6)
#Goes from front to back. outer_base is the outer ring of buttons on
#the base, inner_base is the inner ring of buttons on the base.
#-----------------------------------------------------------------------
seven = JoystickButton(self.setpointStick, 7)
eight = JoystickButton(self.setpointStick, 8)
nine = JoystickButton(self.setpointStick, 9)
ten = JoystickButton(self.setpointStick, 10)
eleven = JoystickButton(self.setpointStick, 11)
twelve = JoystickButton(self.setpointStick, 12)
#-----------------------------------------------------------------------
#Mapping of buttons.
bad_trigger.whileHeld(HatButton(robot, 1))
thumb.whileHeld(TiltButton(robot))
pov_north.whileHeld(Intake(robot, .45, .3))
pov_south.whileHeld(Intake(robot, -.5, -.5))
bottom_left.whileHeld(EarsButton(robot, 1))
bottom_right.whileHeld(EarsButton(robot, .4))
top_left.whileHeld(Intake(robot, -.5, -.5))
drive_thumb.whileHeld(Intake(robot, -.5, -.5))
#Give the message to confirm initialisation
print("Joysticks initialized")
def __init__(self, robot):
self.joy = wpilib.Joystick(0)
# Put Some buttons on the SmartDashboard
SmartDashboard.putData("Elevator Bottom", SetElevatorSetpoint(robot, 0));
SmartDashboard.putData("Elevator Platform", SetElevatorSetpoint(robot, 0.2));
SmartDashboard.putData("Elevator Top", SetElevatorSetpoint(robot, 0.3));
SmartDashboard.putData("Wrist Horizontal", SetWristSetpoint(robot, 0));
SmartDashboard.putData("Raise Wrist", SetWristSetpoint(robot, -45));
SmartDashboard.putData("Open Claw", OpenClaw(robot));
SmartDashboard.putData("Close Claw", CloseClaw(robot));
SmartDashboard.putData("Deliver Soda", Autonomous(robot));
# Create some buttons
d_up = JoystickButton(self.joy, 5)
d_right = JoystickButton(self.joy, 6)
d_down = JoystickButton(self.joy, 7)
d_left = JoystickButton(self.joy, 8)
l2 = JoystickButton(self.joy, 9)
r2 = JoystickButton(self.joy, 10)
l1 = JoystickButton(self.joy, 11)
r1 = JoystickButton(self.joy, 12)
# Connect the buttons to commands
d_up.whenPressed(SetElevatorSetpoint(robot, 0.2))
d_down.whenPressed(SetElevatorSetpoint(robot, -0.2))
d_right.whenPressed(CloseClaw(robot))
d_left.whenPressed(OpenClaw(robot))
r1.whenPressed(PrepareToPickup(robot))
r2.whenPressed(Pickup(robot))
l1.whenPressed(Place(robot))
l2.whenPressed(Autonomous(robot))
def __init__(self, robot):
"""Double joysticks WOOT"""
print("Initializing joysticks...")
#Initialise the stick and the smart dashboard (in case we need stuff for auton):
self.stick = wpilib.Joystick(0)
self.setpointStick = wpilib.Joystick(1)
self.smart_dashboard = NetworkTable.getTable("SmartDashboard")
#Main stick POV.
#-----------------------------------------------------------------------
drive_north = POVButton(self.stick, 0)
drive_northeast = POVButton(self.stick, 45)
drive_east = POVButton(self.stick, 90)
drive_southeast = POVButton(self.stick, 135)
drive_south = POVButton(self.stick, 180)
drive_southwest = POVButton(self.stick, 225)
drive_west = POVButton(self.stick, 270)
drive_northwest = POVButton(self.stick, 315)
#Setpoint stick button mapping.
#-----------------------------------------------------------------------
drive_trigger = JoystickButton(self.stick, 1)
drive_thumb = JoystickButton(self.stick, 2)
drive_bottom_left = JoystickButton(self.stick, 3)
drive_bottom_right = JoystickButton(self.stick, 4)
drive_top_left = JoystickButton(self.stick, 5)
drive_top_right = JoystickButton(self.stick, 6)
#Goes from front to back. outer_base is the outer ring of buttons on
#the base, inner_base is the inner ring of buttons on the base.
#-----------------------------------------------------------------------
drive_outer_base_one = JoystickButton(self.stick, 7)
drive_inner_base_one = JoystickButton(self.stick, 8)
drive_outer_base_two = JoystickButton(self.stick, 9)
drive_inner_base_two = JoystickButton(self.stick, 10)
drive_outer_base_three = JoystickButton(self.stick, 11)
drive_inner_base_three = JoystickButton(self.stick, 12)
#Hat switch POV stuff.
#-----------------------------------------------------------------------
pov_north = POVButton(self.setpointStick, 0)
pov_northeast = POVButton(self.setpointStick, 45)
pov_east = POVButton(self.setpointStick, 90)
pov_southeast = POVButton(self.setpointStick, 135)
pov_south = POVButton(self.setpointStick, 180)
pov_southwest = POVButton(self.setpointStick, 225)
pov_west = POVButton(self.setpointStick, 270)
pov_northwest = POVButton(self.setpointStick, 315)
#Setpoint stick button mapping.
#-----------------------------------------------------------------------
bad_trigger = JoystickButton(self.setpointStick, 1)
thumb = JoystickButton(self.setpointStick, 2)
bottom_left = JoystickButton(self.setpointStick, 3)
bottom_right = JoystickButton(self.setpointStick, 4)
top_left = JoystickButton(self.setpointStick, 5)
top_right = JoystickButton(self.setpointStick, 6)
#Goes from front to back. outer_base is the outer ring of buttons on
#the base, inner_base is the inner ring of buttons on the base.
#-----------------------------------------------------------------------
seven = JoystickButton(self.setpointStick, 7)
eight = JoystickButton(self.setpointStick, 8)
nine = JoystickButton(self.setpointStick, 9)
ten = JoystickButton(self.setpointStick, 10)
eleven = JoystickButton(self.setpointStick, 11)
twelve = JoystickButton(self.setpointStick, 12)
#-----------------------------------------------------------------------
#Mapping of buttons.
bad_trigger.whileHeld(HatButton(robot, 1))
thumb.whileHeld(TiltButton(robot))
pov_north.whileHeld(Intake(robot, .45, .3))
pov_south.whileHeld(Intake(robot, -.5, -.5))
bottom_left.whileHeld(EarsButton(robot, 1))
bottom_right.whileHeld(EarsButton(robot, .4))
top_left.whileHeld(Intake(robot, -.5, -.5))
drive_thumb.whileHeld(Intake(robot, -.5, -.5))
#Give the message to confirm initialisation
print("Joysticks initialized")
class OI:
"""
OI defines the operator interface for Ares.
- two joysticks with many associated buttons
- smartdashboard widgets
"""
# Joysticks
k_driveStickPort = 0
k_launcherStickPort = 1
# DriveStick Commands - controls for driver:
# move, turn, throttle, intake, drive, intake ball
k_intakeBallButton = 3
k_aimIntakeButton = 4
k_driveStopIntakeWheelsButton = 5
k_aimNeutralButton = 6
k_turnThrottleButton = 8
k_driveStrightButton = 11 # experimental
# LauncherStick Commands - controls for mechanism operator:
# aim, shoot, portcullis
k_lightSwitchButton = 2
k_kickBallButton = 3
k_spinIntakeWheelsOutButton = 4
k_stopIntakeWheelsButton = 5
k_pcUpButton = 6 # Pc is portcullis
k_pcDownButton = 7
k_pcBarStopButton = 9
k_pcBarInButton = 10
k_pcBarOutButton = 11
# Auto positions are numbered (1-5), position 1 is special
k_LowBarPosition = 1
# Auto barrier
k_LowBarBarrier = 0
k_MoatBarrier = 1
k_RoughTerrainBarrier = 2
k_RockWallBarrier = 3
k_PortcullisBarrier = 4
k_RampartsBarrier = 5
# Auto strategy
k_NoMoveStrategy = 0
k_CrossStrategy = 1
k_CrossBlindShotStrategy = 2
k_CrossVisionShotStrategy = 3
def __init__(self, robot):
self.robot = robot
self.driveStick = wpilib.Joystick(OI.k_driveStickPort)
self.aimStick = wpilib.Joystick(OI.k_launcherStickPort)
self.initDriveTrain()
self.initLauncher()
self.initPortcullis()
self.initAutonomous()
self.initVision()
self.initSmartDashboard()
def initDriveTrain(self):
ds = self.driveStick
self.dtTurnThrottleButton = JoystickButton(ds, OI.k_turnThrottleButton)
# TODO: convert whileHeld/whenRealsed pair to a single command
self.dtTurnThrottleButton.whileHeld(DTCmds.TurnSpeedSlow(self.robot))
self.dtTurnThrottleButton.whenReleased(DTCmds.TurnSpeedFast(self.robot))
def initLauncher(self):
aS = self.aimStick
dS = self.driveStick
self.kickBallButton = JoystickButton(aS, OI.k_kickBallButton)
self.kickBallButton.whenPressed(ILCmds.LaunchBallCommandGroup(self.robot))
# TODO: Java code had two buttons for stop wheels
self.mechStopWheelsButton = JoystickButton(aS, OI.k_stopIntakeWheelsButton)
self.mechStopWheelsButton.whenPressed(ILCmds.StopWheelsCommand(self.robot))
self.grabBallButton = JoystickButton(dS, OI.k_intakeBallButton)
self.grabBallButton.whenPressed(ILCmds.IntakeBallCommandGroup(self.robot))
self.spinIntakeWheelsOutButton = JoystickButton(aS, OI.k_spinIntakeWheelsOutButton)
self.spinIntakeWheelsOutButton.whenPressed(ILCmds.SpinIntakeWheelsCommand(self.robot, "out"))
self.driveLauncherJumpToIntakeButton = JoystickButton(dS, OI.k_aimIntakeButton)
self.driveLauncherJumpToIntakeButton.whenPressed(ILCmds.LauncherGoToIntakeCommand(self.robot))
# TODO: Java code had two buttons for neutral
self.driveLauncherJumpToNeutralButton = JoystickButton(aS, OI.k_aimNeutralButton)
self.driveLauncherJumpToNeutralButton.whenPressed(ILCmds.LauncherGoToNeutralCommand(self.robot))
def initVision(self):
s = self.aimStick
self.lightSwitchButton = JoystickButton(s, OI.k_lightSwitchButton)
self.lightSwitchButton.whenPressed(VCmds.LightSwitchCmd(self.robot))
def initPortcullis(self):
s = self.aimStick
self.pcUpButton = JoystickButton(s, OI.k_pcUpButton)
self.pcUpButton.whenPressed(PCmds.MoveUp(self.robot))
self.pcDownButton = JoystickButton(s, OI.k_pcDownButton)
self.pcDownButton.whenPressed(PCmds.MoveDown(self.robot))
self.pcBarInButton = JoystickButton(s, OI.k_pcBarInButton)
self.pcBarInButton.whenPressed(PCmds.BarIn(self.robot))
self.pcBarOutButton = JoystickButton(s, OI.k_pcBarOutButton)
self.pcBarOutButton.whenPressed(PCmds.BarOut(self.robot))
self.pcBarStopButton = JoystickButton(s, OI.k_pcBarStopButton)
self.pcBarStopButton.whenPressed(PCmds.BarStop(self.robot))
def initAutonomous(self):
ch = SendableChooser()
ch.addDefault("1: Low Bar", self.k_LowBarPosition)
ch.addObject("2", 2)
ch.addObject("3", 3)
ch.addObject("4", 4)
ch.addObject("5", 5)
self.startingFieldPosition = ch
ch = SendableChooser()
ch.addDefault("Low Bar", self.k_LowBarBarrier)
ch.addObject("Moat", self.k_MoatBarrier)
ch.addObject("Rough Terrain", self.k_RoughTerrainBarrier)
ch.addObject("Rock Wall", self.k_RockWallBarrier)
ch.addObject("Portcullis", self.k_PortcullisBarrier)
ch.addObject("Ramparts", self.k_RampartsBarrier)
self.barrierType = ch
ch = SendableChooser()
ch.addDefault("None", self.k_NoMoveStrategy)
ch.addObject("Cross Only", self.k_CrossStrategy)
ch.addObject("Cross, Blind Shot", self.k_CrossBlindShotStrategy)
ch.addObject("Cross, Vision Shot",self.k_CrossVisionShotStrategy)
self.strategy = ch
def initSmartDashboard(self):
SmartDashboard.putData("AutoFieldPosition", self.startingFieldPosition)
SmartDashboard.putData("AutoBarrierType", self.barrierType)
SmartDashboard.putData("AutoStrategy", self.strategy)
def initVision(self):
s = self.aimStick
self.lightSwitchButton = JoystickButton(s, OI.k_lightSwitchButton)
self.lightSwitchButton.whenPressed(VCmds.LightSwitchCmd(self.robot))
def __init__(self, robot):
"""Warning: Metric tonnes of code here. May need to be tidied up a wee bit."""
self.stick_left = wpilib.Joystick(0)
self.pad = wpilib.Joystick(1)
self.smart_dashboard = NetworkTable.getTable("SmartDashboard")
#Buttons? Aw, man, I love buttons! *bleep bloop*
#-----------------------------------------------
# Create some buttons on the left stick (which is really not, but I don't wanna disturb the preexisting code).
left_trigger = JoystickButton(self.stick_left, 1)
left_thumb = JoystickButton(self.stick_left, 2)
left_three = JoystickButton(self.stick_left, 3)
left_four = JoystickButton(self.stick_left, 4)
left_five = JoystickButton(self.stick_left, 5)
left_six = JoystickButton(self.stick_left, 6)
left_seven = JoystickButton(self.stick_left, 7)
left_eight = JoystickButton(self.stick_left, 8)
left_nine = JoystickButton(self.stick_left, 9)
left_ten = JoystickButton(self.stick_left, 10)
left_eleven = JoystickButton(self.stick_left, 11)
left_twelve = JoystickButton(self.stick_left, 12)
#Create some POV stuff on the left stick, based on angles and the hat switch
left_north = POVButton(self.stick_left, 0)
left_northeast = POVButton(self.stick_left, 45)
left_east = POVButton(self.stick_left, 90)
left_southeast = POVButton(self.stick_left, 135)
left_south = POVButton(self.stick_left, 180)
left_southwest = POVButton(self.stick_left, 225)
left_west = POVButton(self.stick_left, 270)
left_northwest = POVButton(self.stick_left, 315)
#Keypad Buttons
g0 = JoystickButton(self.pad, 1)
g1 = KeyButton(self.smart_dashboard, 10)
g2 = KeyButton(self.smart_dashboard, 11)
g3 = KeyButton(self.smart_dashboard, 12)
g4 = KeyButton(self.smart_dashboard, 13)
g5 = KeyButton(self.smart_dashboard, 14)
g6 = KeyButton(self.smart_dashboard, 15)
g7 = KeyButton(self.smart_dashboard, 16)
g8 = KeyButton(self.smart_dashboard, 17)
g9 = KeyButton(self.smart_dashboard, 18)
g10 = KeyButton(self.smart_dashboard, 19)
g11 = KeyButton(self.smart_dashboard, 20)
g12 = KeyButton(self.smart_dashboard, 21)
g13 = KeyButton(self.smart_dashboard, 22)
g14 = KeyButton(self.smart_dashboard, 23)
g15 = KeyButton(self.smart_dashboard, 24)
g16 = KeyButton(self.smart_dashboard, 25)
g17 = KeyButton(self.smart_dashboard, 26)
g18 = KeyButton(self.smart_dashboard, 27)
g19 = KeyButton(self.smart_dashboard, 28)
g20 = KeyButton(self.smart_dashboard, 29)
g21 = KeyButton(self.smart_dashboard, 30)
g22 = KeyButton(self.smart_dashboard, 31)
topshift = KeyButton(self.smart_dashboard, 32)
bottomshift = KeyButton(self.smart_dashboard, 33)
# Connect buttons & commands
#---------------------------
#Bump commands
left_south.whenPressed(DriveStraight(robot, 0, .25, timeout = .25))
left_north.whenPressed(DriveStraight(robot, 0, -.25, timeout = .25))
left_east.whenPressed(DriveStraight(robot, .25, 0, timeout = .35))
left_west.whenPressed(DriveStraight(robot, -.25, 0, timeout = .35))
#Mast control
left_thumb.whileHeld(Shaker(robot))
left_five.whileHeld(MastButton(robot, .38))
left_six.whileHeld(MastButton(robot, -.38))
#SuperStrafe
left_seven.whenPressed(SuperStrafe64(robot, SuperStrafe64.kForward))
left_eight.whenPressed(SuperStrafe64(robot, SuperStrafe64.kBack))
left_nine.whenPressed(SuperStrafe64(robot, SuperStrafe64.kLeft))
left_ten.whenPressed(SuperStrafe64(robot, SuperStrafe64.kRight))
#Lift presets
g1.whileHeld(LiftGoToLevelShift(robot, 1, topshift, bottomshift))
g2.whileHeld(LiftGoToLevelShift(robot, 2, topshift, bottomshift))
g3.whileHeld(LiftGoToLevelShift(robot, 4, topshift, bottomshift))
g4.whileHeld(LiftGoToLevelShift(robot, 0, topshift, bottomshift))
g5.whenPressed(OpenClaw(robot))
g6.whenPressed(GrabTote(robot))
g7.whenPressed(GrabCan(robot))
#Never, under ANY circumstances, run these during a match.
#g9.whenPressed(RecordMacro(robot, "macro.csv"))
#g10.whenPressed(PlayMacro(robot, "macro.csv"))
#RecordMacro(robot, "macro.csv")
#PlayMacro(robot, "macro.csv")
g11.whenPressed(RecordMacro(robot, "macro_1.csv"))
g12.whenPressed(PlayMacro(robot, "macro_1.csv"))
#RecordMacro(robot, "macro_1.csv")
#PlayMacro(robot, "macro_1.csv")
#g13.whenPressed(RecordMacro(robot, "autonomous.csv"))
#g14.whenPressed(PlayMacro(robot, "autonomous.csv"))
#RecordMacro(robot, "autonomous.csv")
#PlayMacro(robot, "autonomous.csv")
#g15.whenPressed(RecordMacro(robot, "macro_3.csv"))
#g16.whenPressed(PlayMacro(robot, "macro_3.csv"))
#Winchy thing
g17.whileHeld(WinchButton(robot, .5))
g18.whileHeld(WinchButton(robot, -.5))
g19.whenPressed(GrabSpecialTote(robot))
#Mast
g20.whileHeld(MastBack(robot))
g21.whileHeld(MastLevel(robot))
g22.whileHeld(MastForward(robot))
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
Command.getRobot = lambda x=0: self
self.driveSubsystem = DriveSubsystem()
self.controller = Joystick(0)
self.forward = JoystickButton(self.controller, PS4Button["TRIANGLE"])
self.backward = JoystickButton(self.controller, PS4Button["CROSS"])
self.right = JoystickButton(self.controller, PS4Button["CIRCLE"])
self.left = JoystickButton(self.controller, PS4Button["SQUARE"])
self.forward.whenPressed(MoveForward())
self.forward.whenReleased(Stop())
self.backward.whenPressed(MoveBackward())
self.backward.whenReleased(Stop())
self.right.whenPressed(TurnRight())
self.right.whenReleased(Stop())
self.left.whenPressed(TurnLeft())
self.left.whenReleased(Stop())
def autonomousInit(self):
'''Called only at the beginning of autonomous mode'''
pass
def autonomousPeriodic(self):
'''Called every 20ms in autonomous mode'''
pass
def disabledInit(self):
'''Called only at the beginning of disabled mode'''
pass
def disabledPeriodic(self):
'''Called every 20ms in disabled mode'''
pass
def teleopInit(self):
'''Called only at the beginning of teleoperated mode'''
pass
def teleopPeriodic(self):
'''Called every 20ms in teleoperated mode'''
Scheduler.getInstance().run()
class Robot(magicbot.MagicRobot):
# Order of components determines order that execute methods are run
# State Machines
panel_spinner: PanelSpinner
auto_launcher: AutoShoot
# Other components
align: Align
odometry: Odometry
follower: Follower
intake: Intake
control_panel: ControlPanel
launcher: Launcher
drive: Drive
flipped = tunable(False)
ntSolenoid_state = ntproperty("/robot/ntSolenoid_state", 0)
ds_velocity_setpoint = ntproperty('/components/launcher/DS_VEL_SETPOINT',
2100)
limelight_stream_mode = ntproperty('/limelight/stream', 2)
WHEEL_DIAMETER = 0.1524 # Units: Meters
ENCODER_PULSES_PER_REV = 2048 # Ignore quadrature decoding (4x, 8192)
LAUNCHER_GEARING = 1 # No gearing since encoder is on shaft
GEARING = 7.56 # 7.56:1 gear ratio
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1)
self.btn_intake_ = JoystickButton(self.joystick_alt, 5)
self.btn_align = JoystickButton(self.joystick_left, 1)
self.btn_intake_in = JoystickButton(self.joystick_alt, 3)
self.btn_intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_cp_extend = Toggle(self.joystick_left, 4)
self.btn_winch = JoystickButton(self.joystick_alt, 8)
self.btn_cp_motor = JoystickButton(self.joystick_left, 3)
self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12)
self.btn_launcher_idle = Toggle(self.joystick_alt, 10)
self.btn_launcher_motor_close = JoystickButton(self.joystick_alt,
11) # Initiation Line
self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9)
self.btn_slow_movement = JoystickButton(self.joystick_right, 1)
self.btn_intake_solenoid = Toggle(self.joystick_alt, 2)
self.btn_scissor_extend = Toggle(self.joystick_alt, 7)
self.btn_color_sensor = JoystickButton(self.joystick_left, 5)
self.btn_cp_stop = JoystickButton(self.joystick_left, 2)
self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6)
self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1)
self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6)
# Set up motors for encoders
self.master_left = CANSparkMax(1, MotorType.kBrushless)
self.master_right = CANSparkMax(4, MotorType.kBrushless)
self.encoder_constant = (1 /
self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.master_left.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.right_encoder = self.master_right.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.master_left, CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(
self.master_right, CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(
CANSparkMax(8, MotorType.kBrushless),
CANSparkMax(9, MotorType.kBrushless))
self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7)
self.hook_motor = WPI_VictorSPX(3)
# Control Panel Spinner
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.cp_solenoid = wpilib.DoubleSolenoid(
5, 4) # Reversed numbers so kForward is extended
self.cp_motor = WPI_VictorSPX(2)
self.ultrasonic = Ultrasonic(3, 4)
self.ultrasonic.setAutomaticMode(True)
self.ultrasonic.setEnabled(True)
# Intake
self.intake_motor = WPI_VictorSPX(1)
self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 1)
self.intake_switch = wpilib.DigitalInput(2)
# Launcher
# self.launcher_spark = CANSparkMax(40, MotorType.kBrushed)
# self.launcher_spark.setInverted(True)
self.launcher_motor = CANSparkMax(10, MotorType.kBrushed)
self.launcher_motor.setInverted(True)
self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed)
self.launcher_motor_follower.follow(self.launcher_motor)
self.launcher_solenoid = wpilib.Solenoid(0)
# Don't use index pin and change to k1X encoding to decrease rate measurement jitter
self.launcher_encoder = self.launcher_motor.getEncoder(
CANEncoder.EncoderType.kQuadrature, 8192)
self.rpm_controller = self.launcher_motor.getPIDController()
self.launcher_sensor = wpilib.Ultrasonic(0, 1)
self.launcher_sensor.setAutomaticMode(True)
self.launcher_sensor.setEnabled(True)
self.launcher_encoder.setPosition(0)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Limelight
self.limelight = Limelight()
# Camera Stream
CameraServer.launch('camera/camera.py:main')
# Robot motion control
self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code
self.drive_feedforward = DRIVE_FEEDFORWARD
self.trajectories = load_trajectories()
# LED strip
self.led_driver = BlinkinLED(0)
def autonomous(self):
self.right_motors.setInverted(True)
super().autonomous()
self.limelight.changePipeline(0)
def disabledInit(self):
self.navx.reset()
self.limelight_stream_mode = 0
def disabledPeriodic(self):
self.limelight.averagePose()
def teleopInit(self):
self.right_motors.setInverted(False)
self.drive.squared_inputs = True
self.drive.speed_constant = 1.05
self.limelight.changePipeline(0)
self.drive.rotational_constant = 0.5
self.inverse = 1
def teleopPeriodic(self):
# if self.btn_invert_y_axis.get():
# self.flipped = True
# self.inverse *= -1
# else:
# self.flipped = False
# self.logger.info(self.ultrasonic.getRangeInches())
if self.btn_invert_y_axis.get():
self.inverse = 1
else:
self.inverse = -1
if self.btn_rotation_sensitivity.get():
self.drive.rotational_constant = 0.1
self.drive.move(self.inverse * self.joystick_left.getY(),
self.joystick_right.getX())
# Align (Overrides self.drive.move() because it's placed after)
if self.btn_align.get() and self.limelight.targetExists():
self.drive.set_target(self.limelight.getYaw(), relative=True)
if self.btn_align.get():
self.limelight.TurnLightOn(True)
self.drive.align()
else:
self.limelight.TurnLightOn(False)
self.drive.set_target(None)
if self.btn_slow_movement.get():
# 10% of original values
self.drive.rotational_constant = 0.54
self.drive.speed_constant = 0.5
# Control Panel Spinner
self.control_panel.set_solenoid(self.btn_cp_extend.get())
if self.btn_cp_extend.get():
self.ntSolenoid_state = True
else:
self.ntSolenoid_state = False
if self.btn_scissor_extend.get():
self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
else:
self.scissor_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
# Color Sensor
if self.btn_color_sensor.get():
self.panel_spinner.spin_to()
if self.btn_cp_motor.get():
self.control_panel.spin(0.5)
# Launcher
if self.btn_launcher_motor.get():
self.launcher.setVelocity(4630)
elif self.btn_launcher_motor_close.get():
self.launcher.setVelocity(3900)
elif self.btn_launcher_motor_dynamic.get():
# self.limelight.TurnLightOn(True)
self.launcher.setVelocity(self.ds_velocity_setpoint)
# if self.limelight.targetExists():
# self.launcher.setVelocityFromDistance(self.limelight.pitch_angle, 4670)
elif self.btn_launcher_idle.get():
self.launcher.setPercentOutput(1)
if self.btn_launcher_solenoid.get():
self.auto_launcher.fire_when_ready()
if self.btn_cp_stop.get():
self.panel_spinner.done()
# Intake
if self.btn_intake_out.get():
self.intake.spin_inside(1)
elif self.btn_intake_in.get():
self.intake.spin(-1, 0.4)
elif self.btn_intake_bottom_out.get():
self.intake.spin_lower(-0.4)
if self.btn_intake_solenoid.get():
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
else:
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kForward)
# Winch
if self.btn_winch.get():
self.winch_motors.set(0.65)
else:
self.winch_motors.set(
0
) # Must use set(0) when not pressed because there is no component
# Hook
if self.joystick_alt.getPOV(0) == 90:
self.hook_motor.set(0.5)
elif self.joystick_alt.getPOV(0) == 270:
self.hook_motor.set(-0.5)
else:
self.hook_motor.set(0)
if self.joystick_alt.getPOV(0) == 0:
self.launcher.fire()
def createObjects(self):
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_launcher_solenoid = JoystickButton(self.joystick_alt, 1)
self.btn_intake_ = JoystickButton(self.joystick_alt, 5)
self.btn_align = JoystickButton(self.joystick_left, 1)
self.btn_intake_in = JoystickButton(self.joystick_alt, 3)
self.btn_intake_out = JoystickButton(self.joystick_alt, 4)
self.btn_cp_extend = Toggle(self.joystick_left, 4)
self.btn_winch = JoystickButton(self.joystick_alt, 8)
self.btn_cp_motor = JoystickButton(self.joystick_left, 3)
self.btn_launcher_motor = JoystickButton(self.joystick_alt, 12)
self.btn_launcher_idle = Toggle(self.joystick_alt, 10)
self.btn_launcher_motor_close = JoystickButton(self.joystick_alt,
11) # Initiation Line
self.btn_launcher_motor_dynamic = JoystickButton(self.joystick_alt, 9)
self.btn_slow_movement = JoystickButton(self.joystick_right, 1)
self.btn_intake_solenoid = Toggle(self.joystick_alt, 2)
self.btn_scissor_extend = Toggle(self.joystick_alt, 7)
self.btn_color_sensor = JoystickButton(self.joystick_left, 5)
self.btn_cp_stop = JoystickButton(self.joystick_left, 2)
self.btn_invert_y_axis = JoystickButton(self.joystick_left, 6)
self.btn_rotation_sensitivity = JoystickButton(self.joystick_right, 1)
self.btn_intake_bottom_out = JoystickButton(self.joystick_alt, 6)
# Set up motors for encoders
self.master_left = CANSparkMax(1, MotorType.kBrushless)
self.master_right = CANSparkMax(4, MotorType.kBrushless)
self.encoder_constant = (1 /
self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.master_left.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.right_encoder = self.master_right.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant /
60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.master_left, CANSparkMax(3, MotorType.kBrushless))
self.right_motors = wpilib.SpeedControllerGroup(
self.master_right, CANSparkMax(6, MotorType.kBrushless))
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.left_motors,
self.right_motors)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(
CANSparkMax(8, MotorType.kBrushless),
CANSparkMax(9, MotorType.kBrushless))
self.scissor_solenoid = wpilib.DoubleSolenoid(6, 7)
self.hook_motor = WPI_VictorSPX(3)
# Control Panel Spinner
self.colorSensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
self.cp_solenoid = wpilib.DoubleSolenoid(
5, 4) # Reversed numbers so kForward is extended
self.cp_motor = WPI_VictorSPX(2)
self.ultrasonic = Ultrasonic(3, 4)
self.ultrasonic.setAutomaticMode(True)
self.ultrasonic.setEnabled(True)
# Intake
self.intake_motor = WPI_VictorSPX(1)
self.intake_motor_lower = CANSparkMax(40, MotorType.kBrushed)
self.intake_solenoid = wpilib.DoubleSolenoid(2, 1)
self.intake_switch = wpilib.DigitalInput(2)
# Launcher
# self.launcher_spark = CANSparkMax(40, MotorType.kBrushed)
# self.launcher_spark.setInverted(True)
self.launcher_motor = CANSparkMax(10, MotorType.kBrushed)
self.launcher_motor.setInverted(True)
self.launcher_motor_follower = CANSparkMax(12, MotorType.kBrushed)
self.launcher_motor_follower.follow(self.launcher_motor)
self.launcher_solenoid = wpilib.Solenoid(0)
# Don't use index pin and change to k1X encoding to decrease rate measurement jitter
self.launcher_encoder = self.launcher_motor.getEncoder(
CANEncoder.EncoderType.kQuadrature, 8192)
self.rpm_controller = self.launcher_motor.getPIDController()
self.launcher_sensor = wpilib.Ultrasonic(0, 1)
self.launcher_sensor.setAutomaticMode(True)
self.launcher_sensor.setEnabled(True)
self.launcher_encoder.setPosition(0)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Limelight
self.limelight = Limelight()
# Camera Stream
CameraServer.launch('camera/camera.py:main')
# Robot motion control
self.kinematics = KINEMATICS # Use kinematics from inner trajectory generator code
self.drive_feedforward = DRIVE_FEEDFORWARD
self.trajectories = load_trajectories()
# LED strip
self.led_driver = BlinkinLED(0)
def initDriveTrain(self):
ds = self.driveStick
self.dtTurnThrottleButton = JoystickButton(ds, OI.k_turnThrottleButton)
# TODO: convert whileHeld/whenRealsed pair to a single command
self.dtTurnThrottleButton.whileHeld(DTCmds.TurnSpeedSlow(self.robot))
self.dtTurnThrottleButton.whenReleased(DTCmds.TurnSpeedFast(self.robot))
