def robotInit(self):
self.gamepad = Gamepad(port=1)
self.sh = wpilib.CANTalon(5)
self.it = wpilib.CANTalon(6)
self.flywheelSpeedLog = LogState("Flywheel speed")
class Climber(Module):
def lock(self):
if not self.locked:
self.locked = True
self.cm.changeControlMode(wpilib.CANTalon.ControlMode.Position)
self.cm.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.cm.setPID(1.0, 0.0, 3.0, 0.0)
position = self.cm.getPosition()
self.cm.set(position)
print("Locked")
def unlock(self):
if self.locked :
self.locked = False
self.cm.changeControlMode(wpilib.CANTalon.ControlMode.PercentVbus)
print("Unlocked")
def robotInit(self):
self.gamepad = Gamepad(port = 1)
self.cm = wpilib.CANTalon(4)
#cm stands for climb motor
def teleopInit(self):
print("SPECIAL GAMEPAD")
print(" Left Joystick up: Climb")
print(" Left Joystick down: Descend")
print(" Up Dpad: Lock motor")
print(" Down Dpad: Unlock motor")
self.locked = False
self.lockmode = False
def teleopPeriodic(self):
if self.gamepad.getRawButton(Gamepad.UP):
if not self.lockmode:
self.lockmode = True
print("Lock mode enabled")
if self.gamepad.getRawButton(Gamepad.DOWN):
if self.lockmode:
self.lockmode = False
print("Lock mode disabled")
if self.gamepad.getLY()==0 and self.lockmode:
self.lock()
else:
self.unlock()
self.cm.set(self.gamepad.getLY() * -.5)
def disabledInit(self):
pass
def robotInit(self, talonPort, ticksPerRotation=4000, maxVelocity=10):
self.talonPort = talonPort
self.Talon = wpilib.CANTalon(talonPort)
self.Talon.setPID(1, 0, 1, 0)
self.Talon.changeControlMode(wpilib.CANTalon.ControlMode.Position)
#self.Talon.changeControlMode(wpilib.CANTalon.ControlMode.Speed)
self.goalPosition = self.Talon.getPosition()
self.maxVelocity = maxVelocity
self.ticksPerRotation = ticksPerRotation
self.gamepad = Gamepad(0)
self.adjustFactor = 1.0
self.PIDNumber = 0
def robotInit(self):
self.gamepad = Gamepad(port=0)
fl = wpilib.CANTalon(2)
fr = wpilib.CANTalon(1)
bl = wpilib.CANTalon(0)
br = wpilib.CANTalon(3)
self.talons = [fl, fr, bl, br]
for talon in self.talons:
talon.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.driveModeLog = LogState("Drive mode")
self.currentPID = None
self.fastPID = (3.0, 0.0, 3.0, 0.0)
self.fastPIDScale = 0.1
self.slowPID = (30.0, 0.0, 3.0, 0.0)
self.slowPIDScale = 0.01
self.pidLog = LogState("Drive PID")
self._setPID(self.fastPID)
# 4156 ticks per wheel rotation
# encoder has 100 raw ticks -- with a QuadEncoder that makes 400 ticks
# the motor gear has 18 teeth and the wheel has 187 teeth
# 187 / 18 * 400 = 4155.5556 = ~4156
# TODO: recalculate ticks per rotation
self.holoDrive = HolonomicDrive(fl, fr, bl, br, 4156)
self.holoDrive.invertDrive(True)
self.holoDrive.setWheelOffset(math.radians(22.5)) #angle of rollers
self.drive = AccelerationFilterDrive(self.holoDrive)
self.ahrs = AHRS.create_spi() # the NavX
self.drive = FieldOrientedDrive(self.drive,
self.ahrs,
offset=math.pi / 2)
self.drive.zero()
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
self.normalScale = 0.3
self.fastScale = 0.5
self.slowScale = 0.05
self.joystickExponent = 2
def robotInit(self):
self.gamepad = Gamepad(1)
self.LeftFly = ctre.CANTalon(4)
self.RightFly = ctre.CANTalon(5)
self.LeftFly.setPID(1, 0.0009, 1, 0.0)
self.RightFly.setPID(1, 0.0009, 1, 0.0)
self.LeftFly.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.RightFly.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.Intake = ctre.CANTalon(8)
self.Intake.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.motorSpeed = 0
self.hold = False
self.flywheelsLog = LogState("Flywheels")
self.holdLog = LogState("Hold flywheel speed")
self.intakeLog = LogState("Intake")
def robotInit(self, normalScale=.5, fastScale=1.0, slowScale=.2):
"""
``normalScale``, ``fastScale`` and ``slowScale`` are optional - these
are the scales (from 0.0 to 1.0) for drive speed when driving normally,
when the Fast button is pressed, and when the Slow button is pressed.
"""
print("Left Bumper: Slower")
print("Left Joystick: Faster")
self.gamepad = Gamepad(port=0)
self.drive = None
self.normalScale = normalScale
self.fastScale = fastScale
self.slowScale = slowScale
self.talons = []
self.currentPID = None
self.normalPID = None
self.slowPID = None
def robotInit(self, normalScale=.5, fastScale=1.0, slowScale=.2):
"""
``normalScale``, ``fastScale`` and ``slowScale`` are optional - these
are the scales (from 0.0 to 1.0) for drive speed when driving normally,
when the Fast button is pressed, and when the Slow button is pressed.
"""
print("Left Bumper: Slower")
print("Left Joystick: Faster")
self.gamepad = Gamepad(port = 0)
self.drive = None
self.normalScale = normalScale
self.fastScale = fastScale
self.slowScale = slowScale
self.talons = [ ]
self.currentPID = None
self.normalPID = None
self.slowPID = None
class Shooter(Module):
def robotInit(self):
self.gamepad = Gamepad(port=1)
self.sh = wpilib.CANTalon(5)
self.it = wpilib.CANTalon(6)
self.flywheelSpeedLog = LogState("Flywheel speed")
def teleopInit(self):
print(" A: Flywheel")
print(" B: Intake")
print(" X: Outtake")
self.flywheelSpeed = .76
self.flywheelSpeedLog.update(self.flywheelSpeed)
def teleopPeriodic(self):
if self.gamepad.buttonPressed(Gamepad.BACK):
self.flywheelSpeed -= .01
if self.gamepad.buttonPressed(Gamepad.START):
self.flywheelSpeed += .01
self.flywheelSpeedLog.update(self.flywheelSpeed)
if self.gamepad.getRawButton(Gamepad.A):
self.sh.set(-self.flywheelSpeed)
else:
self.sh.set(0)
if self.gamepad.getRawButton(Gamepad.B):
self.it.set(0.2)
elif self.gamepad.getRawButton(Gamepad.X):
self.it.set(-0.2)
else:
self.it.set(0)
self.gamepad.updateButtons()
class StingrayShooter(Module):
def robotInit(self):
self.gamepad = Gamepad(1)
self.LeftFly = ctre.CANTalon(4)
self.RightFly = ctre.CANTalon(5)
self.LeftFly.setPID(1, 0.0009, 1, 0.0)
self.RightFly.setPID(1, 0.0009, 1, 0.0)
self.LeftFly.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.RightFly.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.Intake = ctre.CANTalon(8)
self.Intake.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.motorSpeed = 0
self.hold = False
self.flywheelsLog = LogState("Flywheels")
self.holdLog = LogState("Hold flywheel speed")
self.intakeLog = LogState("Intake")
def teleopPeriodic(self):
if not self.hold:
buttonPressed = False
speed = 3
if self.gamepad.getRawButton(Gamepad.A):
buttonPressed = True
speed += 1
if self.gamepad.getRawButton(Gamepad.B):
buttonPressed = True
speed += 2
if self.gamepad.getRawButton(Gamepad.X):
buttonPressed = True
speed += 4
if self.gamepad.getRawButton(Gamepad.Y):
buttonPressed = True
speed += 8
if buttonPressed:
self.motorSpeed = -float(speed) / 18.0
else:
self.motorSpeed = 0.0
self.flywheelsLog.update(self.getFlywheelSpeedString())
self.RightFly.set(self.motorSpeed)
self.LeftFly.set(self.motorSpeed)
if self.gamepad.getRawButton(Gamepad.START):
self.hold = True
self.holdLog.update(self.getFlywheelSpeedString())
elif self.gamepad.getRawButton(Gamepad.BACK):
self.hold = False
self.holdLog.update("Disabled")
if self.gamepad.getRawButton(Gamepad.RB):
# intake forwards
self.Intake.set(.5)
self.intakeLog.update("Forward")
elif self.gamepad.getRawButton(Gamepad.LB):
# intake backwards
self.Intake.set(-.5)
self.intakeLog.update("Backward")
else:
self.Intake.set(0.0)
self.intakeLog.update("Off")
def getFlywheelSpeedString(self):
return str(int(round(-self.motorSpeed * 100))) + "%"
class DriveTest(wpilib.IterativeRobot):
"""
Can be extended to allow driving a robot with a custom drivetrain using the
gamepad.
"""
def robotInit(self, normalScale=.5, fastScale=1.0, slowScale=.2):
"""
``normalScale``, ``fastScale`` and ``slowScale`` are optional - these
are the scales (from 0.0 to 1.0) for drive speed when driving normally,
when the Fast button is pressed, and when the Slow button is pressed.
"""
print("Left Bumper: Slower")
print("Left Joystick: Faster")
self.gamepad = Gamepad(port=0)
self.drive = None
self.normalScale = normalScale
self.fastScale = fastScale
self.slowScale = slowScale
self.talons = []
self.currentPID = None
self.normalPID = None
self.slowPID = None
def initDrive(self,
drive,
driveMode=DriveInterface.DriveMode.POSITION,
talons=[],
normalPID=None,
slowPID=None):
"""
``drive`` is a ``seamonsters.drive.DriveInterface``. ``driveMode`` is a
``seamonsters.drive.DriveInterface.DriveMode`` - if given, this is the
initial drive mode that the DriveInterface is set to. ``talons`` is a
list of CANTalons (you only need to specify this if you want to use
alternate slow-mode PIDs). ``normalPID`` and ``slowPID`` should be
tuples of 4 floats (P, I, D, F). ``slowPID`` is used while the Slow
button is pressed. Both are optional.
"""
self.drive = drive
self.drive.setDriveMode(driveMode)
self.talons = talons
self.currentPID = None
self.normalPID = normalPID
self.slowPID = slowPID
def teleopPeriodic(self):
if self.drive == None:
return
# change drive mode with A, B, and X
if self.gamepad.getRawButton(Gamepad.A):
print("Voltage mode!")
self.drive.setDriveMode(DriveInterface.DriveMode.VOLTAGE)
elif self.gamepad.getRawButton(Gamepad.B):
print("Speed mode!")
self.drive.setDriveMode(DriveInterface.DriveMode.SPEED)
elif self.gamepad.getRawButton(Gamepad.X):
print("Position mode!")
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
scale = self.normalScale
if self.gamepad.getRawButton(Gamepad.LJ): # faster button
scale = self.fastScale
if self.gamepad.getRawButton(Gamepad.LB): # slower button
scale = self.slowScale
self._setPID(self.slowPID)
else:
self._setPID(self.normalPID)
turn = -self.gamepad.getRX() * abs(self.gamepad.getRX()) \
* (scale / 2)
magnitude = self.gamepad.getLMagnitude()**2 * scale
direction = self.gamepad.getLDirection()
self.drive.drive(magnitude, direction, turn)
def _setPID(self, pid):
if pid == None:
return
if pid == self.currentPID:
return
self.currentPID = pid
for talon in self.talons:
talon.setPID(pid[0], pid[1], pid[2], pid[3])
class PIDTest(wpilib.IterativeRobot):
def robotInit(self, talonPort, ticksPerRotation=4000, maxVelocity=10):
self.talonPort = talonPort
self.Talon = wpilib.CANTalon(talonPort)
self.Talon.setPID(1, 0, 1, 0)
self.Talon.changeControlMode(wpilib.CANTalon.ControlMode.Position)
#self.Talon.changeControlMode(wpilib.CANTalon.ControlMode.Speed)
self.goalPosition = self.Talon.getPosition()
self.maxVelocity = maxVelocity
self.ticksPerRotation = ticksPerRotation
self.gamepad = Gamepad(0)
self.adjustFactor = 1.0
self.PIDNumber = 0
def teleopInit(self):
print("PID Adjustor")
print("Hold RT and move the left joystick to change motor speed")
print("Press Start and Back to toggle the selected value (P, I, D, F)")
print("Press Left/Right to increase/decrease the adjust factor")
print("Press Up/Down to increase/decrease the selected value")
print()
print("Using CANTalon:", self.talonPort)
print("Currently adjusting: P")
self.goalPosition = self.Talon.getPosition()
def teleopPeriodic(self):
self.gamepad.updateButtons()
if self.gamepad.buttonPressed(Gamepad.START):
self.PIDNumber += 1
if self.PIDNumber > 3:
self.PIDNumber = 0
self._printAdjustedValue()
if self.gamepad.buttonPressed(Gamepad.BACK):
self.PIDNumber -= 1
if self.PIDNumber < 0:
self.PIDNumber = 3
self._printAdjustedValue()
if self.gamepad.buttonPressed(Gamepad.LEFT):
self.adjustFactor /= 2.0
self._printAdjustFactor()
if self.gamepad.buttonPressed(Gamepad.RIGHT):
self.adjustFactor *= 2.0
self._printAdjustFactor()
elif self.gamepad.buttonPressed(Gamepad.UP):
self._changePID(self.adjustFactor)
self._printValues()
elif self.gamepad.buttonPressed(Gamepad.DOWN):
self._changePID(-self.adjustFactor)
self._printValues()
if self.gamepad.getRawButton(Gamepad.RT):
if not (abs(self.goalPosition - self.Talon.getPosition()) \
> self.ticksPerRotation):
self.goalPosition += self.gamepad.getLY() * -1 \
* self.maxVelocity
self.Talon.set(self.goalPosition)
print("Speed: " + str(self.Talon.getEncVelocity()))
def _changePID(self, number):
if self.PIDNumber == 0:
self.Talon.setP(self.Talon.getP() + number)
elif self.PIDNumber == 1:
self.Talon.setI(self.Talon.getI() + number)
elif self.PIDNumber == 2:
self.Talon.setD(self.Talon.getD() + number)
elif self.PIDNumber == 3:
self.Talon.setF(self.Talon.getF() + number)
def _printAdjustedValue(self):
print("Currently adjusting: ", end="")
if self.PIDNumber == 0:
print("P")
elif self.PIDNumber == 1:
print("I")
elif self.PIDNumber == 2:
print("D")
elif self.PIDNumber == 3:
print("F")
def _printAdjustFactor(self):
print("Factor:", self.adjustFactor)
def _printValues(self):
print("P: " + str(self.Talon.getP()) \
+ " I: " + str(self.Talon.getI()) \
+ " D: " + str(self.Talon.getD()) \
+ " F: " + str(self.Talon.getF()))
class DriveTest (wpilib.IterativeRobot):
def robotInit(self, normalScale=.5, fastScale=1.0, slowScale=.2):
"""
``normalScale``, ``fastScale`` and ``slowScale`` are optional - these
are the scales (from 0.0 to 1.0) for drive speed when driving normally,
when the Fast button is pressed, and when the Slow button is pressed.
"""
print("Left Bumper: Slower")
print("Left Joystick: Faster")
self.gamepad = Gamepad(port = 0)
self.drive = None
self.normalScale = normalScale
self.fastScale = fastScale
self.slowScale = slowScale
self.talons = [ ]
self.currentPID = None
self.normalPID = None
self.slowPID = None
def initDrive(self, drive, driveMode=DriveInterface.DriveMode.POSITION,
talons=[ ], normalPID=None, slowPID=None):
"""
``drive`` is a ``seamonsters.drive.DriveInterface``. ``driveMode`` is a
``seamonsters.drive.DriveInterface.DriveMode`` - if given, this is the
initial drive mode that the DriveInterface is set to. ``talons`` is a
list of CANTalons (you only need to specify this if you want to use
alternate slow-mode PIDs). ``normalPID`` and ``slowPID`` should be
tuples of 4 floats (P, I, D, F). ``slowPID`` is used while the Slow
button is pressed. Both are optional.
"""
self.drive = drive
self.drive.setDriveMode(driveMode)
self.talons = talons
self.currentPID = None
self.normalPID = normalPID
self.slowPID = slowPID
def teleopPeriodic(self):
if self.drive == None:
return
# change drive mode with A, B, and X
if self.gamepad.getRawButton(Gamepad.A):
print("Voltage mode!")
self.drive.setDriveMode(DriveInterface.DriveMode.VOLTAGE)
elif self.gamepad.getRawButton(Gamepad.B):
print("Speed mode!")
self.drive.setDriveMode(DriveInterface.DriveMode.SPEED)
elif self.gamepad.getRawButton(Gamepad.X):
print("Position mode!")
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
scale = self.normalScale
if self.gamepad.getRawButton(Gamepad.LJ): # faster button
scale = self.fastScale
if self.gamepad.getRawButton(Gamepad.LB): # slower button
scale = self.slowScale
self._setPID(self.slowPID)
else:
self._setPID(self.normalPID)
turn = -self.gamepad.getRX() * abs(self.gamepad.getRX()) \
* (scale / 2)
magnitude = self.gamepad.getLMagnitude()**2 * scale
direction = self.gamepad.getLDirection()
self.drive.drive(magnitude, direction, turn)
def _setPID(self, pid):
if pid == None:
return
if pid == self.currentPID:
return
self.currentPID = pid
for talon in self.talons:
talon.setPID(pid[0], pid[1], pid[2], pid[3])
def robotInit(self):
self.gamepad = Gamepad(port = 1)
self.cm = wpilib.CANTalon(4)
class DriveBot(Module):
def robotInit(self):
self.gamepad = Gamepad(port=0)
fl = wpilib.CANTalon(2)
fr = wpilib.CANTalon(1)
bl = wpilib.CANTalon(0)
br = wpilib.CANTalon(3)
self.talons = [fl, fr, bl, br]
for talon in self.talons:
talon.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.driveModeLog = LogState("Drive mode")
self.currentPID = None
self.fastPID = (3.0, 0.0, 3.0, 0.0)
self.fastPIDScale = 0.1
self.slowPID = (30.0, 0.0, 3.0, 0.0)
self.slowPIDScale = 0.01
self.pidLog = LogState("Drive PID")
self._setPID(self.fastPID)
# 4156 ticks per wheel rotation
# encoder has 100 raw ticks -- with a QuadEncoder that makes 400 ticks
# the motor gear has 18 teeth and the wheel has 187 teeth
# 187 / 18 * 400 = 4155.5556 = ~4156
# TODO: recalculate ticks per rotation
self.holoDrive = HolonomicDrive(fl, fr, bl, br, 4156)
self.holoDrive.invertDrive(True)
self.holoDrive.setWheelOffset(math.radians(22.5)) #angle of rollers
self.drive = AccelerationFilterDrive(self.holoDrive)
self.ahrs = AHRS.create_spi() # the NavX
self.drive = FieldOrientedDrive(self.drive,
self.ahrs,
offset=math.pi / 2)
self.drive.zero()
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
self.normalScale = 0.3
self.fastScale = 0.5
self.slowScale = 0.05
self.joystickExponent = 2
def teleopInit(self):
print("Left Bumper: Slower")
print("Left Trigger: Faster")
self.holoDrive.zeroEncoderTargets()
def teleopPeriodic(self):
# change drive mode with A, B, and X
if self.gamepad.getRawButton(Gamepad.A):
self.drive.setDriveMode(DriveInterface.DriveMode.VOLTAGE)
elif self.gamepad.getRawButton(Gamepad.B):
self.drive.setDriveMode(DriveInterface.DriveMode.SPEED)
elif self.gamepad.getRawButton(Gamepad.X):
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
self.driveModeLog.update(self._driveModeName(
self.drive.getDriveMode()))
scale = self.normalScale
if self.gamepad.getRawButton(Gamepad.LT): # faster button
scale = self.fastScale
if self.gamepad.getRawButton(Gamepad.LB): # slower button
scale = self.slowScale
turn = self._joystickPower(-self.gamepad.getRX()) * (scale / 2)
magnitude = self._joystickPower(self.gamepad.getLMagnitude()) * scale
direction = self.gamepad.getLDirection()
driveScale = max(magnitude, abs(turn * 2))
self._setPID(self._lerpPID(driveScale))
self.drive.drive(magnitude, direction, turn)
def _driveModeName(self, driveMode):
if driveMode == DriveInterface.DriveMode.VOLTAGE:
return "Voltage"
if driveMode == DriveInterface.DriveMode.SPEED:
return "Speed"
if driveMode == DriveInterface.DriveMode.POSITION:
return "Position"
return "Unknown"
def _setPID(self, pid):
self.pidLog.update(pid)
if pid == self.currentPID:
return
self.currentPID = pid
for talon in self.talons:
talon.setPID(pid[0], pid[1], pid[2], pid[3])
def _lerpPID(self, magnitude):
if magnitude <= self.slowPIDScale:
return self.slowPID
elif magnitude >= self.fastPIDScale:
return self.fastPID
else:
# 0 - 1
scale = (magnitude - self.slowPIDScale) / \
(self.fastPIDScale - self.slowPIDScale)
pidList = []
for i in range(0, 4):
slowValue = self.slowPID[i]
fastValue = self.fastPID[i]
value = (fastValue - slowValue) * scale + slowValue
pidList.append(value)
return tuple(pidList)
def _joystickPower(self, value):
newValue = float(abs(value))**float(self.joystickExponent)
if value < 0:
newValue = -newValue
return newValue
def robotInit(self):
### CONSTANTS ###
# normal speed scale, out of 1:
self.normalScale = 0.44
# speed scale when fast button is pressed:
self.fastScale = 0.9
# speed scale when slow button is pressed:
self.slowScale = 0.07
self.joystickExponent = 2
self.fastJoystickExponent = .5
self.slowJoystickExponent = 4
# if the joystick direction is within this number of radians on either
# side of straight up, left, down, or right, it will be rounded
self.driveDirectionDeadZone = math.radians(10)
# rate of increase of velocity per 1/50th of a second:
accelerationRate = .04
# PIDF values for fast driving:
self.fastPID = (1.0, 0.0009, 3.0, 0.0)
# speed at which fast PID's should be used:
self.fastPIDScale = 0.15
# PIDF values for slow driving:
self.slowPID = (30.0, 0.0009, 3.0, 0.0)
# speed at which slow PID's should be used:
self.slowPIDScale = 0.01
pidLookBackRange = 10
### END OF CONSTANTS ###
### FLAGS ###
self.fieldOriented = True
self.pidLogEnabled = False
self.currentLogEnabled = True
### END OF FLAGS ###
self.gamepad = Gamepad(port=0)
fl = wpilib.CANTalon(2)
fr = wpilib.CANTalon(1)
bl = wpilib.CANTalon(0)
br = wpilib.CANTalon(3)
self.talons = [fl, fr, bl, br]
for talon in self.talons:
talon.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.driveModeLog = LogState("Drive mode")
self.currentPID = None
self.pidLog = LogState("Drive PID")
self._setPID(self.fastPID)
self.driveScales = [0.0 for i in range(0, pidLookBackRange)]
# 2833 ticks per wheel rotation
# encoder has 100 raw ticks -- with a QuadEncoder that makes 400 ticks
# the motor gear has 12 teeth and the wheel has 85 teeth
# 85 / 12 * 400 = 2833.333 = ~2833
self.holoDrive = HolonomicDrive(fl, fr, bl, br, 2833)
self.holoDrive.invertDrive(True)
self.holoDrive.setWheelOffset(math.radians(45.0)) #angle of rollers
self.filterDrive = AccelerationFilterDrive(self.holoDrive,
accelerationRate)
if self.fieldOriented:
self.ahrs = AHRS.create_spi() # the NavX
self.drive = FieldOrientedDrive(self.filterDrive,
self.ahrs,
offset=0)
else:
self.drive = self.filterDrive
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
self.pdp = wpilib.PowerDistributionPanel()
self.currentLog = LogState("Current")
class DriveBot(Module):
def robotInit(self):
### CONSTANTS ###
# normal speed scale, out of 1:
self.normalScale = 0.44
# speed scale when fast button is pressed:
self.fastScale = 0.9
# speed scale when slow button is pressed:
self.slowScale = 0.07
self.joystickExponent = 2
self.fastJoystickExponent = .5
self.slowJoystickExponent = 4
# if the joystick direction is within this number of radians on either
# side of straight up, left, down, or right, it will be rounded
self.driveDirectionDeadZone = math.radians(10)
# rate of increase of velocity per 1/50th of a second:
accelerationRate = .04
# PIDF values for fast driving:
self.fastPID = (1.0, 0.0009, 3.0, 0.0)
# speed at which fast PID's should be used:
self.fastPIDScale = 0.15
# PIDF values for slow driving:
self.slowPID = (30.0, 0.0009, 3.0, 0.0)
# speed at which slow PID's should be used:
self.slowPIDScale = 0.01
pidLookBackRange = 10
### END OF CONSTANTS ###
### FLAGS ###
self.fieldOriented = True
self.pidLogEnabled = False
self.currentLogEnabled = True
### END OF FLAGS ###
self.gamepad = Gamepad(port=0)
fl = wpilib.CANTalon(2)
fr = wpilib.CANTalon(1)
bl = wpilib.CANTalon(0)
br = wpilib.CANTalon(3)
self.talons = [fl, fr, bl, br]
for talon in self.talons:
talon.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.driveModeLog = LogState("Drive mode")
self.currentPID = None
self.pidLog = LogState("Drive PID")
self._setPID(self.fastPID)
self.driveScales = [0.0 for i in range(0, pidLookBackRange)]
# 2833 ticks per wheel rotation
# encoder has 100 raw ticks -- with a QuadEncoder that makes 400 ticks
# the motor gear has 12 teeth and the wheel has 85 teeth
# 85 / 12 * 400 = 2833.333 = ~2833
self.holoDrive = HolonomicDrive(fl, fr, bl, br, 2833)
self.holoDrive.invertDrive(True)
self.holoDrive.setWheelOffset(math.radians(45.0)) #angle of rollers
self.filterDrive = AccelerationFilterDrive(self.holoDrive,
accelerationRate)
if self.fieldOriented:
self.ahrs = AHRS.create_spi() # the NavX
self.drive = FieldOrientedDrive(self.filterDrive,
self.ahrs,
offset=0)
else:
self.drive = self.filterDrive
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
self.pdp = wpilib.PowerDistributionPanel()
self.currentLog = LogState("Current")
def teleopInit(self):
print("DRIVE GAMEPAD:")
print(" Left Trigger: Slower")
print(" Right Trigger: Faster")
print(" A: Voltage mode")
print(" B: Speed mode")
print(" X: Position mode")
self.holoDrive.zeroEncoderTargets()
if self.fieldOriented:
self.drive.zero()
def autonomousInit(self):
self.holoDrive.zeroEncoderTargets()
if self.fieldOriented:
self.drive.zero()
def teleopPeriodic(self):
# change drive mode with A, B, and X
if self.gamepad.getRawButton(Gamepad.A):
self.drive.setDriveMode(DriveInterface.DriveMode.VOLTAGE)
elif self.gamepad.getRawButton(Gamepad.B):
self.drive.setDriveMode(DriveInterface.DriveMode.SPEED)
elif self.gamepad.getRawButton(Gamepad.X):
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
self.driveModeLog.update(self._driveModeName(
self.drive.getDriveMode()))
scale = self.normalScale
turnScale = self.normalScale
exponent = self.joystickExponent
if self.gamepad.getRawButton(Gamepad.RT): # faster button
scale = self.fastScale
exponent = self.fastJoystickExponent
if self.gamepad.getRawButton(Gamepad.LT): # slower button
scale = self.slowScale
turnScale = self.slowScale
exponent = self.slowJoystickExponent
turn = self._joystickPower(-self.gamepad.getRX(), exponent) * turnScale
magnitude = self._joystickPower(self.gamepad.getLMagnitude(),
exponent) * scale
direction = self.gamepad.getLDirection()
# constrain direction to be between 0 and 2pi
if direction < 0:
circles = math.ceil(-direction / (math.pi * 2))
direction += circles * math.pi * 2
direction %= math.pi * 2
direction = self.roundDirection(direction, 0)
direction = self.roundDirection(direction, math.pi / 2.0)
direction = self.roundDirection(direction, math.pi)
direction = self.roundDirection(direction, 3.0 * math.pi / 2.0)
direction = self.roundDirection(direction, math.pi * 2)
self.drive.drive(magnitude, direction, turn)
driveScale = max(self.filterDrive.getFilteredMagnitude(),
abs(self.filterDrive.getFilteredTurn() * 2))
self.driveScales.append(driveScale)
self.driveScales.pop(0)
self._setPID(self._lerpPID(max(self.driveScales)))
if self.currentLogEnabled:
self.currentLog.update(
self.pdp.getCurrent(12) + # drive motors
self.pdp.getCurrent(13) + self.pdp.getCurrent(14) +
self.pdp.getCurrent(15) + self.pdp.getCurrent(3) # climber
)
def _driveModeName(self, driveMode):
if driveMode == DriveInterface.DriveMode.VOLTAGE:
return "Voltage"
if driveMode == DriveInterface.DriveMode.SPEED:
return "Speed"
if driveMode == DriveInterface.DriveMode.POSITION:
return "Position"
return "Unknown"
def _setPID(self, pid):
if self.pidLogEnabled:
self.pidLog.update(pid)
if pid == self.currentPID:
return
self.currentPID = pid
for talon in self.talons:
talon.setPID(pid[0], pid[1], pid[2], pid[3])
def _lerpPID(self, magnitude):
if magnitude <= self.slowPIDScale:
return self.slowPID
elif magnitude >= self.fastPIDScale:
return self.fastPID
else:
# 0 - 1
scale = (magnitude - self.slowPIDScale) / \
(self.fastPIDScale - self.slowPIDScale)
pidList = []
for i in range(0, 4):
slowValue = self.slowPID[i]
fastValue = self.fastPID[i]
value = (fastValue - slowValue) * scale + slowValue
pidList.append(value)
return tuple(pidList)
def _joystickPower(self, value, exponent):
newValue = float(abs(value))**float(exponent)
if value < 0:
newValue = -newValue
return newValue
def roundDirection(self, value, target):
if abs(value - target) <= self.driveDirectionDeadZone:
return target
else:
return value