def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.frontLeft = WPI_TalonSRX(DRIVETRAIN_FRONT_LEFT_MOTOR)
self.leftTalon = WPI_TalonSRX(DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight = WPI_TalonSRX(DRIVETRAIN_FRONT_RIGHT_MOTOR)
self.rightTalon = WPI_TalonSRX(DRIVETRAIN_REAR_RIGHT_MOTOR)
# Set the front motors to be the followers of the rear motors
self.frontLeft.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_RIGHT_MOTOR)
# Add the motors to the speed controller groups and create the differential drivetrain
self.leftDrive = SpeedControllerGroup(self.frontLeft, self.leftTalon)
self.rightDrive = SpeedControllerGroup(self.frontRight,
self.rightTalon)
self.diffDrive = DifferentialDrive(self.leftDrive, self.rightDrive)
# Setup the default motor controller setup
self.initControllerSetup()
# Map the pigeon. This will be connected to an unused Talon.
self.talonPigeon = WPI_TalonSRX(DRIVETRAIN_PIGEON)
self.pigeonIMU = PigeonIMU(self.talonPigeon)
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.rightTalon = WPI_TalonSRX(INTAKE_RIGHT_MOTOR)
self.leftTalon = WPI_TalonSRX(INTAKE_LEFT_MOTOR)
def createObjects(self):
"""
Initialize testbench components.
"""
self.joystick = wpilib.Joystick(0)
self.brushless = wpilib.NidecBrushless(9, 9)
self.spark = wpilib.Spark(4)
self.lf_victor = wpilib.Victor(0)
self.lr_victor = wpilib.Victor(1)
self.rf_victor = wpilib.Victor(2)
self.rr_victor = wpilib.Victor(3)
self.lf_talon = WPI_TalonSRX(5)
self.lr_talon = WPI_TalonSRX(10)
self.rf_talon = WPI_TalonSRX(15)
self.rr_talon = WPI_TalonSRX(20)
self.drive = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_victor, self.lr_victor,
self.lf_talon, self.lr_talon),
wpilib.SpeedControllerGroup(self.rf_victor, self.rr_victor,
self.rf_talon, self.rr_talon))
wpilib.CameraServer.launch()
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.talon = WPI_TalonSRX(BOOM_MOTOR)
# Setup the default motor controller setup
self.initOpenLoop()
self.initClosedLoop()
def createObjects(self):
self.joystick_left = wpilib.Joystick(0)
self.joystick_right = wpilib.Joystick(1)
self.lf_motor = WPI_TalonSRX(10)
self.lr_motor = WPI_TalonSRX(15)
self.rf_motor = WPI_TalonSRX(20)
self.rr_motor = WPI_TalonSRX(25)
# Drivetrain
self.train = wpilib.drive.DifferentialDrive(
wpilib.SpeedControllerGroup(self.lf_motor, self.lr_motor),
wpilib.SpeedControllerGroup(self.rf_motor, self.rr_motor))
def __init__(self):
super().__init__()
self.logger = logging.getLogger(self.getName())
# Configure motors
self.leftMasterTalon = WPI_TalonSRX(RobotMap.LEFT_MASTER_TALON)
self.leftSlaveTalon = WPI_TalonSRX(RobotMap.LEFT_SLAVE_TALON)
self.rightMasterTalon = WPI_TalonSRX(RobotMap.RIGHT_MASTER_TALON)
self.rightSlaveTalon = WPI_TalonSRX(RobotMap.RIGHT_SLAVE_TALON)
self.leftSlaveTalon.follow(self.leftMasterTalon)
self.rightSlaveTalon.follow(self.rightMasterTalon)
self.leftMasterTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
self.rightMasterTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
self.leftMasterTalon.setInverted(True)
self.leftSlaveTalon.setInverted(True)
self.configureDrivePID()
# Configure shift solenoid
self.shiftSolenoid = DoubleSolenoid(RobotMap.SHIFT_IN_SOLENOID,
RobotMap.SHIFT_OUT_SOLENOID)
def __init__(self):
super().__init__(p=0.015, i=0.0001, d=0.0)
self._deadband = 0.1
self._turnOutput = 0.0
# Configure motors
motors = [
WPI_TalonSRX(i)
for i in [RM.DRIVE_LF, RM.DRIVE_LB, RM.DRIVE_RF, RM.DRIVE_RB]
]
for i, motor in enumerate(motors):
# motor.configFactoryDefault()
motor.enableVoltageCompensation(True)
motor.configOpenLoopRamp(1.4, 10)
motor.setNeutralMode(NeutralMode.Brake)
# Invert left side motors?
# if i <= 1:
# motor.setInverted(True)
# Set up PIDSubsystem parameters
self.setInputRange(0.0, 360.0)
self.pidController = self.getPIDController()
self.pidController.setContinuous(True)
self.pidController.setAbsoluteTolerance(1.0)
self.setSetpoint(0.0)
# Enable this is you use the PID functionality
# self.pidController.enable()
self.drive = MecanumDrive(*motors)
self.drive.setExpiration(1)
self.drive.setSafetyEnabled(False)
self.drive.setDeadband(0.1)
def __init__(self):
super().__init__()
self.leftIntakeTalon = WPI_TalonSRX(RobotMap.LEFT_INTAKE_TALON)
self.rightIntakeTalon = WPI_TalonSRX(RobotMap.RIGHT_INTAKE_TALON)
self.intakeVictor = WPI_VictorSPX(RobotMap.END_EFFECTOR_VICTOR)
self.intakeSolenoid = DoubleSolenoid(RobotMap.INTAKE_IN_SOLENOID,
RobotMap.INTAKE_OUT_SOLENOID)
self.placingSolenoid = DoubleSolenoid(RobotMap.PLACE_IN_SOLENOID,
RobotMap.PLACE_OUT_SOLENOID)
self.leftIntakeTalon.configContinuousCurrentLimit(
6, Constants.TIMEOUT_MS)
self.rightIntakeTalon.configContinuousCurrentLimit(
6, Constants.TIMEOUT_MS)
self.setPosition(IntakeOutput.Position.UP)
self.setArmPower(0)
self.mode = None # This cannot be set here because the buttons do not yet exist.
def __init__(self):
super().__init__("Drivetrain")
self.leftleader = WPI_TalonSRX(CAN_LEFT_LEADER)
set_motor(self.leftleader, WPI_TalonSRX.NeutralMode.Brake, False)
self.leftfollower = WPI_VictorSPX(CAN_LEFT_FOLLOWER)
set_motor(self.leftfollower, WPI_VictorSPX.NeutralMode.Brake, False)
self.leftfollower.follow(self.leftleader)
self.rightleader = WPI_TalonSRX(CAN_RIGHT_LEADER)
set_motor(self.rightleader, WPI_TalonSRX.NeutralMode.Brake, True)
self.rightfollower = WPI_VictorSPX(CAN_RIGHT_FOLLOWER)
set_motor(self.rightfollower, WPI_VictorSPX.NeutralMode.Brake, True)
self.rightfollower.follow(self.rightleader)
self.DS = wpilib.DoubleSolenoid(0, 1)
self.drive = DifferentialDrive(self.leftleader, self.rightleader)
self.drive.maxOutput = 1.0
def __init__(self):
super().__init__()
self.liftDrive = WPI_TalonSRX(RobotMap.BACK_DRIVE_TALON)
self.liftSpark = CANSparkMax(RobotMap.LIFT_SPARK, MotorType.kBrushless)
self.liftSpark.restoreFactoryDefaults()
self.liftSpark.setIdleMode(IdleMode.kCoast)
self.liftSpark.setSmartCurrentLimit(40)
self.liftEncoder = self.liftSpark.getEncoder()
self.liftPID = self.liftSpark.getPIDController()
self.setLiftPIDConstants()
self.elevatorSpark = CANSparkMax(RobotMap.ELEVATOR_SPARK,
MotorType.kBrushless)
self.elevatorSpark.restoreFactoryDefaults()
self.elevatorSpark.setIdleMode(IdleMode.kCoast)
self.elevatorSpark.setInverted(True)
self.liftSpark.setSmartCurrentLimit(60)
self.elevatorEncoder = self.elevatorSpark.getEncoder()
self.elevatorPID = self.elevatorSpark.getPIDController()
self.setElevatorPIDConstants()
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
class MyRobot(MagicRobot):
#
# Define components here
#
drive = drive.Drive
arm = arm.Arm
#
# Define constants here
#
# Robot attributes
WHEEL_DIAMETER = 0.5 # 6 inches
ENCODER_COUNTS_PER_REV = 360
# Pathfinder constants
MAX_VELOCITY = 5 # ft/s
MAX_ACCELERATION = 6
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 autonomous(self):
"""
Prepare for and start autonomous mode.
"""
self.drive.reset()
self.drive.squared_inputs = False
self.drive.rotational_constant = 0.5
# Call autonomous
super().autonomous()
def teleopPeriodic(self):
"""
Place code here that does things as a result of operator
actions
"""
self.drive.move(
-self.joystick_left.getY(),
self.joystick_right.getX(),
self.btn_fine_movement.get(),
)
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.
"""
# 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 DriveTrain(Subsystem):
"""
This subsystem controls the drivetrain for the robot.
"""
def __init__(self):
super().__init__()
self.logger = logging.getLogger(self.getName())
# Configure motors
self.leftMasterTalon = WPI_TalonSRX(RobotMap.LEFT_MASTER_TALON)
self.leftSlaveTalon = WPI_TalonSRX(RobotMap.LEFT_SLAVE_TALON)
self.rightMasterTalon = WPI_TalonSRX(RobotMap.RIGHT_MASTER_TALON)
self.rightSlaveTalon = WPI_TalonSRX(RobotMap.RIGHT_SLAVE_TALON)
self.leftSlaveTalon.follow(self.leftMasterTalon)
self.rightSlaveTalon.follow(self.rightMasterTalon)
self.leftMasterTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
self.rightMasterTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, Constants.TIMEOUT_MS)
self.leftMasterTalon.setInverted(True)
self.leftSlaveTalon.setInverted(True)
self.configureDrivePID()
# Configure shift solenoid
self.shiftSolenoid = DoubleSolenoid(RobotMap.SHIFT_IN_SOLENOID,
RobotMap.SHIFT_OUT_SOLENOID)
def configureDrivePID(self):
# Slot 0 = Distance PID
# Slot 1 = Velocity PID
self.leftMasterTalon.config_kF(0, Constants.LEFT_DISTANCE_F,
Constants.TIMEOUT_MS)
self.leftMasterTalon.config_kP(0, Constants.LEFT_DISTANCE_P,
Constants.TIMEOUT_MS)
self.leftMasterTalon.config_kI(0, Constants.LEFT_DISTANCE_I,
Constants.TIMEOUT_MS)
self.leftMasterTalon.config_kD(0, Constants.LEFT_DISTANCE_D,
Constants.TIMEOUT_MS)
self.leftMasterTalon.config_kF(1, Constants.LEFT_VELOCITY_F,
Constants.TIMEOUT_MS)
self.leftMasterTalon.config_kP(1, Constants.LEFT_VELOCITY_P,
Constants.TIMEOUT_MS)
self.leftMasterTalon.config_kI(1, Constants.LEFT_VELOCITY_I,
Constants.TIMEOUT_MS)
self.leftMasterTalon.config_kD(1, Constants.LEFT_VELOCITY_D,
Constants.TIMEOUT_MS)
self.rightMasterTalon.config_kF(0, Constants.RIGHT_DISTANCE_F,
Constants.TIMEOUT_MS)
self.rightMasterTalon.config_kP(0, Constants.RIGHT_DISTANCE_P,
Constants.TIMEOUT_MS)
self.rightMasterTalon.config_kI(0, Constants.RIGHT_DISTANCE_I,
Constants.TIMEOUT_MS)
self.rightMasterTalon.config_kD(0, Constants.RIGHT_DISTANCE_D,
Constants.TIMEOUT_MS)
self.rightMasterTalon.config_kF(1, Constants.RIGHT_VELOCITY_F,
Constants.TIMEOUT_MS)
self.rightMasterTalon.config_kP(1, Constants.RIGHT_VELOCITY_P,
Constants.TIMEOUT_MS)
self.rightMasterTalon.config_kI(1, Constants.RIGHT_VELOCITY_I,
Constants.TIMEOUT_MS)
self.rightMasterTalon.config_kD(1, Constants.RIGHT_VELOCITY_D,
Constants.TIMEOUT_MS)
self.leftMasterTalon.configMotionAcceleration(6385)
self.rightMasterTalon.configMotionAcceleration(6385)
self.leftMasterTalon.configMotionCruiseVelocity(6385)
self.rightMasterTalon.configMotionCruiseVelocity(6385)
def initDefaultCommand(self):
self.setDefaultCommand(JoystickDrive())
def zeroEncoders(self):
self.leftMasterTalon.setSelectedSensorPosition(0)
self.rightMasterTalon.setSelectedSensorPosition(0)
def set(self, type, leftMagnitude, rightMagnitude):
assert (isinstance(type, ControlMode))
if type == ControlMode.PercentOutput:
self.leftMasterTalon.set(ControlMode.PercentOutput, leftMagnitude)
self.rightMasterTalon.set(ControlMode.PercentOutput,
rightMagnitude)
elif type == ControlMode.Velocity:
self.leftMasterTalon.set(ControlMode.Velocity, leftMagnitude)
self.rightMasterTalon.set(ControlMode.Velocity, rightMagnitude)
elif type == ControlMode.MotionMagic:
self.leftMasterTalon.set(ControlMode.MotionMagic, leftMagnitude)
self.rightMasterTalon.set(ControlMode.MotionMagic, rightMagnitude)
def setPIDSlot(self, slot):
self.leftMasterTalon.selectProfileSlot(slot, 0)
self.rightMasterTalon.selectProfileSlot(slot, 0)
def getPositions(self):
return self.leftMasterTalon.getSelectedSensorPosition(
), self.rightMasterTalon.getSelectedSensorPosition()
def getVelocities(self):
return self.leftMasterTalon.getSelectedSensorVelocity(
), self.rightMasterTalon.getSelectedSensorVelocity()
def customArcadeDrive(self, xSpeed, zRotation, squareInputs):
deadband = 0.1
xSpeed, zRotation = self.limit(xSpeed, zRotation)
xSpeed, zRotation = self.applyDeadband(deadband, xSpeed, zRotation)
if squareInputs:
xSpeed = math.copysign(xSpeed * xSpeed, xSpeed)
zRotation = math.copysign(zRotation * zRotation, zRotation)
maxInput = math.copysign(max(abs(xSpeed), abs(zRotation)), xSpeed)
if xSpeed >= 0:
if zRotation >= 0:
leftMotorOutput = maxInput
rightMotorOutput = xSpeed - zRotation
else:
leftMotorOutput = xSpeed + zRotation
rightMotorOutput = maxInput
else:
if zRotation >= 0:
leftMotorOutput = xSpeed + zRotation
rightMotorOutput = maxInput
else:
leftMotorOutput = maxInput
rightMotorOutput = xSpeed - zRotation
# Fine tune control
if abs(xSpeed) > abs(zRotation):
self.leftMasterTalon.set(ControlMode.PercentOutput,
self.limit(leftMotorOutput) * 0.55)
self.rightMasterTalon.set(ControlMode.PercentOutput,
self.limit(rightMotorOutput) * -0.55)
else:
self.leftMasterTalon.set(ControlMode.PercentOutput,
self.limit(leftMotorOutput) * 0.55)
self.rightMasterTalon.set(ControlMode.PercentOutput,
self.limit(rightMotorOutput) * -0.55)
def limit(self, *args):
return_list = []
for arg in args:
if arg > 1.0:
return_list.append(1.0)
elif arg < -1.0:
return_list.append(1.0)
else:
return_list.append(arg)
return return_list[0] if len(return_list) == 1 else return_list
def applyDeadband(self, deadband, *args):
return_list = []
for arg in args:
if abs(arg) < deadband:
return_list.append(0.0)
else:
return_list.append(arg)
return return_list[0] if len(return_list) == 1 else return_list
class LiftElevator(Subsystem):
def __init__(self):
super().__init__()
self.liftDrive = WPI_TalonSRX(RobotMap.BACK_DRIVE_TALON)
self.liftSpark = CANSparkMax(RobotMap.LIFT_SPARK, MotorType.kBrushless)
self.liftSpark.restoreFactoryDefaults()
self.liftSpark.setIdleMode(IdleMode.kCoast)
self.liftSpark.setSmartCurrentLimit(40)
self.liftEncoder = self.liftSpark.getEncoder()
self.liftPID = self.liftSpark.getPIDController()
self.setLiftPIDConstants()
self.elevatorSpark = CANSparkMax(RobotMap.ELEVATOR_SPARK,
MotorType.kBrushless)
self.elevatorSpark.restoreFactoryDefaults()
self.elevatorSpark.setIdleMode(IdleMode.kCoast)
self.elevatorSpark.setInverted(True)
self.liftSpark.setSmartCurrentLimit(60)
self.elevatorEncoder = self.elevatorSpark.getEncoder()
self.elevatorPID = self.elevatorSpark.getPIDController()
self.setElevatorPIDConstants()
# self.liftEncoder.setPosition(0)
def setLiftPIDConstants(self):
self.liftPID.setFF(Constants.LIFT_ELEVATOR_FF)
self.liftPID.setP(Constants.LIFT_ELEVATOR_P)
self.liftPID.setI(Constants.LIFT_ELEVATOR_I)
self.liftPID.setD(Constants.LIFT_ELEVATOR_D)
self.liftPID.setSmartMotionAllowedClosedLoopError(0, 0)
self.liftPID.setSmartMotionMaxVelocity(Constants.LIFT_MAX_VELOCITY, 0)
self.liftPID.setSmartMotionMaxAccel(Constants.LIFT_MAX_ACCEL, 0)
self.liftPID.setSmartMotionMinOutputVelocity(0, 0)
def setElevatorPIDConstants(self):
self.elevatorPID.setFF(Constants.LIFT_ELEVATOR_FF)
self.elevatorPID.setP(Constants.LIFT_ELEVATOR_P)
self.elevatorPID.setI(Constants.LIFT_ELEVATOR_I)
self.elevatorPID.setD(Constants.LIFT_ELEVATOR_D)
self.elevatorPID.setSmartMotionMaxVelocity(
Constants.ELEVATOR_MAX_VELOCITY, 0)
self.elevatorPID.setSmartMotionMaxAccel(Constants.ELEVATOR_MAX_ACCEL,
0)
self.elevatorPID.setSmartMotionAllowedClosedLoopError(0, 0)
self.elevatorPID.setSmartMotionMinOutputVelocity(0, 0)
def initDefaultCommand(self):
pass
def setLiftReference(self, targetPos):
self.liftPID.setReference(targetPos, ControlType.kSmartMotion)
def setElevatorReference(self, targetPos):
self.elevatorPID.setReference(targetPos, ControlType.kSmartMotion)
def setDrive(self, magnitude):
self.liftDrive.set(magnitude)
def setLift(self, targetSpeed):
self.liftDrive.set(targetSpeed)
def setElevator(self, targetSpeed):
self.elevatorSpark.set(targetSpeed)
def setLiftElevator(self, targetLiftSpeed, targetElevatorSpeed):
self.setLift(targetLiftSpeed)
self.setElevator(targetElevatorSpeed)
def stopLiftElevator(self):
self.elevatorSpark.set(0)
self.liftSpark.set(0)
def resetEncoders(self):
self.liftSpark.setEncPosition(0)
self.elevatorSpark.setEncPosition(0)
def getLiftElevatorAmps(self):
return self.liftSpark.getOutputCurrent(
), self.elevatorSpark.getOutputCurrent()
def getLiftPos(self):
return self.liftEncoder.getPosition()
def getElevatorPos(self):
return self.elevatorEncoder.getPosition()
def getLiftElevatorPos(self):
return self.getLiftPos(), self.getElevatorPos()
def getLiftElevatorTemps(self):
return self.liftSpark.getMotorTemperature(
), self.elevatorSpark.getMotorTemperature()
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)
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 Boom(Subsystem):
"""
The boom subsystem is used by the operator and also the boom controller. There is a pot
mounted to the linear actuator which will provide feedback for the boom controller.
"""
ENCODER_TICS_PER_REV = 102.4 # 10-turn analog pot on 10-bit ADC: 1024 / 10
POT_INTAKE_POSITION_DEG = 84 * (3600 / 1023)
POT_SWITCH_POSITION_DEG = 348 * (3600 / 1023) # ** TODO **
POT_SCALE_POSITION_DEG = 848 * (3600 / 1023) # ** TODO **
POT_SCALE_POSITION = 825
POT_SCALE_UPPER_ERROR = 1005
POT_SCALE_LOWER_ERROR = 645
POT_ERROR_LIMIT = 360.0
# FORWARD_SOFT_LIMIT = 840
# REVERSE_SOFT_LIMIT = 90
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.talon = WPI_TalonSRX(BOOM_MOTOR)
# Setup the default motor controller setup
self.initOpenLoop()
self.initClosedLoop()
def initOpenLoop(self):
"""
This method will setup the default settings of the motor controllers.
"""
# Set to percent output mode at neutral
self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, 0.0)
# Add a ramp-rate limiter to joystick control
self.talon.configOpenLoopRamp(0.2, 10)
# Add current limiter
# self.talon.configPeakCurrentLimit(BOOM_MAX_CURRENT, 10)
# self.talon.configPeakCurrentDuration(0, 10) # Necessary to avoid errata
# self.talon.configContinuousCurrentLimit(BOOM_MAX_CURRENT, 10)
# self.talon.enableCurrentLimit(True)
# Add soft limits
# self.talon.configForwardSoftLimitThreshold(self.FORWARD_SOFT_LIMIT, 0)
# self.talon.configForwardSoftLimitEnable(True, 0)
# self.talon.configReverseSoftLimitThreshold(self.REVERSE_SOFT_LIMIT, 0)
# self.talon.configReverseSoftLimitEnable(True, 0)
# Configured forward and reverse limit switch of Talon to be from a feedback connector and
# be normally open
self.talon.configForwardLimitSwitchSource(
LimitSwitchSource.FeedbackConnector,
LimitSwitchNormal.NormallyOpen, 0, 10)
self.talon.configReverseLimitSwitchSource(
LimitSwitchSource.FeedbackConnector,
LimitSwitchNormal.NormallyOpen, 0, 10)
# Set brake/coast mode
self.talon.setNeutralMode(WPI_TalonSRX.NeutralMode.Brake)
def initClosedLoop(self):
# PIDF slot index 0 is for single-position moves
self.talon.config_kP(0, 5.0, 10)
self.talon.config_kI(0, 0.0, 10)
self.talon.config_kD(0, 0.0, 10)
self.talon.config_kF(0, 20, 10)
# PIDF slot index 1 is for multi-position moves
self.talon.config_kP(1, 1.0, 10)
self.talon.config_kI(1, 0.0, 10)
self.talon.config_kD(1, 0.0, 10)
self.talon.config_kF(1, 22, 10)
# Use analog potentiometer
self.initAnalogPotentiometer()
def initAnalogPotentiometer(self):
"""
This method will initialize the analog potentiometer for position feedback.
"""
self.talon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.Analog, 0, 10)
def getPotPositionInDegrees(self):
"""
This method will return the potentiometer position.
"""
# Lead: 0.20", Potentiometer for a 6.85:1 ratio, 12 / 0.2 = 60 => 60 / 6.85 = 8.76
# The potentiometer will turn 8.76 times during a 12-inch travel
# 0V - 5V maps to 0 - 1023
degreesPerADC = 3600 / 1024
degrees = self.talon.getSensorCollection().getAnalogInRaw(
) * degreesPerADC
# logger.debug('Pot Degrees: %1.2f' % (degrees))
return degrees
def getPotPosition(self):
"""
This method will return the potentiometer position.
"""
return self.talon.getSensorCollection().getAnalogInRaw()
def getPotVelocityInDegPer100ms(self):
"""
This method will return the potentiometer velocity in deg / 100ms
"""
return self.talon.getSensorCollection().getAnalogInVel()
def getActiveMPPosition(self):
"""
This method will return the active motion profile position
"""
return self.talon.getActiveTrajectoryPosition()
def getActiveMPVelocity(self):
"""
This method will return the active motion profile position
"""
return self.talon.getActiveTrajectoryVelocity()
def getPrimaryClosedLoopTarget(self):
"""
This method will return the closed loop target.
"""
return self.talon.getClosedLoopTarget(0)
def getPrimaryClosedLoopError(self):
"""
This method will return the closed loop error for the primary conrol loop.
"""
return self.talon.getClosedLoopError(0)
def initDefaultCommand(self):
"""
This method will set the default command for this subsystem.
"""
self.setDefaultCommand(BoomJoystick(self.robot))
class DriveTrain(Subsystem):
"""
The DriveTrain subsytem is used by the driver as well as the Pathfinder and Motion Profile
controllers. The default command is DriveJoystick. Each side of the differential drive is
connected to CTRE's magnetic encoders.
"""
ENCODER_TICKS_PER_REV = 4096
MP_SLOT0_SELECT = 0
MP_SLOT1_SELECT = 0
def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.frontLeft = WPI_TalonSRX(DRIVETRAIN_FRONT_LEFT_MOTOR)
self.leftTalon = WPI_TalonSRX(DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight = WPI_TalonSRX(DRIVETRAIN_FRONT_RIGHT_MOTOR)
self.rightTalon = WPI_TalonSRX(DRIVETRAIN_REAR_RIGHT_MOTOR)
# Set the front motors to be the followers of the rear motors
self.frontLeft.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_RIGHT_MOTOR)
# Add the motors to the speed controller groups and create the differential drivetrain
self.leftDrive = SpeedControllerGroup(self.frontLeft, self.leftTalon)
self.rightDrive = SpeedControllerGroup(self.frontRight,
self.rightTalon)
self.diffDrive = DifferentialDrive(self.leftDrive, self.rightDrive)
# Setup the default motor controller setup
self.initControllerSetup()
# Map the pigeon. This will be connected to an unused Talon.
self.talonPigeon = WPI_TalonSRX(DRIVETRAIN_PIGEON)
self.pigeonIMU = PigeonIMU(self.talonPigeon)
def initControllerSetup(self):
"""
This method will setup the default settings of the motor controllers.
"""
# Feedback sensor phase
self.leftTalon.setSensorPhase(True)
self.rightTalon.setSensorPhase(True)
# Diable the motor-safety
self.diffDrive.setSafetyEnabled(False)
# Enable brake/coast mode
self.leftTalon.setNeutralMode(WPI_TalonSRX.NeutralMode.Coast)
self.rightTalon.setNeutralMode(WPI_TalonSRX.NeutralMode.Coast)
# This function will intiliaze the drivetrain motor controllers to the factory defaults.
# Only values which do not match the factory default will be written. Any values which
# are explicity listed will be skipped (ie any values written prior in this method).
# ***** TODO ***** #
def initiaizeDrivetrainMotionProfileControllers(self, stream_rate):
"""
This method will initialize the Talon's for motion profiling
"""
# Invert right motors
self.rightTalon.setInverted(True)
self.frontRight.setInverted(True)
# Enable voltage compensation for 12V
self.leftTalon.configVoltageCompSaturation(12.0, 10)
self.leftTalon.enableVoltageCompensation(True)
self.rightTalon.configVoltageCompSaturation(12.0, 10)
self.rightTalon.enableVoltageCompensation(True)
# PIDF slot index 0 is for autonomous wheel postion
# There are 4096 encoder units per rev. 1 rev of the wheel is pi * diameter. That
# evaluates to 2607.6 encoder units per foot. For the feed-forward system, we expect very
# tight position control, so use a P-gain which drives full throttle at 8" of error. This
# evaluates to 0.588 = (1.0 * 1023) / (8 / 12 * 2607.6)
self.leftTalon.config_kP(0, 0.0, 10)
self.leftTalon.config_kI(0, 0.0, 10)
self.leftTalon.config_kD(0, 0.0, 10)
self.leftTalon.config_kF(0, 1023 / 12, 10) # 10-bit ADC / 12 V
self.leftTalon.config_IntegralZone(0, 100, 10)
self.leftTalon.configClosedLoopPeakOutput(0, 1.0, 10)
self.rightTalon.config_kP(0, 0.0, 10)
self.rightTalon.config_kI(0, 0.0, 10)
self.rightTalon.config_kD(0, 0.0, 10)
self.rightTalon.config_kF(0, 1023 / 12, 10) # 10-bit ADC / 12 V
self.rightTalon.config_IntegralZone(0, 100, 10)
self.rightTalon.configClosedLoopPeakOutput(0, 1.0, 10)
# PIDF slot index 1 is for autonomous heading
self.leftTalon.config_kP(1, 0, 10)
self.leftTalon.config_kI(1, 0, 10)
self.leftTalon.config_kD(1, 0, 10)
self.leftTalon.config_kF(1, 0, 10)
self.leftTalon.config_IntegralZone(1, 100, 10)
self.leftTalon.configClosedLoopPeakOutput(1, 1.0, 10)
self.rightTalon.config_kP(1, 0, 10)
self.rightTalon.config_kI(1, 0, 10)
self.rightTalon.config_kD(1, 0, 10)
self.rightTalon.config_kF(1, 0, 10)
self.rightTalon.config_IntegralZone(1, 100, 10)
self.rightTalon.configClosedLoopPeakOutput(1, 1.0, 10)
# Change the control frame period
self.leftTalon.changeMotionControlFramePeriod(stream_rate)
self.rightTalon.changeMotionControlFramePeriod(stream_rate)
# Initilaize the quadrature encoders and pigeon IMU
self.initQuadratureEncoder()
self.initPigeonIMU()
def cleanUpDrivetrainMotionProfileControllers(self):
'''
This mothod will be called to cleanup the Talon's motion profiling
'''
# Invert right motors again so the open-loop joystick driving works
self.rightTalon.setInverted(False)
self.frontRight.setInverted(False)
# Change the control frame period back to the default
framePeriod = TALON_DEFAULT_MOTION_CONTROL_FRAME_PERIOD_MS
self.leftTalon.changeMotionControlFramePeriod(framePeriod)
self.rightTalon.changeMotionControlFramePeriod(framePeriod)
def initPigeonIMU(self):
# false means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
# true means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
self.rightTalon.configAuxPIDPolarity(False, 10)
self.leftTalon.configAuxPIDPolarity(True, 10)
# select a gadgeteer pigeon for remote 0
self.rightTalon.configRemoteFeedbackFilter(
self.talonPigeon.getDeviceID(),
RemoteSensorSource.GadgeteerPigeon_Yaw, 0, 10)
self.leftTalon.configRemoteFeedbackFilter(
self.talonPigeon.getDeviceID(),
RemoteSensorSource.GadgeteerPigeon_Yaw, 0, 10)
# Select the remote feedback sensor for PID1
self.rightTalon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.RemoteSensor0, 1, 10)
self.leftTalon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.RemoteSensor0, 1, 10)
# Using the config feature, scale units to 3600 per rotation. This is nice as it keeps
# 0.1 deg resolution, and is fairly intuitive.
self.rightTalon.configSelectedFeedbackCoefficient(3600 / 8192, 1, 10)
self.leftTalon.configSelectedFeedbackCoefficient(3600 / 8192, 1, 10)
# Zero the sensor
self.pigeonIMU.setYaw(0, 10)
self.pigeonIMU.setAccumZAngle(0, 10)
def initQuadratureEncoder(self):
"""
This method will initialize the encoders for quadrature feedback.
"""
self.leftTalon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10)
self.rightTalon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10)
self.leftTalon.getSensorCollection().setQuadraturePosition(0, 10)
self.rightTalon.getSensorCollection().setQuadraturePosition(0, 10)
def getLeftQuadraturePosition(self):
"""
This method will return the left-side sensor quadrature position. The sign needs to
manually be handled here since this function is used to provide the sensor postion outide
of the talon.
"""
return -self.leftTalon.getSensorCollection().getQuadraturePosition()
def getRightQuadraturePosition(self):
"""
This method will return the right-side sensor quadrature position. The sign needs to
manually be handled here since this function is used to provide the sensor postion outide
of the talon.
"""
return self.rightTalon.getSensorCollection().getQuadraturePosition()
def setQuadratureStatusFramePeriod(self, sample_period_ms):
"""
This method will set the status frame persiod of the quadrature encoder
"""
self.leftTalon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
sample_period_ms, 10)
self.rightTalon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
sample_period_ms, 10)
def setDefaultQuadratureStatusFramePeriod(self):
"""
This method will set the status frame persiod of the quadrature encoder back to the factory
default.
"""
self.leftTalon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
TALON_DEFAULT_QUADRATURE_STATUS_FRAME_PERIOD_MS, 10)
self.rightTalon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_3_Quadrature,
TALON_DEFAULT_QUADRATURE_STATUS_FRAME_PERIOD_MS, 10)
def pathFinderDrive(self, leftOutput, rightOutput):
"""
This method will take the Pathfinder Controller motor output and apply them to the
drivetrain.
"""
self.leftTalon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftOutput)
self.rightTalon.set(WPI_TalonSRX.ControlMode.PercentOutput,
-rightOutput)
def getLeftVelocity(self):
return self.leftTalon.getSensorCollection().getQuadratureVelocity()
def getRightVelocity(self):
return self.rightTalon.getSensorCollection().getQuadratureVelocity()
def getLeftVoltage(self):
return self.leftTalon.getMotorOutputVoltage()
def getRightVoltage(self):
return self.rightTalon.getMotorOutputVoltage()
def initDefaultCommand(self):
"""
This method will set the default command for this subsystem.
"""
self.setDefaultCommand(DriveJoystick(self.robot))
class IntakeOutput(Subsystem):
"""
This subsystem controls the Intake or Output of the Panels or Cargo.
"""
def __init__(self):
super().__init__()
self.leftIntakeTalon = WPI_TalonSRX(RobotMap.LEFT_INTAKE_TALON)
self.rightIntakeTalon = WPI_TalonSRX(RobotMap.RIGHT_INTAKE_TALON)
self.intakeVictor = WPI_VictorSPX(RobotMap.END_EFFECTOR_VICTOR)
self.intakeSolenoid = DoubleSolenoid(RobotMap.INTAKE_IN_SOLENOID,
RobotMap.INTAKE_OUT_SOLENOID)
self.placingSolenoid = DoubleSolenoid(RobotMap.PLACE_IN_SOLENOID,
RobotMap.PLACE_OUT_SOLENOID)
self.leftIntakeTalon.configContinuousCurrentLimit(
6, Constants.TIMEOUT_MS)
self.rightIntakeTalon.configContinuousCurrentLimit(
6, Constants.TIMEOUT_MS)
self.setPosition(IntakeOutput.Position.UP)
self.setArmPower(0)
self.mode = None # This cannot be set here because the buttons do not yet exist.
def initDefaultCommand(self):
pass
def setPosition(self, pos):
if pos == IntakeOutput.Position.DOWN:
self.intakeSolenoid.set(DoubleSolenoid.Value.kReverse)
elif pos == IntakeOutput.Position.UP:
self.intakeSolenoid.set(DoubleSolenoid.Value.kForward)
def setArmPower(self, power):
self.leftIntakeTalon.set(-power)
self.rightIntakeTalon.set(power)
def panelPlace(self, putortake):
if putortake == IntakeOutput.PanelPutOrTake.PUT:
self.placingSolenoid.set(DoubleSolenoid.Value.kForward)
elif putortake == IntakeOutput.PanelPutOrTake.TAKE:
self.placingSolenoid.set(DoubleSolenoid.Value.kReverse)
def giveOrTakeCargo(self, giveortake):
if giveortake == IntakeOutput.CargeGiveOrTake.GIVE:
self.intakeVictor.set(ControlMode.PercentOutput, 1)
elif giveortake == IntakeOutput.CargeGiveOrTake.TAKE:
self.intakeVictor.set(ControlMode.PercentOutput, -0.5)
elif giveortake == IntakeOutput.CargeGiveOrTake.NADA:
self.intakeVictor.set(ControlMode.PercentOutput, 0)
def getPistonPos(self):
return self.placingSolenoid.get() == DoubleSolenoid.Value.kForward
def setMode(self, mode):
assert (isinstance(mode, IntakeOutput.BallOrHatchMode))
self.mode = mode
class Position(enum.IntEnum):
DOWN = 0
UP = 1
class PanelPutOrTake(enum.IntEnum):
PUT = 0
TAKE = 1
class CargeGiveOrTake(enum.IntEnum):
NADA = 0
GIVE = 1
TAKE = 2
class BallOrHatchMode(enum.IntEnum):
BALL = 0
HATCH = 1
class IntakeInOutNeutral(enum.IntEnum):
NEUTRAL = 0
IN = 1
OUT = 2
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')
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))