class Chassis(Subsystem):
def __init__(self):
super().__init__("Chassis")
self.spark_L1 = Spark(robotmap.spark_L1)
self.spark_L2 = Spark(robotmap.spark_L2)
self.spark_R1 = Spark(robotmap.spark_R1)
self.spark_R2 = Spark(robotmap.spark_R2)
self.spark_group_L = SpeedControllerGroup(self.spark_L1, self.spark_L2)
self.spark_group_R = SpeedControllerGroup(self.spark_R1, self.spark_R2)
self.drive = DifferentialDrive(self.spark_group_L, self.spark_group_R)
self.gyro = ADXRS450_Gyro(robotmap.gyro)
self.encoder_L = Encoder(0, 1)
self.encoder_R = Encoder(2, 3)
self.dist_pulse_L = pi * 6 / 2048
self.dist_pulse_R = pi * 6 / 425
@classmethod
def setDriveSpd(cls, spd_drive_new):
robotmap.spd_chassis_drive = spd_drive_new
@classmethod
def setRotateSpd(cls, spd_rotate_new):
robotmap.spd_chassis_rotate = spd_rotate_new
def stop(self):
self.drive.stopMotor()
def curvatureDrive(self, spd_x, spd_z):
self.drive.curvatureDrive(spd_x, spd_z, True)
def joystickDrive(self):
self.drive.curvatureDrive(
-(oi.joystick.getRawAxis(1)) * robotmap.spd_chassis_drive,
oi.joystick.getRawAxis(4) * robotmap.spd_chassis_rotate, True)
def setupEncoder(self):
self.encoder_L.setDistancePerPulse(self.dist_pulse_L)
self.encoder_R.setDistancePerPulse(self.dist_pulse_R)
self.encoder_L.reset()
self.encoder_R.reset()
def getGyroAngle(self):
return self.gyro.getAngle()
def resetGyro(self):
self.gyro.reset()
def gyroDrive(self, spd_temp, amt_turn=None):
if spd_temp and amt_turn is not None:
self.curvatureDrive(spd_temp, amt_turn)
elif amt_turn is None:
self.curvatureDrive(spd_temp, 0.0)
else:
raise ("GyroDrive() failed!")
class Chassis(Subsystem):
def __init__(self):
super().__init__("Chassis")
self.l_controller = CANSparkMax(0, MotorType.kBrushless)
self.r_controller = CANSparkMax(1, MotorType.kBrushless)
self.drive = DifferentialDrive(self.l_controller, self.r_controller)
def joystick_drive(self, x_axis: float, y_axis: float) -> None:
self.drive.curvatureDrive(x_axis, y_axis, True)
def initDefaultCommand(self):
from commands.joystick_drive import JoystickDrive
self.setDefaultCommand(JoystickDrive())
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 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)
