def robotInit(self):
self.joystick = wpilib.Joystick(0)
wheelATalon = ctre.WPI_TalonSRX(21)
wheelBTalon = ctre.WPI_TalonSRX(22)
for talon in [wheelATalon, wheelBTalon]:
talon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,
0, 0)
wheelA = sea.AngledWheel(wheelATalon,
1.0,
0.0,
math.pi / 2,
encoderCountsPerFoot=31291.1352,
maxVoltageVelocity=16)
wheelB = sea.AngledWheel(wheelBTalon,
-1.0,
0.0,
math.pi / 2,
encoderCountsPerFoot=31291.1352,
maxVoltageVelocity=16)
self.superDrive = sea.SuperHolonomicDrive()
self.superDrive.addWheel(wheelA)
self.superDrive.addWheel(wheelB)
for wheel in self.superDrive.wheels:
wheel.driveMode = ctre.ControlMode.PercentOutput
def initDrivetrain(self):
#talons
leftTalon = ctre.WPI_TalonSRX(0)
rightTalon = ctre.WPI_TalonSRX(1)
#configure talons
for talon in [leftTalon, rightTalon]:
talon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,
0, 0)
#create wheels
leftWheel = sea.AngledWheel(leftTalon, -1, 0, math.pi / 2, 31291.1352,
16)
rightWheel = sea.AngledWheel(rightTalon, 1, 0, math.pi / 2, 31291.1352,
16)
#add wheels to drivetrain
self.drivetrain = sea.SuperHolonomicDrive()
self.drivetrain.addWheel(leftWheel)
self.drivetrain.addWheel(rightWheel)
for wheel in self.drivetrain.wheels:
wheel.driveMode = ctre.ControlMode.PercentOutput
#sets up drivetrain to work in the simulator
sea.setSimulatedDrivetrain(self.drivetrain)
def robotInit(self):
self.joystick = wpilib.Joystick(0)
wheelADriveTalon = ctre.WPI_TalonSRX(1)
wheelARotateTalon = ctre.WPI_TalonSRX(0)
wheelBDriveTalon = ctre.WPI_TalonSRX(3)
wheelBRotateTalon = ctre.WPI_TalonSRX(2)
wheelCDriveTalon = ctre.WPI_TalonSRX(5)
wheelCRotateTalon = ctre.WPI_TalonSRX(4)
for talon in [
wheelADriveTalon, wheelARotateTalon, wheelBDriveTalon,
wheelBRotateTalon, wheelCDriveTalon, wheelCRotateTalon
]:
talon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,
0, 0)
wheelADrive = sea.AngledWheel(wheelADriveTalon,
.75,
.75,
0,
encoderCountsPerFoot=31291.1352,
maxVoltageVelocity=16)
wheelBDrive = sea.AngledWheel(wheelBDriveTalon,
-.75,
.75,
0,
encoderCountsPerFoot=31291.1352,
maxVoltageVelocity=16)
wheelCDrive = sea.AngledWheel(wheelCDriveTalon,
0,
-.75,
0,
encoderCountsPerFoot=31291.1352,
maxVoltageVelocity=16)
wheelARotate = sea.SwerveWheel(wheelADrive, wheelARotateTalon, 1612.8,
True)
wheelBRotate = sea.SwerveWheel(wheelBDrive, wheelBRotateTalon, 1612.8,
True)
wheelCRotate = sea.SwerveWheel(wheelCDrive, wheelCRotateTalon, 1680,
True) # 1670, 1686, 1680
self.superDrive = sea.SuperHolonomicDrive()
sea.setSimulatedDrivetrain(self.superDrive)
for wheelrotate in [wheelARotate, wheelBRotate, wheelCRotate]:
self.superDrive.addWheel(wheelrotate)
for wheel in self.superDrive.wheels:
wheel.driveMode = ctre.ControlMode.PercentOutput
self.robotOrigin = None
self.app = None
sea.startDashboard(self, swervebot_app.SwerveBotDashboard)
def _makeWheel(superDrive,
sparkMaxNum1,
sparkMaxNum2,
sparkMaxNum3,
motorType,
xPos,
yPos,
reverse=False):
sparkMax1 = sea.createSpark(sparkMaxNum1, motorType)
sparkMax2 = sea.createSpark(sparkMaxNum2, motorType)
sparkMax3 = sea.createSpark(sparkMaxNum3, motorType)
for sparkMax in [sparkMax1, sparkMax2, sparkMax3]:
sparkMax.restoreFactoryDefaults()
sparkMax.setIdleMode(rev.CANSparkMax.IdleMode.kBrake)
superDrive.motors.append(sparkMax)
# maxVoltageVelocity = 5 ft per second * 60 seconds = 300 rpm
# probably wrong though because 16 works
wheelDiameter = 6 / 12 # 6 inches converted to feet
wheelCircumference = wheelDiameter * math.pi
angledWheel = sea.AngledWheel(sparkMax1,
xPos,
yPos,
math.radians(90),
wheelCircumference,
16,
reverse=reverse)
angledWheel.addMotor(sparkMax2)
angledWheel.addMotor(sparkMax3)
superDrive.addWheel(angledWheel)
def _makeSwerveWheel(superDrive, driveTalonNum, rotateTalonNum, xPos, yPos):
driveTalon = ctre.WPI_TalonSRX(driveTalonNum)
rotateTalon = ctre.WPI_TalonSRX(rotateTalonNum)
driveTalon.configFactoryDefault(0)
rotateTalon.configFactoryDefault(0)
driveTalon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder, 0,
0)
rotateTalon.configSelectedFeedbackSensor(ctre.FeedbackDevice.QuadEncoder,
0, 0)
driveTalon.setSensorPhase(False)
rotateTalon.setSensorPhase(True)
driveTalon.setNeutralMode(ctre.NeutralMode.Brake)
rotateTalon.setNeutralMode(ctre.NeutralMode.Brake)
driveTalon.setStatusFramePeriod(
ctre.WPI_TalonSRX.StatusFrame.Status_1_General, 50, 0)
driveTalon.setStatusFramePeriod(
ctre.WPI_TalonSRX.StatusFrame.Status_2_Feedback0, 50, 0)
rotateTalon.setStatusFramePeriod(
ctre.WPI_TalonSRX.StatusFrame.Status_1_General, 100, 0)
rotateTalon.setStatusFramePeriod(
ctre.WPI_TalonSRX.StatusFrame.Status_2_Feedback0, 100, 0)
# rotate talon PIDs (drive talon is configured by DriveGear)
rotateTalon.config_kP(0, 30.0, 0)
rotateTalon.config_kI(0, 0.0, 0)
rotateTalon.config_kD(0, 24.0, 0)
# Drive motor:
# 8192 counts per encoder revolution
# 4 : 1 gear ratio
# 8192 * 4 = 32768 counts per wheel rotation
# Wheel diameter: 3.97 in.
# Wheel circumference: 3.97 * pi / 12 = 1.03934 ft
# 32768 / 1.03934 = 31527.59199 counts per foot
angledWheel = sea.AngledWheel(driveTalon,
xPos,
yPos,
0,
encoderCountsPerFoot=31527.59199,
maxVoltageVelocity=16,
reverse=True)
# Steer motor:
# 537.6 counts per revolution
# 3 : 1 gear ratio
# 537.6 * 3 = 1612.8 counts per wheel rotation
swerveWheel = sea.SwerveWheel(angledWheel,
rotateTalon,
encoderCountsPerRev=1612.8,
rotationVelocity=math.pi * 2 * 1.77331,
offsetX=0,
offsetY=-0.06883518,
reverseSteerMotor=True)
superDrive.addWheel(swerveWheel)
def robotInit(self):
leftSpark = rev.CANSparkMax(1, rev.MotorType.kBrushless)
rightSpark = rev.CANSparkMax(2, rev.MotorType.kBrushless)
for spark in [leftSpark, rightSpark]:
spark.restoreFactoryDefaults()
spark.setIdleMode(rev.IdleMode.kBrake)
leftWheel = sea.AngledWheel(leftSpark, -1, 0, math.pi / 2, 1, 16)
rightWheel = sea.AngledWheel(rightSpark, 1, 0, math.pi / 2, 1, 16)
self.drivetrain = sea.SuperHolonomicDrive()
self.drivetrain.addWheel(leftWheel)
self.drivetrain.addWheel(rightWheel)
for wheel in self.drivetrain.wheels:
wheel.driveMode = rev.ControlType.kVelocity
sea.setSimulatedDrivetrain(self.drivetrain)
def makeSwerveWheel(self, driveTalonNum, rotateTalonNum, xPos, yPos,
encoderCountsPerRev, reverseSteerMotor):
driveTalon = ctre.WPI_TalonSRX(driveTalonNum)
rotateTalon = ctre.WPI_TalonSRX(rotateTalonNum)
driveTalon.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, 0)
rotateTalon.configSelectedFeedbackSensor(
ctre.FeedbackDevice.QuadEncoder, 0, 0)
angledWheel = sea.AngledWheel(driveTalon,
xPos,
yPos,
0,
encoderCountsPerFoot=31291.1352,
maxVoltageVelocity=16)
swerveWheel = sea.SwerveWheel(angledWheel, rotateTalon,
encoderCountsPerRev, reverseSteerMotor)
self.superDrive.addWheel(swerveWheel)