def robotInit(self):
self.gamepad = Gamepad(port=1)
self.sh = wpilib.CANTalon(5)
self.it = wpilib.CANTalon(6)
self.flywheelSpeedLog = LogState("Flywheel speed")
def autonomousInit(self):
self.vision = vision.Vision()
self.turnAlignLog = LogState("Turn align")
self.targetWidthLog = LogState("Target width")
self.currentSpeed = 0
if dashboard.getSwitch("Drive voltage mode", False):
self.holoDrive.setDriveMode(DriveInterface.DriveMode.VOLTAGE)
else:
self.holoDrive.setDriveMode(DriveInterface.DriveMode.POSITION)
def __init__(self, drive, vision):
super().__init__()
self.drive = drive
self.vision = vision
if TurnAlignCommand.log == None:
TurnAlignCommand.log = LogState("Turn alignment")
def __init__(self, drive, vision):
super().__init__()
self.drive = drive
self.vision = vision
self.tolerance = .02 # fraction of width
if StrafeAlignCommand.log == None:
StrafeAlignCommand.log = LogState("Strafe alignment")
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 __init__(self, channels):
"""
``channels`` is a list of channel numbers (0 - 15) to monitor.
"""
self.channels = channels
self.pdp = wpilib.PowerDistributionPanel()
# updates every 0.8 seconds
self.logStates = [LogState("PDP " + str(c), 0.8) for c in channels]
def __init__(self, ports):
"""
ports is an array of integers for the CANTalon ports to test
"""
super().__init__()
self.talons = [wpilib.CANTalon(p) for p in ports]
self.logStates = [LogState("Talon " + str(p)) for p in ports]
for t in self.talons:
t.setFeedbackDevice(wpilib.CANTalon.FeedbackDevice.QuadEncoder)
def __init__(self):
self.flywheelMotor = wpilib.CANTalon(5)
self.speedVoltage = .76
self.speedSpeed = 21000
# encoder resolution is 512 (* 4)
self.flywheelMotor.setPID(0.15, 0.0, 5.0, 0)
self.flywheelMotor.setFeedbackDevice(
wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.switchSpeedMode()
self.flywheelMotor.changeControlMode(
wpilib.CANTalon.ControlMode.PercentVbus)
self.talonSpeedModeEnabled = False
self.voltageModeStartupCount = 0
self.speedLog = LogState("Flywheel speed")
self.controlModeLog = LogState("Flywheel mode")
class Test(wpilib.IterativeRobot):
def robotInit(self):
self.ahrs = AHRS.create_spi() # the NavX
#self.ahrs = AHRS.create_i2c() # other communication protocol option
self.yawLog = LogState("Yaw")
self.angleLog = LogState("Angle")
#wpilib.SmartDashboard.putString("thisIsAKey", "this is a value!")
def teleopPeriodic(self):
# angles are floats, in degrees clockwise
# yaw ranges from -180 to 180
self.yawLog.update(self.ahrs.getYaw())
# angle starts at 0 and accumulates rotations. After 2 full rotations
# clockwise it will be 720
self.angleLog.update(self.ahrs.getAngle())
def __init__(self, drive, ahrs, offset=0):
super().__init__()
self.drive = drive
self.ahrs = ahrs
self.offsetAmount = offset
# prevent isFinished() returning True
self.origin = self._getYawRadians() + math.pi * 2
if StaticRotationCommand.log == None:
StaticRotationCommand.log = LogState("Rotation offset")
def __init__(self):
self.flywheelMotors = [ctre.CANTalon(5), ctre.CANTalon(6)]
self.speedVoltage = .5
self.speedSpeed = 27632 * self.speedVoltage
# encoder resolution is 512 (* 4)
for motor in self.flywheelMotors:
motor.setPID(0.15, 0.0, 5.0, 0)
motor.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
for motor in self.flywheelMotors:
motor.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.talonSpeedModeEnabled = False
self.voltageModeStartupCount = 0
self.speedLog = LogState("Flywheel speed")
self.controlModeLog = LogState("Flywheel mode")
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()
def __init__(self, drive, vision, buffer=21.0):
super().__init__()
self.drive = drive
self.visionary = vision
self.buffer = buffer #inches
self.tolerance = 1
self.pegFocalDistance = 661.96
self.pegRealTargetDistance = 8.25
# prevent isFinished() from returning True
self.distance = self.buffer + self.tolerance + 1
if DriveToTargetDistanceCommand.log == None:
DriveToTargetDistanceCommand.log = LogState("Distance offset")
def robotInit(self):
self.gamepad = seamonsters.gamepad.globalGamepad(port=0)
self.climberMotor = ctre.CANTalon(4)
self.lockLog = LogState("Climber lock mode")
self.statusLog = LogState("Climber status")
self.currentLog = LogState("Climber current")
#self.encoderLog = LogState("Climber encoder")
self.encoderLog = None
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")
class Climber(Module):
def lock(self):
if not self.locked:
self.locked = True
self.climberMotor.changeControlMode(
ctre.CANTalon.ControlMode.Position)
self.climberMotor.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.climberMotor.setPID(1.0, 0.0, 3.0, 0.0)
self.lockPosition = self.climberMotor.getPosition()
self.climberMotor.set(self.lockPosition)
def unlock(self):
if self.locked:
self.locked = False
self.climberMotor.changeControlMode(
ctre.CANTalon.ControlMode.PercentVbus)
def robotInit(self):
self.gamepad = seamonsters.gamepad.globalGamepad(port=0)
self.climberMotor = ctre.CANTalon(4)
self.lockLog = LogState("Climber lock mode")
self.statusLog = LogState("Climber status")
self.currentLog = LogState("Climber current")
#self.encoderLog = LogState("Climber encoder")
self.encoderLog = None
def teleopInit(self):
print(" Right trigger: Climb up")
print(" Left trigger: Climb down")
print(" Right bumper: Climb up slowly (lock mode only)")
print(" Left bumper: Climb down slowly")
print(" B: Lock motor")
print(" X: Unlock motor")
self.locked = False
self.lockmode = False
self.enabled = True
self.lockPosition = None
self.climberMotor.changeControlMode(
ctre.CANTalon.ControlMode.PercentVbus)
def teleopPeriodic(self):
if self.gamepad.getRawButton(Gamepad.B):
if not self.lockmode:
self.lockmode = True
#When the A button is pressed the motor locks, so the robot wont fall down the rope
if self.gamepad.getRawButton(Gamepad.X):
if self.lockmode:
self.lockmode = False
self.enabled = True
#When the Right Joystick is pressed down the lockmode is disabled
if not self.enabled:
self.lockLog.update("Climber disabled!")
self.statusLog.update("Locked, disabled!")
elif self.lockmode:
self.lockLog.update("On!")
else:
self.lockLog.update("Off")
climbSpeed = self.gamepad.getRTrigger() - self.gamepad.getLTrigger()
if abs(climbSpeed) < 0.05:
climbSpeed = 0
if climbSpeed == 0 and self.lockmode:
self.lock()
if self.enabled:
self.statusLog.update("Locked!")
if self.gamepad.getRawButton(Gamepad.RB):
self.lockPosition += 1500
elif self.gamepad.getRawButton(Gamepad.LB):
self.lockPosition -= 1500
elif self.enabled:
self.unlock()
self.climberMotor.set(climbSpeed)
self.statusLog.update("Unlocked")
self.currentLog.update(self.climberMotor.getOutputCurrent())
#if self.climberMotor.getOutputCurrent() >= 200:
# self.lock()
# self.enabled = False
if self.encoderLog != None:
self.encoderLog.update(self.climberMotor.getPosition())
class Flywheels:
def __init__(self):
self.flywheelMotor = wpilib.CANTalon(5)
self.speedVoltage = .76
self.speedSpeed = 21000
# encoder resolution is 512 (* 4)
self.flywheelMotor.setPID(0.15, 0.0, 5.0, 0)
self.flywheelMotor.setFeedbackDevice(
wpilib.CANTalon.FeedbackDevice.QuadEncoder)
self.switchSpeedMode()
self.flywheelMotor.changeControlMode(
wpilib.CANTalon.ControlMode.PercentVbus)
self.talonSpeedModeEnabled = False
self.voltageModeStartupCount = 0
self.speedLog = LogState("Flywheel speed")
self.controlModeLog = LogState("Flywheel mode")
def switchSpeedMode(self):
self.speedModeEnabled = True
def switchVoltageMode(self):
self.speedModeEnabled = False
def inSpeedMode(self):
return self.speedModeEnabled
def _talonSpeedMode(self):
if not self.talonSpeedModeEnabled:
self.talonSpeedModeEnabled = True
self.flywheelMotor.changeControlMode(
wpilib.CANTalon.ControlMode.Speed)
def _talonVoltageMode(self):
if self.talonSpeedModeEnabled:
self.talonSpeedModeEnabled = False
self.flywheelMotor.changeControlMode(
wpilib.CANTalon.ControlMode.PercentVbus)
def _updateLogs(self):
self.speedLog.update(-self.flywheelMotor.getEncVelocity())
if self.speedModeEnabled:
self.controlModeLog.update("Speed")
else:
self.controlModeLog.update("Voltage!")
def spinFlywheels(self):
if self.speedModeEnabled:
if self.voltageModeStartupCount < 50:
self._talonVoltageMode()
self.flywheelMotor.set(-self.speedVoltage)
else:
self._talonSpeedMode()
self.flywheelMotor.set(-self.speedSpeed)
self.voltageModeStartupCount += 1
else:
self._talonVoltageMode()
self.flywheelMotor.set(-self.speedVoltage)
self._updateLogs()
def stopFlywheels(self):
self._talonVoltageMode()
self.flywheelMotor.set(0)
self.voltageModeStartupCount = 0
self._updateLogs()
def reverseFlywheels(self):
self._talonVoltageMode()
self.flywheelMotor.set(self.speedVoltage / 2)
self.voltageModeStartupCount = 0
self._updateLogs()
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 Flywheels:
def __init__(self):
self.flywheelMotors = [ctre.CANTalon(5), ctre.CANTalon(6)]
self.speedVoltage = .5
self.speedSpeed = 27632 * self.speedVoltage
# encoder resolution is 512 (* 4)
for motor in self.flywheelMotors:
motor.setPID(0.15, 0.0, 5.0, 0)
motor.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
for motor in self.flywheelMotors:
motor.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
self.talonSpeedModeEnabled = False
self.voltageModeStartupCount = 0
self.speedLog = LogState("Flywheel speed")
self.controlModeLog = LogState("Flywheel mode")
def switchSpeedMode(self):
self.speedModeEnabled = True
def switchVoltageMode(self):
self.speedModeEnabled = False
def inSpeedMode(self):
return self.speedModeEnabled
def _talonSpeedMode(self):
if not self.talonSpeedModeEnabled:
self.talonSpeedModeEnabled = True
for motor in self.flywheelMotors:
motor.changeControlMode(ctre.CANTalon.ControlMode.Speed)
def _talonVoltageMode(self):
if self.talonSpeedModeEnabled:
self.talonSpeedModeEnabled = False
for motor in self.flywheelMotors:
motor.changeControlMode(ctre.CANTalon.ControlMode.PercentVbus)
def _updateLogs(self):
self.speedLog.update(
str(-self.flywheelMotors[0].getEncVelocity()) + ", " +
str(-self.flywheelMotors[1].getEncVelocity()))
if self.speedModeEnabled:
self.controlModeLog.update("Speed")
else:
self.controlModeLog.update("Voltage!")
def spinFlywheels(self):
if self.speedModeEnabled:
if self.voltageModeStartupCount < 50:
self._talonVoltageMode()
for motor in self.flywheelMotors:
motor.set(-self.speedVoltage)
else:
self._talonSpeedMode()
for motor in self.flywheelMotors:
motor.set(-self.speedSpeed)
self.voltageModeStartupCount += 1
else:
self._talonVoltageMode()
for motor in self.flywheelMotors:
motor.set(-self.speedVoltage)
self._updateLogs()
def stopFlywheels(self):
self._talonVoltageMode()
for motor in self.flywheelMotors:
motor.set(0)
self.voltageModeStartupCount = 0
self._updateLogs()
def reverseFlywheels(self):
self._talonVoltageMode()
for motor in self.flywheelMotors:
motor.set(self.speedVoltage / 2)
self.voltageModeStartupCount = 0
self._updateLogs()
def robotInit(self):
### CONSTANTS ###
# 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 = 1.0
# PIDF values for fast driving:
fastPID = (1.0, 0.0009, 3.0, 0.0)
# speed at which fast PID's should be used:
fastPIDScale = 0.09
# PIDF values for slow driving:
slowPID = (30.0, 0.0009, 3.0, 0.0)
# speed at which slow PID's should be used:
slowPIDScale = 0.01
pidLookBackRange = 10
self.maxVelocity = 650
### END OF CONSTANTS ###
self.driverGamepad = seamonsters.gamepad.globalGamepad(port=0)
fl = ctre.CANTalon(2)
fr = ctre.CANTalon(1)
bl = ctre.CANTalon(0)
br = ctre.CANTalon(3)
self.talons = [fl, fr, bl, br]
for talon in self.talons:
talon.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.driveModeLog = LogState("Drive mode")
self._setPID(fastPID)
# 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
ticksPerWheelRotation = 2833
self.holoDrive = HolonomicDrive(fl, fr, bl, br, ticksPerWheelRotation)
self.holoDrive.invertDrive(True)
self.holoDrive.setWheelOffset(math.radians(45.0)) # angle of rollers
self.pidDrive = DynamicPIDDrive(self.holoDrive, self.talons, slowPID,
slowPIDScale, fastPID, fastPIDScale,
pidLookBackRange)
self.filterDrive = AccelerationFilterDrive(self.pidDrive,
accelerationRate)
self.ahrs = AHRS.create_spi() # the NavX
self.fieldDrive = FieldOrientedDrive(self.filterDrive,
self.ahrs,
offset=0)
self.fieldDrive.zero()
self.fieldDriveLog = LogState("Field oriented")
self.pdp = wpilib.PowerDistributionPanel()
self.currentLog = LogState("Drive current", logFrequency=2.0)
self.encoderLog = LogState("Wheel encoders")
self.speedLog = LogState("Wheel speeds")
if self.pdp.getVoltage() < 12:
print("Battery Level below 12 volts!!!")
class DriveBot(Module):
def robotInit(self):
### CONSTANTS ###
# 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 = 1.0
# PIDF values for fast driving:
fastPID = (1.0, 0.0009, 3.0, 0.0)
# speed at which fast PID's should be used:
fastPIDScale = 0.09
# PIDF values for slow driving:
slowPID = (30.0, 0.0009, 3.0, 0.0)
# speed at which slow PID's should be used:
slowPIDScale = 0.01
pidLookBackRange = 10
self.maxVelocity = 650
### END OF CONSTANTS ###
self.driverGamepad = seamonsters.gamepad.globalGamepad(port=0)
fl = ctre.CANTalon(2)
fr = ctre.CANTalon(1)
bl = ctre.CANTalon(0)
br = ctre.CANTalon(3)
self.talons = [fl, fr, bl, br]
for talon in self.talons:
talon.setFeedbackDevice(ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.driveModeLog = LogState("Drive mode")
self._setPID(fastPID)
# 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
ticksPerWheelRotation = 2833
self.holoDrive = HolonomicDrive(fl, fr, bl, br, ticksPerWheelRotation)
self.holoDrive.invertDrive(True)
self.holoDrive.setWheelOffset(math.radians(45.0)) # angle of rollers
self.pidDrive = DynamicPIDDrive(self.holoDrive, self.talons, slowPID,
slowPIDScale, fastPID, fastPIDScale,
pidLookBackRange)
self.filterDrive = AccelerationFilterDrive(self.pidDrive,
accelerationRate)
self.ahrs = AHRS.create_spi() # the NavX
self.fieldDrive = FieldOrientedDrive(self.filterDrive,
self.ahrs,
offset=0)
self.fieldDrive.zero()
self.fieldDriveLog = LogState("Field oriented")
self.pdp = wpilib.PowerDistributionPanel()
self.currentLog = LogState("Drive current", logFrequency=2.0)
self.encoderLog = LogState("Wheel encoders")
self.speedLog = LogState("Wheel speeds")
if self.pdp.getVoltage() < 12:
print("Battery Level below 12 volts!!!")
def autonomousInit(self):
self.vision = vision.Vision()
self.turnAlignLog = LogState("Turn align")
self.targetWidthLog = LogState("Target width")
self.currentSpeed = 0
if dashboard.getSwitch("Drive voltage mode", False):
self.holoDrive.setDriveMode(DriveInterface.DriveMode.VOLTAGE)
else:
self.holoDrive.setDriveMode(DriveInterface.DriveMode.POSITION)
def autonomousPeriodic(self):
contours = self.vision.getContours()
targetCenter = vision.Vision.targetCenter(contours)
targetDimensions = vision.Vision.targetDimensions(contours)
if targetCenter == None or targetDimensions == None:
print("No vision!!")
self.filterDrive.drive(0, 0, 0)
return
targetX = float(targetCenter[0]) / float(vision.Vision.WIDTH)
centerDistance = targetX - vision.Vision.CENTER
self.turnAlignLog.update("{0:.5f}".format(centerDistance))
turnAmount = centerDistance * abs(centerDistance) * 0.7
targetWidth = float(targetDimensions[0]) / float(vision.Vision.WIDTH)
self.targetWidthLog.update("{0:.5f}".format(targetWidth))
distance = 0.55 - targetWidth
driveSpeed = distance * abs(distance) * 1.5
if driveSpeed > 0.25:
driveSpeed = 0.25
if driveSpeed < -0.2:
driveSpeed = -0.2
self.currentSpeed += (driveSpeed - self.currentSpeed) / 3
self.filterDrive.drive(driveSpeed, math.pi / 2, -turnAmount)
def teleopInit(self):
print("DRIVE GAMEPAD:")
print(" Left Joystick: Strafe/Drive")
print(" Right Joystick: Turn")
print(" Left Joystick Button: Slower")
print(" Right Joystick Button: Faster")
print(" A: Brake")
print(" Y: Shake")
print(" Start: Position Mode")
print(" Back: Voltage mode")
self.holoDrive.zeroEncoderTargets()
self.holoDrive.setMaxVelocity(self.maxVelocity)
self.count = 0
self.teleopCommand = None
if dashboard.getSwitch("Field oriented drive", False):
self.drive = self.fieldDrive
else:
self.drive = self.filterDrive
if dashboard.getSwitch("Drive voltage mode", False):
self.holoDrive.setDriveMode(DriveInterface.DriveMode.VOLTAGE)
else:
self.holoDrive.setDriveMode(DriveInterface.DriveMode.POSITION)
if dashboard.getSwitch("Slow driving", True):
self.normalScale = 0.15
self.fastScale = 0.15
self.slowScale = 0.07
self.normalTurnScale = 0.15
self.fastTurnScale = 0.2
self.joystickExponent = 2
self.fastJoystickExponent = 2
self.slowJoystickExponent = 4
else:
# normal speed scale, out of 1:
self.normalScale = 0.37
# speed scale when fast button is pressed:
self.fastScale = 1.0
# speed scale when slow button is pressed:
self.slowScale = 0.07
# normal turning speed scale:
self.normalTurnScale = 0.25
# turning speed scale when fast button is pressed
self.fastTurnScale = 0.34
self.joystickExponent = 2
self.fastJoystickExponent = .5
self.slowJoystickExponent = 4
self.wheelsLocked = False
self.encoderLoggingEnabled = dashboard.getSwitch(
"Encoder logging", False)
def teleopPeriodic(self):
self.count = self.count + 1
current = 0
for talon in self.talons:
current += talon.getOutputCurrent()
if current > 50:
self.currentLog.update(str(current) + "!")
else:
self.currentLog.update(current)
if self.encoderLoggingEnabled:
encoderLogText = ""
for talon in self.talons:
encoderLogText += str(talon.getPosition()) + " "
self.encoderLog.update(encoderLogText)
speedLogText = ""
for talon in self.talons:
speedLogText += str(talon.getEncVelocity()) + " "
self.speedLog.update(speedLogText)
# change drive mode with back and start
if self.driverGamepad.getRawButton(Gamepad.BACK):
self.drive.setDriveMode(DriveInterface.DriveMode.VOLTAGE)
elif self.driverGamepad.getRawButton(Gamepad.START):
self.drive.setDriveMode(DriveInterface.DriveMode.POSITION)
self.driveModeLog.update(self._driveModeName(
self.drive.getDriveMode()))
if self.driverGamepad.getRawButton(Gamepad.A) and \
self.driverGamepad.getLMagnitude() == 0 and \
self.driverGamepad.getRX() == 0:
# lock wheels and don't allow driving
if not self.wheelsLocked:
print("Locking wheels")
for talon in self.talons:
talon.enable()
talon.changeControlMode(ctre.CANTalon.ControlMode.Position)
talon.set(talon.getPosition())
talon.setPID(30.0, 0.0009, 3.0, 0.0)
self.wheelsLocked = True
return
else:
self.wheelsLocked = False
if self.driverGamepad.getRawButton(Gamepad.RB) \
and self.driverGamepad.getRawButton(Gamepad.LB):
print("Zero field oriented.")
self.fieldDrive.zero()
if self.drive is self.fieldDrive:
self.fieldDriveLog.update("Enabled")
else:
self.fieldDriveLog.update("Disabled")
scale = self.normalScale
turnScale = self.normalTurnScale
exponent = self.joystickExponent
if self.driverGamepad.getRawButton(Gamepad.RJ): # faster button
scale = self.fastScale
turnScale = self.fastTurnScale
exponent = self.fastJoystickExponent
if self.driverGamepad.getRawButton(Gamepad.LJ): # slower button
scale = self.slowScale
turnScale = self.slowScale
exponent = self.slowJoystickExponent
turn = self._joystickPower(-self.driverGamepad.getRX(),
exponent) * turnScale
magnitude = self._joystickPower(self.driverGamepad.getLMagnitude(),
exponent) * scale
direction = self.driverGamepad.getLDirection()
accelFilter = True
if self.driverGamepad.getRawButton(Gamepad.Y):
magnitude = 1
if self.count % 10 >= 5:
direction = math.pi
else:
direction = 0
accelFilter = False
if accelFilter:
self.drive.drive(magnitude, direction, turn)
else:
self.pidDrive.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):
for talon in self.talons:
talon.setPID(pid[0], pid[1], pid[2], pid[3])
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
def robotInit(self):
self.ahrs = AHRS.create_spi() # the NavX
#self.ahrs = AHRS.create_i2c() # other communication protocol option
self.yawLog = LogState("Yaw")
self.angleLog = LogState("Angle")
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
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
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 Climber(Module):
def lock(self):
if not self.locked:
self.locked = True
self.climberMotor.changeControlMode(
wpilib.CANTalon.ControlMode.Position)
self.climberMotor.setFeedbackDevice(
wpilib.CANTalon.FeedbackDevice.QuadEncoder)
if config.PRACTICE_BOT:
self.climberMotor.setPID(5.0, 0.0, 3.0, 0.0)
else:
self.climberMotor.setPID(1.0, 0.0, 3.0, 0.0)
self.lockPosition = self.climberMotor.getPosition()
self.climberMotor.set(self.lockPosition)
def unlock(self):
if self.locked:
self.locked = False
self.climberMotor.changeControlMode(
wpilib.CANTalon.ControlMode.PercentVbus)
def robotInit(self):
self.secondaryGamepad = seamonsters.gamepad.globalGamepad(port=1)
self.climberMotor = wpilib.CANTalon(4)
self.lockLog = LogState("Climber lock mode")
self.statusLog = LogState("Climber status")
self.currentLog = LogState("Climber current")
#self.encoderLog = LogState("Climber encoder")
self.encoderLog = None
def teleopInit(self):
print("SPECIAL GAMEPAD:")
print(" Y: Shake robot")
print(" Left Joystick up: Climb")
print(" Left Joystick down: Descend")
print(" Right Joystick: Slow mode (lock mode only)")
print(" A: Lock motor")
print(" Press down Right Joystick: Unlock motor")
self.locked = False
self.lockmode = False
self.enabled = True
self.lockPosition = None
self.climberMotor.changeControlMode(
wpilib.CANTalon.ControlMode.PercentVbus)
def teleopPeriodic(self):
if self.secondaryGamepad.getRawButton(Gamepad.A):
if not self.lockmode:
self.lockmode = True
#When the A button is pressed the motor locks, so the robot wont fall down the rope
if self.secondaryGamepad.getRawButton(Gamepad.RJ):
if self.lockmode:
self.lockmode = False
self.enabled = True
#When the Right Joystick is pressed down the lockmode is disabled
if not self.enabled:
self.lockLog.update("Climber disabled!")
self.statusLog.update("Locked, disabled!")
elif self.lockmode:
self.lockLog.update("On!")
else:
self.lockLog.update("Off")
if self.secondaryGamepad.getLY() == 0 and self.lockmode:
self.lock()
if self.enabled:
self.statusLog.update("Locked!")
self.lockPosition += self.secondaryGamepad.getRY() * 1000
elif self.enabled:
self.unlock()
self.climberMotor.set(self.secondaryGamepad.getLY())
self.statusLog.update("Unlocked")
self.currentLog.update(self.climberMotor.getOutputCurrent())
#if self.climberMotor.getOutputCurrent() >= 200:
# self.lock()
# self.enabled = False
if self.encoderLog != None:
self.encoderLog.update(self.climberMotor.getPosition())
def disabledInit(self):
pass