def __init__(self, robot):
super().__init__()
self.robot = robot
# Map the CIM motors to the TalonSRX's
self.frontLeft = WPI_TalonSRX(DRIVETRAIN_FRONT_LEFT_MOTOR)
self.leftTalon = WPI_TalonSRX(DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight = WPI_TalonSRX(DRIVETRAIN_FRONT_RIGHT_MOTOR)
self.rightTalon = WPI_TalonSRX(DRIVETRAIN_REAR_RIGHT_MOTOR)
# Set the front motors to be the followers of the rear motors
self.frontLeft.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_LEFT_MOTOR)
self.frontRight.set(WPI_TalonSRX.ControlMode.Follower,
DRIVETRAIN_REAR_RIGHT_MOTOR)
# Add the motors to the speed controller groups and create the differential drivetrain
self.leftDrive = SpeedControllerGroup(self.frontLeft, self.leftTalon)
self.rightDrive = SpeedControllerGroup(self.frontRight,
self.rightTalon)
self.diffDrive = DifferentialDrive(self.leftDrive, self.rightDrive)
# Setup the default motor controller setup
self.initControllerSetup()
# Map the pigeon. This will be connected to an unused Talon.
self.talonPigeon = WPI_TalonSRX(DRIVETRAIN_PIGEON)
self.pigeonIMU = PigeonIMU(self.talonPigeon)
def robotInit(self):
# Note the lack of the camera server initialization here.
# Initializing drive motors
RLMotor = Spark(self.RLMotorChannel)
RRMotor = Spark(self.RRMotorChannel)
FLMotor = Spark(self.FLMotorChannel)
FRMotor = Spark(self.FRMotorChannel)
# Grouping drive motors into left and right.
self.Left = SpeedControllerGroup(RLMotor, FLMotor)
self.Right = SpeedControllerGroup(RRMotor, FRMotor)
# Initializing drive Joystick.
self.DriveStick = Joystick(self.DriveStickChannel)
# Initializing drive function.
self.drive = DifferentialDrive(self.Left, self.Right)
# Sets the right side motors to be inverted.
self.drive.setRightSideInverted(rightSideInverted=True)
# Turns the drive off after .1 seconds of inactivity.
self.drive.setExpiration(0.1)
# Components is a dictionary that contains any variables you want to put into it. All of the variables put into
# components dictionary is given to the autonomous modes.
self.components = {"drive": self.drive}
# Sets up the autonomous mode selector by telling it where the autonomous modes are at and what the autonomous
# modes should inherit.
self.automodes = autonomous.AutonomousModeSelector(
"Sim_Autonomous", self.components)
def __init__(self):
left_rear_m = TalonSRX(1)
self.left_front_m = TalonSRX(2)
self.right_front_m = TalonSRX(0)
right_rear_m = TalonSRX(2)
left_drive = SpeedControllerGroup(self.left_front_m, left_rear_m)
right_drive = SpeedControllerGroup(self.right_front_m, right_rear_m)
self.robot_drive = DifferentialDrive(left_drive,right_drive)
class Robot(TimedRobot):
def robotInit(self):
# Right Motors
self.RightMotor1 = WPI_TalonFX(1)
self.RightSensor1 = SensorCollection(self.RightMotor1)
self.RightMotor2 = WPI_TalonFX(2)
self.RightMotor3 = WPI_TalonFX(3)
self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1,
self.RightMotor2,
self.RightMotor3)
# Left Motors
self.LeftMotor1 = WPI_TalonFX(4)
self.LeftMotor2 = WPI_TalonFX(5)
self.LeftMotor3 = WPI_TalonFX(6)
self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1,
self.LeftMotor2,
self.LeftMotor3)
# self.drive = DifferentialDrive(self.leftDriveMotors,
# self.rightDriveMotors)
self.testEncoder = Encoder(1, 2, 3, Encoder.EncodingType.k4X)
self.dashboard = NetworkTables.getTable("SmartDashboard")
def disabledInit(self):
pass
def autonomousInit(self):
pass
def teleopInit(self):
pass
def testInit(self):
pass
def robotPeriodic(self):
self.dashboard.putNumber("Encoder", self.RightSensor1.getQuadratureVelocity()/2048*60)
def disabledPeriodic(self):
pass
def autonomousPeriodic(self):
speed = self.testEncoder.get() / 1028
self.leftDriveMotors.set(speed)
self.rightDriveMotors.set(speed)
def teleopPeriodic(self):
pass
def testPeriodic(self):
pass
def __init__(self, controller: XboxController):
super().__init__()
frontLeftMotor, backLeftMotor = PWMVictorSPX(0), PWMVictorSPX(1)
frontRightMotor, backRightMotor = PWMVictorSPX(2), PWMVictorSPX(3)
self.leftMotors = SpeedControllerGroup(frontLeftMotor, backLeftMotor)
self.rightMotors = SpeedControllerGroup(frontRightMotor,
backRightMotor)
self.drivetrain = DifferentialDrive(self.leftMotors, self.rightMotors)
self.controller = controller
def __init__(self):
super().__init__()
# Create the motor controllers and their respective speed controllers.
self.leftMotors = SpeedControllerGroup(
PWMSparkMax(constants.kLeftMotor1Port),
PWMSparkMax(constants.kLeftMotor2Port),
)
self.rightMotors = SpeedControllerGroup(
PWMSparkMax(constants.kRightMotor1Port),
PWMSparkMax(constants.kRightMotor2Port),
)
# Create the differential drivetrain object, allowing for easy motor control.
self.drive = DifferentialDrive(self.leftMotors, self.rightMotors)
# Create the encoder objects.
self.leftEncoder = Encoder(
constants.kLeftEncoderPorts[0],
constants.kLeftEncoderPorts[1],
constants.kLeftEncoderReversed,
)
self.rightEncoder = Encoder(
constants.kRightEncoderPorts[0],
constants.kRightEncoderPorts[1],
constants.kRightEncoderReversed,
)
# Configure the encoder so it knows how many encoder units are in one rotation.
self.leftEncoder.setDistancePerPulse(
constants.kEncoderDistancePerPulse)
self.rightEncoder.setDistancePerPulse(
constants.kEncoderDistancePerPulse)
# Create the gyro, a sensor which can indicate the heading of the robot relative
# to a customizable position.
self.gyro = ADXRS450_Gyro()
# Create the an object for our odometry, which will utilize sensor data to
# keep a record of our position on the field.
self.odometry = DifferentialDriveOdometry(self.gyro.getRotation2d())
# Reset the encoders upon the initilization of the robot.
self.resetEncoders()
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
def __init__(self, robot):
super().__init__("drivetrainsim")
self.robot = robot
self.counter = 0 # used for updating the log
self.x, self.y = 0, 0
# initialize sensors
self.navx = navx.AHRS.create_spi()
# initialize motors
self.spark_neo_left_front = wpilib.Jaguar(1)
self.spark_neo_left_rear = wpilib.Jaguar(2)
self.spark_neo_right_front = wpilib.Jaguar(3)
self.spark_neo_right_rear = wpilib.Jaguar(4)
motor_type = rev.MotorType.kBrushless
self.spark_neo_left_front1 = rev.CANSparkMax(1, motor_type)
self.spark_neo_left_rear1 = rev.CANSparkMax(2, motor_type)
self.spark_neo_right_front1 = rev.CANSparkMax(3, motor_type)
self.spark_neo_right_rear1 = rev.CANSparkMax(4, motor_type)
self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(self.spark_neo_left_front1)
self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(self.spark_neo_left_rear1)
self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(self.spark_neo_right_front1)
self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(self.spark_neo_right_rear1)
# create drivetrain from motors
self.speedgroup_left = SpeedControllerGroup(self.spark_neo_left_front, self.spark_neo_left_rear)
self.speedgroup_right = SpeedControllerGroup(self.spark_neo_right_front, self.spark_neo_right_rear)
self.drive = wpilib.drive.DifferentialDrive(self.speedgroup_left, self.speedgroup_right)
# self.drive = wpilib.drive.DifferentialDrive(self.spark_neo_left_front, self.spark_neo_right_front)
self.drive.setMaxOutput(1.0)
# initialize encoders - doing this after motors because they may be part of the motor controller
self.l_encoder = wpilib.Encoder(0, 1, True)
self.r_encoder = wpilib.Encoder(2, 3, True)
self.l_encoder.setDistancePerPulse(drive_constants.k_encoder_distance_per_pulse_m)
self.r_encoder.setDistancePerPulse(drive_constants.k_encoder_distance_per_pulse_m)
# odometry for tracking the robot pose
self.odometry = wpimath.kinematics.DifferentialDriveOdometry(geo.Rotation2d.fromDegrees( -self.navx.getAngle() ))
def robotInit(self):
# Right Motors
self.RightMotor1 = WPI_TalonFX(1)
self.RightMotor2 = WPI_TalonFX(2)
self.RightMotor3 = WPI_TalonFX(3)
self.rightDriveMotors = SpeedControllerGroup(self.RightMotor1,
self.RightMotor2,
self.RightMotor3)
# Left Motors
self.LeftMotor1 = WPI_TalonFX(4)
self.LeftMotor2 = WPI_TalonFX(5)
self.LeftMotor3 = WPI_TalonFX(6)
self.leftDriveMotors = SpeedControllerGroup(self.LeftMotor1,
self.LeftMotor2,
self.LeftMotor3)
self.drive = DifferentialDrive(self.leftDriveMotors,
self.rightDriveMotors)
self.testEncoder = Encoder(1, 2, 3)
self.dashboard = NetworkTables.getTable("SmartDashboard")
def __init__(self, robot, l_f_port=0, l_r_port=1, r_f_port=2, r_r_port=3):
"""
Initialize ports for bot
var naming scheme: (side)_(position)_(object), ex: l_f_port is left_front_port
"""
super().__init__()
self.robot = robot
# Motors on Left Side
self.l_f_motor = Spark(l_f_port)
self.l_r_motor = Spark(l_r_port)
# Motors on Right Side
self.r_f_motor = Spark(r_f_port)
self.r_r_motor = Spark(r_r_port)
# Motor groups
self.l_group = SpeedControllerGroup(self.l_f_motor, self.l_r_motor)
self.r_group = SpeedControllerGroup(self.r_f_motor, self.r_r_motor)
self.drive = DifferentialDrive(self.l_group, self.r_group)
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 __init__(self,
timer: Timer,
left_motors: list,
right_motors: list,
control_system: int = ControlSystem.BASIC,
invert_left: bool = True,
invert_right: bool = False):
self.CONTROL_SYSTEM = control_system
self.timer = timer
self.left_motors = left_motors
self.right_motors = right_motors
# Set control mode
if self.CONTROL_SYSTEM == ControlSystem.BUILTIN:
self.left_master = SpeedControllerGroup(*self.left_motors)
self.right_master = SpeedControllerGroup(*self.right_motors)
self.drivetrain = wpilib.drive.DifferentialDrive(
self.left_master, self.right_master)
self.drivetrain.setSafetyEnabled(True)
self.drivetrain.tankDrive(0.0, 0.0)
self.drivetrain.setExpiration(0.5)
else:
for motor in self.left_motors:
motor: TalonSRX
motor.setNeutralMode(NeutralMode.Brake)
motor.set(motor.ControlMode.PercentOutput, 0.0)
motor.setInverted(invert_left)
for motor in self.right_motors:
motor: TalonSRX
motor.setNeutralMode(NeutralMode.Brake)
motor.set(motor.ControlMode.PercentOutput, 0.0)
motor.setInverted(invert_right)
def robotInit(self):
""" Initalizes all subsystems and user controls. """
if self.numSubsystems > 0:
# Create drive subsystems
pwmOfs: int = 0
if self.enableDrive:
leftMotors = SpeedControllerGroup(wpilib.VictorSP(0), wpilib.VictorSP(1))
rightMotors = SpeedControllerGroup(wpilib.VictorSP(2), wpilib.VictorSP(3))
gamePad: GenericHID = XboxController(0)
drive: Drive = Drive(99, leftMotors, rightMotors)
drive.setDefaultCommand(DriveArcade(drive, leftMotors, rightMotors, gamePad))
pwmOfs += 4
for i in range(0, self.numSubsystems):
pwm: int = pwmOfs + i * 2
leftMotor: SpeedController = wpilib.VictorSP(pwm)
rightMotor: SpeedController = wpilib.VictorSP(pwm + 1)
drive = Drive(i, leftMotor, rightMotor)
SmartDashboard.putData("Forward " + str(i), DrivePower(drive, 0.2, 0.2))
SmartDashboard.putData("Backward " + str(i), DrivePower(drive, -0.2, -0.2))
# Add command to dashboard to track time for one periodic pass
self.performance = Performance()
SmartDashboard.putData("Measure Performance", self.performance)
def __init__(self, robot):
super().__init__("drivetrain")
self.robot = robot
self.counter = 0 # used for updating the log
self.x, self.y = 0, 0
self.error_dict = {}
# initialize sensors
self.navx = navx.AHRS.create_spi()
# initialize motors and encoders
motor_type = rev.MotorType.kBrushless
#self.spark_neo_left_front = rev.CANSparkMax(1, motor_type)
#self.spark_neo_left_rear = rev.CANSparkMax(2, motor_type)
#self.spark_neo_right_front = rev.CANSparkMax(3, motor_type)
#self.spark_neo_right_rear = rev.CANSparkMax(4, motor_type)
self.spark_neo_left_front = rev.CANSparkMax(4, motor_type)
self.spark_neo_left_rear = rev.CANSparkMax(3, motor_type)
self.spark_neo_right_front = rev.CANSparkMax(2, motor_type)
self.spark_neo_right_rear = rev.CANSparkMax(1, motor_type)
self.controllers = [
self.spark_neo_left_front, self.spark_neo_left_rear,
self.spark_neo_right_front, self.spark_neo_right_rear
]
self.spark_PID_controller_right_front = self.spark_neo_right_front.getPIDController(
)
self.spark_PID_controller_right_rear = self.spark_neo_right_rear.getPIDController(
)
self.spark_PID_controller_left_front = self.spark_neo_left_front.getPIDController(
)
self.spark_PID_controller_left_rear = self.spark_neo_left_rear.getPIDController(
)
self.pid_controllers = [
self.spark_PID_controller_left_front,
self.spark_PID_controller_left_rear,
self.spark_PID_controller_right_front,
self.spark_PID_controller_right_rear
]
# swap encoders to get sign right
# changing them up for mecanum vs WCD
self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(
self.spark_neo_left_front)
self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(
self.spark_neo_left_rear)
self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(
self.spark_neo_right_front)
self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(
self.spark_neo_right_rear)
self.encoders = [
self.sparkneo_encoder_1, self.sparkneo_encoder_2,
self.sparkneo_encoder_3, self.sparkneo_encoder_4
]
# copy these so the sim and the real reference the same encoders
self.l_encoder = self.sparkneo_encoder_1
self.r_encoder = self.sparkneo_encoder_3
# Configure encoders and controllers
# should be wheel_diameter * pi / gear_ratio - and for the old double reduction gear box
# the gear ratio was 4.17:1. With the shifter (low gear) I think it was a 12.26.
# the new 2020 gearbox is 9.52'
#gear_ratio = 12.75
#gear_ratio = 4.17 # pretty fast WCD gearbox
#conversion_factor = 6.0 * 0.0254 * 3.1416 / gear_ratio # do this in meters from now on
conversion_factor = drive_constants.k_sparkmax_conversion_factor_meters
for ix, encoder in enumerate(self.encoders):
self.error_dict.update({
'conv_pos_' + str(ix):
encoder.setPositionConversionFactor(conversion_factor)
})
self.error_dict.update({
'conv_vel_' + str(ix):
encoder.setVelocityConversionFactor(conversion_factor / 60.0)
}) # native is rpm
burn_flash = False
if burn_flash:
for ix, controller in enumerate(self.controllers):
self.error_dict.update(
{'burn_' + str(ix): controller.burnFlash()})
# TODO - figure out if I want to invert the motors or the encoders
#inverted = False # needs this to be True for the toughbox
#self.spark_neo_left_front.setInverted(inverted) # inverting a controller
#self.r_encoder.setInverted(True) # inverting an encoder
# create drivetrain from motors
self.speedgroup_left = SpeedControllerGroup(self.spark_neo_left_front,
self.spark_neo_left_rear)
self.speedgroup_right = SpeedControllerGroup(
self.spark_neo_right_front, self.spark_neo_right_rear)
self.drive = wpilib.drive.DifferentialDrive(self.speedgroup_left,
self.speedgroup_right)
# self.drive = wpilib.drive.DifferentialDrive(self.spark_neo_left_front, self.spark_neo_right_front)
self.drive.setMaxOutput(1.0)
self.drive.setSafetyEnabled(True)
self.drive.setExpiration(0.1)
# odometry for tracking the robot pose
self.odometry = wpimath.kinematics.DifferentialDriveOdometry(
geo.Rotation2d.fromDegrees(-self.navx.getAngle()))
class Drivetrain(SubsystemBase):
def __init__(self):
super().__init__()
# Create the motor controllers and their respective speed controllers.
self.leftMotors = SpeedControllerGroup(
PWMSparkMax(constants.kLeftMotor1Port),
PWMSparkMax(constants.kLeftMotor2Port),
)
self.rightMotors = SpeedControllerGroup(
PWMSparkMax(constants.kRightMotor1Port),
PWMSparkMax(constants.kRightMotor2Port),
)
# Create the differential drivetrain object, allowing for easy motor control.
self.drive = DifferentialDrive(self.leftMotors, self.rightMotors)
# Create the encoder objects.
self.leftEncoder = Encoder(
constants.kLeftEncoderPorts[0],
constants.kLeftEncoderPorts[1],
constants.kLeftEncoderReversed,
)
self.rightEncoder = Encoder(
constants.kRightEncoderPorts[0],
constants.kRightEncoderPorts[1],
constants.kRightEncoderReversed,
)
# Configure the encoder so it knows how many encoder units are in one rotation.
self.leftEncoder.setDistancePerPulse(
constants.kEncoderDistancePerPulse)
self.rightEncoder.setDistancePerPulse(
constants.kEncoderDistancePerPulse)
# Create the gyro, a sensor which can indicate the heading of the robot relative
# to a customizable position.
self.gyro = ADXRS450_Gyro()
# Create the an object for our odometry, which will utilize sensor data to
# keep a record of our position on the field.
self.odometry = DifferentialDriveOdometry(self.gyro.getRotation2d())
# Reset the encoders upon the initilization of the robot.
self.resetEncoders()
def periodic(self):
"""
Called periodically when it can be called. Updates the robot's
odometry with sensor data.
"""
self.odometry.update(
self.gyro.getRotation2d(),
self.leftEncoder.getDistance(),
self.rightEncoder.getDistance(),
)
def getPose(self):
"""Returns the current position of the robot using it's odometry."""
return self.odometry.getPose()
def getWheelSpeeds(self):
"""Return an object which represents the wheel speeds of our drivetrain."""
speeds = DifferentialDriveWheelSpeeds(self.leftEncoder.getRate(),
self.rightEncoder.getRate())
return speeds
def resetOdometry(self, pose):
""" Resets the robot's odometry to a given position."""
self.resetEncoders()
self.odometry.resetPosition(pose, self.gyro.getRotation2d())
def arcadeDrive(self, fwd, rot):
"""Drive the robot with standard arcade controls."""
self.drive.arcadeDrive(fwd, rot)
def tankDriveVolts(self, leftVolts, rightVolts):
"""Control the robot's drivetrain with voltage inputs for each side."""
# Set the voltage of the left side.
self.leftMotors.setVoltage(leftVolts)
# Set the voltage of the right side. It's
# inverted with a negative sign because it's motors need to spin in the negative direction
# to move forward.
self.rightMotors.setVoltage(-rightVolts)
# Resets the timer for this motor's MotorSafety
self.drive.feed()
def resetEncoders(self):
"""Resets the encoders of the drivetrain."""
self.leftEncoder.reset()
self.rightEncoder.reset()
def getAverageEncoderDistance(self):
"""
Take the sum of each encoder's traversed distance and divide it by two,
since we have two encoder values, to find the average value of the two.
"""
return (self.leftEncoder.getDistance() +
self.rightEncoder.getDistance()) / 2
def getLeftEncoder(self):
"""Returns the left encoder object."""
return self.leftEncoder
def getRightEncoder(self):
"""Returns the right encoder object."""
return self.rightEncoder
def setMaxOutput(self, maxOutput):
"""Set the max percent output of the drivetrain, allowing for slower control."""
self.drive.setMaxOutput(maxOutput)
def zeroHeading(self):
"""Zeroes the gyro's heading."""
self.gyro.reset()
def getHeading(self):
"""Return the current heading of the robot."""
return self.gyro.getRotation2d().getDegrees()
def getTurnRate(self):
"""Returns the turning rate of the robot using the gyro."""
# The minus sign negates the value.
return -self.gyro.getRate()
class Intake(Subsystem):
def __init__(self):
super().__init__("Intake")
self.talon_1 = WPI_TalonSRX(robotmap.talon_intake_1)
self.talon_2 = WPI_TalonSRX(robotmap.talon_intake_2)
self.talon_group = SpeedControllerGroup(self.talon_1, self.talon_2)
@classmethod
def setSpd(cls, spd_new):
robotmap.spd_intake = spd_new
def intake(self, spd_temp=None, is_fixed=None):
self.talon_1.setInverted(True)
self.talon_2.setInverted(False)
if (spd_temp is None) and (is_fixed is None):
if oi.joystick.getAxis(2) - oi.joystick.getAxis(3) >= 0.8:
self.talon_group.set(0.8)
elif oi.joystick.getAxis(2) - oi.joystick.getAxis(3) < 0.8:
self.talon_group.set(
oi.joystick.getAxis(2) - oi.joystick.getAxis(3))
elif (spd_temp is not None) and (is_fixed is None):
self.talon_group.set(spd_temp)
elif (spd_temp is not None) and (is_fixed is not None):
self.talon_group.set(robotmap.spd_intake)
else:
raise ("intake() fail!")
def eject(self, spd_temp=None, is_fixed=None):
self.talon_1.setInverted(False)
self.talon_2.setInverted(True)
if (spd_temp is None) and (is_fixed is None):
self.talon_group.set(oi.joystick.getAxis(3))
elif (spd_temp is not None) and (is_fixed is None):
self.talon_group.set(spd_temp)
elif (spd_temp is not None) and (is_fixed is not None):
self.talon_group.set(robotmap.spd_intake)
else:
raise ("eject() fail!")
def dislodge(self):
self.eject(True)
time.sleep(0.1)
self.intake(True)
time.sleep(0.2)
def stop(self):
self.talon_group.stopMotor()
def initDefaultCommand(self):
pass
class DriveTrainSim(Subsystem):
# ----------------- INITIALIZATION -----------------------
def __init__(self, robot):
super().__init__("drivetrainsim")
self.robot = robot
self.counter = 0 # used for updating the log
self.x, self.y = 0, 0
# initialize sensors
self.navx = navx.AHRS.create_spi()
# initialize motors
self.spark_neo_left_front = wpilib.Jaguar(1)
self.spark_neo_left_rear = wpilib.Jaguar(2)
self.spark_neo_right_front = wpilib.Jaguar(3)
self.spark_neo_right_rear = wpilib.Jaguar(4)
motor_type = rev.MotorType.kBrushless
self.spark_neo_left_front1 = rev.CANSparkMax(1, motor_type)
self.spark_neo_left_rear1 = rev.CANSparkMax(2, motor_type)
self.spark_neo_right_front1 = rev.CANSparkMax(3, motor_type)
self.spark_neo_right_rear1 = rev.CANSparkMax(4, motor_type)
self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(self.spark_neo_left_front1)
self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(self.spark_neo_left_rear1)
self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(self.spark_neo_right_front1)
self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(self.spark_neo_right_rear1)
# create drivetrain from motors
self.speedgroup_left = SpeedControllerGroup(self.spark_neo_left_front, self.spark_neo_left_rear)
self.speedgroup_right = SpeedControllerGroup(self.spark_neo_right_front, self.spark_neo_right_rear)
self.drive = wpilib.drive.DifferentialDrive(self.speedgroup_left, self.speedgroup_right)
# self.drive = wpilib.drive.DifferentialDrive(self.spark_neo_left_front, self.spark_neo_right_front)
self.drive.setMaxOutput(1.0)
# initialize encoders - doing this after motors because they may be part of the motor controller
self.l_encoder = wpilib.Encoder(0, 1, True)
self.r_encoder = wpilib.Encoder(2, 3, True)
self.l_encoder.setDistancePerPulse(drive_constants.k_encoder_distance_per_pulse_m)
self.r_encoder.setDistancePerPulse(drive_constants.k_encoder_distance_per_pulse_m)
# odometry for tracking the robot pose
self.odometry = wpimath.kinematics.DifferentialDriveOdometry(geo.Rotation2d.fromDegrees( -self.navx.getAngle() ))
def initDefaultCommand(self):
""" When other commands aren't using the drivetrain, allow arcade drive with the joystick. """
self.setDefaultCommand(DriveByJoystick(self.robot))
# ----------------- DRIVE METHODS -----------------------
def arcade_drive(self, thrust, twist):
""" wrapper for the current drive mode, really should just be called drive or move """
self.drive.arcadeDrive(xSpeed=thrust, zRotation=twist, squareInputs=True)
def stop(self):
""" stop the robot """
self.drive.arcadeDrive(xSpeed=0, zRotation=0, squareInputs=True)
# ----------------- SIMULATION AND TELEMETRY METHODS -----------------------
def get_pose(self):
# return self.odometry.getPoseMeters() # 2021 only?
return self.odometry.getPose()
def get_wheel_speeds(self):
return wpimath.kinematics.DifferentialDriveWheelSpeeds(self.l_encoder.getRate(), self.r_encoder.getRate())
def reset_encoders(self):
self.l_encoder.reset()
self.r_encoder.reset()
def get_rate(self, encoder): # spark maxes and regular encoders use different calls... annoying
return encoder.getRate()
def get_position(self, encoder):
return encoder.getDistance()
def set_position(self, encoder, position):
encoder.setDistance(position)
def reset_odometry(self, pose):
self.reset_encoders()
self.odometry.resetPosition(pose, geo.Rotation2d.fromDegrees(-self.navx.getAngle()))
def tank_drive_volts(self, left_volts, right_volts):
self.speedgroup_left.setVoltage(left_volts)
self.speedgroup_right.setVoltage(right_volts)
self.drive.feed()
def get_rotation2d(self):
return geo.Rotation2d.fromDegrees(-self.navx.getAngle())
def get_average_encoder_distance(self):
return (self.l_encoder.getDistance() - self.r_encoder.getDistance())/2 # used in PID drive straight
def get_average_encoder_rate(self):
return (self.l_encoder.getRate() + self.r_encoder.getRate())/2
def zero_heading(self):
self.navx.reset()
def reset(self):
pass
def periodic(self) -> None:
"""Perform odometry and update dash with telemetry"""
self.counter += 1
self.odometry.update(geo.Rotation2d.fromDegrees(-self.navx.getAngle()), self.l_encoder.getDistance(), -self.r_encoder.getDistance())
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position (only good for WCD)'
pose = self.get_pose()
SmartDashboard.putString('drive_pose', f'[{pose.X():2.2f}, {pose.Y():2.2f}, {pose.rotation().degrees():2.2f}]' )
if self.counter % 100 == 0:
pass
def __init__(self, robot):
super().__init__("drivetrain")
#Subsystem.__init__("drivetrain")
self.robot = robot
self.error_dict = {}
# Add constants and helper variables
self.twist_power_maximum = 0.5
self.strafe_power_maximum = 0.5
self.thrust_power_maximum = 0.5
self.mecanum_power_limit = 1.0
self.max_velocity = 500 # rpm for mecanum velocity
self.current_thrust = 0
self.current_twist = 0
self.current_strafe = 0
self.acceleration_limit = 0.05
self.counter = 0
# due to limitations in displaying digits in the Shuffleboard, we'll multiply these by 1000 and divide when updating the controllers
self.PID_multiplier = 1000.
self.PID_dict_pos = {
'kP': 0.010,
'kI': 5.0e-7,
'kD': 0.40,
'kIz': 0,
'kFF': 0.002,
'kMaxOutput': 0.99,
'kMinOutput': -0.99
}
self.PID_dict_vel = {
'kP': 2 * 0.00015,
'kI': 1.5 * 8.0e-7,
'kD': 0.00,
'kIz': 0,
'kFF': 0.00022,
'kMaxOutput': 0.99,
'kMinOutput': -0.99
}
self.encoder_offsets = [
0, 0, 0, 0
] # added because the encoders do not reset fast enough for autonomous
# Smart Motion Coefficients - these don't seem to be writing for some reason... python is old? just set with rev's program for now
self.smartmotion_maxvel = 1000 # rpm
self.smartmotion_maxacc = 500
self.current_limit = 100
self.ramp_rate = 0.0
# tracking the robot across the field... easier with WCD
self.x = 0
self.y = 0
self.previous_distance = 0
self.is_limited = False
self.deadband = 0.05
# Configure drive motors
#if True: # or could be if self.robot.isReal():
if self.robot.isReal():
motor_type = rev.MotorType.kBrushless
self.spark_neo_right_front = rev.CANSparkMax(1, motor_type)
self.spark_neo_right_rear = rev.CANSparkMax(2, motor_type)
self.spark_neo_left_front = rev.CANSparkMax(3, motor_type)
self.spark_neo_left_rear = rev.CANSparkMax(4, motor_type)
self.controllers = [
self.spark_neo_left_front, self.spark_neo_left_rear,
self.spark_neo_right_front, self.spark_neo_right_rear
]
self.spark_PID_controller_right_front = self.spark_neo_right_front.getPIDController(
)
self.spark_PID_controller_right_rear = self.spark_neo_right_rear.getPIDController(
)
self.spark_PID_controller_left_front = self.spark_neo_left_front.getPIDController(
)
self.spark_PID_controller_left_rear = self.spark_neo_left_rear.getPIDController(
)
self.pid_controllers = [
self.spark_PID_controller_left_front,
self.spark_PID_controller_left_rear,
self.spark_PID_controller_right_front,
self.spark_PID_controller_right_rear
]
# wpilib.Timer.delay(0.02)
# swap encoders to get sign right
# changing them up for mechanum vs WCD
self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(
self.spark_neo_left_front)
self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(
self.spark_neo_left_rear)
self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(
self.spark_neo_right_front)
self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(
self.spark_neo_right_rear)
self.encoders = [
self.sparkneo_encoder_1, self.sparkneo_encoder_2,
self.sparkneo_encoder_3, self.sparkneo_encoder_4
]
# Configure encoders and controllers
# should be wheel_diameter * pi / gear_ratio - and for the old double reduction gear box
# the gear ratio was 4.17:1. With the shifter (low gear) I think it was a 12.26.
# then new 2020 gearbox is 9.52
gear_ratio = 9.52
#gear_ratio = 12.75
conversion_factor = 8.0 * 3.141 / gear_ratio
for ix, encoder in enumerate(self.encoders):
self.error_dict.update({
'conv_' + str(ix):
encoder.setPositionConversionFactor(conversion_factor)
})
# wpilib.Timer.delay(0.02)
# TODO - figure out if I want to invert the motors or the encoders
inverted = False # needs this to be True for the toughbox
self.spark_neo_left_front.setInverted(inverted)
self.spark_neo_left_rear.setInverted(inverted)
self.spark_neo_right_front.setInverted(inverted)
self.spark_neo_right_rear.setInverted(inverted)
self.configure_controllers()
#self.display_PIDs()
else: # for simulation only, but the CANSpark is getting closer to behaving in sim
# get a pretend drivetrain for the simulator
self.spark_neo_left_front = wpilib.Talon(1)
self.spark_neo_left_rear = wpilib.Talon(2)
self.spark_neo_right_front = wpilib.Talon(3)
self.spark_neo_right_rear = wpilib.Talon(4)
# Not sure if speedcontrollergroups work with the single sparkmax in python - seems to complain
# drive_type = 'mechanum' # comp bot
drive_type = 'wcd' # practice bot, sim as of 1/16/2021
if drive_type == 'wcd':
# WCD
print("Enabling WCD drive!")
if robot.isReal():
err_1 = self.spark_neo_left_rear.follow(
self.spark_neo_left_front)
err_2 = self.spark_neo_right_rear.follow(
self.spark_neo_right_front)
if err_1 != rev.CANError.kOk or err_2 != rev.CANError.kOk:
print(
f"Warning: drivetrain follower issue with neo2 returning {err_1} and neo4 returning {err_2}"
)
self.speedgroup_left = SpeedControllerGroup(
self.spark_neo_left_front)
self.speedgroup_right = SpeedControllerGroup(
self.spark_neo_right_front)
self.differential_drive = DifferentialDrive(
self.speedgroup_left, self.speedgroup_right)
self.drive = self.differential_drive
self.differential_drive.setMaxOutput(1.0)
if drive_type == 'mechanum':
# Mechanum
# TODO: Reset followers in software
print("Enabling mechanum drive!")
self.speedgroup_lfront = SpeedControllerGroup(
self.spark_neo_left_front)
self.speedgroup_lrear = SpeedControllerGroup(
self.spark_neo_left_rear)
self.speedgroup_rfront = SpeedControllerGroup(
self.spark_neo_right_front)
self.speedgroup_rrear = SpeedControllerGroup(
self.spark_neo_right_rear)
self.mechanum_drive = MecanumDrive(self.speedgroup_lfront,
self.speedgroup_lrear,
self.speedgroup_rfront,
self.speedgroup_rrear)
# self.mechanum_drive = MecanumDrive(self.spark_neo_left_front, self.spark_neo_left_rear, self.spark_neo_right_front, self.spark_neo_right_rear)
self.mechanum_drive.setMaxOutput(self.mecanum_power_limit)
self.drive = self.mechanum_drive
self.drive.setSafetyEnabled(True)
self.drive.setExpiration(0.1)
class DriveTrain(Subsystem):
# ----------------- INITIALIZATION -----------------------
def __init__(self, robot):
super().__init__("drivetrain")
self.robot = robot
self.counter = 0 # used for updating the log
self.x, self.y = 0, 0
self.error_dict = {}
# initialize sensors
self.navx = navx.AHRS.create_spi()
# initialize motors and encoders
motor_type = rev.MotorType.kBrushless
#self.spark_neo_left_front = rev.CANSparkMax(1, motor_type)
#self.spark_neo_left_rear = rev.CANSparkMax(2, motor_type)
#self.spark_neo_right_front = rev.CANSparkMax(3, motor_type)
#self.spark_neo_right_rear = rev.CANSparkMax(4, motor_type)
self.spark_neo_left_front = rev.CANSparkMax(4, motor_type)
self.spark_neo_left_rear = rev.CANSparkMax(3, motor_type)
self.spark_neo_right_front = rev.CANSparkMax(2, motor_type)
self.spark_neo_right_rear = rev.CANSparkMax(1, motor_type)
self.controllers = [
self.spark_neo_left_front, self.spark_neo_left_rear,
self.spark_neo_right_front, self.spark_neo_right_rear
]
self.spark_PID_controller_right_front = self.spark_neo_right_front.getPIDController(
)
self.spark_PID_controller_right_rear = self.spark_neo_right_rear.getPIDController(
)
self.spark_PID_controller_left_front = self.spark_neo_left_front.getPIDController(
)
self.spark_PID_controller_left_rear = self.spark_neo_left_rear.getPIDController(
)
self.pid_controllers = [
self.spark_PID_controller_left_front,
self.spark_PID_controller_left_rear,
self.spark_PID_controller_right_front,
self.spark_PID_controller_right_rear
]
# swap encoders to get sign right
# changing them up for mecanum vs WCD
self.sparkneo_encoder_1 = rev.CANSparkMax.getEncoder(
self.spark_neo_left_front)
self.sparkneo_encoder_2 = rev.CANSparkMax.getEncoder(
self.spark_neo_left_rear)
self.sparkneo_encoder_3 = rev.CANSparkMax.getEncoder(
self.spark_neo_right_front)
self.sparkneo_encoder_4 = rev.CANSparkMax.getEncoder(
self.spark_neo_right_rear)
self.encoders = [
self.sparkneo_encoder_1, self.sparkneo_encoder_2,
self.sparkneo_encoder_3, self.sparkneo_encoder_4
]
# copy these so the sim and the real reference the same encoders
self.l_encoder = self.sparkneo_encoder_1
self.r_encoder = self.sparkneo_encoder_3
# Configure encoders and controllers
# should be wheel_diameter * pi / gear_ratio - and for the old double reduction gear box
# the gear ratio was 4.17:1. With the shifter (low gear) I think it was a 12.26.
# the new 2020 gearbox is 9.52'
#gear_ratio = 12.75
#gear_ratio = 4.17 # pretty fast WCD gearbox
#conversion_factor = 6.0 * 0.0254 * 3.1416 / gear_ratio # do this in meters from now on
conversion_factor = drive_constants.k_sparkmax_conversion_factor_meters
for ix, encoder in enumerate(self.encoders):
self.error_dict.update({
'conv_pos_' + str(ix):
encoder.setPositionConversionFactor(conversion_factor)
})
self.error_dict.update({
'conv_vel_' + str(ix):
encoder.setVelocityConversionFactor(conversion_factor / 60.0)
}) # native is rpm
burn_flash = False
if burn_flash:
for ix, controller in enumerate(self.controllers):
self.error_dict.update(
{'burn_' + str(ix): controller.burnFlash()})
# TODO - figure out if I want to invert the motors or the encoders
#inverted = False # needs this to be True for the toughbox
#self.spark_neo_left_front.setInverted(inverted) # inverting a controller
#self.r_encoder.setInverted(True) # inverting an encoder
# create drivetrain from motors
self.speedgroup_left = SpeedControllerGroup(self.spark_neo_left_front,
self.spark_neo_left_rear)
self.speedgroup_right = SpeedControllerGroup(
self.spark_neo_right_front, self.spark_neo_right_rear)
self.drive = wpilib.drive.DifferentialDrive(self.speedgroup_left,
self.speedgroup_right)
# self.drive = wpilib.drive.DifferentialDrive(self.spark_neo_left_front, self.spark_neo_right_front)
self.drive.setMaxOutput(1.0)
self.drive.setSafetyEnabled(True)
self.drive.setExpiration(0.1)
# odometry for tracking the robot pose
self.odometry = wpimath.kinematics.DifferentialDriveOdometry(
geo.Rotation2d.fromDegrees(-self.navx.getAngle()))
def initDefaultCommand(self):
""" When other commands aren't using the drivetrain, allow arcade drive with the joystick. """
self.setDefaultCommand(DriveByJoystick(self.robot))
# ----------------- DRIVE METHODS -----------------------
def arcade_drive(self, thrust, twist):
""" wrapper for the current drive mode, really should just be called drive or move """
self.drive.arcadeDrive(xSpeed=thrust,
zRotation=twist,
squareInputs=True)
#[controller.setVoltage(thrust) for controller in self.controllers]
#self.drive.feed()
def stop(self):
""" stop the robot """
self.drive.arcadeDrive(xSpeed=0, zRotation=0, squareInputs=True)
# ----------------- SIMULATION AND TELEMETRY METHODS -----------------------
def get_pose(self):
# return self.odometry.getPoseMeters() # 2021 only?
return self.odometry.getPose()
def get_wheel_speeds(self):
return wpimath.kinematics.DifferentialDriveWheelSpeeds(
self.l_encoder.getVelocity(), self.r_encoder.getVelocity())
def reset_encoders(self):
self.l_encoder.setPosition(0)
self.r_encoder.setPosition(0)
def get_rate(
self, encoder
): # spark maxes and regular encoders use different calls... annoying
return encoder.getVelocity()
def get_position(self, encoder):
return encoder.getPosition()
def set_position(self, encoder, position):
encoder.setPosition(position)
def reset_odometry(self, pose):
self.reset_encoders()
self.odometry.resetPosition(
pose, geo.Rotation2d.fromDegrees(-self.navx.getAngle()))
def tank_drive_volts(self, left_volts, right_volts):
self.speedgroup_left.setVoltage(left_volts)
self.speedgroup_right.setVoltage(right_volts)
self.drive.feed()
def get_rotation2d(self):
return geo.Rotation2d.fromDegrees(-self.navx.getAngle())
def get_average_encoder_distance(self):
return (self.l_encoder.getPosition() -
self.r_encoder.getPosition()) / 2
def get_average_encoder_rate(self):
return (self.l_encoder.getVelocity() +
self.r_encoder.getVelocity()) / 2
def zero_heading(self):
self.navx.reset()
def reset(self):
self.zero_heading()
self.reset_encoders()
def periodic(self) -> None:
"""Perform odometry and update dash with telemetry"""
self.counter += 1
self.odometry.update(geo.Rotation2d.fromDegrees(-self.navx.getAngle()),
self.l_encoder.getPosition(),
-self.r_encoder.getPosition())
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position (only good for WCD)'
pose = self.get_pose()
SmartDashboard.putString(
'drive_pose',
f'[{pose.X():2.2f}, {pose.Y():2.2f}, {pose.rotation().degrees():2.2f}]'
)
SmartDashboard.putString(
'drive_encoders LR',
f'[{self.get_position(self.l_encoder):2.3f}, {self.get_position(self.r_encoder):2.2f}]'
)
SmartDashboard.putString('drive_heading',
f'[{-self.navx.getAngle():2.3f}]')
if self.counter % 100 == 0:
pass
# self.display_PIDs()
msg = f"Positions: ({self.l_encoder.getPosition():2.2f}, {self.r_encoder.getPosition():2.2}, {self.navx.getAngle():2.2})"
msg = msg + f" Rates: ({self.l_encoder.getVelocity():2.2f}, {self.r_encoder.getVelocity():2.2f}) Time: {Timer.getFPGATimestamp() - self.robot.enabled_time:2.1f}"
SmartDashboard.putString("alert", msg)
SmartDashboard.putString("sparks", str(self.error_dict))
def __init__(self):
super().__init__("Intake")
self.talon_1 = WPI_TalonSRX(robotmap.talon_intake_1)
self.talon_2 = WPI_TalonSRX(robotmap.talon_intake_2)
self.talon_group = SpeedControllerGroup(self.talon_1, self.talon_2)