def robotInit(self):
self.control_stick = wpilib.Joystick(0)
self.open_claw = ButtonDebouncer(self.control_stick, 1)
self.close_claw = ButtonDebouncer(self.control_stick, 2)
self.claw = claw.Claw(2, 0)
class Robot(wpilib.IterativeRobot):
def robotInit(self):
self.control_stick = wpilib.Joystick(0)
self.open_claw = ButtonDebouncer(self.control_stick, 1)
self.close_claw = ButtonDebouncer(self.control_stick, 2)
self.claw = claw.Claw(2, 0)
def disabledInit(self):
pass
def disabledPeriodic(self):
pass
def autonomousInit(self):
pass
def autonomousPeriodic(self):
pass
def teleopInit(self):
pass
def teleopPeriodic(self):
if self.open_claw.get():
self.claw.open()
elif self.close_claw.get():
self.claw.close()
self.claw.update()
def __init__(self, robot, control_stick):
self.robot = robot
self.stick = control_stick
self.prefs = wpilib.Preferences.getInstance()
self.toggle_foc_button = ButtonDebouncer(self.stick, 7)
self.zero_yaw_button = ButtonDebouncer(self.stick, 3)
self.switch_camera_button = ButtonDebouncer(self.stick, 4)
self.low_speed_button = ButtonDebouncer(self.stick, 9)
self.high_speed_button = ButtonDebouncer(self.stick, 10)
def __init__(self, robot):
self.robot = robot
self.stick = wpilib.Joystick(0)
self.throttle = wpilib.Joystick(1)
self.claw_const_pressure_active = False
self.prefs = wpilib.Preferences.getInstance()
self.toggle_foc_button = ButtonDebouncer(self.stick, 2)
self.zero_yaw_button = ButtonDebouncer(self.stick, 3)
self.switch_camera_button = ButtonDebouncer(self.stick, 4)
self.low_speed_button = ButtonDebouncer(self.stick, 9)
self.high_speed_button = ButtonDebouncer(self.stick, 10)
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)
class MyRobot(magicbot.MagicRobot):
def createObjects(self):
self.leftStick = wpilib.Joystick(2)
self.elevatorStick = wpilib.Joystick(1)
self.rightStick = wpilib.Joystick(0)
self.elevatorMotorOne = wpilib.Victor(2)
self.elevatorMotorTwo = wpilib.Victor(3)
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.myRobot = DifferentialDrive(self.left, self.right)
self.elevator = wpilib.SpeedControllerGroup(self.elevatorMotorOne,
self.elevatorMotorTwo)
self.elevatorPot = wpilib.AnalogPotentiometer(0)
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.gearshift.set(1)
def teleopInit(self):
pass
def teleopPeriodic(self):
self.myRobot.tankDrive(-self.leftStick.getY(), -self.rightStick.getY())
if (self.trigger.get()):
if (self.gearshift.get() == 1):
self.gearshift.set(2)
elif (self.gearshift.get() == 2):
self.gearshift.set(1)
self.elevator.set(self.elevatorStick.getY())
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):
self.leftStick = wpilib.Joystick(2)
self.elevatorStick = wpilib.Joystick(1)
self.rightStick = wpilib.Joystick(0)
self.elevatorMotorOne = wpilib.Victor(2)
self.elevatorMotorTwo = wpilib.Victor(3)
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.myRobot = DifferentialDrive(self.left, self.right)
self.elevator = wpilib.SpeedControllerGroup(self.elevatorMotorOne,
self.elevatorMotorTwo)
self.elevatorPot = wpilib.AnalogPotentiometer(0)
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.gearshift.set(1)
def robotInit(self):
self.stick = wpilib.Joystick(self.controller_index)
self.__prefs = wpilib.Preferences.getInstance()
self.lift_main = TalonSRX(self.main_lift_id)
self.lift_follower = TalonSRX(self.follower_id)
self.lift_main.configSelectedFeedbackSensor(
TalonSRX.FeedbackDevice.PulseWidthEncodedPosition, 0, 0)
self.lift_main.selectProfileSlot(0, 0)
self.lift_follower.set(TalonSRX.ControlMode.Follower,
self.main_lift_id)
self.last_out = 0
self.back = ButtonDebouncer(self.stick, 2)
self.fwd = ButtonDebouncer(self.stick, 3)
self.__load_config()
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.gyro = wpilib.AnalogGyro(0)
self.gyro.reset()
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
NetworkTables.initialize(server='127.0.0.1')
self.smartdash = NetworkTables.getTable('SmartDashboard')
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
wpilib.CameraServer.launch("vision.py:main")
self.ll = NetworkTables.getTable("limelight")
self.ll.putNumber('ledStatus', 1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(2)
self.rightStick = wpilib.Joystick(1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.smartdash.putNumber('tx', 1)
self.gearshift.set(1)
self.pdp = wpilib.PowerDistributionPanel(0)
self.accelerometer = wpilib.BuiltInAccelerometer()
self.gyro.initSendable
self.myRobot.initSendable
self.gearshift.initSendable
self.pdp.initSendable
self.accelerometer.initSendable
self.acc = wpilib.AnalogAccelerometer(3)
self.setpoint = 90.0
self.P = .3
self.I = 0
self.D = 0
self.integral = 0
self.previous_error = 0
self.rcw = 0
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)
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
def createObjects(self):
"""
Create basic components (motor controllers, joysticks, etc.).
"""
# NavX
self.navx = navx.AHRS.create_spi()
# Initialize SmartDashboard
self.sd = NetworkTable.getTable('SmartDashboard')
# Joysticks
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.secondary_joystick = wpilib.Joystick(2)
# Triggers and buttons
self.secondary_trigger = ButtonDebouncer(self.secondary_joystick, 1)
# Drive motors
self.fr_module = swervemodule.SwerveModule(ctre.CANTalon(30), wpilib.VictorSP(3), wpilib.AnalogInput(0), sd_prefix='fr_module', zero=1.85, has_drive_encoder=True)
self.fl_module = swervemodule.SwerveModule(ctre.CANTalon(20), wpilib.VictorSP(1), wpilib.AnalogInput(2), sd_prefix='fl_module', zero=3.92, inverted=True)
self.rr_module = swervemodule.SwerveModule(ctre.CANTalon(10), wpilib.VictorSP(2), wpilib.AnalogInput(1), sd_prefix='rr_module', zero=4.59)
self.rl_module = swervemodule.SwerveModule(ctre.CANTalon(5), wpilib.VictorSP(0), wpilib.AnalogInput(3), sd_prefix='rl_module', zero=2.44, has_drive_encoder=True, inverted=True)
# Drive control
self.field_centric_button = ButtonDebouncer(self.left_joystick, 6)
self.predict_position = ButtonDebouncer(self.left_joystick, 7)
self.field_centric_drive = True
self.field_centric_hot_switch = toggle_button.TrueToggleButton(self.left_joystick, 1)
self.left_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 4)
self.right_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 5)
self.align_button = toggle_button.TrueToggleButton(self.right_joystick, 10)
# Shooting motors
self.shooter_motor = ctre.CANTalon(15)
self.belt_motor = wpilib.spark.Spark(9)
self.light_controller = wpilib.VictorSP(8)
# Pistons for gear picker
self.picker = wpilib.DoubleSolenoid(6, 7)
self.pivot = wpilib.DoubleSolenoid(4, 5)
self.pessure_sensor = pressure_sensor.REVAnalogPressureSensor(navx.pins.getNavxAnalogInChannel(0))
# Toggling button on secondary joystick
self.pivot_toggle_button = ButtonDebouncer(self.secondary_joystick, 2)
# Or, up and down buttons on right joystick
self.pivot_down_button = ButtonDebouncer(self.right_joystick, 2)
self.pivot_up_button = ButtonDebouncer(self.right_joystick, 3)
# Climb motors
self.climb_motor1 = wpilib.spark.Spark(4)
self.climb_motor2 = wpilib.spark.Spark(5)
# Camera gimble
self.gimbal_yaw = wpilib.Servo(6)
self.gimbal_pitch = wpilib.Servo(7)
# PDP
self.pdp = wpilib.PowerDistributionPanel(0)
class MyRobot(magicbot.MagicRobot):
drive = swervedrive.SwerveDrive
shooter = shooter.Shooter
gear_picker = gearpicker.GearPicker
climber = climber.Climber
gimbal = gimbal.Gimbal
field_centric = FieldCentric
tracker = PositionTracker
fc_tracker = FCPositionTracker
y_ctrl = YPosController
x_ctrl = XPosController
fc_y_ctrl = FCYPosController
fc_x_ctrl = FCXPosController
angle_ctrl = AngleController
moving_angle_ctrl = MovingAngleController
pos_history = PositionHistory
auto_align = AutoAlign
gamepad_mode = tunable(False)
def createObjects(self):
"""
Create basic components (motor controllers, joysticks, etc.).
"""
# NavX
self.navx = navx.AHRS.create_spi()
# Initialize SmartDashboard
self.sd = NetworkTable.getTable('SmartDashboard')
# Joysticks
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.secondary_joystick = wpilib.Joystick(2)
# Triggers and buttons
self.secondary_trigger = ButtonDebouncer(self.secondary_joystick, 1)
# Drive motors
self.fr_module = swervemodule.SwerveModule(ctre.CANTalon(30), wpilib.VictorSP(3), wpilib.AnalogInput(0), sd_prefix='fr_module', zero=1.85, has_drive_encoder=True)
self.fl_module = swervemodule.SwerveModule(ctre.CANTalon(20), wpilib.VictorSP(1), wpilib.AnalogInput(2), sd_prefix='fl_module', zero=3.92, inverted=True)
self.rr_module = swervemodule.SwerveModule(ctre.CANTalon(10), wpilib.VictorSP(2), wpilib.AnalogInput(1), sd_prefix='rr_module', zero=4.59)
self.rl_module = swervemodule.SwerveModule(ctre.CANTalon(5), wpilib.VictorSP(0), wpilib.AnalogInput(3), sd_prefix='rl_module', zero=2.44, has_drive_encoder=True, inverted=True)
# Drive control
self.field_centric_button = ButtonDebouncer(self.left_joystick, 6)
self.predict_position = ButtonDebouncer(self.left_joystick, 7)
self.field_centric_drive = True
self.field_centric_hot_switch = toggle_button.TrueToggleButton(self.left_joystick, 1)
self.left_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 4)
self.right_shimmy = toggle_button.TrueToggleButton(self.right_joystick, 5)
self.align_button = toggle_button.TrueToggleButton(self.right_joystick, 10)
# Shooting motors
self.shooter_motor = ctre.CANTalon(15)
self.belt_motor = wpilib.spark.Spark(9)
self.light_controller = wpilib.VictorSP(8)
# Pistons for gear picker
self.picker = wpilib.DoubleSolenoid(6, 7)
self.pivot = wpilib.DoubleSolenoid(4, 5)
self.pessure_sensor = pressure_sensor.REVAnalogPressureSensor(navx.pins.getNavxAnalogInChannel(0))
# Toggling button on secondary joystick
self.pivot_toggle_button = ButtonDebouncer(self.secondary_joystick, 2)
# Or, up and down buttons on right joystick
self.pivot_down_button = ButtonDebouncer(self.right_joystick, 2)
self.pivot_up_button = ButtonDebouncer(self.right_joystick, 3)
# Climb motors
self.climb_motor1 = wpilib.spark.Spark(4)
self.climb_motor2 = wpilib.spark.Spark(5)
# Camera gimble
self.gimbal_yaw = wpilib.Servo(6)
self.gimbal_pitch = wpilib.Servo(7)
# PDP
self.pdp = wpilib.PowerDistributionPanel(0)
def robotInit(self):
super().robotInit()
wpilib.CameraServer.launch('camera/vision.py:main')
def autonomous(self):
"""
Prepare for autonomous mode.
"""
self.field_centric.set_raw_values = True
self.drive.allow_reverse = False
self.drive.wait_for_align = True
self.drive.threshold_input_vectors = True
super().autonomous()
def disabledPeriodic(self):
"""
Repeat periodically while robot is disabled.
Usually emptied.
Sometimes used to easily test sensors and other things.
"""
self.update_sd()
def disabledInit(self):
"""
Do once right away when robot is disabled.
"""
def teleopInit(self):
"""
Do when teleoperated mode is started.
"""
self.drive.flush() # This is a poor solution to the drive system maintain speed/direction
self.field_centric.set_raw_values = False
self.drive.allow_reverse = True
self.drive.wait_for_align = False
self.drive.squared_inputs = True
self.drive.threshold_input_vectors = True
def move(self, x, y, rcw):
if self.right_joystick.getRawButton(1):
rcw *= 0.75
if not self.field_centric_drive or self.left_joystick.getRawButton(1):
self.drive.move(x, y, rcw)
else:
self.field_centric.move(x, y)
self.drive.set_rcw(rcw)
def teleopPeriodic(self):
"""
Do periodically while robot is in teleoperated mode.
"""
# Drive system
if not self.gamepad_mode or self.ds.isFMSAttached():
self.move(self.left_joystick.getY()*-1, self.left_joystick.getX()*-1, self.right_joystick.getX())
else:
self.move(self.left_joystick.getRawAxis(1)*-1, self.left_joystick.getRawAxis(0), self.left_joystick.getRawAxis(2)*-1)
if self.field_centric_button.get():
if not self.field_centric_drive:
self.navx.reset()
self.field_centric_drive = not self.field_centric_drive
if self.left_joystick.getRawButton(2):
self.drive.request_wheel_lock = True
if self.right_joystick.getRawButton(4):
self.drive.set_raw_strafe(0.25)
elif self.right_joystick.getRawButton(5):
self.drive.set_raw_strafe(-0.25)
if self.right_joystick.getRawButton(3):
self.drive.set_raw_fwd(0.35)
elif self.right_joystick.getRawButton(2):
self.drive.set_raw_fwd(-0.35)
# Gear picker
if self.pivot_toggle_button.get():
if self.gear_picker._pivot_state == 1:
self.gear_picker.pivot_down()
else:
self.gear_picker.pivot_up()
if self.secondary_trigger.get():
self.gear_picker.actuate_picker()
# Climber
if self.left_joystick.getRawButton(3) or self.secondary_joystick.getRawButton(4):
self.climber.climb(-1)
if self.secondary_joystick.getRawButton(6):
self.climber.climb(-0.5)
if self.secondary_joystick.getRawButton(5):
self.light_controller.set(1)
else:
self.light_controller.set(0)
# Shooter
if self.secondary_joystick.getRawButton(3):
self.shooter.shoot()
else:
self.shooter.stop()
# Secondary driver gimble control
if self.secondary_joystick.getRawButton(12):
# scale.scale params: (input, input_min, input_max, output_min, output_max)
self.gimbal.yaw = scale.scale(self.secondary_joystick.getX()*-1, -1, 1, 0, 0.14)
self.gimbal.pitch = scale.scale(self.secondary_joystick.getY()*-1, -1, 1, 0.18, 0.72)
# Auto align test
if self.right_joystick.getRawButton(10):
self.auto_align.align()
if self.align_button.get_released():
self.auto_align.done()
self.update_sd()
def update_sd(self):
self.sd.putNumber('current/climb1_current_draw', self.pdp.getCurrent(1))
self.sd.putNumber('current/climb2_current_draw', self.pdp.getCurrent(2))
self.sd.putNumber('current/rr_rotate_current_draw', self.pdp.getCurrent(8))
self.sd.putNumber('current/fl_rotate_current_draw', self.pdp.getCurrent(7))
self.sd.putNumber('pneumatics/tank_pressure', self.pessure_sensor.getPressure())
self.drive.update_smartdash()
class Robot(MagicRobot):
drivetrain = Drivetrain
climber = Climber
elevator = Elevator
intake = Intake
mode = RobotMode.switch
rumbling = False
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)
def up_mode(self):
self.mode += 1
def down_mode(self):
self.mode -= 1
def autonomous(self):
self.intake.reset_wrist()
super().autonomous()
def teleopInit(self):
self.intake.reset_wrist_up()
def teleopPeriodic(self):
self.right = -self.right_drive_joystick.getRawAxis(1)
self.left = -self.left_drive_joystick.getRawAxis(1)
self.right = 0 if abs(self.right) < 0.1 else self.right
self.left = 0 if abs(self.left) < 0.1 else self.left
self.drivetrain.tank(math.copysign(self.right**2, self.right),
math.copysign(self.left**2, self.left))
# outtake
if self.operator_joystick.getRawAxis(3) > 0.01:
self.intake.set_speed(
self.operator_joystick.getRawAxis(3) * 0.8 + 0.2)
elif self.operator_joystick.getRawButton(3):
self.intake.intake()
else:
self.intake.hold()
elevator_speed = -self.operator_joystick.getY(0)
if self.down_button.get():
self.down_mode()
elif self.up_button.get():
self.up_mode()
joystick_val = -self.operator_joystick.getRawAxis(1)
if joystick_val > 0.2:
joystick_val -= 0.2
elif joystick_val < -0.2:
joystick_val += 0.2
else:
joystick_val = 0.0
self.elevator.move_setpoint(joystick_val / 50.0 * 20)
if self.right_bumper_button.get():
self.intake.wrist_down()
self.intake.intake()
self.intake.open_arm()
if self.intake.has_cube():
self.rumbling = True
else:
self.intake.close_arm()
if self.left_bumper_button.get():
self.climber.set_speed(-self.operator_joystick.getRawAxis(5))
else:
wrist_speed = self.operator_joystick.getRawAxis(5)
wrist_speed = 0 if abs(wrist_speed) < 0.2 else wrist_speed
self.intake.move_wrist_setpoint(wrist_speed)
if self.has_cube_button.get():
self.intake.toggle_has_cube()
self.operator_joystick.setRumble(
wpilib.Joystick.RumbleType.kRightRumble, 1 if self.rumbling else 0)
self.operator_joystick.setRumble(
wpilib.Joystick.RumbleType.kLeftRumble, 1 if self.rumbling else 0)
self.rumbling = False
def disabledPeriodic(self):
self.drivetrain._update_odometry()
self.elevator.reset_position()
def testPeriodic(self):
self.wrist_motor.set(self.operator_joystick.getRawAxis(5) * 0.6)
self.elevator_winch.set(self.operator_joystick.getRawAxis(1))
self.intake_solenoid.set(wpilib.DoubleSolenoid.Value.kReverse)
self.left_drive_motor.set(0)
self.right_drive_motor.set(0)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
"""Robot initialization function"""
# object that handles basic drive operations
self.left = wpilib.Victor(0)
self.right = wpilib.Victor(1)
self.gyro = wpilib.AnalogGyro(0)
self.gyro.reset()
self.myRobot = DifferentialDrive(self.left, self.right)
self.myRobot.setExpiration(0.1)
NetworkTables.initialize(server='127.0.0.1')
self.smartdash = NetworkTables.getTable('SmartDashboard')
self.gearshift = wpilib.DoubleSolenoid(0, 0, 1)
wpilib.CameraServer.launch("vision.py:main")
self.ll = NetworkTables.getTable("limelight")
self.ll.putNumber('ledStatus', 1)
# joysticks 1 & 2 on the driver station
self.leftStick = wpilib.Joystick(2)
self.rightStick = wpilib.Joystick(1)
self.trigger = ButtonDebouncer(self.rightStick, 1, period=.5)
self.smartdash.putNumber('tx', 1)
self.gearshift.set(1)
self.pdp = wpilib.PowerDistributionPanel(0)
self.accelerometer = wpilib.BuiltInAccelerometer()
self.gyro.initSendable
self.myRobot.initSendable
self.gearshift.initSendable
self.pdp.initSendable
self.accelerometer.initSendable
self.acc = wpilib.AnalogAccelerometer(3)
self.setpoint = 90.0
self.P = .3
self.I = 0
self.D = 0
self.integral = 0
self.previous_error = 0
self.rcw = 0
#wpilib.DriverStation.reportWarning(str(self.myRobot.isRightSideInverted()),False)
def PID(self):
error = self.setpoint - self.gyro.getAngle()
self.integral = self.integral + (error * .02)
derivative = (error - self.previous_error) / .02
self.rcw = self.P * error + self.I * self.integral + self.D * derivative
def autonomousInit(self):
self.myRobot.setSafetyEnabled(False)
def autonomousPeriodic(self):
pass
def teleopInit(self):
"""Executed at the start of teleop mode"""
self.myRobot.setSafetyEnabled(True)
def teleopPeriodic(self):
"""Runs the motors with tank steering"""
self.myRobot.arcadeDrive(self.rightStick.getY(),
-self.rightStick.getZ() * .4)
self.smartdash.putNumber('gyrosensor', self.gyro.getAngle())
if (self.rightStick.getRawButton(4)):
self.ll.putNumber('ledMode', 1)
if (self.rightStick.getRawButton(3)):
self.right.set(.5)
if (self.trigger.get()):
if (self.gearshift.get() == 1):
self.gearshift.set(2)
elif (self.gearshift.get() == 2):
self.gearshift.set(1)
if (self.rightStick.getRawButton(2)):
self.tx = self.ll.getNumber("tx", None)
if (not type(self.tx) == type(0.0)):
self.tx = 0.0
if (self.tx > 1.0):
self.myRobot.arcadeDrive(0, self.tx * .03)
elif (self.tx < -1.0):
self.myRobot.tankDrive(0, self.tx * .03)
else:
self.myRobot.tankDrive(0, 0)
self.pdp.getVoltage()
class Robot(wpilib.TimedRobot):
def threshold(self, value, limit):
if (abs(value) < limit):
return 0
else:
return round(value, 2)
def robotInit(self):
self.kSlotIdx = 0
self.kPIDLoopIdx = 0
self.kTimeoutMs = 10
# Sets the speed
self.speed = 0.4
self.ySpeed = 1
self.tSpeed = 0.75
self.baseIntakeSpeed = 5
self.previousAvg = 0
self.currentAvg = 0
# Smart Dashboard
self.sd = NetworkTables.getTable('SmartDashboard')
# joysticks 1 & 2 on the driver station
self.driverJoystick = wpilib.Joystick(0)
# Create a simple timer (docs: https://robotpy.readthedocs.io/projects/wpilib/en/latest/wpilib/Timer.html#wpilib.timer.Timer.get)
self.timer = wpilib.Timer()
# TODO: Fix module number
self.compressor = wpilib.Compressor()
# TODO: Fix module numbers
self.intake = Intake(0, 0, 0, 0, 0, 0, 0)
# Talon CAN devices
# TODO: Fix module numbers
self.frontLeftTalon = WPI_TalonSRX(2)
self.rearLeftTalon = WPI_TalonSRX(0)
self.frontRightTalon = WPI_TalonSRX(3)
self.rearRightTalon = WPI_TalonSRX(1)
self.rightPistonButton = ButtonDebouncer(driverJoystick, 4)
self.leftPistonButton = ButtonDebouncer(driverJoystick, 5)
# Enable auto breaking
self.frontLeftTalon.setNeutralMode(NeutralMode.Brake)
self.rearLeftTalon.setNeutralMode(NeutralMode.Brake)
self.frontRightTalon.setNeutralMode(NeutralMode.Brake)
self.rearRightTalon.setNeutralMode(NeutralMode.Brake)
# Setup encoders
self.frontLeftTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.rearLeftTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.frontRightTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.rearRightTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# Setup encoders
self.leftEncoder = self.rearLeftTalon
self.rightEncoder = self.rearRightTalon
def autonomousInit(self):
"""Called only at the beginning of autonomous mode."""
pass
def autonomousPeriodic(self):
"""Called every 20ms in autonomous mode."""
pass
def teleopInit(self):
self.compressor.start()
self.frontLeftTalon.set(ControlMode.Speed)
self.frontRightTalon.set(ControlMode.Speed)
self.rearLeftTalon.set(ControlMode.Speed)
self.rearRightTalon.set(ControlMode.Speed)
pass
def teleopPeriodic(self):
# Get max speed
self.speed = (-self.driverJoystick.getRawAxis(3) + 1) / 2
self.intake.test(True, self.rightPistonButton.get())
self.intake.test(False, self.leftPistonButton.get())
# Get turn and movement speeds
self.tAxis = self.threshold(self.driverJoystick.getRawAxis(2),
0.05) * self.tSpeed * self.speed
self.yAxis = self.threshold(-self.driverJoystick.getRawAxis(1),
0.05) * self.ySpeed * self.speed
# Calculate right and left speeds
leftSpeed = self.yAxis + self.tAxis
rightSpeed = self.yAxis - self.tAxis
# Update Motors
self.frontLeftTalon.set(ControlMode.PercentOutput, leftSpeed)
self.rearLeftTalon.set(ControlMode.PercentOutput, leftSpeed)
self.frontRightTalon.set(ControlMode.PercentOutput, rightSpeed)
self.rearRightTalon.set(ControlMode.PercentOutput, rightSpeed)
# Update SmartDashboard
self.sd.putNumber(
"Left Encoder",
self.leftEncoder.getSelectedSensorPosition(self.kPIDLoopIdx))
self.sd.putNumber(
"Right Encoder",
self.rightEncoder.getSelectedSensorPosition(self.kPIDLoopIdx))
def robotInit(self):
self.kSlotIdx = 0
self.kPIDLoopIdx = 0
self.kTimeoutMs = 10
# Sets the speed
self.speed = 0.4
self.ySpeed = 1
self.tSpeed = 0.75
self.baseIntakeSpeed = 5
self.previousAvg = 0
self.currentAvg = 0
# Smart Dashboard
self.sd = NetworkTables.getTable('SmartDashboard')
# joysticks 1 & 2 on the driver station
self.driverJoystick = wpilib.Joystick(0)
# Create a simple timer (docs: https://robotpy.readthedocs.io/projects/wpilib/en/latest/wpilib/Timer.html#wpilib.timer.Timer.get)
self.timer = wpilib.Timer()
# TODO: Fix module number
self.compressor = wpilib.Compressor()
# TODO: Fix module numbers
self.intake = Intake(0, 0, 0, 0, 0, 0, 0)
# Talon CAN devices
# TODO: Fix module numbers
self.frontLeftTalon = WPI_TalonSRX(2)
self.rearLeftTalon = WPI_TalonSRX(0)
self.frontRightTalon = WPI_TalonSRX(3)
self.rearRightTalon = WPI_TalonSRX(1)
self.rightPistonButton = ButtonDebouncer(driverJoystick, 4)
self.leftPistonButton = ButtonDebouncer(driverJoystick, 5)
# Enable auto breaking
self.frontLeftTalon.setNeutralMode(NeutralMode.Brake)
self.rearLeftTalon.setNeutralMode(NeutralMode.Brake)
self.frontRightTalon.setNeutralMode(NeutralMode.Brake)
self.rearRightTalon.setNeutralMode(NeutralMode.Brake)
# Setup encoders
self.frontLeftTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.rearLeftTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.frontRightTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.rearRightTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# Setup encoders
self.leftEncoder = self.rearLeftTalon
self.rightEncoder = self.rearRightTalon
class Teleop:
last_applied_control = np.array([0, 0, 0])
foc_enabled = False
def __init__(self, robot, control_stick):
self.robot = robot
self.stick = control_stick
self.prefs = wpilib.Preferences.getInstance()
self.toggle_foc_button = ButtonDebouncer(self.stick, 7)
self.zero_yaw_button = ButtonDebouncer(self.stick, 3)
self.switch_camera_button = ButtonDebouncer(self.stick, 4)
self.low_speed_button = ButtonDebouncer(self.stick, 9)
self.high_speed_button = ButtonDebouncer(self.stick, 10)
def update_smart_dashboard(self):
wpilib.SmartDashboard.putBoolean('FOC Enabled', self.foc_enabled)
def buttons(self):
if self.robot.imu.is_present():
if self.zero_yaw_button.get():
self.navx.reset()
if self.toggle_foc_button.get():
self.foc_enabled = not self.foc_enabled
if self.switch_camera_button.get():
current_camera = (self.prefs.getInt('Selected Camera') + 1) % 1
self.prefs.putInt('Selected Camera', current_camera)
def lift_control(self):
liftPct = self.stick.getRawAxis(constants.liftAxis)
if constants.liftInv:
liftPct *= -1
# normalize liftPct to [0, 1]
liftPct += 1
liftPct /= 2
tgt_height = liftPct * constants.lift_height
self.robot.lift.set_height(tgt_height)
def drive(self):
"""
Drive the robot directly using a joystick.
"""
ctrl = np.array([
self.stick.getRawAxis(constants.fwdAxis),
self.stick.getRawAxis(constants.strAxis)
])
if constants.fwdInv:
ctrl[0] *= -1
if constants.strInv:
ctrl[1] *= -1
linear_control_active = True
if abs(np.sqrt(np.sum(ctrl**2))) < 0.1:
ctrl[0] = 0
ctrl[1] = 0
linear_control_active = False
if (self.robot.imu.is_present() and self.foc_enabled):
# perform FOC coordinate transform
hdg = self.robot.imu.get_robot_heading()
# Right-handed passive (alias) transform matrix
foc_transform = np.array([[np.cos(hdg), np.sin(hdg)],
[-np.sin(hdg), np.cos(hdg)]])
ctrl = np.squeeze(np.matmul(foc_transform, ctrl))
tw = self.stick.getRawAxis(constants.rcwAxis)
if constants.rcwInv:
tw *= -1
rotation_control_active = True
if abs(tw) < 0.15:
tw = 0
rotation_control_active = False
tw *= constants.turn_sensitivity
if linear_control_active or rotation_control_active:
self.last_applied_control = np.array([ctrl[0], ctrl[1], tw])
speed_coefficient = 0.75
if self.low_speed_button.get():
speed_coefficient = 0.25
elif self.high_speed_button.get():
speed_coefficient = 1
self.robot.drivetrain.drive(ctrl[0] * speed_coefficient,
ctrl[1] * speed_coefficient,
tw * speed_coefficient,
max_wheel_speed=constants.teleop_speed)
else:
self.robot.drivetrain.drive(self.last_applied_control[0],
self.last_applied_control[1],
self.last_applied_control[2],
max_wheel_speed=0)
class Robot(magicbot.MagicRobot):
# Automations
# TODO: bad name
seek_target: seek_target.SeekTarget
# Controllers
# recorder: recorder.Recorder
# Components
follower: trajectory_follower.TrajectoryFollower
drive: drive.Drive
lift: lift.Lift
hatch_manipulator: hatch_manipulator.HatchManipulator
cargo_manipulator: cargo_manipulator.CargoManipulator
climber: climber.Climber
ENCODER_PULSE_PER_REV = 1024
WHEEL_DIAMETER = 0.5
"""
manual_lift_control = tunable(True)
stabilize = tunable(False)
stabilizer_threshold = tunable(30)
stabilizer_aggression = tunable(5)
"""
"""
time = tunable(0)
voltage = tunable(0)
yaw = tunable(0)
"""
def createObjects(self):
"""
Initialize robot components.
"""
# For using teleop in autonomous
self.robot = self
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.button_strafe_left = JoystickButton(self.joystick_left, 4)
self.button_strafe_right = JoystickButton(self.joystick_left, 5)
self.button_strafe_forward = JoystickButton(self.joystick_left, 3)
self.button_strafe_backward = JoystickButton(self.joystick_left, 2)
self.button_slow_rotation = JoystickButton(self.joystick_right, 4)
self.button_lift_actuate = ButtonDebouncer(self.joystick_alt, 2)
self.button_manual_lift_control = ButtonDebouncer(self.joystick_alt, 6)
self.button_hatch_kick = JoystickButton(self.joystick_alt, 1)
self.button_cargo_push = JoystickButton(self.joystick_alt, 5)
self.button_cargo_pull = JoystickButton(self.joystick_alt, 3)
self.button_cargo_pull_lightly = JoystickButton(self.joystick_alt, 4)
self.button_climb_front = JoystickButton(self.joystick_right, 3)
self.button_climb_back = JoystickButton(self.joystick_right, 2)
self.button_target = JoystickButton(self.joystick_right, 8)
# 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_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) *
self.WHEEL_DIAMETER * math.pi)
self.r_encoder = wpilib.Encoder(0, 1)
self.r_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder = wpilib.Encoder(2, 3)
self.l_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder.setReverseDirection(True)
# Drivetrain
self.train = wpilib.drive.MecanumDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
# Functional motors
self.lift_motor = WPI_TalonSRX(40)
self.lift_motor.setSensorPhase(True)
self.lift_switch = wpilib.DigitalInput(4)
self.lift_solenoid = wpilib.DoubleSolenoid(2, 3)
self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1)
self.left_cargo_intake_motor = WPI_TalonSRX(35)
# TODO: electricians soldered one motor in reverse.
# self.left_cargo_intake_motor.setInverted(True)
self.right_cargo_intake_motor = WPI_TalonSRX(30)
"""
self.cargo_intake_motors = wpilib.SpeedControllerGroup(self.left_cargo_intake_motor,
self.right_cargo_intake_motor)
"""
self.right_cargo_intake_motor.follow(self.left_cargo_intake_motor)
self.front_climb_piston = wpilib.DoubleSolenoid(4, 5)
self.back_climb_piston = wpilib.DoubleSolenoid(6, 7)
# Tank Drivetrain
"""
self.tank_train = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
"""
# Load trajectories
self.generated_trajectories = load_trajectories()
# NavX
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Utility
# self.ds = wpilib.DriverStation.getInstance()
# self.timer = wpilib.Timer()
self.pdp = wpilib.PowerDistributionPanel(0)
self.compressor = wpilib.Compressor()
# Camera server
wpilib.CameraServer.launch('camera/camera.py:main')
wpilib.LiveWindow.disableAllTelemetry()
def robotPeriodic(self):
"""
Executed periodically regardless of mode.
"""
# self.time = int(self.timer.getMatchTime())
# self.voltage = self.pdp.getVoltage()
# self.yaw = self.navx.getAngle() % 360
pass
def autonomous(self):
"""
Prepare for and start autonomous mode.
"""
# Call autonomous
super().autonomous()
def disabledInit(self):
"""
Executed once right away when robot is disabled.
"""
# Reset Gyro to 0
self.navx.reset()
def disabledPeriodic(self):
"""
Executed periodically while robot is disabled.
Useful for testing.
"""
pass
def teleopInit(self):
"""
Executed when teleoperated mode begins.
"""
self.lift.zero = self.lift_motor.getSelectedSensorPosition()
self.lift.current_position = 5000000
self.compressor.start()
def teleopPeriodic(self):
"""
Executed periodically while robot is in teleoperated mode.
"""
# Read from joysticks and move drivetrain accordingly
self.drive.move(x=-self.joystick_left.getY(),
y=self.joystick_left.getX(),
rot=self.joystick_right.getX(),
real=True,
slow_rot=self.button_slow_rotation.get())
"""
self.drive.strafe(self.button_strafe_left.get(),
self.button_strafe_right.get(),
self.button_strafe_forward.get(),
self.button_strafe_backward.get())
"""
"""
for button in range(7, 12 + 1):
if self.joystick_alt.getRawButton(button):
self.lift.target(button)
"""
# if self.manual_lift_control:
self.lift.move(-self.joystick_alt.getY())
"""
else:
# self.lift.correct(-self.joystick_alt.getY())
# self.lift.approach()
pass
"""
"""
if self.button_manual_lift_control:
# self.manual_lift_control = not self.manual_lift_control
pass
"""
if self.button_hatch_kick.get():
self.hatch_manipulator.extend()
else:
self.hatch_manipulator.retract()
if self.button_target.get():
self.seek_target.seek()
if self.button_lift_actuate.get():
self.lift.actuate()
if self.button_cargo_push.get():
self.cargo_manipulator.push()
elif self.button_cargo_pull.get():
self.cargo_manipulator.pull()
elif self.button_cargo_pull_lightly.get():
self.cargo_manipulator.pull_lightly()
if self.button_climb_front.get():
self.climber.extend_front()
else:
self.climber.retract_front()
if self.button_climb_back.get():
self.climber.extend_back()
else:
self.climber.retract_back()
"""
def createObjects(self):
"""
Initialize robot components.
"""
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_claw = ButtonDebouncer(self.joystick_left, 1)
self.btn_forearm = ButtonDebouncer(self.joystick_right, 1)
self.btn_up = JoystickButton(self.joystick_left, 3)
self.btn_down = JoystickButton(self.joystick_left, 2)
self.btn_climb = JoystickButton(self.joystick_right, 11)
# Buttons on alternative joystick
self.btn_claw_alt = ButtonDebouncer(self.joystick_alt, 1)
self.btn_forearm_alt = ButtonDebouncer(self.joystick_alt, 2)
self.btn_climb_alt = JoystickButton(self.joystick_alt, 3)
# Buttons for toggling control options and aides
self.btn_unified_control = ButtonDebouncer(self.joystick_alt, 8)
self.btn_record = ButtonDebouncer(self.joystick_left, 6)
self.btn_stabilize = ButtonDebouncer(self.joystick_alt, 12)
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_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
# Follow front wheels with rear to save CAN packets
self.lr_motor.follow(self.lf_motor)
self.rr_motor.follow(self.rf_motor)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.lf_motor, self.rf_motor)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(wpilib.Victor(7), wpilib.Victor(8))
# Motion Profiling
self.position_controller = motion_profile.PositionController()
# Arm
self.elevator = wpilib.Victor(5)
self.forearm = wpilib.DoubleSolenoid(2, 3)
self.claw = wpilib.DoubleSolenoid(0, 1)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Utility
self.ds = wpilib.DriverStation.getInstance()
self.timer = wpilib.Timer()
self.pdp = wpilib.PowerDistributionPanel(0)
self.compressor = wpilib.Compressor()
# Camera server
wpilib.CameraServer.launch('camera/camera.py:main')
def createObjects(self):
"""
Initialize robot components.
"""
# For using teleop in autonomous
self.robot = self
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.button_strafe_left = JoystickButton(self.joystick_left, 4)
self.button_strafe_right = JoystickButton(self.joystick_left, 5)
self.button_strafe_forward = JoystickButton(self.joystick_left, 3)
self.button_strafe_backward = JoystickButton(self.joystick_left, 2)
self.button_slow_rotation = JoystickButton(self.joystick_right, 4)
self.button_lift_actuate = ButtonDebouncer(self.joystick_alt, 2)
self.button_manual_lift_control = ButtonDebouncer(self.joystick_alt, 6)
self.button_hatch_kick = JoystickButton(self.joystick_alt, 1)
self.button_cargo_push = JoystickButton(self.joystick_alt, 5)
self.button_cargo_pull = JoystickButton(self.joystick_alt, 3)
self.button_cargo_pull_lightly = JoystickButton(self.joystick_alt, 4)
self.button_climb_front = JoystickButton(self.joystick_right, 3)
self.button_climb_back = JoystickButton(self.joystick_right, 2)
self.button_target = JoystickButton(self.joystick_right, 8)
# 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_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) *
self.WHEEL_DIAMETER * math.pi)
self.r_encoder = wpilib.Encoder(0, 1)
self.r_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder = wpilib.Encoder(2, 3)
self.l_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder.setReverseDirection(True)
# Drivetrain
self.train = wpilib.drive.MecanumDrive(self.lf_motor, self.lr_motor,
self.rf_motor, self.rr_motor)
# Functional motors
self.lift_motor = WPI_TalonSRX(40)
self.lift_motor.setSensorPhase(True)
self.lift_switch = wpilib.DigitalInput(4)
self.lift_solenoid = wpilib.DoubleSolenoid(2, 3)
self.hatch_solenoid = wpilib.DoubleSolenoid(0, 1)
self.left_cargo_intake_motor = WPI_TalonSRX(35)
# TODO: electricians soldered one motor in reverse.
# self.left_cargo_intake_motor.setInverted(True)
self.right_cargo_intake_motor = WPI_TalonSRX(30)
"""
self.cargo_intake_motors = wpilib.SpeedControllerGroup(self.left_cargo_intake_motor,
self.right_cargo_intake_motor)
"""
self.right_cargo_intake_motor.follow(self.left_cargo_intake_motor)
self.front_climb_piston = wpilib.DoubleSolenoid(4, 5)
self.back_climb_piston = wpilib.DoubleSolenoid(6, 7)
# Tank Drivetrain
"""
self.tank_train = wpilib.drive.DifferentialDrive(wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
"""
# Load trajectories
self.generated_trajectories = load_trajectories()
# NavX
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Utility
# self.ds = wpilib.DriverStation.getInstance()
# self.timer = wpilib.Timer()
self.pdp = wpilib.PowerDistributionPanel(0)
self.compressor = wpilib.Compressor()
# Camera server
wpilib.CameraServer.launch('camera/camera.py:main')
wpilib.LiveWindow.disableAllTelemetry()
class MyRobot(magicbot.MagicRobot):
# Shorten a bunch of things
targetGoal = targetGoal.TargetGoal
shootBall = shootBall.ShootBall
winch = winch.Winch
light = light.Light
lightSwitch = lightOff.LightSwitch
intake = intake.Arm
drive = drive.Drive
enable_camera_logging = ntproperty('/camera/logging_enabled', True)
auto_aim_button = ntproperty('/SmartDashboard/Drive/autoAim', False, writeDefault = False)
"""Create basic components (motor controllers, joysticks, etc.)"""
def createObjects(self):
# Joysticks
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
# Motors (l/r = left/right, f/r = front/rear)
self.lf_motor = wpilib.CANTalon(5)
self.lr_motor = wpilib.CANTalon(10)
self.rf_motor = wpilib.CANTalon(15)
self.rr_motor = wpilib.CANTalon(20)
# Drivetrain object
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
# Left and right arm motors (there's two, which both control the raising and lowering the arm)
self.leftArm = wpilib.CANTalon(25)
self.rightArm = wpilib.CANTalon(30)
# Motor that spins the bar at the end of the arm.
# There was originally going to be one on the right, but we decided against that in the end.
# In retrospect, that was probably a mistake.
self.leftBall = wpilib.Talon(9)
# Motor that reels in the winch to lift the robot.
self.winchMotor = wpilib.Talon(0)
# Motor that opens the winch.
self.kickMotor = wpilib.Talon(1)
# Aiming flashlight
self.flashlight = wpilib.Relay(0)
# Timer to keep light from staying on for too long
self.lightTimer = wpilib.Timer()
# Flashlight has three intensities. So, when it's turning off, it has to go off on, off on, off.
# self.turningOffState keeps track of which on/off it's on.
self.turningOffState = 0
# Is currently on or off? Used to detect if UI button is pressed.
self.lastState = False
# Drive encoders; measure how much the motor has spun
self.rf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontRightMotor, True)
self.lf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontLeftMotor)
# Distance sensors
self.back_sensor = distance_sensors.SharpIRGP2Y0A41SK0F(0)
self.ultrasonic = wpilib.AnalogInput(1)
# NavX (purple board on top of the RoboRIO)
self.navX = navx.AHRS.create_spi()
# Initialize SmartDashboard, the table of robot values
self.sd = NetworkTable.getTable('SmartDashboard')
# How much will the control loop pause in between (0.025s = 25ms)
self.control_loop_wait_time = 0.025
# Button to reverse controls
self.reverseButton = ButtonDebouncer(self.joystick1, 1)
# Initiate functional buttons on joysticks
self.shoot = ButtonDebouncer(self.joystick2, 1)
self.raiseButton = ButtonDebouncer(self.joystick2, 3)
self.lowerButton = ButtonDebouncer(self.joystick2, 2)
self.lightButton = ButtonDebouncer(self.joystick1, 6)
def autonomous(self):
"""Prepare for autonomous mode"""
# Reset Gyro to 0
self.drive.reset_gyro_angle()
# Call autonomous
magicbot.MagicRobot.autonomous(self)
def disabledPeriodic(self):
"""Repeat periodically while robot is disabled. Usually emptied. Sometimes used to easily test sensors and other things."""
pass
def disabledInit(self):
"""Do once right away when robot is disabled."""
self.enable_camera_logging = True
self.drive.disable_camera_tracking()
def teleopInit(self):
"""Do when teleoperated mode is started."""
self.drive.reset_drive_encoders()
self.sd.putValue('startTheTimer', True)
self.intake.target_position = None
self.intake.target_index = None
self.drive.disable_camera_tracking()
self.enable_camera_logging = False
def teleopPeriodic(self):
"""Do periodically while robot is in teleoperated mode."""
# Get the joystick values and move as much as they say.
self.drive.move(-self.joystick1.getY(), self.joystick2.getX())
# If reverse control button is pressed,
if self.reverseButton.get():
# Reverse the drivetrain direction
self.drive.switch_direction()
# If outtake button is pressed,
if self.joystick2.getRawButton(5):
# Then spit ball out.
self.intake.outtake()
# Or, if intake button is pressed,
elif self.joystick2.getRawButton(4):
# Then suck button in.
self.intake.intake()
# If shoot button pressed
if self.shoot.get():
# Automatically shoot ball
self.shootBall.shoot()
"""There's two sets of arm buttons. The first automatically raises and lowers the arm the proper amount, whereas the second will let you manually raise and lower it more precise amounts."""
# If automatic arm raise button is pressed,
if self.raiseButton.get():
# Raise arm
self.intake.raise_arm()
# Or, if automatic arm lower button is pressed, (won't do both at once)
elif self.lowerButton.get():
# Lower arm
self.intake.lower_arm()
# If manual arm raise button is pressed,
if self.joystick1.getRawButton(3):
# Raise arm
self.intake.set_manual(-1)
# If manual arm lower button is pressed, (this one can be activated both at one time)
if self.joystick1.getRawButton(2):
# Lower arm
self.intake.set_manual(1)
# Flashlight
# Automatically turn flashlight off at the starting. It will only be made true if NT value is true.
lightButton = False
# Store whether flashlight button is pressed on dashboard
uiButton = self.sd.getValue('LightBulb', False)
# If the value has changed
if uiButton != self.lastState:
# Flashlight on
lightButton = True
# Update self.lastState to the new state
self.lastState = uiButton
# If light button on joystick or light button is pressed and turn-off state is 0
if (self.lightButton.get() or lightButton) and self.turningOffState == 0:
# Turn on flashlight
self.lightSwitch.switch()
# If joystick button 5 or UI autoaim button is pressed
if self.joystick1.getRawButton(5) or self.auto_aim_button:
# Start targeting goal
self.targetGoal.target()
# Winch
# If joystick1 button 7 is pressed
if self.joystick1.getRawButton(7):
# Set off winch
self.winch.deploy_winch()
# If joystick1 button 8 is pressed
if self.joystick1.getRawButton(8):
# Reel in winch
self.winch.winch()
# If button 9 on joystick1 pressed and robot is backwards
if self.joystick1.getRawButton(9) and self.drive.isTheRobotBackwards:
# Move
self.drive.move(.5, 0)
# Testing angles in pit or when not in competition
# If Field Management System isn't attached
if not self.ds.isFMSAttached():
# If button 10 on joystick1 is pressed
if self.joystick1.getRawButton(10):
# Calibrate rotation angle to 35deg
self.drive.angle_rotation(35)
# If button 9 on joystick1 is pressed
elif self.joystick1.getRawButton(9): # Could be problematic if robot is backwards
# Calibrate robot rotation angle to 0
self.drive.angle_rotation(0)
# If button 10 on joystick2 is pressed
elif self.joystick2.getRawButton(10):
# Activate vision things
self.drive.enable_camera_tracking()
self.drive.align_to_tower()
class Teleop:
last_applied_control = np.array([0, 0, 0])
foc_enabled = False
def __init__(self, robot):
self.robot = robot
self.stick = wpilib.Joystick(0)
self.throttle = wpilib.Joystick(1)
self.claw_const_pressure_active = False
self.prefs = wpilib.Preferences.getInstance()
self.toggle_foc_button = ButtonDebouncer(self.stick, 2)
self.zero_yaw_button = ButtonDebouncer(self.stick, 3)
self.switch_camera_button = ButtonDebouncer(self.stick, 4)
self.low_speed_button = ButtonDebouncer(self.stick, 9)
self.high_speed_button = ButtonDebouncer(self.stick, 10)
def update_smart_dashboard(self):
wpilib.SmartDashboard.putBoolean('FOC Enabled', self.foc_enabled)
def buttons(self):
if self.robot.imu.is_present():
if self.zero_yaw_button.get():
self.robot.imu.reset()
if self.toggle_foc_button.get():
self.foc_enabled = not self.foc_enabled
if self.switch_camera_button.get():
current_camera = (self.prefs.getInt('Selected Camera', 0) + 1) % 2
self.prefs.putInt('Selected Camera', current_camera)
def lift_control(self):
liftPct = self.throttle.getRawAxis(constants.liftAxis)
if self.throttle.getRawButton(5):
self.robot.lift.set_soft_limit_status(False)
else:
self.robot.lift.set_soft_limit_status(True)
if constants.liftInv:
liftPct *= -1
if abs(liftPct) < constants.lift_deadband:
self.robot.lift.setLiftPower(self.robot.lift.sustain)
return
liftPct *= constants.lift_coeff
wpilib.SmartDashboard.putNumber("Lift Power", liftPct)
self.robot.lift.setLiftPower(liftPct)
def claw_control(self):
clawPct = self.throttle.getRawAxis(constants.clawAxis)
if constants.clawInv:
clawPct *= -1
# NOTE: positive = in
# negative = out
if abs(clawPct) < constants.claw_deadband:
if self.claw_const_pressure_active:
clawPct = .05
else:
clawPct = 0
else:
if clawPct < -constants.claw_deadband:
self.claw_const_pressure_active = False
elif clawPct > constants.claw_deadband:
self.claw_const_pressure_active = True
if clawPct > 0:
clawPct *= constants.claw_in_coeff
else:
clawPct *= constants.claw_out_coeff
self.robot.claw.set_power(clawPct)
def winch_control(self):
# if self.throttle.getRawButton(1):
# if (
# abs(self.robot.winch.talon.getSelectedSensorPosition(0))
# < abs(constants.winch_slack)
# ):
# self.robot.winch.forward()
# self.robot.lift.setLiftPower(0)
# else:
# self.robot.winch.forward()
# self.robot.lift.setLiftPower(constants.sync_power)
# elif self.throttle.getRawButton(3):
# self.robot.winch.forward()
# elif self.throttle.getRawButton(2):
# self.robot.winch.reverse()
# else:
# self.robot.winch.stop()
if self.throttle.getRawButton(3):
self.robot.winch.forward()
self.robot.lift.setLiftPower(0)
elif (self.throttle.getRawButton(2)
and not wpilib.DriverStation.getInstance().isFMSAttached()):
self.robot.winch.reverse()
else:
self.robot.winch.stop()
def drive(self):
"""
Drive the robot directly using a joystick.
"""
ctrl = np.array([self.stick.getRawAxis(1), self.stick.getRawAxis(0)])
if constants.fwdInv:
ctrl[0] *= -1
if constants.strInv:
ctrl[1] *= -1
if abs(ctrl[0]) < 0.1:
ctrl[0] = 0
if abs(ctrl[1]) < 0.1:
ctrl[1] = 0
linear_control_active = True
if abs(np.sqrt(np.sum(ctrl**2))) < 0.1:
ctrl[0] = 0
ctrl[1] = 0
linear_control_active = False
if (self.robot.imu.is_present() and self.foc_enabled):
# perform FOC coordinate transform
hdg = self.robot.imu.get_robot_heading()
# Right-handed passive (alias) transform matrix
foc_transform = np.array([[np.cos(hdg), np.sin(hdg)],
[-np.sin(hdg), np.cos(hdg)]])
ctrl = np.squeeze(np.matmul(foc_transform, ctrl))
tw = self.stick.getRawAxis(2)
if constants.rcwInv:
tw *= -1
rotation_control_active = True
if abs(tw) < 0.15:
tw = 0
rotation_control_active = False
tw *= constants.turn_sensitivity
if linear_control_active or rotation_control_active:
self.last_applied_control = np.array([ctrl[0], ctrl[1], tw])
speed_coefficient = 0.75
if self.low_speed_button.get():
speed_coefficient = 0.25
elif self.high_speed_button.get():
speed_coefficient = 1
self.robot.drivetrain.drive(ctrl[0] * speed_coefficient,
ctrl[1] * speed_coefficient,
tw * speed_coefficient,
max_wheel_speed=constants.teleop_speed)
else:
self.robot.drivetrain.drive(self.last_applied_control[0],
self.last_applied_control[1],
self.last_applied_control[2],
max_wheel_speed=0)
def createObjects(self):
# Joysticks
self.joystick1 = wpilib.Joystick(0)
self.joystick2 = wpilib.Joystick(1)
# Motors (l/r = left/right, f/r = front/rear)
self.lf_motor = wpilib.CANTalon(5)
self.lr_motor = wpilib.CANTalon(10)
self.rf_motor = wpilib.CANTalon(15)
self.rr_motor = wpilib.CANTalon(20)
# Drivetrain object
self.robot_drive = wpilib.RobotDrive(self.lf_motor, self.lr_motor, self.rf_motor, self.rr_motor)
# Left and right arm motors (there's two, which both control the raising and lowering the arm)
self.leftArm = wpilib.CANTalon(25)
self.rightArm = wpilib.CANTalon(30)
# Motor that spins the bar at the end of the arm.
# There was originally going to be one on the right, but we decided against that in the end.
# In retrospect, that was probably a mistake.
self.leftBall = wpilib.Talon(9)
# Motor that reels in the winch to lift the robot.
self.winchMotor = wpilib.Talon(0)
# Motor that opens the winch.
self.kickMotor = wpilib.Talon(1)
# Aiming flashlight
self.flashlight = wpilib.Relay(0)
# Timer to keep light from staying on for too long
self.lightTimer = wpilib.Timer()
# Flashlight has three intensities. So, when it's turning off, it has to go off on, off on, off.
# self.turningOffState keeps track of which on/off it's on.
self.turningOffState = 0
# Is currently on or off? Used to detect if UI button is pressed.
self.lastState = False
# Drive encoders; measure how much the motor has spun
self.rf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontRightMotor, True)
self.lf_encoder = driveEncoders.DriveEncoders(self.robot_drive.frontLeftMotor)
# Distance sensors
self.back_sensor = distance_sensors.SharpIRGP2Y0A41SK0F(0)
self.ultrasonic = wpilib.AnalogInput(1)
# NavX (purple board on top of the RoboRIO)
self.navX = navx.AHRS.create_spi()
# Initialize SmartDashboard, the table of robot values
self.sd = NetworkTable.getTable('SmartDashboard')
# How much will the control loop pause in between (0.025s = 25ms)
self.control_loop_wait_time = 0.025
# Button to reverse controls
self.reverseButton = ButtonDebouncer(self.joystick1, 1)
# Initiate functional buttons on joysticks
self.shoot = ButtonDebouncer(self.joystick2, 1)
self.raiseButton = ButtonDebouncer(self.joystick2, 3)
self.lowerButton = ButtonDebouncer(self.joystick2, 2)
self.lightButton = ButtonDebouncer(self.joystick1, 6)
class Panthera(magicbot.MagicRobot):
drive: drive.Drive
winch: winch.Winch
arm: arm.Arm
recorder: recorder.Recorder
time = tunable(0)
plates = tunable('')
voltage = tunable(0)
rotation = tunable(0)
unified_control = tunable(False)
recording = tunable(False)
stabilize = tunable(False)
stabilizer_threshold = tunable(30)
stabilizer_aggression = tunable(5)
def createObjects(self):
"""
Initialize robot components.
"""
# Joysticks
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.joystick_alt = wpilib.Joystick(2)
# Buttons
self.btn_claw = ButtonDebouncer(self.joystick_left, 1)
self.btn_forearm = ButtonDebouncer(self.joystick_right, 1)
self.btn_up = JoystickButton(self.joystick_left, 3)
self.btn_down = JoystickButton(self.joystick_left, 2)
self.btn_climb = JoystickButton(self.joystick_right, 11)
# Buttons on alternative joystick
self.btn_claw_alt = ButtonDebouncer(self.joystick_alt, 1)
self.btn_forearm_alt = ButtonDebouncer(self.joystick_alt, 2)
self.btn_climb_alt = JoystickButton(self.joystick_alt, 3)
# Buttons for toggling control options and aides
self.btn_unified_control = ButtonDebouncer(self.joystick_alt, 8)
self.btn_record = ButtonDebouncer(self.joystick_left, 6)
self.btn_stabilize = ButtonDebouncer(self.joystick_alt, 12)
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_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
# Follow front wheels with rear to save CAN packets
self.lr_motor.follow(self.lf_motor)
self.rr_motor.follow(self.rf_motor)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(self.lf_motor, self.rf_motor)
# Winch
self.winch_motors = wpilib.SpeedControllerGroup(wpilib.Victor(7), wpilib.Victor(8))
# Motion Profiling
self.position_controller = motion_profile.PositionController()
# Arm
self.elevator = wpilib.Victor(5)
self.forearm = wpilib.DoubleSolenoid(2, 3)
self.claw = wpilib.DoubleSolenoid(0, 1)
# NavX (purple board on top of the RoboRIO)
self.navx = navx.AHRS.create_spi()
self.navx.reset()
# Utility
self.ds = wpilib.DriverStation.getInstance()
self.timer = wpilib.Timer()
self.pdp = wpilib.PowerDistributionPanel(0)
self.compressor = wpilib.Compressor()
# Camera server
wpilib.CameraServer.launch('camera/camera.py:main')
def robotPeriodic(self):
"""
Executed periodically regardless of mode.
"""
self.time = int(self.timer.getMatchTime())
self.voltage = self.pdp.getVoltage()
self.rotation = self.navx.getAngle() % 360
def autonomous(self):
"""
Prepare for and start autonomous mode.
"""
self.compressor.stop()
self.drive.squared_inputs = False
self.drive.rotational_constant = 0.5
self.plates = ''
wpilib.Timer.delay(1)
# Read data on plate colors from FMS.
# 3.10: "The FMS provides the ALLIANCE color assigned to each PLATE to the Driver Station software. Immediately following the assignment of PLATE color prior to the start of AUTO."
# Will fetch a string of three characters ('L' or 'R') denoting position of the current alliance's on the switches and scale, with the nearest structures first.
# More information: http://wpilib.screenstepslive.com/s/currentCS/m/getting_started/l/826278-2018-game-data-details
self.plates = self.ds.getGameSpecificMessage()
# Call autonomous
super().autonomous()
def disabledInit(self):
"""
Executed once right away when robot is disabled.
"""
# Reset Gyro to 0
self.navx.reset()
def disabledPeriodic(self):
"""
Executed periodically while robot is disabled.
Useful for testing.
"""
pass
def teleopInit(self):
"""
Executed when teleoperated mode begins.
"""
self.compressor.start()
self.drive.squared_inputs = True
self.drive.rotational_constant = 0.5
def teleopPeriodic(self):
"""
Executed periodically while robot is in teleoperated mode.
"""
# Read from joysticks and move drivetrain accordingly
self.drive.move(-self.joystick_left.getY(),
self.joystick_right.getX(),
self.btn_fine_movement.get())
if self.stabilize:
if abs(self.navx.getPitch()) > self.stabilizer_threshold:
self.drive.move(self.navx.getPitch() / 180 * self.stabilizer_aggression, 0)
if self.btn_stabilize.get():
self.stabilize = not self.stabilize
if self.btn_unified_control.get():
self.unified_control = not self.unified_control
# Winch
if (self.btn_climb.get() and self.unified_control) or self.btn_climb_alt.get():
self.winch.run()
# Arm
if (self.btn_claw.get() and self.unified_control) or self.btn_claw_alt.get():
self.arm.actuate_claw()
if (self.btn_forearm.get() and self.unified_control) or self.btn_forearm_alt.get():
self.arm.actuate_forearm()
self.arm.move(-self.joystick_alt.getY())
if self.unified_control:
if self.btn_up.get():
self.arm.up()
if self.btn_down.get():
self.arm.down()
if self.btn_record.get():
if self.recording:
self.recording = False
self.recorder.stop()
else:
self.recording = True
self.recorder.start(self.voltage)
if self.recording:
self.recorder.capture((self.joystick_left, self.joystick_right, self.joystick_alt))
