class Robot(wpilib.IterativeRobot):
#: Which PID slot to pull gains from. Starting 2018, you can choose from
#: 0,1,2 or 3. Only the first two (0,1) are visible in web-based
#: configuration.
kSlotIdx = 0
#: Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For
#: now we just want the primary one.
kPIDLoopIdx = 0
#: set to zero to skip waiting for confirmation, set to nonzero to wait and
#: report to DS if action fails.
kTimeoutMs = 10
def robotInit(self):
self.talon = WPI_TalonSRX(3)
self.joy = wpilib.Joystick(0)
self.loops = 0
self.timesInMotionMagic = 0
# first choose the sensor
self.talon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.talon.setSensorPhase(True)
self.talon.setInverted(False)
# Set relevant frame periods to be at least as fast as periodic rate
self.talon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs
)
self.talon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs
)
# set the peak and nominal outputs
self.talon.configNominalOutputForward(0, self.kTimeoutMs)
self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
self.talon.configPeakOutputForward(1, self.kTimeoutMs)
self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
# set closed loop gains in slot0 - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0.2, self.kTimeoutMs)
self.talon.config_kP(0, 0.2, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, self.kTimeoutMs)
# set acceleration and vcruise velocity - see documentation
self.talon.configMotionCruiseVelocity(15000, self.kTimeoutMs)
self.talon.configMotionAcceleration(6000, self.kTimeoutMs)
# zero the sensor
self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def teleopPeriodic(self):
"""
This function is called periodically during operator control
"""
# get gamepad axis - forward stick is positive
leftYstick = -1.0 * self.joy.getY()
# calculate the percent motor output
motorOutput = self.talon.getMotorOutputPercent()
# prepare line to print
sb = []
sb.append("\tOut%%: %.3f" % motorOutput)
sb.append(
"\tVel: %.3f" % self.talon.getSelectedSensorVelocity(self.kPIDLoopIdx)
)
if self.joy.getRawButton(1):
# Motion Magic - 4096 ticks/rev * 10 Rotations in either direction
targetPos = leftYstick * 4096 * 10.0
self.talon.set(WPI_TalonSRX.ControlMode.MotionMagic, targetPos)
# append more signals to print when in speed mode.
sb.append("\terr: %s" % self.talon.getClosedLoopError(self.kPIDLoopIdx))
sb.append("\ttrg: %.3f" % targetPos)
else:
# Percent voltage mode
self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftYstick)
# instrumentation
self.processInstrumentation(self.talon, sb)
def processInstrumentation(self, tal, sb):
# smart dash plots
wpilib.SmartDashboard.putNumber(
"SensorVel", tal.getSelectedSensorVelocity(self.kPIDLoopIdx)
)
wpilib.SmartDashboard.putNumber(
"SensorPos", tal.getSelectedSensorPosition(self.kPIDLoopIdx)
)
wpilib.SmartDashboard.putNumber(
"MotorOutputPercent", tal.getMotorOutputPercent()
)
wpilib.SmartDashboard.putNumber(
"ClosedLoopError", tal.getClosedLoopError(self.kPIDLoopIdx)
)
# check if we are motion-magic-ing
if tal.getControlMode() == WPI_TalonSRX.ControlMode.MotionMagic:
self.timesInMotionMagic += 1
else:
self.timesInMotionMagic = 0
if self.timesInMotionMagic > 10:
# print the Active Trajectory Point Motion Magic is servoing towards
wpilib.SmartDashboard.putNumber(
"ClosedLoopTarget", tal.getClosedLoopTarget(self.kPIDLoopIdx)
)
if not self.isSimulation():
wpilib.SmartDashboard.putNumber(
"ActTrajVelocity", tal.getActiveTrajectoryVelocity()
)
wpilib.SmartDashboard.putNumber(
"ActTrajPosition", tal.getActiveTrajectoryPosition()
)
wpilib.SmartDashboard.putNumber(
"ActTrajHeading", tal.getActiveTrajectoryHeading()
)
# periodically print to console
self.loops += 1
if self.loops >= 10:
self.loops = 0
print(" ".join(sb))
# clear line cache
sb.clear()
class LifterComponent(Events):
class EVENTS:
on_control_move = "on_control_move"
on_manual_move = "on_manual move"
# RPM Multipliers
# ELEVATOR_MULTIPLIER = 1635.15 / 6317.67
ELEVATOR_MULTIPLIER = 2102.35 / 3631.33
CARRIAGE_MULTIPLIER = 1.0 - ELEVATOR_MULTIPLIER
# Max heights of each stage.
ELEVATOR_MAX_HEIGHT = 40
CARRIAGE_MAX_HEIGHT = 40
# Conversion factors (counts to inches)
# CHANGE THESE VALUES
ELEVATOR_CONV_FACTOR = 0.00134
CARRIAGE_CONV_FACTOR = 0.000977
el_down = -1
el_up = 1
carriage_down = -1
carriage_up = 1
MAX_SPEED = 0.5
ELEVATOR_ZERO = 0.0
CARRIAGE_ZERO = 0.0
TIMEOUT_MS = 0
ELEVATOR_kF = 0.0
ELEVATOR_kP = 0.1
ELEVATOR_kI = 0.0
ELEVATOR_kD = 0.0
# ALLOWABLE_ERROR = 2
CARRIAGE_ALLOWABLE_ERROR = int(2 / CARRIAGE_CONV_FACTOR)
ELEVATOR_ALLOWABLE_ERROR = int(2 / ELEVATOR_CONV_FACTOR)
# positions = {
# "floor": 2.0,
# "portal": 34.0,
# "scale_low": 48.0,
# "scale_mid": 60.0,
# "scale_high": 72.0,
# "max_height": 84.0
# }
positions = {
"floor": 0.5,
"portal": 34.0,
"scale_low": 52.0,
"scale_mid": 68.0,
"scale_high": 78.0
}
def __init__(self):
Events.__init__(self)
self.elevator_motor = WPI_TalonSRX(5)
self.elevator_bottom_switch = DigitalInput(9)
self.carriage_motor = WPI_TalonSRX(3)
self.carriage_bottom_switch = DigitalInput(1)
self.carriage_top_switch = DigitalInput(2)
self._create_events([
LifterComponent.EVENTS.on_control_move,
LifterComponent.EVENTS.on_manual_move
])
self._is_reset = False
# configure elevator motor and encoder
self.elevator_motor.setNeutralMode(NeutralMode.Brake)
self.elevator_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSensorPhase(True)
self.elevator_motor.setInverted(True)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configAllowableClosedloopError(
0, LifterComponent.ELEVATOR_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configForwardSoftLimitThreshold(
int(LifterComponent.ELEVATOR_MAX_HEIGHT /
LifterComponent.ELEVATOR_CONV_FACTOR), 0)
self.elevator_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.elevator_motor.setCurrentLimit()
# self.elevator_motor.EnableCurrentLimit()
# self.elevator_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
# configure carriage motor and encoder
self.carriage_motor.setNeutralMode(NeutralMode.Brake)
self.carriage_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.setSensorPhase(True)
self.carriage_motor.setInverted(True)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configAllowableClosedloopError(
0, LifterComponent.CARRIAGE_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configForwardSoftLimitThreshold(
int(LifterComponent.CARRIAGE_MAX_HEIGHT /
LifterComponent.CARRIAGE_CONV_FACTOR), 0)
self.carriage_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.carriage_motor.setCurrentLimit()
# self.carriage_motor.EnableCurrentLimit()
# self.carriage_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
def set_elevator_speed(self, speed):
if (speed > 0 and self.current_elevator_position >= LifterComponent.ELEVATOR_MAX_HEIGHT - 2) \
or (speed < 0 and self.elevator_bottom_switch.get()):
self.elevator_motor.set(0)
else:
self.elevator_motor.set(speed)
self.trigger_event(LifterComponent.EVENTS.on_manual_move)
def set_carriage_speed(self, speed):
if (speed > 0 and self.carriage_top_switch.get()) \
or (speed < 0 and self.carriage_bottom_switch.get()):
self.carriage_motor.set(0)
else:
self.carriage_motor.set(speed)
self.trigger_event(LifterComponent.EVENTS.on_manual_move)
def reset_sensors(self):
self.carriage_motor.setSelectedSensorPosition(
0, 0, LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSelectedSensorPosition(
0, 0, LifterComponent.TIMEOUT_MS)
self._is_reset = True
@property
def current_elevator_position(self) -> float:
return self.elevator_motor.getSelectedSensorPosition(
0) * LifterComponent.ELEVATOR_CONV_FACTOR
@property
def current_carriage_position(self) -> float:
return self.carriage_motor.getSelectedSensorPosition(
0) * LifterComponent.CARRIAGE_CONV_FACTOR
@property
def current_position(self) -> float:
return self.current_elevator_position + self.current_carriage_position
def stop_lift(self):
self.elevator_motor.stopMotor()
self.carriage_motor.stopMotor()
def elevator_to_target_position(self, inches: float):
if self._is_reset:
self.elevator_motor.set(
WPI_TalonSRX.ControlMode.Position,
inches / LifterComponent.ELEVATOR_CONV_FACTOR)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def carriage_to_target_position(self, inches: float):
if self._is_reset:
self.carriage_motor.set(
WPI_TalonSRX.ControlMode.Position,
inches / LifterComponent.CARRIAGE_CONV_FACTOR)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def lift_to_distance(self, inches):
i = inches + 6
elevator = min(i * LifterComponent.ELEVATOR_MULTIPLIER,
LifterComponent.ELEVATOR_MAX_HEIGHT)
carriage = i - elevator
print("lift_to_distance carriage" + str(carriage))
print("lift_to_distance elevate" + str(elevator))
print("lift_to_distance lifter" + str(carriage + elevator))
self.elevator_to_target_position(elevator)
self.carriage_to_target_position(carriage)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def is_at_position(self, position: str) -> bool:
return self.is_at_distance(LifterComponent.positions[position])
def is_at_distance(self, inches: float) -> bool:
return inches - 5 < self.current_position < inches + 5
def closest_position(self) -> tuple:
# returns the key for the position closest to the current position
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
return min(
positions,
key=lambda position: abs(self.current_position - position[1]))
def next_position(self) -> str:
position = self.closest_position()
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
index = positions.index(position)
if index == len(positions) - 1:
return position[0]
return positions[index + 1][0]
def prev_position(self) -> str:
position = self.closest_position()
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
index = positions.index(position)
if index == 0:
return position[0]
return positions[index - 1][0]
class Robot(magicbot.MagicRobot):
location: PosApprox
powertrain: Powertrain
encoders: Encoders
navx: NavX
drivetrain: Drivetrain
limelight: Limelight
shooter: Shooter
serializer: Serializer
color_sensor: WheelOfFortuneSensor
fortune_controller: ColorWheelController
intake_lift: IntakeLift
intake_roller: IntakeRoller
intake: Intake
climber: Climber
serializer: Serializer
def createObjects(self):
# initialize physical objects
#limelight
self.limelight_table = NetworkTables.getTable("limelight")
# drivetrain
self.limelight_table = NetworkTables.getTable('limelight')
motor_objects_left = [
CANSparkMax(port, MotorType.kBrushless)
for port in RobotMap.Drivetrain.motors_left
]
self.left_motors = wpilib.SpeedControllerGroup(motor_objects_left[0],
*motor_objects_left[1:])
motor_objects_right = [
CANSparkMax(port, MotorType.kBrushless)
for port in RobotMap.Drivetrain.motors_right
]
self.right_motors = wpilib.SpeedControllerGroup(
motor_objects_right[0], *motor_objects_right[1:])
for motor in motor_objects_left + motor_objects_right:
config_spark(motor, RobotMap.Drivetrain.motor_config)
self.differential_drive = wpilib.drive.DifferentialDrive(
self.left_motors, self.right_motors)
self.differential_drive.setMaxOutput(
RobotMap.Drivetrain.max_motor_power)
self.left_encoder = wpilib.Encoder(RobotMap.Encoders.left_encoder_b,
RobotMap.Encoders.left_encoder_a)
self.right_encoder = wpilib.Encoder(RobotMap.Encoders.right_encoder_b,
RobotMap.Encoders.right_encoder_a)
self.right_encoder.setReverseDirection(False)
self.left_encoder.setDistancePerPulse(
RobotMap.Encoders.distance_per_pulse)
self.right_encoder.setDistancePerPulse(
RobotMap.Encoders.distance_per_pulse)
self.navx_ahrs = navx.AHRS.create_spi()
self.driver = Driver(wpilib.XboxController(0))
self.sysop = Sysop(wpilib.XboxController(1))
# intake
self.intake_lift_motor = WPI_TalonSRX(RobotMap.IntakeLift.motor_port)
self.intake_lift_motor.configPeakOutputForward(
RobotMap.IntakeLift.max_power)
self.intake_lift_motor.configPeakOutputReverse(
-RobotMap.IntakeLift.max_power)
config_talon(self.intake_lift_motor, RobotMap.IntakeLift.motor_config)
self.intake_lift_motor.setSelectedSensorPosition(0)
self.intake_roller_motor = WPI_TalonSRX(
RobotMap.IntakeRoller.motor_port)
self.intake_roller_motor.configPeakOutputForward(
RobotMap.IntakeRoller.max_power)
self.intake_roller_motor.configPeakOutputReverse(
-RobotMap.IntakeRoller.max_power)
config_talon(self.intake_roller_motor,
RobotMap.IntakeRoller.motor_config)
#self.intake_roller_motor.setSelectedSensorPosition(0)
# climber
self.climber_motor = WPI_TalonSRX(RobotMap.Climber.motor_port)
config_talon(self.climber_motor, RobotMap.Climber.motor_config)
# color wheel
self.color_wheel_motor = WPI_TalonSRX(RobotMap.ColorWheel.motor_port)
config_talon(self.color_wheel_motor, RobotMap.ColorWheel.motor_config)
# shooter
self.shooter_motor = WPI_TalonSRX(RobotMap.Shooter.motor_port)
config_talon(self.shooter_motor, RobotMap.Shooter.motor_config)
# serializer
self.serializer_motor = WPI_TalonSRX(RobotMap.Serializer.motor_port)
config_talon(self.serializer_motor, RobotMap.Serializer.motor_config)
self.i2c_color_sensor = ColorSensorV3(wpilib.I2C.Port.kOnboard)
# controllers and electrical stuff
self.driver = Driver(wpilib.XboxController(0))
self.sysop = Sysop(wpilib.XboxController(1))
NetworkTables.initialize(server="roborio")
# camera server
wpilib.CameraServer.launch()
def reset_subsystems(self):
self.drivetrain.reset()
self.climber.reset()
self.limelight.reset()
def teleopInit(self):
self.reset_subsystems()
self.serializer.turn_on()
def teleopPeriodic(self):
#print(self.limelight.get)
#print(self.color_wheel_motor.__dir__());
#print("execute method")
try:
# drive the drivetrain as needed
driver_x, driver_y = self.driver.get_curvature_output()
# manually handled driving
if self.drivetrain.state == DrivetrainState.MANUAL_DRIVE:
self.drivetrain.curvature_drive(driver_y, driver_x)
# set turbo mode
self.drivetrain.set_power_scaling(self.driver.process_turbo_mode())
# # check and see if we need to activate driver assists
# if self.drivetrain.ready_straight_assist() and self.driver.ready_straight_assist():
# self.drivetrain.drive_straight(driver_y)
# elif self.drivetrain.state == DrivetrainState.AIDED_DRIVE_STRAIGHT:
# self.drivetrain.state = DrivetrainState.MANUAL_DRIVE
# revert to manual control if enabled
if self.driver.get_manual_control_override():
self.drivetrain.state = DrivetrainState.MANUAL_DRIVE
except:
print("DRIVETRAIN ERROR")
try:
# move intake lift
if self.sysop.get_intake_lower():
self.intake_lift.send_down()
elif self.sysop.get_intake_raise():
self.intake_lift.send_up()
except:
print("INTAKE LIFT ERROR")
try:
#print(self.intake_roller_motor.getSelectedSensorPosition());
# intake rollers
if self.sysop.get_intake_intake():
self.intake_roller.intake()
elif self.sysop.get_intake_outtake():
self.intake_roller.outtake()
else:
self.intake_roller.stop()
except:
print("INTAKE ROLLER ERROR")
try:
if self.sysop.get_target_aim():
self.shooter.aim_and_fire()
elif self.sysop.get_shooter_stop():
self.shooter.stop_fire()
self.drivetrain.reset_state()
elif self.sysop.get_change_shooter_power() != 0:
self.shooter.adjust_power(
self.sysop.get_change_shooter_power())
except:
traceback.print_exc()
print("AUTO AIM ERROR")
try:
self.climber.set_power(self.sysop.get_climb_axis())
except:
print("CLIMBER ERROR")
try:
manual_fortune_input = self.sysop.get_manual_fortune_axis()
fms_color_position = self.ds.getGameSpecificMessage()
dash.putString("Color sensor color", self.color_sensor.get_color())
dash.putString("FMS Color", fms_color_position)
if manual_fortune_input != 0:
self.fortune_controller.manual_power(manual_fortune_input)
# elif self.sysop.get_position_control() and fms_color_position != "":
# self.fortune_controller.position_control({
# "B": WheelOfFortuneColor.BLUE,
# "R": WheelOfFortuneColor.GREEN,
# "G": WheelOfFortuneColor.RED,
# "Y": WheelOfFortuneColor.YELLOW,
# }[fms_color_position])
# TODO this is commented out because it'll be more effective to use manual control
# elif self.sysop.get_rotation_control():
# self.fortune_controller.rotation_control()
elif self.fortune_controller.state == ColorWheelState.MANUAL:
self.fortune_controller.manual_power(0)
except:
print("COLOR WHEEL ERROR")
class Robot(wpilib.IterativeRobot):
#: Which PID slot to pull gains from. Starting 2018, you can choose from
#: 0,1,2 or 3. Only the first two (0,1) are visible in web-based
#: configuration.
kSlotIdx = 0
#: Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For
#: now we just want the primary one.
kPIDLoopIdx = 0
#: set to zero to skip waiting for confirmation, set to nonzero to wait and
#: report to DS if action fails.
kTimeoutMs = 10
def robotInit(self):
self.talon = WPI_TalonSRX(3)
self.joy = wpilib.Joystick(0)
self.loops = 0
self.lastButton1 = False
self.targetPos = 0
# choose the sensor and sensor direction
self.talon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# choose to ensure sensor is positive when output is positive
self.talon.setSensorPhase(True)
# choose based on what direction you want forward/positive to be.
# This does not affect sensor phase.
self.talon.setInverted(False)
# set the peak and nominal outputs, 12V means full
self.talon.configNominalOutputForward(0, self.kTimeoutMs)
self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
self.talon.configPeakOutputForward(1, self.kTimeoutMs)
self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
# Set the allowable closed-loop error, Closed-Loop output will be
# neutral within this range. See Table in Section 17.2.1 for native
# units per rotation.
self.talon.configAllowableClosedloopError(0, self.kPIDLoopIdx,
self.kTimeoutMs)
# set closed loop gains in slot0, typically kF stays zero - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0, self.kTimeoutMs)
self.talon.config_kP(0, 0.1, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, self.kTimeoutMs)
# zero the sensor
self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx,
self.kTimeoutMs)
def teleopPeriodic(self):
"""
This function is called periodically during operator control
"""
# get gamepad axis - forward stick is positive
leftYstick = self.joy.getY()
# calculate the percent motor output
motorOutput = self.talon.getMotorOutputPercent()
button1 = self.joy.getRawButton(1)
button2 = self.joy.getRawButton(2)
# deadband gamepad
if abs(leftYstick) < 0.1:
leftYstick = 0
# prepare line to print
sb = []
sb.append("\tOut%%: %.3f" % motorOutput)
sb.append("\tPos: %.3fu" %
self.talon.getSelectedSensorPosition(self.kPIDLoopIdx))
if self.lastButton1 and button1:
# Position mode - button just pressed
# 10 Rotations * 4096 u/rev in either direction
self.targetPos = leftYstick * 4096 * 10.0
self.talon.set(WPI_TalonSRX.ControlMode.Position, self.targetPos)
# on button2 just straight drive
if button2:
# Percent voltage mode
self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftYstick)
if self.talon.getControlMode() == WPI_TalonSRX.ControlMode.Position:
# append more signals to print when in speed mode.
sb.append("\terr: %s" %
self.talon.getClosedLoopError(self.kPIDLoopIdx))
sb.append("\ttrg: %.3f" % self.targetPos)
# periodically print to console
self.loops += 1
if self.loops >= 10:
self.loops = 0
print(" ".join(sb))
# save button state for on press detect
self.lastButton1 = button1
class CargoMech():
kSlotIdx = 0
kPIDLoopIdx = 0
kTimeoutMs = 10
def initialize(self):
timeout = 15
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.1
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.05
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.motor = Talon(map.intake)
self.motor.configContinuousCurrentLimit(20,timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(30,timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(100,timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.intakeSpeed = 0.9
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#choose sensor
self.wrist.configSelectedFeedbackSensor(
Talon.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,)
self.wrist.setSensorPhase(False) #!!!!!
# Set relevant frame periods to be at least as fast as periodic rate
self.wrist.setStatusFramePeriod(
Talon.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs)
self.wrist.setStatusFramePeriod(
Talon.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs)
# set the peak and nominal outputs
self.wrist.configNominalOutputForward(0, self.kTimeoutMs)
self.wrist.configNominalOutputReverse(0, self.kTimeoutMs)
self.wrist.configPeakOutputForward(0.6, self.kTimeoutMs)
self.wrist.configPeakOutputReverse(-0.25, self.kTimeoutMs)
self.kF = self.getFeedForward(self.getNumber("Wrist F Gain" , 0))
self.kP = self.getNumber("Wrist kP" , 0)
self.kI = self.getNumber("Wrist kI" , 0)
self.kD = self.getNumber("Wrist kD" , 0)
# set closed loop gains in slot0 - see documentation */
self.wrist.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.wrist.config_kF(0, self.kF, self.kTimeoutMs)
self.wrist.config_kP(0, self.kP, self.kTimeoutMs)
self.wrist.config_kI(0, 0, self.kTimeoutMs)
self.wrist.config_kD(0, 0, self.kTimeoutMs)
# set acceleration and vcruise velocity - see documentation
self.wrist.configMotionCruiseVelocity(400, self.kTimeoutMs) #!!!!!
self.wrist.configMotionAcceleration(500, self.kTimeoutMs) #!!!!!
# zero the sensor
self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def intake(self, mode):
''' Intake/Outtake/Stop Intake the cargo (turn wheels inward)'''
if mode == "intake": self.motor.set(self.intakeSpeed)
elif mode == "outtake": self.motor.set(-1 * self.intakeSpeed)
elif mode == "stop": self.motor.set(0)
def moveWrist(self,mode):
'''move wrist in and out of robot'''
if mode == "up": self.wrist.set(self.getPowerSimple("up"))
elif mode == "down": self.wrist.set(-1 * self.getPowerSimple("down"))
elif mode == "upVolts": self.wrist.set(self.wristUpVolts/self.maxVolts)
elif mode == "downVolts": self.wrist.set(-1 * self.wristDownVolts/ self.maxVolts)
elif mode == "upMagic":
if self.lastMode != mode:
self.wrist.config_kF(0, self.kF, self.kTimeoutMs)
self.wrist.config_kP(0, self.kP, self.kTimeoutMs)
self.wrist.config_kI(0, 0, self.kTimeoutMs)
self.wrist.config_kD(0, 0, self.kTimeoutMs)
# Motion Magic - 4096 ticks/rev * 10 Rotations in either direction
self.wrist.set(Talon.ControlMode.MotionMagic, self.targetPosUp)
# append more signals to print when in speed mode.
self.sb.append("\terr: %s" % self.wrist.getClosedLoopError(self.kPIDLoopIdx))
self.sb.append("\ttrg: %.3f" % self.targetPosUp)
elif mode == "downMagic":
if self.lastMode != mode:
self.wrist.config_kF(0, self.kF, self.kTimeoutMs)
self.wrist.config_kP(0, self.kP, self.kTimeoutMs)
self.wrist.config_kI(0, 0, self.kTimeoutMs)
self.wrist.config_kD(0, 0, self.kTimeoutMs)
# Motion Magic - 4096 ticks/rev * 10 Rotations in either direction
self.wrist.set(Talon.ControlMode.MotionMagic, self.targetPosDown)
# append more signals to print when in speed mode.
self.sb.append("\terr: %s" % self.wrist.getClosedLoopError(self.kPIDLoopIdx))
self.sb.append("\ttrg: %.3f" % self.targetPosDown)
elif mode == "stop":
self.wrist.set(0)
else:
self.wrist.set(self.getGravitySimple())
self.lastMode = mode
def periodic(self):
deadband = 0.4
if self.xbox.getRawAxis(map.intakeCargo)>deadband: self.intake("intake")
elif self.xbox.getRawAxis(map.outtakeCargo)>deadband: self.intake("outtake")
else:
self.intake("stop")
if self.xbox.getRawButton(map.wristUp): self.moveWrist("up")
elif self.xbox.getRawButton(map.wristDown): self.moveWrist("down")
elif self.joystick0.getRawButton(map.wristUpVolts): self.moveWrist("upVolts")
elif self.joystick0.getRawButton(map.wristDownVolts): self.moveWrist("downVolts")
elif self.joystick0.getRawButton(map.wristUpMagic): self.moveWrist("upMagic")
elif self.joystick0.getRawButton(map.wristDownMagic): self.moveWrist("downMagic")
else:
self.moveWrist("gravity")
# calculate the percent motor output
motorOutput = self.wrist.getMotorOutputPercent()
self.sb = []
# prepare line to print
self.sb.append("\tOut%%: %.3f" % motorOutput)
self.sb.append("\tVel: %.3f" % self.wrist.getSelectedSensorVelocity(self.kPIDLoopIdx))
# instrumentation
self.processInstrumentation(self.wrist, self.sb)
def disable(self): self.intake("stop")
def processInstrumentation(self, tal, sb):
# smart dash plots
wpilib.SmartDashboard.putNumber("SensorVel", tal.getSelectedSensorVelocity(self.kPIDLoopIdx))
wpilib.SmartDashboard.putNumber("SensorPos", tal.getSelectedSensorPosition(self.kPIDLoopIdx))
wpilib.SmartDashboard.putNumber("MotorOutputPercent", tal.getMotorOutputPercent())
wpilib.SmartDashboard.putNumber("ClosedLoopError", tal.getClosedLoopError(self.kPIDLoopIdx))
# check if we are motion-magic-ing
if tal.getControlMode() == Talon.ControlMode.MotionMagic:
self.timesInMotionMagic += 1
else:
self.timesInMotionMagic = 0
if self.timesInMotionMagic > 10:
# print the Active Trajectory Point Motion Magic is servoing towards
wpilib.SmartDashboard.putNumber(
"ClosedLoopTarget", tal.getClosedLoopTarget(self.kPIDLoopIdx)
)
if not wpilib.RobotBase.isSimulation():
wpilib.SmartDashboard.putNumber(
"ActTrajVelocity", tal.getActiveTrajectoryVelocity()
)
wpilib.SmartDashboard.putNumber(
"ActTrajPosition", tal.getActiveTrajectoryPosition()
)
wpilib.SmartDashboard.putNumber(
"ActTrajHeading", tal.getActiveTrajectoryHeading()
)
# periodically print to console
self.loops += 1
if self.loops >= 10:
self.loops = 0
print(" ".join(self.sb))
# clear line cache
self.sb.clear()
def getAngle(self):
pos = self.getPosition()
angle = abs(pos * 115/self.targetPosDown)
return angle - 25
def getPosition(self):
return self.wrist.getQuadraturePosition()
def getFeedForward(self, gain):
angle = self.getAngle()
return angle*gain
def getPowerSimple(self, direction):
'''true direction is up into robot
false direction is down out of robot'''
angle = self.getAngle()
power = abs(self.simpleGainGravity * math.sin(math.radians(angle)) + self.simpleGain)
if angle > 80:
if direction == "down":
power = 0
if angle < -20:
if direction == "up":
power = 0
return power
def getGravitySimple(self):
angle = self.getAngle()
power = self.simpleGainGravity * math.sin(math.radians(angle))
return power
def getNumber(self, key, defVal):
val = SmartDashboard.getNumber(key, None)
if val == None:
val = defVal
SmartDashboard.putNumber(key, val)
return val
def dashboardInit(self):
pass
def dashboardPeriodic(self):
self.wristUp = self.getNumber("WristUpSpeed" , 0.5)
self.wristDown = self.getNumber("WristDownSpeed" , 0.2)
self.wristUpVolts = self.getNumber("WristUpVoltage" , 5)
self.wristDownVolts = self.getNumber("WristDownVoltage" , 2)
self.kF = self.getFeedForward(self.getNumber("Wrist F Gain" , 0))
self.kP = self.getNumber("Wrist kP" , 0)
self.kI = self.getNumber("Wrist kI" , 0)
self.kD = self.getNumber("Wrist kD" , 0)
self.simpleGain = self.getNumber("Wrist Simple Gain", 0.5)
self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", 0.07)
SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition())
SmartDashboard.putNumber("Wrist Angle" , self.getAngle())
SmartDashboard.putNumber("Wrist Power Up" , self.getPowerSimple("up"))
SmartDashboard.putNumber("Wrist Power Down" , self.getPowerSimple("down"))
class Climber(Subsystem):
def __init__(self, robot):
self.robot = robot
self.lights = FROGLights()
self.climbMotor = Talon(self.robot.kClimber['climb_motor'])
self.checkSwitch = wpilib.DigitalInput(
self.robot.kClimber['check_switch'])
self.solenoid = wpilib.DoubleSolenoid(
self.robot.pneumaticControlModuleCANID,
self.robot.kClimber['solenoid'], self.robot.kClimber['solenoid_2'])
self.driverOne = self.robot.dStick
self.latchToggleCount = 2
self.handlerState = 0
self.climbMotor.configPeakOutputForward(1, 0)
self.climbMotor.configPeakOutputReverse(-1, 0)
def climberFunction(self):
self.climbLatch()
if self.driverOne.getPOV() == 0:
if self.getCheckSwitch():
self.lightsHandler(2)
else:
self.lightsHandler(1)
self.climbMotor.set(0.75)
elif self.driverOne.getPOV() == 180:
if self.getCheckSwitch():
self.lightsHandler(2)
else:
self.lightsHandler(1)
self.climbMotor.set(-0.75)
else:
if self.getCheckSwitch():
self.lightsHandler(2)
self.climbMotor.set(0)
wpilib.SmartDashboard.putBoolean('Latch Switch',
self.checkSwitch.get() == 1)
def climbLatch(self):
if self.driverOne.getRawButtonReleased(1):
self.latchToggleCount += 1
if self.latchToggleCount % 2 == 1:
self.solenoid.set(self.solenoid.Value.kReverse)
else:
self.solenoid.set(self.solenoid.Value.kForward)
def getCheckSwitch(self):
return not self.checkSwitch.get()
if not self.checkSwitch.get():
self.lightsHandler(1)
def lightsHandler(self, tempHandlerState):
if tempHandlerState != self.handlerState:
if tempHandlerState == 1:
self.lights.sendDontClimb()
elif tempHandlerState == 2:
self.lights.sendOKToClimb()
self.handlerState = tempHandlerState
class DriveSubsystem(Subsystem):
def __init__(self, robot):
super().__init__("Drive")
self.robot = robot
self.left_master = WPI_TalonSRX(robotmap.DRIVELEFTMASTER)
self.right_master = WPI_TalonSRX(robotmap.DRIVERIGHTMASTER)
self.left_slave = WPI_TalonSRX(robotmap.DRIVELEFTSLAVE)
self.right_slave = WPI_TalonSRX(robotmap.DRIVERIGHTSLAVE)
self.shifter_high = Solenoid(robotmap.PCM1_CANID,
robotmap.SHIFTER_HIGH)
self.shifter_low = Solenoid(robotmap.PCM1_CANID, robotmap.SHIFTER_LOW)
self.differential_drive = DifferentialDrive(self.left_master,
self.right_master)
self.TalonConfig()
self.shiftLow()
def teleDrive(self, xSpeed, zRotation):
if self.robot.oi.getController1().B().get():
scale = SmartDashboard.getNumber("creep_mult", 0.3)
xSpeed = xSpeed * scale
zRotation = zRotation * scale
self.differential_drive.arcadeDrive(xSpeed, zRotation, False)
def getLeftPosition(self):
return self.left_master.getSelectedSensorPosition(0)
def getRightPosition(self):
return self.right_master.getSelectedSensorPosition(0)
def getRightSpeed(self):
return self.right_master.getSelectedSensorVelocity(0)
def getLeftSpeed(self):
return self.unitsToInches(
self.left_master.getSelectedSensorVelocity(0))
def getErrorLeft(self):
return self.left_master.getClosedLoopError(0)
def getErrorRight(self):
return self.right_master.getClosedLoopError(0)
def isLeftSensorConnected(self):
return self.left_master.getSensorCollection(
).getPulseWidthRiseToRiseUs() != 0
def isRightSensorConnected(self):
return self.right_master.getSensorCollection(
).getPulseWidthRiseToRiseUs() != 0
def resetEncoders(self):
self.left_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS)
self.right_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS)
self.left_master.getSensorCollection().setQuadraturePosition(
0, robotmap.TIMEOUT_MS)
self.right_master.getSensorCollection().setQuadraturePosition(
0, robotmap.TIMEOUT_MS)
def setMotionMagicLeft(self, setpoint):
SmartDashboard.putNumber("setpoint_left_units",
self.inchesToUnits(setpoint))
self.left_master.set(ControlMode.MotionMagic,
self.inchesToUnits(setpoint))
def setMotionMagicRight(self, setpoint):
self.right_master.set(ControlMode.MotionMagic,
self.inchesToUnits(-setpoint))
def isMotionMagicNearTarget(self):
retval = False
if self.left_master.getActiveTrajectoryPosition(
) == self.left_master.getClosedLoopTarget(0):
if self.right_master.getActiveTrajectoryPosition(
) == self.right_master.getClosedLoopTarget(0):
if abs(self.left_master.getClosedLoopError(0)) < 300:
if abs(self.right_master.getClosedLoopError(0)) < 300:
retval = True
return retval
def shiftHigh(self):
self.shifter_high.set(True)
self.shifter_low.set(False)
def shiftLow(self):
self.shifter_high.set(False)
self.shifter_low.set(True)
def stop(self):
self.left_master.set(ControlMode.PercentOutput, 0.0)
self.right_master.set(ControlMode.PercentOutput, 0.0)
def unitsToInches(self, units):
return units * robotmap.DRIVE_WHEEL_CIRCUMFERENCE / robotmap.DRIVE_ENCODER_PPR
def inchesToUnits(self, inches):
return inches * robotmap.DRIVE_ENCODER_PPR / robotmap.DRIVE_WHEEL_CIRCUMFERENCE
def autoInit(self):
self.resetEncoders()
self.left_master.setNeutralMode(NeutralMode.Brake)
self.left_slave.setNeutralMode(NeutralMode.Brake)
self.right_master.setNeutralMode(NeutralMode.Brake)
self.right_slave.setNeutralMode(NeutralMode.Brake)
self.differential_drive.setSafetyEnabled(False)
self.shiftLow()
def teleInit(self):
self.left_master.setNeutralMode(NeutralMode.Coast)
self.left_slave.setNeutralMode(NeutralMode.Coast)
self.right_master.setNeutralMode(NeutralMode.Coast)
self.right_slave.setNeutralMode(NeutralMode.Coast)
self.differential_drive.setSafetyEnabled(True)
self.shiftLow()
def TalonConfig(self):
self.left_master.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotmap.TIMEOUT_MS)
self.right_master.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotmap.TIMEOUT_MS)
self.left_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS)
self.right_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS)
self.left_master.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10,
robotmap.TIMEOUT_MS)
self.left_master.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10,
robotmap.TIMEOUT_MS)
self.right_master.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10,
robotmap.TIMEOUT_MS)
self.right_master.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10,
robotmap.TIMEOUT_MS)
self.left_master.setSensorPhase(False)
self.right_master.setSensorPhase(False)
self.left_master.setInverted(True)
self.left_slave.setInverted(True)
self.right_master.setInverted(True)
self.right_slave.setInverted(True)
#Makes talons force zero voltage across when zero output to resist motion
self.left_master.setNeutralMode(NeutralMode.Brake)
self.left_slave.setNeutralMode(NeutralMode.Brake)
self.right_master.setNeutralMode(NeutralMode.Brake)
self.right_slave.setNeutralMode(NeutralMode.Brake)
self.left_slave.set(ControlMode.Follower, robotmap.DRIVELEFTMASTER)
self.right_slave.set(ControlMode.Follower, robotmap.DRIVERIGHTMASTER)
#Closed Loop Voltage Limits
self.left_master.configNominalOutputForward(0.0, robotmap.TIMEOUT_MS)
self.right_master.configNominalOutputForward(0.0, robotmap.TIMEOUT_MS)
self.left_master.configNominalOutputReverse(-0.0, robotmap.TIMEOUT_MS)
self.right_master.configNominalOutputReverse(-0.0, robotmap.TIMEOUT_MS)
self.left_master.configPeakOutputForward(1.0, robotmap.TIMEOUT_MS)
self.right_master.configPeakOutputForward(1.0, robotmap.TIMEOUT_MS)
self.left_master.configPeakOutputReverse(-1.0, robotmap.TIMEOUT_MS)
self.right_master.configPeakOutputReverse(-1.0, robotmap.TIMEOUT_MS)
def configGains(self, f, p, i, d, izone, cruise, accel):
self.left_master.selectProfileSlot(0, 0)
self.left_master.config_kF(0, f, robotmap.TIMEOUT_MS)
self.left_master.config_kP(0, p, robotmap.TIMEOUT_MS)
self.left_master.config_kI(0, i, robotmap.TIMEOUT_MS)
self.left_master.config_kD(0, d, robotmap.TIMEOUT_MS)
self.left_master.config_IntegralZone(0, izone, robotmap.TIMEOUT_MS)
self.right_master.selectProfileSlot(0, 0)
self.right_master.config_kF(0, f, robotmap.TIMEOUT_MS)
self.right_master.config_kP(0, p, robotmap.TIMEOUT_MS)
self.right_master.config_kI(0, i, robotmap.TIMEOUT_MS)
self.right_master.config_kD(0, d, robotmap.TIMEOUT_MS)
self.right_master.config_IntegralZone(0, izone, robotmap.TIMEOUT_MS)
self.left_master.configMotionCruiseVelocity(cruise,
robotmap.TIMEOUT_MS)
self.left_master.configMotionAcceleration(accel, robotmap.TIMEOUT_MS)
self.right_master.configMotionCruiseVelocity(cruise,
robotmap.TIMEOUT_MS)
self.right_master.configMotionAcceleration(accel, robotmap.TIMEOUT_MS)
class DriveTrain(Subsystem):
'''
'Tank Drive' system set up with 2 motors per side, one a "master"
with a mag encoder attached and the other "slave" controller set
to follow the "master".
'''
def __init__(self, robot):
self.robot = robot
self.ahrs = AHRS.create_spi()
self.ahrs.reset()
# self.angleAdjustment = self.ahrs.getAngle()
# self.ahrs.setAngleAdjustment(self.angleAdjustment)
# Initialize all controllers
self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master'])
self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave'])
self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master'])
self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave'])
wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster",
self.driveLeftMaster)
wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster",
self.driveRightMaster)
# Connect the slaves to the masters on each side
self.driveLeftSlave.follow(self.driveLeftMaster)
self.driveRightSlave.follow(self.driveRightMaster)
self.driveLeftMaster.configNominalOutputForward(0, 0)
self.driveLeftMaster.configNominalOutputReverse(0, 0)
self.driveRightMaster.configNominalOutputForward(0, 0)
self.driveRightMaster.configNominalOutputReverse(0, 0)
self.speed = .4
self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)
self.driveRightMaster.configPeakOutputForward(self.speed, 0)
self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)
self.driveLeftMaster.configClosedLoopRamp(.2, 0)
self.driveRightMaster.configClosedLoopRamp(.2, 0)
self.driveLeftMaster.setSafetyEnabled(False)
self.driveRightMaster.setSafetyEnabled(False)
# Makes sure both sides' controllers show green and use positive
# values to move the bot forward.
self.driveLeftSlave.setInverted(False)
self.driveLeftMaster.setInverted(False)
self.driveRightSlave.setInverted(True)
self.driveRightMaster.setInverted(True)
self.PID()
"""
Initializes the count for toggling which side of the
robot will be considered the front when driving.
"""
self.robotFrontToggleCount = 2
# Configures each master to use the attached Mag Encoders
self.driveLeftMaster.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
0)
self.driveRightMaster.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
0)
# Reverses the encoder direction so forward movement always
# results in a positive increase in the encoder ticks.
self.driveLeftMaster.setSensorPhase(True)
self.driveRightMaster.setSensorPhase(True)
self.driveLeftMaster.setSelectedSensorPosition(0, 0, 0)
self.driveRightMaster.setSelectedSensorPosition(0, 0, 0)
# these supposedly aren't part of the WPI_TalonSRX class
# self.driveLeftMaster.setSelectedSensorPostion(0, 0, 10)
# self.driveRightMaster.setSelectedSensorPosition(0, 0, 10)
# Throw data on the SmartDashboard so we can work with it.
# SD.putNumber(
# 'Left Quad Pos.',
# self.driveLeftMaster.getQuadraturePosition())
# SD.putNumber(
# 'Right Quad Pos.',
# self.driveRightMaster.getQuadraturePosition())
self.leftVel = None
self.leftPos = None
self.rightVel = None
self.rightPos = None
# self.driveLeftMaster.config_kP(0, .3, 10)
self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
self.driveControllerRight.setInverted(True)
self.drive = DifferentialDrive(self.driveControllerLeft,
self.driveControllerRight)
self.drive.setSafetyEnabled(False)
self.previousError = 0
super().__init__()
def autoInit(self):
self.speed = .5
self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)
self.driveRightMaster.configPeakOutputForward(self.speed, 0)
self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)
self.driveLeftMaster.config_kP(0, .115, 0)
self.driveRightMaster.config_kP(0, .115, 0)
# self.driveLeftMaster.config_kP(0, .185, 0)
# self.driveRightMaster.config_kP(0, .185, 0)
# self.driveLeftMaster.config_kP(0, 20, 0)
# self.driveRightMaster.config_kP(0, 20, 0)
self.driveLeftMaster.config_kF(0, 0.0, 0)
self.driveRightMaster.config_kF(0, 0.0, 0)
def teleInit(self):
self.speed = .55
self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)
self.driveRightMaster.configPeakOutputForward(self.speed, 0)
self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)
self.driveLeftMaster.config_kP(0, 0.0, 0)
self.driveRightMaster.config_kP(0, 0.0, 0)
self.driveLeftMaster.config_kF(0, 0.313, 0)
self.driveRightMaster.config_kF(0, 0.313, 0)
def moveToPosition(self, position):
self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Position,
position)
self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Position,
position)
def stop(self):
self.drive.stopMotor()
def arcade(self, speed, rotation):
# self.updateSD()
if self.robot.dStick.getRawButtonReleased(3):
self.robotFrontToggleCount += 1
"""
This if statement acts as a toggle to change which motors are
inverted, completely changing the "front" of the robot. This is
useful for when we are about to climb.
"""
if self.robotFrontToggleCount % 2 == 0:
self.drive.arcadeDrive(speed, rotation, True)
else:
self.drive.arcadeDrive(-speed, rotation, True)
def arcadeWithRPM(self, speed, rotation, maxRPM):
# self.updateSD()
self.driveLeftMaster.setSafetyEnabled(False)
if self.robot.dStick.getRawButtonReleased(3):
self.robotFrontToggleCount += 1
if self.robotFrontToggleCount % 2 == 0:
XSpeed = wpilib.RobotDrive.limit(speed)
else:
XSpeed = wpilib.RobotDrive.limit(-speed)
XSpeed = self.applyDeadband(XSpeed, .02)
ZRotation = wpilib.RobotDrive.limit(rotation)
ZRotation = self.applyDeadband(ZRotation, .02)
XSpeed = math.copysign(XSpeed * XSpeed, XSpeed)
ZRotation = math.copysign(ZRotation * ZRotation, ZRotation)
maxInput = math.copysign(max(abs(XSpeed), abs(ZRotation)), XSpeed)
if XSpeed >= 0.0:
if ZRotation >= 0.0:
leftMotorSpeed = maxInput
rightMotorSpeed = XSpeed - ZRotation
else:
leftMotorSpeed = XSpeed + ZRotation
rightMotorSpeed = maxInput
else:
if ZRotation >= 0.0:
leftMotorSpeed = XSpeed + ZRotation
rightMotorSpeed = maxInput
else:
leftMotorSpeed = maxInput
rightMotorSpeed = XSpeed - ZRotation
leftMotorSpeed = wpilib.RobotDrive.limit(leftMotorSpeed)
rightMotorSpeed = wpilib.RobotDrive.limit(rightMotorSpeed)
leftMotorRPM = leftMotorSpeed * maxRPM
rightMotorRPM = rightMotorSpeed * maxRPM
self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity,
leftMotorRPM)
self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity,
rightMotorRPM)
def updateSD(self):
leftVel = self.driveLeftMaster.getSelectedSensorVelocity(0)
leftPos = self.driveLeftMaster.getSelectedSensorPosition(0)
rightVel = self.driveRightMaster.getSelectedSensorVelocity(0)
rightPos = self.driveRightMaster.getSelectedSensorPosition(0)
# calculate side deltas
if self.leftVel:
leftVelDelta = leftVel - self.leftVel
else:
leftVelDelta = 0
if self.leftPos:
leftPosDelta = leftPos - self.leftPos
else:
leftPosDelta = 0
if self.rightVel:
rightVelDelta = rightVel - self.rightVel
else:
rightVelDelta = 0
if self.rightPos:
rightPosDelta = rightPos - self.rightPos
else:
rightPosDelta = 0
# calculate delta of delta
differenceVel = leftVelDelta - rightVelDelta
differencePos = leftPosDelta - rightPosDelta
SD.putNumber("LeftSensorVel", leftVel)
SD.putNumber("LeftSensorPos", leftPos)
SD.putNumber("RightSensorVel", rightVel)
SD.putNumber("RightSensorPos", rightPos)
SD.putNumber('LeftVelDelta', leftVelDelta)
SD.putNumber('LeftPosDelta', leftPosDelta)
SD.putNumber('RightVelDelta', rightVelDelta)
SD.putNumber('RightPosDelta', rightPosDelta)
SD.putNumber('DifferenceVel', differenceVel)
SD.putNumber('DifferencePos', differencePos)
SD.putNumber('Angle', self.ahrs.getAngle())
SD.putNumber('Angle Adjustment', self.ahrs.getAngleAdjustment())
self.leftVel = leftVel
self.leftPos = leftPos
self.rightVel = rightVel
self.rightPos = rightPos
# kP = self.driveLeftMaster.configGetParameter(
# self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)
# SmartDashboard.putNumber('Left Proportional', kP)
# these may give the derivitive an integral of the PID once
# they are set. For now, they just show 0
#SD.putNumber(
# 'Left Derivative',
# self.driveLeftMaster.getErrorDerivative(0))
#SD.putNumber(
# 'Left Integral',
# self.driveLeftMaster.getIntegralAccumulator(0))
def applyDeadband(self, value, deadband):
"""Returns 0.0 if the given value is within the specified range around zero. The remaining range
between the deadband and 1.0 is scaled from 0.0 to 1.0.
:param value: value to clip
:param deadband: range around zero
"""
if abs(value) > deadband:
if value < 0.0:
return (value - deadband) / (1.0 - deadband)
else:
return (value + deadband) / (1.0 - deadband)
return 0.0
def setAngle(self, angle, tolerance):
#self.tolerance = tolerance
#self.calculateAdjustedSetpoint(angle)
self.turnController.setSetpoint(angle)
if ((self.ahrs.getYaw() <= (angle + tolerance))
and (self.ahrs.getYaw() >= (angle - tolerance))):
self.turnController.disable()
self.driveLeftMaster.set(0)
self.driveRightMaster.set(0)
else:
self.turnController.enable()
self.drive.arcadeDrive(0, self.output)
#self.leftTurnController.setSetpoint(angle)
def isInGyroPosition(self):
SD.putNumber('Is in gyro position',
((self.ahrs.getYaw() <=
(self.turnController.getSetpoint() +
self.robot.autonomous.ANGLE_TOLERANCE)) and
(self.ahrs.getYaw() >=
(self.turnController.getSetpoint() -
self.robot.autonomous.ANGLE_TOLERANCE))))
return ((self.ahrs.getYaw() <= (self.turnController.getSetpoint() +
self.robot.autonomous.ANGLE_TOLERANCE))
and (self.ahrs.getYaw() >=
(self.turnController.getSetpoint() -
self.robot.autonomous.ANGLE_TOLERANCE)))
def calculateAdjustedSetpoint(self, angle):
self.startingYaw = self.robot.autonomous.startingYaw
adjustedAngle = angle + self.startingYaw
if adjustedAngle < -180:
undershot = adjustedAngle + 180
adjustedAngle = 180 + undershot
elif adjustedAngle > 180:
overshot = adjustedAngle - 180
adjustedAngle = -180 + overshot
self.adjustedSetpoint = adjustedAngle
def PID(self):
self.kP = .045
self.kI = 0.00
self.kD = 0.00
self.kF = 0.00
self.turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.ahrs,
output=self)
self.turnController.setInputRange(-180, 180)
self.turnController.setOutputRange(-0.55, 0.55)
self.turnController.disable()
def pidWrite(self, output):
self.output = output
class cubeGrabber(Subsystem):
def __init__(self, robot):
self.robot = robot
self.armMotor = Talon(self.robot.kCubeGrabber['right_arm'])
self.armMotor.configPeakOutputForward(1, 0)
self.armMotor.configPeakOutputReverse(-1, 0)
self.armUS = wpilib.AnalogInput(self.robot.kCubeGrabber['ultra_sonic'])
self.armSwitch = wpilib.DigitalInput(self.robot.kCubeGrabber['switch'])
self.armClosePosition = self.robot.kCubeGrabber['close']
self.armOpenPosition = self.robot.kCubeGrabber['open']
self.driverTwo = self.robot.cStick
self.armSolenoid = wpilib.Solenoid(
self.robot.pneumaticControlModuleCANID,
self.robot.kCubeGrabber['solenoid'])
"""
Initializes the predetermined distances for grabber/cube interaction.
"""
self.spinDistance = 12
self.closeDistance = 7
"""
Initializes Toggle Counts for the arm functionality.
"""
self.armModeToggleCount = 2
self.openToggleCount = 3
super().__init__()
def grabberFunction(self):
"""
Updates the arm ultra sonic sensor divided by a
predetermined ratio to get the output to inches.
"""
self.cubeDistanceIn = self.armUS.getValue() / 8.417
"""
If statement adding to the toggle count as the
driver changes from automatic grabbing to manual grabbing.
"""
if self.robot.cStick.getRawButtonReleased(3):
self.armModeToggleCount = self.armModeToggleCount + 1
#self.armGrabReset()
"""
If statement controlling whether the arm functionality
is automatic or manual, determined by the Arm Mode Toggle
remainder when divided by 2.
"""
if self.armModeToggleCount % 2 == 0:
self.armGrabAuto()
"""Does not rumble the controller when in Automatic Grabbing"""
self.robot.cStick.setRumble(0, 0)
else:
self.armGrabManual()
"""Does rumble the controller when in Manual Grabbing"""
self.robot.cStick.setRumble(0, 0.5)
"""
These send information over to the SmartDashboard
"""
wpilib.SmartDashboard.putNumber("cubeGrabber Ultra Sonic",
self.cubeDistanceIn)
wpilib.SmartDashboard.putNumber("cubeGrabber Limit Switch",
self.armSwitch.get())
"""
Code for resetting the Cube cubeGrabber
"""
def armGrabReset(self):
"""
Stops the motors and returns arms to default open position.
"""
self.armMotor.set(0)
self.armSolenoid.set(self.armOpenPosition)
"""
Code for automatic cube grabbing
"""
def armGrabAuto(self):
"""
1) Checks if the left trigger on Driver Two's controller is pressed in more
than a quarter of the way down, then closes the Arms and spins the motors outwards.
This spits out the cube and has first priority.
2) Checks if the limit switch on the arm is pressed.
Will stop the motors and keep the arms closed and has second priority.
3) Checks if the cube is close enough to start spinning the intake wheels inward and keeps the arms open.
4) Checks if the cube is in range to close the arms. Spins motors inward and keeps arms closed.
5)If nothing is sensed the arms will be in a default position with the motors stopped and the arms open.
"""
if self.driverTwo.getRawAxis(2) > .25:
self.armMotor.set(1)
self.armSolenoid.set(self.armClosePosition)
"""This will prevent the arms from changing position when changing to Manual Mode."""
self.openToggleCount = 2
elif self.armSwitch.get() == 1:
self.armMotor.set(0)
self.armSolenoid.set(self.armClosePosition)
"""This will prevent the arms from changing position when changing to Manual Mode."""
self.openToggleCount = 2
elif (self.cubeDistanceIn <= self.spinDistance) and (
self.cubeDistanceIn > self.closeDistance):
self.armMotor.set(-1)
self.armSolenoid.set(self.armOpenPosition)
"""This will prevent the arms from changing position when changing to Manual Mode."""
self.openToggleCount = 3
elif (self.cubeDistanceIn <= self.closeDistance):
self.armMotor.set(-1)
self.armSolenoid.set(self.armClosePosition)
"""This will prevent the arms from changing position when changing to Manual Mode."""
self.openToggleCount = 2
else:
self.armMotor.set(0)
self.armSolenoid.set(self.armOpenPosition)
"""This will prevent the arms from changing position when changing to Manual Mode."""
self.openToggleCount = 3
"""
Code for manual cube grabbing
"""
def armGrabManual(self):
"""
Combines the trigger axes on Driver Two's controller to create one "axis".
"""
self.manualArmSpeed = -self.driverTwo.getRawAxis(
2) + self.driverTwo.getRawAxis(3)
"""
If statement adding to the toggle count that will determine whether the arms are closed or open.
"""
if self.driverTwo.getRawButtonReleased(1):
self.openToggleCount = self.openToggleCount + 1
"""
If statement determining whether the arms are closed or opened
determined by the open toggle count remainder when divided by two.
"""
if self.openToggleCount % 2 == 0:
self.armSolenoid.set(self.armClosePosition)
else:
self.armSolenoid.set(self.armOpenPosition)
"""
Sets the speed of the intake wheels, using the makeshift axis made above as diret input.
"""
self.armMotor.set(-self.manualArmSpeed)
def getSwitch(self):
return self.armSwitch.get() == 1
class Elevator(Subsystem):
kSwitch = -20000
kScale = -50000
kStart = -5000
kBottom = 0
def __init__(self, robot):
self.robot = robot
self.elevator = Talon(self.robot.kElevator['elevator_motor'])
self.elevator.setInverted(True)
self.driverTwo = self.robot.cStick
self.elevator.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.elevator.setSensorPhase(True)
self.elevator.setSelectedSensorPosition(0, 0, 0)
self.elevator.configPeakOutputForward(.2, 0)
self.elevator.configPeakOutputReverse(-.95, 0)
self.elevator.configNominalOutputForward(0, 0)
self.elevator.configNominalOutputReverse(0, 0)
self.elevator.setSafetyEnabled(False)
self.smartToggle = True
self.smartPosition = 1
super().__init__()
def elevatorFunction(self):
# if self.elevator.isRevLimitSwitchClosed():
# self.elevator.setSelectedSensorPosition(0, 0, 0)
#self.elevator.set(self.driverTwo.getRawAxis(1))
self.smartPositioning()
#wpilib.SmartDashboard.putNumber('elevator amperage', self.elevator.getOutputCurrent())
# def updateSD(self):
def setElevatorPosition(self, position):
# if self.elevator.isRevLimitSwitchClosed():
# self.elevator.setSelectedSensorPosition(0, 0, 0)
#
self.elevator.set(ctre.talonsrx.TalonSRX.ControlMode.Position, position)
def smartPositioning(self):
if self.driverTwo.getRawButtonReleased(5):
self.smartPosition -= 1
self.smartToggle = True
if self.smartPosition < 0:
self.smartPosition = 0
elif self.driverTwo.getRawButtonReleased(6):
self.smartPosition += 1
self.smartToggle = True
if self.smartPosition > 2:
self.smartPosition = 2
elif (self.driverTwo.getRawAxis(1) > 0.2) or (self.driverTwo.getRawAxis(1) < -0.2):
self.smartToggle = False
if self.smartToggle:
if self.smartPosition == 2:
self.setElevatorPosition(self.kScale)
elif self.smartPosition == 1:
self.setElevatorPosition(self.kSwitch)
elif self.smartPosition == 0:
self.elevator.set(0)
else:
self.elevator.set(self.driverTwo.getRawAxis(1))
if self.elevator.getSelectedSensorPosition(0) < (self.kSwitch + self.kScale)/2:
self.smartPosition = 2
elif self.elevator.getSelectedSensorPosition(0) < self.kSwitch/2:
self.smartPosition = 1
else:
self.smartPosition = 0
def calibrateBottomAutonomous(self):
if self.elevator.isRevLimitSwitchClosed():
self.elevator.setSelectedSensorPosition(0, 0, 0)
self.elevator.set(0)
# self.calibrateStep = 1
else:
self.elevator.set(-.5)
def getBottomLimit(self):
return self.elevator.isFwdLimitSwitchClosed()
def telemetry(self):
wpilib.SmartDashboard.putNumber('Lift Position', self.elevator.getSelectedSensorPosition(0))
wpilib.SmartDashboard.putBoolean('Forward Limit', self.elevator.isFwdLimitSwitchClosed())
wpilib.SmartDashboard.putBoolean('Reverse Limit', self.elevator.isRevLimitSwitchClosed())