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 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()
"""