class MyRobot(wpilib.TimedRobot):
"""Main robot class"""
# NetworkTables API for controlling this
#: Speed to set the motors to
autospeed = ntproperty("/robot/autospeed",
defaultValue=0,
writeDefault=True)
#: Test data that the robot sends back
telemetry = ntproperty("/robot/telemetry",
defaultValue=(0, ) * 9,
writeDefault=False)
prior_autospeed = 0
WHEEL_DIAMETER = 0.5
ENCODER_PULSE_PER_REV = 4096
PIDIDX = 0
def robotInit(self):
"""Robot-wide initialization code should go here"""
self.lstick = wpilib.Joystick(0)
# Left front
left_front_motor = ctre.WPI_TalonSRX(1)
left_front_motor.setInverted(False)
left_front_motor.setSensorPhase(False)
self.left_front_motor = left_front_motor
# Right front
right_front_motor = ctre.WPI_TalonSRX(2)
right_front_motor.setInverted(False)
right_front_motor.setSensorPhase(False)
self.right_front_motor = right_front_motor
# Left rear -- follows front
left_rear_motor = ctre.WPI_TalonSRX(3)
left_rear_motor.setInverted(False)
left_rear_motor.setSensorPhase(False)
left_rear_motor.follow(left_front_motor)
# Right rear -- follows front
right_rear_motor = ctre.WPI_TalonSRX(4)
right_rear_motor.setInverted(False)
right_rear_motor.setSensorPhase(False)
right_rear_motor.follow(right_front_motor)
#
# Configure drivetrain movement
#
self.drive = DifferentialDrive(left_front_motor, right_front_motor)
self.drive.setDeadband(0)
#
# Configure encoder related functions -- getpos and getrate should return
# ft and ft/s
#
encoder_constant = ((1 / self.ENCODER_PULSE_PER_REV) *
self.WHEEL_DIAMETER * math.pi)
left_front_motor.configSelectedFeedbackSensor(
left_front_motor.FeedbackDevice.QuadEncoder, self.PIDIDX, 10)
self.l_encoder_getpos = (
lambda: left_front_motor.getSelectedSensorPosition(
self.PIDIDX) * encoder_constant)
self.l_encoder_getrate = (
lambda: left_front_motor.getSelectedSensorVelocity(
self.PIDIDX) * encoder_constant * 10)
right_front_motor.configSelectedFeedbackSensor(
right_front_motor.FeedbackDevice.QuadEncoder, self.PIDIDX, 10)
self.r_encoder_getpos = (
lambda: left_front_motor.getSelectedSensorPosition(
self.PIDIDX) * encoder_constant)
self.r_encoder_getrate = (
lambda: left_front_motor.getSelectedSensorVelocity(
self.PIDIDX) * encoder_constant * 10)
#
# Configure gyro
#
# You only need to uncomment the below lines if you want to characterize wheelbase radius
# Otherwise you can leave this area as-is
self.gyro_getangle = lambda: 0
# Uncomment for the KOP gyro
# Note that the angle from all implementors of the Gyro class must be negated because
# getAngle returns a clockwise angle, and we require a counter-clockwise one
# gyro = ADXRS450_Gyro()
# self.gyro_getangle = lambda: -1 * gyro.getAngle()
# Set the update rate instead of using flush because of a NetworkTables bug
# that affects ntcore and pynetworktables
# -> probably don't want to do this on a robot in competition
NetworkTables.setUpdateRate(0.010)
def disabledInit(self):
self.logger.info("Robot disabled")
self.drive.tankDrive(0, 0)
def disabledPeriodic(self):
pass
def robotPeriodic(self):
# feedback for users, but not used by the control program
sd = wpilib.SmartDashboard
sd.putNumber("l_encoder_pos", self.l_encoder_getpos())
sd.putNumber("l_encoder_rate", self.l_encoder_getrate())
sd.putNumber("r_encoder_pos", self.r_encoder_getpos())
sd.putNumber("r_encoder_rate", self.r_encoder_getrate())
sd.putNumber("gyro_angle", self.gyro_getangle())
def teleopInit(self):
self.logger.info("Robot in operator control mode")
def teleopPeriodic(self):
self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX())
def autonomousInit(self):
self.logger.info("Robot in autonomous mode")
def autonomousPeriodic(self):
"""
If you wish to just use your own robot program to use with the data
logging program, you only need to copy/paste the logic below into
your code and ensure it gets called periodically in autonomous mode
Additionally, you need to set NetworkTables update rate to 10ms using
the setUpdateRate call.
Note that reading/writing self.autospeed and self.telemetry are
NetworkTables operations (using pynetworktables's ntproperty), so
if you don't read/write NetworkTables in your implementation it won't
actually work.
"""
# Retrieve values to send back before telling the motors to do something
now = wpilib.Timer.getFPGATimestamp()
l_encoder_position = self.l_encoder_getpos()
l_encoder_rate = self.l_encoder_getrate()
r_encoder_position = self.r_encoder_getpos()
r_encoder_rate = self.r_encoder_getrate()
battery = self.ds.getBatteryVoltage()
l_motor_volts = self.left_front_motor.getMotorOutputVoltage()
r_motor_volts = self.right_front_motor.getMotorOutputVoltage()
gyro_angle = self.gyro_getangle()
# Retrieve the commanded speed from NetworkTables
autospeed = self.autospeed
self.prior_autospeed = autospeed
# command motors to do things
self.drive.tankDrive(autospeed, autospeed, False)
# send telemetry data array back to NT
self.telemetry = (
now,
battery,
autospeed,
l_motor_volts,
r_motor_volts,
l_encoder_position,
r_encoder_position,
l_encoder_rate,
r_encoder_rate,
gyro_angle,
)
class Drivetrain:
navx = navx.AHRS
left_motor_master = WPI_TalonFX
left_motor_slave = WPI_TalonFX
left_motor_slave2 = WPI_TalonFX
right_motor_master = WPI_TalonFX
right_motor_slave = WPI_TalonFX
right_motor_slave2 = WPI_TalonFX
shifter_solenoid = Solenoid
arcade_cutoff = tunable(0.1)
angle_correction_cutoff = tunable(0.05)
angle_correction_factor_low_gear = tunable(0.1)
angle_correction_factor_high_gear = tunable(0.1)
angle_correction_max = tunable(0.2)
little_rotation_cutoff = tunable(0.1)
def __init__(self):
self.pending_differential_drive = None
self.force_differential_drive = False
self.pending_gear = LOW_GEAR
self.pending_position = None
self.pending_reset = False
self.og_yaw = None
self.pending_manual_drive = None
self.is_manual_mode = False
# Set encoders
self.left_motor_master.configSelectedFeedbackSensor(
ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0)
self.right_motor_master.configSelectedFeedbackSensor(
ctre.TalonFXFeedbackDevice.IntegratedSensor, 0, 0)
self.left_motor_master.setSensorPhase(True)
# Set slave motors
self.left_motor_slave.set(ctre.TalonFXControlMode.Follower,
self.left_motor_master.getDeviceID())
self.left_motor_slave2.set(ctre.TalonFXControlMode.Follower,
self.left_motor_master.getDeviceID())
self.right_motor_slave.set(ctre.TalonFXControlMode.Follower,
self.right_motor_master.getDeviceID())
self.right_motor_slave2.set(ctre.TalonFXControlMode.Follower,
self.right_motor_master.getDeviceID())
# Set up drive control
self.robot_drive = DifferentialDrive(self.left_motor_master,
self.right_motor_master)
self.robot_drive.setDeadband(0)
self.robot_drive.setSafetyEnabled(False)
def is_left_encoder_connected(self):
return self.left_motor_master.getPulseWidthRiseToRiseUs() != 0
def is_right_encoder_connected(self):
return self.right_motor_master.getPulseWidthRiseToRiseUs() != 0
def reset_position(self):
self.left_motor_master.setQuadraturePosition(0, TALON_TIMEOUT)
self.right_motor_master.setQuadraturePosition(0, TALON_TIMEOUT)
def get_position(self):
'''
Returns averaged quadrature position in inches.
'''
left_position = self.get_left_encoder()
right_position = self.get_right_encoder()
position = (((left_position + right_position) / 2) *
(1 / UNITS_PER_REV) * DISTANCE_PER_REV)
return position
def drive(self, *args):
self.differential_drive(*args)
def differential_drive(self,
y,
rotation=0,
squared=True,
force=False,
quick_turn=False,
use_curvature=False):
if not self.force_differential_drive:
self.pending_differential_drive = DifferentialDriveConfig(
y=y,
rotation=rotation,
squared=squared,
quick_turn=quick_turn,
use_curvature=use_curvature)
self.force_differential_drive = force
def turn(self, rotation=0, force=False):
self.differential_drive(0, rotation, squared=False, force=force)
def reset_angle_correction(self):
self.navx.reset()
def angle_corrected_differential_drive(self, y, rotation=0):
'''
Heading must be reset first. (drivetrain.reset_angle_correction())
'''
# Scale angle to reduce max turn
rotation = util.scale(rotation, -1, 1, -0.65, 0.65)
# Scale y-speed in high gear
if self.pending_gear == HIGH_GEAR:
y = util.scale(y, -1, 1, -0.75, 0.75)
use_curvature = False
quick_turn = False
# Small rotation at lower speeds - and also do a quick_turn instead of
# the normal curvature-based mode.
if abs(y) <= self.little_rotation_cutoff:
rotation = util.abs_clamp(rotation, 0, 0.7)
quick_turn = True
# NEVER USE CURVATURE
# Curvature drive for high gear and high speedz
# if self.pending_gear == HIGH_GEAR and abs(y) >= 0.5:
# use_curvature = True
# Angle correction
if abs(rotation) <= self.angle_correction_cutoff:
heading = self.navx.getYaw()
if not self.og_yaw:
self.og_yaw = heading
factor = self.angle_correction_factor_high_gear if \
self.pending_gear == HIGH_GEAR else \
self.angle_correction_factor_low_gear
rotation = util.abs_clamp(-factor * (heading - self.og_yaw), 0,
self.angle_correction_max)
else:
self.og_yaw = None
self.differential_drive(y,
rotation,
quick_turn=quick_turn,
use_curvature=use_curvature)
def shift_low_gear(self):
self.pending_gear = LOW_GEAR
def shift_high_gear(self):
self.pending_gear = HIGH_GEAR
def shift_toggle(self):
if self.pending_gear == HIGH_GEAR:
self.pending_gear = LOW_GEAR
else:
self.pending_gear = HIGH_GEAR
def manual_drive(self, left, right):
self.pending_manual_drive = [left, right]
def get_left_encoder(self):
return -self.left_motor_master.getQuadraturePosition()
def get_right_encoder(self):
return self.right_motor_master.getQuadraturePosition()
def get_left_encoder_meters(self):
return self.get_left_encoder() * \
(1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS
def get_right_encoder_meters(self):
return self.get_right_encoder() * \
(1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS
def get_left_encoder_velocity(self):
return -self.left_motor_master.getQuadratureVelocity()
def get_right_encoder_velocity(self):
return self.right_motor_master.getQuadratureVelocity()
def get_left_encoder_velocity_meters(self):
return self.get_left_encoder_velocity() * \
(1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS
def get_right_encoder_velocity_meters(self):
return self.get_right_encoder_velocity() * \
(1 / UNITS_PER_REV) * DISTANCE_PER_REV_METERS
def set_manual_mode(self, is_manual):
self.is_manual_mode = is_manual
if not is_manual:
self.pending_manual_drive = None
def execute(self):
# print('exec drivetrain', self.pending_differential_drive)
# print('dist_traveled_meters', self.get_left_encoder_meters(),
# self.get_right_encoder_meters())
# Shifter
self.shifter_solenoid.set(self.pending_gear)
# Manual
if self.is_manual_mode:
if self.pending_manual_drive:
left, right = self.pending_manual_drive
# left = self.robot_drive.applyDeadband(left, DEADBAND)
# right = self.robot_drive.applyDeadband(right, DEADBAND)
self.left_motor_master.set(-left)
self.right_motor_master.set(right)
self.pending_manual_drive = None
return
# Drive
if self.pending_differential_drive:
if self.pending_differential_drive.use_curvature and \
abs(self.pending_differential_drive.y) > \
self.arcade_cutoff and \
USE_CURVATURE_DRIVE:
self.robot_drive.curvatureDrive(
self.pending_differential_drive.y,
-self.pending_differential_drive.rotation,
isQuickTurn=self.pending_differential_drive.quick_turn)
else:
self.robot_drive.arcadeDrive(
self.pending_differential_drive.y,
-self.pending_differential_drive.rotation,
squareInputs=self.pending_differential_drive.squared)
self.pending_differential_drive = None
self.force_differential_drive = False
def on_disable(self):
self.robot_drive.arcadeDrive(0, 0)
def get_state(self):
return {
'pending_gear': self.pending_gear,
'pending_differential_drive': self.pending_differential_drive
}
def put_state(self, state):
self.pending_gear = state['pending_gear']
self.pending_differential_drive = DifferentialDriveConfig._make(
state['pending_differential_drive'])
def limelight_turn(self):
self.llt = NetworkTables.getTable('limelight')
self.tv = self.llt.getNumber('tv', 0)
self.tx = self.llt.getNumber('tx', 0)
if self.tv:
self.turn(self.get_position() + self.tx)
class Robot(wpilib.IterativeRobot):
WHEEL_DIAMETER = 0.1524 # Units: Meters
# Currently unused
# ENCODER_PULSE_PER_REV = 42
GEARING = 7.56 # 7.56:1 gear ratio
auto_speed_entry = ntproperty('/robot/autospeed', 0.0)
telemetry_entry = ntproperty('/robot/telemetry', [0.0], writeDefault=False)
rotate_entry = ntproperty('/robot/rotate', False)
l_encoder_pos = ntproperty('/l_encoder_pos', 0)
l_encoder_rate = ntproperty('/l_encoder_rate', 0)
r_encoder_pos = ntproperty('/r_encoder_pos', 0)
r_encoder_rate = ntproperty('/r_encoder_rate', 0)
def robotInit(self):
self.prior_autospeed = 0
self.joystick = wpilib.Joystick(0)
self.left_motor_master = CANSparkMax(1, MotorType.kBrushless)
self.right_motor_master = CANSparkMax(4, MotorType.kBrushless)
# Set up Speed Controller Groups
self.left_motors = wpilib.SpeedControllerGroup(
self.left_motor_master,
CANSparkMax(3, MotorType.kBrushless)
)
self.right_motors = wpilib.SpeedControllerGroup(
self.right_motor_master,
CANSparkMax(6, MotorType.kBrushless)
)
# Configure Gyro
# Note that the angle from the NavX and all implementors of wpilib Gyro
# must be negated because getAngle returns a clockwise positive angle
self.gyro = navx.AHRS.create_spi()
# Configure drivetrain movement
self.drive = DifferentialDrive(self.left_motors, self.right_motors)
self.drive.setDeadband(0)
# Configure encoder related functions -- getDistance and getrate should return
# units and units/s
self.encoder_constant = (1 / self.GEARING) * self.WHEEL_DIAMETER * math.pi
self.left_encoder = self.left_motor_master.getEncoder()
self.left_encoder.setPositionConversionFactor(self.encoder_constant)
self.left_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.right_encoder = self.right_motor_master.getEncoder()
self.right_encoder.setPositionConversionFactor(self.encoder_constant)
self.right_encoder.setVelocityConversionFactor(self.encoder_constant / 60)
self.left_encoder.setPosition(0)
self.right_encoder.setPosition(0)
# Set the update rate instead of using flush because of a ntcore bug
# -> probably don't want to do this on a robot in competition
NetworkTables.getDefault().setUpdateRate(0.010)
def disabledInit(self):
print('Robot disabled')
self.drive.tankDrive(0, 0)
def robotPeriodic(self):
# feedback for users, but not used by the control program
self.l_encoder_pos = self.left_encoder.getPosition()
self.l_encoder_rate = self.left_encoder.getVelocity()
self.r_encoder_pos = self.right_encoder.getPosition()
self.r_encoder_rate = self.right_encoder.getVelocity()
def teleopInit(self):
print('Robot in operator control mode')
def teleopPeriodic(self):
self.drive.arcadeDrive(-self.joystick.getY(), self.joystick.getX())
print(f'Left Distance: {self.left_encoder.getPosition()}')
def autonomousInit(self):
print('Robot in autonomous mode')
# If you wish to just use your own robot program to use with the data logging
# program, you only need to copy/paste the logic below into your code and
# ensure it gets called periodically in autonomous mode
# Additionally, you need to set NetworkTables update rate to 10ms using the
# setUpdateRate call.
def autonomousPeriodic(self):
# Retrieve values to send back before telling the motors to do somethin
now = wpilib.Timer.getFPGATimestamp()
leftPosition = self.left_encoder.getPosition()
leftRate = self.left_encoder.getVelocity()
rightPosition = self.right_encoder.getPosition()
rightRate = self.right_encoder.getVelocity()
battery = wpilib.RobotController.getInputVoltage()
motorVolts = battery * abs(self.prior_autospeed)
leftMotorVolts = motorVolts
rightMotorVolts = motorVolts
# Retrieve the commanded speed from NetworkTables
autospeed = self.auto_speed_entry
self.prior_autospeed = autospeed
# command motors to do things
self.drive.tankDrive((-1 if self.rotate_entry else 1) * autospeed, autospeed, False)
# send telemetry data array back to NT
number_array = [
now, battery, autospeed, leftMotorVolts, rightMotorVolts,
leftPosition, rightPosition, leftRate, rightRate,
math.radians(-self.gyro.getAngle())
]
self.telemetry_entry = number_array
class MyRobot(wpilib.TimedRobot):
'''Main robot class'''
# NetworkTables API for controlling this
#: Speed to set the motors to
autospeed = ntproperty('/robot/autospeed',
defaultValue=0,
writeDefault=True)
#: Test data that the robot sends back
telemetry = ntproperty('/robot/telemetry',
defaultValue=(0, ) * 9,
writeDefault=False)
prior_autospeed = 0
WHEEL_DIAMETER = 0.5
ENCODER_PULSE_PER_REV = 360
def robotInit(self):
'''Robot-wide initialization code should go here'''
self.lstick = wpilib.Joystick(0)
left_front_motor = wpilib.Spark(1)
left_front_motor.setInverted(False)
right_front_motor = wpilib.Spark(2)
right_front_motor.setInverted(False)
left_rear_motor = wpilib.Spark(3)
left_rear_motor.setInverted(False)
right_rear_motor = wpilib.Spark(4)
right_rear_motor.setInverted(False)
#
# Configure drivetrain movement
#
l = wpilib.SpeedControllerGroup(left_front_motor, left_rear_motor)
r = wpilib.SpeedControllerGroup(right_front_motor, right_rear_motor)
self.drive = DifferentialDrive(l, r)
self.drive.setSafetyEnabled(False)
self.drive.setDeadband(0)
#
# Configure encoder related functions -- getpos and getrate should return
# ft and ft/s
#
encoder_constant = (
1 / self.ENCODER_PULSE_PER_REV) * self.WHEEL_DIAMETER * math.pi
l_encoder = wpilib.Encoder(0, 1)
l_encoder.setDistancePerPulse(encoder_constant)
self.l_encoder_getpos = l_encoder.getDistance
self.l_encoder_getrate = l_encoder.getRate
r_encoder = wpilib.Encoder(2, 3)
r_encoder.setDistancePerPulse(encoder_constant)
self.r_encoder_getpos = r_encoder.getDistance
self.r_encoder_getrate = r_encoder.getRate
# Set the update rate instead of using flush because of a NetworkTables bug
# that affects ntcore and pynetworktables
# -> probably don't want to do this on a robot in competition
NetworkTables.setUpdateRate(0.010)
def disabledInit(self):
self.logger.info("Robot disabled")
self.drive.tankDrive(0, 0)
def disabledPeriodic(self):
pass
def robotPeriodic(self):
# feedback for users, but not used by the control program
sd = wpilib.SmartDashboard
sd.putNumber('l_encoder_pos', self.l_encoder_getpos())
sd.putNumber('l_encoder_rate', self.l_encoder_getrate())
sd.putNumber('r_encoder_pos', self.r_encoder_getpos())
sd.putNumber('r_encoder_rate', self.r_encoder_getrate())
def teleopInit(self):
self.logger.info("Robot in operator control mode")
def teleopPeriodic(self):
self.drive.arcadeDrive(-self.lstick.getY(), self.lstick.getX())
def autonomousInit(self):
self.logger.info("Robot in autonomous mode")
def autonomousPeriodic(self):
'''
If you wish to just use your own robot program to use with the data
logging program, you only need to copy/paste the logic below into
your code and ensure it gets called periodically in autonomous mode
Additionally, you need to set NetworkTables update rate to 10ms using
the setUpdateRate call.
Note that reading/writing self.autospeed and self.telemetry are
NetworkTables operations (using pynetworktables's ntproperty), so
if you don't read/write NetworkTables in your implementation it won't
actually work.
'''
# Retrieve values to send back before telling the motors to do something
now = wpilib.Timer.getFPGATimestamp()
l_encoder_position = self.l_encoder_getpos()
l_encoder_rate = self.l_encoder_getrate()
r_encoder_position = self.r_encoder_getpos()
r_encoder_rate = self.r_encoder_getrate()
battery = self.ds.getBatteryVoltage()
motor_volts = battery * abs(self.prior_autospeed)
l_motor_volts = motor_volts
r_motor_volts = motor_volts
# Retrieve the commanded speed from NetworkTables
autospeed = self.autospeed
self.prior_autospeed = autospeed
# command motors to do things
self.drive.tankDrive(autospeed, autospeed, False)
# send telemetry data array back to NT
self.telemetry = (now, battery, autospeed, l_motor_volts,
r_motor_volts, l_encoder_position,
r_encoder_position, l_encoder_rate, r_encoder_rate)
class DriverComponent:
CONV_FACTOR = 0.0524 * 0.846
LINEAR_SAMPLE_RATE = 28
ANGULAR_SAMPLE_RATE = 2
def __init__(self):
left_front = WPI_TalonSRX(6)
left_rear = WPI_TalonSRX(1)
right_front = WPI_TalonSRX(4)
right_rear = WPI_TalonSRX(7)
left = SpeedControllerGroup(left_front, left_rear)
right = SpeedControllerGroup(right_front, right_rear)
self.left_encoder_motor = left_rear
self.right_encoder_motor = right_front
self.gear_solenoid = DoubleSolenoid(0, 1)
self.driver_gyro = ADXRS450_Gyro()
self.drive_train = DifferentialDrive(left, right)
# setup encoders
self.left_encoder_motor.setSensorPhase(True)
self.drive_train.setDeadband(0.1)
self.moving_linear = [0] * DriverComponent.LINEAR_SAMPLE_RATE
self.moving_angular = [0] * DriverComponent.ANGULAR_SAMPLE_RATE
def set_curve_raw(self, linear, angular):
if -0.1 < linear < 0.1:
self.drive_train.curvatureDrive(linear, angular, True)
else:
self.drive_train.curvatureDrive(linear, angular, False)
def set_curve(self, linear, angular):
self.moving_linear.append(linear)
self.moving_angular.append(angular)
if len(self.moving_linear) > DriverComponent.LINEAR_SAMPLE_RATE:
self.moving_linear.pop(0)
if len(self.moving_angular) > DriverComponent.ANGULAR_SAMPLE_RATE:
self.moving_angular.pop(0)
l_speed = sum([
x / DriverComponent.LINEAR_SAMPLE_RATE for x in self.moving_linear
])
a_speed = sum([
x / DriverComponent.ANGULAR_SAMPLE_RATE
for x in self.moving_angular
])
l_speed = math.sin(l_speed * math.pi / 2)
if -0.1 < l_speed < 0.1:
self.drive_train.curvatureDrive(l_speed, a_speed, True)
else:
self.drive_train.curvatureDrive(l_speed, a_speed, False)
def reset_drive_sensors(self):
self.driver_gyro.reset()
self.left_encoder_motor.setSelectedSensorPosition(0, 0, 0)
self.right_encoder_motor.setSelectedSensorPosition(0, 0, 0)
@property
def current_distance(self):
return DriverComponent.CONV_FACTOR * self.left_encoder_motor.getSelectedSensorPosition(
0)
def current_gear(self):
if self.gear_solenoid.get() is DoubleSolenoid.Value.kForward:
return GearMode.HIGH
if self.gear_solenoid.get() is DoubleSolenoid.Value.kReverse:
return GearMode.LOW
if self.gear_solenoid.get() is DoubleSolenoid.Value.kOff:
return GearMode.OFF
def toggle_gear(self):
if self.current_gear() is GearMode.LOW:
self.set_high_gear()
if self.current_gear() is GearMode.HIGH:
self.set_low_gear()
def set_low_gear(self):
print("shift low")
self.gear_solenoid.set(DoubleSolenoid.Value.kReverse)
def set_high_gear(self):
print("shift high")
self.gear_solenoid.set(DoubleSolenoid.Value.kForward)
