def __init__(self):
# Initialize all controllers
self.driveLeftMaster = Talon(kDriveTrain.lmId)
self.driveLeftSlave = Talon(kDriveTrain.lsId)
self.driveRightMaster = Talon(kDriveTrain.rmId)
self.driveRightSlave = Talon(kDriveTrain.rsId)
# Connect the slaves to the masters on each side
self.driveLeftSlave.follow(self.driveLeftMaster)
self.driveRightSlave.follow(self.driveRightMaster)
# Makes sure both sides' controllers show green and use positive
# values to move the bot forward.
# CURRENTLY DISABLED WHEN USING WITH DifferentialDrive
# self.driveLeftSlave.setInverted(False)
# self.driveLeftMaster.setInverted(False)
self.driveRightSlave.setInverted(True)
self.driveRightMaster.setInverted(True)
# Configures each master to use the attached Mag Encoders
self.driveLeftMaster.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.driveRightMaster.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
# Reverses the encoder direction so forward movement always
# results in a positive increase in the encoder ticks.
self.driveLeftMaster.setSensorPhase(True)
self.driveRightMaster.setSensorPhase(True)
# Throw data on the SmartDashboard so we can work with it.
SD.putNumber('Left Quad Pos.',
self.driveLeftMaster.getQuadraturePosition())
SD.putNumber('Right Quad Pos.',
self.driveRightMaster.getQuadraturePosition())
# these may give the derivitive an integral of the PID once
# they are set. For now, they just show 0
SD.putNumber('Left Derivative',
self.driveLeftMaster.getErrorDerivative(0))
SD.putNumber('Left Integral',
self.driveLeftMaster.getIntegralAccumulator(0))
self.leftVel = None
self.leftPos = None
self.rightVel = None
self.rightPos = None
# self.driveLeftMaster.config_kP(0, .3, 10)
# kP = self.driveLeftMaster.configGetParameter(
# self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)
# SmartDashboard.putNumber('Left Proportional', kP)
self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
self.driveControllerRight.setInverted(True)
self.drive = DifferentialDrive(self.driveControllerLeft,
self.driveControllerRight)
def __init__(self, robot):
self.robot = robot
self.leftArm = Talon(self.robot.kCubeGrabber['left_arm'])
self.rightArm = Talon(self.robot.kCubeGrabber['right_arm'])
self.armUS = wpilib.AnalogInput(self.robot.kCubeGrabber['ultra_sonic'])
self.armSwitch = wpilib.DigitalInput(self.robot.kCubeGrabber['switch'])
self.armClosePosition = self.robot.kCubeGrabber['close']
self.armOpenPosition = self.robot.kCubeGrabber['open']
self.driverTwo = self.robot.cStick
self.armSolenoid = wpilib.Solenoid(self.robot.pneumaticControlModuleCANID, self.robot.kCubeGrabber['solenoid'])
"""
Initializes the predetermined distances for grabber/cube interaction.
"""
self.spinDistance = 12
self.closeDistance = 7
"""
Initializes Toggle Counts for the arm functionality.
"""
self.armModeToggleCount = 2
self.openToggleCount = 3
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
SmartDashboard.putNumber("Gravity Power", 0)
self.F = 0.25
SmartDashboard.putNumber("F Gain", self.F)
#self.angle = 0
#SmartDashboard.putNumber("angle", self.angle)
self.range = -1200
self.povPressed = False
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
SmartDashboard.putNumber("Wrist Up Volts", self.wristUpVolts)
SmartDashboard.putNumber("Wrist Down Volts", self.wristDownVolts)
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.gPower = 0
self.input = 0
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
[self.kP, self.kI, self.kD] = [0, 0, 0]
cargoController = PIDController(self.kP,
self.kI,
self.kD,
source=self.getAngle,
output=self.__setGravity__)
self.cargoController = cargoController
self.cargoController.disable()
def initialize(self):
timeout = 15
self.wristPowerSet = 0
SmartDashboard.putNumber("Wrist Power Set", self.wristPowerSet)
self.maxVolts = 10
self.wristUpVolts = 4
self.wristDownVolts = -4
self.xbox = oi.getJoystick(2)
self.out = 0
#below is the talon on the intake
self.intake = Talon(map.intake)
self.intake.setNeutralMode(2)
self.intake.configVoltageCompSaturation(self.maxVolts)
self.intake.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.intake.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.intake.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.intake.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setNeutralMode(2)
self.wrist.configClearPositionOnLimitF(True)
self.wrist.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wrist.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wrist.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wrist.enableCurrentLimit(True)
self.bottomSwitch = DI(map.bottomSwitch)
self.topSwitch = DI(map.topSwitch)
def initialize(self):
timeout = 15
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.motor = Talon(map.intake)
self.motor.configContinuousCurrentLimit(20,timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(30,timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(100,timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.intakeSpeed = 0.9
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def __init__(self, robot):
self.robot = robot
self.lights = FROGLights()
self.climbMotor = Talon(self.robot.kClimber['climb_motor'])
self.checkSwitch = wpilib.DigitalInput(
self.robot.kClimber['check_switch'])
self.solenoid = wpilib.DoubleSolenoid(
self.robot.pneumaticControlModuleCANID,
self.robot.kClimber['solenoid'], self.robot.kClimber['solenoid_2'])
self.driverOne = self.robot.dStick
self.latchToggleCount = 2
self.handlerState = 0
self.climbMotor.configPeakOutputForward(1, 0)
self.climbMotor.configPeakOutputReverse(-1, 0)
def initialize(self):
#makes control objects
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.joystick1 = oi.getJoystick(1)
#makes solenoid objects to be used in kick and slide functions
self.kicker = Solenoid(map.hatchKick)
self.slider = Solenoid(map.hatchSlide)
self.maxVolts = 10
timeout = 0
self.wheels = Talon(map.hatchWheels)
self.wheels.setNeutralMode(2)
self.wheels.configVoltageCompSaturation(self.maxVolts)
self.wheels.configContinuousCurrentLimit(20,
timeout) #20 Amps per motor
self.wheels.configPeakCurrentLimit(
30, timeout) #30 Amps during Peak Duration
self.wheels.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.wheels.enableCurrentLimit(True)
self.wheels.setInverted(True)
self.powerIn = 0.9
self.powerOut = -0.9
#sets kicker and slide solenoids to in
self.kick("in")
self.slide("in")
#starts lastkick/slide booleans
self.lastKick = False
self.lastSlide = False
self.hasHatch = False
self.outPower = 0
def __init__(self, robot):
self.robot = robot
self.elevator = Talon(self.robot.kElevator['elevator_motor'])
self.elevator.setInverted(True)
self.driverTwo = self.robot.cStick
self.elevator.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.elevator.setSensorPhase(True)
self.elevator.setSelectedSensorPosition(0, 0, 0)
self.elevator.configPeakOutputForward(.2, 0)
self.elevator.configPeakOutputReverse(-.95, 0)
self.elevator.configNominalOutputForward(0, 0)
self.elevator.configNominalOutputReverse(0, 0)
self.elevator.setSafetyEnabled(False)
self.smartToggle = True
self.smartPosition = 1
super().__init__()
def initialize(self, robot):
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
if map.robotId == map.astroV1:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10, 0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backBottomSensor)
self.frontSwitch = DigitalInput(map.frontBottomSensor)
self.upSwitch = DigitalInput(map.upSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = 0 #degrees
self.climbSpeed = 0.5
self.wheelSpeed = 0.5
self.backHold = -0.15 #holds back stationary if extended ADJUST**
self.frontHold = -0.1 #holds front stationary if extended
self.kP = 0.4 #proportional gain for angle to power
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
def __init__(self, robot):
super().__init__('Drive')
SmartDashboard.putNumber("DriveStraight_P", 0.075)
SmartDashboard.putNumber("DriveStraight_I", 0.0)
SmartDashboard.putNumber("DriveStraight_D", 0.42)
# OLD GAINS 0.075, 0, 0.42
SmartDashboard.putNumber("DriveAngle_P", 0.009)
SmartDashboard.putNumber("DriveAngle_I", 0.0)
SmartDashboard.putNumber("DriveAngle_D", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_P", 0.025)
SmartDashboard.putNumber("DriveStraightAngle_I", 0.0)
SmartDashboard.putNumber("DriveStraightAngle_D", 0.01)
self.driveStyle = "Tank"
SmartDashboard.putString("DriveStyle", self.driveStyle)
#SmartDashboard.putData("Mode", self.mode)
self.robot = robot
self.lime = self.robot.limelight
self.nominalPID = 0.15
self.nominalPIDAngle = 0.22 # 0.11 - v2
self.preferences = Preferences.getInstance()
timeout = 0
TalonLeft = Talon(map.driveLeft1)
TalonRight = Talon(map.driveRight1)
leftInverted = True
rightInverted = False
TalonLeft.setInverted(leftInverted)
TalonRight.setInverted(rightInverted)
VictorLeft1 = Victor(map.driveLeft2)
VictorLeft2 = Victor(map.driveLeft3)
VictorLeft1.follow(TalonLeft)
VictorLeft2.follow(TalonLeft)
VictorRight1 = Victor(map.driveRight2)
VictorRight2 = Victor(map.driveRight3)
VictorRight1.follow(TalonRight)
VictorRight2.follow(TalonRight)
for motor in [VictorLeft1, VictorLeft2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(leftInverted)
for motor in [VictorRight1, VictorRight2]:
motor.clearStickyFaults(timeout)
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.setInverted(rightInverted)
for motor in [TalonLeft, TalonRight]:
motor.setSafetyEnabled(False)
#motor.setExpiration(2 * self.robot.period)
motor.clearStickyFaults(timeout) #Clears sticky faults
motor.configContinuousCurrentLimit(40, timeout) #15 Amps per motor
motor.configPeakCurrentLimit(
70, timeout) #20 Amps during Peak Duration
motor.configPeakCurrentDuration(
300, timeout) #Peak Current for max 100 ms
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(12,
timeout) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
motor.configOpenLoopRamp(0.2,
timeout) #number of seconds from 0 to 1
self.left = TalonLeft
self.right = TalonRight
self.navx = navx.AHRS.create_spi()
self.leftEncoder = Encoder(map.leftEncoder[0], map.leftEncoder[1])
self.leftEncoder.setDistancePerPulse(self.leftConv)
self.leftEncoder.setSamplesToAverage(10)
self.rightEncoder = Encoder(map.rightEncoder[0], map.rightEncoder[1])
self.rightEncoder.setDistancePerPulse(self.rightConv)
self.rightEncoder.setSamplesToAverage(10)
self.zero()
#PID for Drive
self.TolDist = 0.1 #feet
[kP, kI, kD, kF] = [0.027, 0.00, 0.20, 0.00]
if wpilib.RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.25, 0.00, 1.00, 0.00]
distController = PIDController(kP,
kI,
kD,
kF,
source=self.__getDistance__,
output=self.__setDistance__)
distController.setInputRange(0, 50) #feet
distController.setOutputRange(-0.6, 0.6)
distController.setAbsoluteTolerance(self.TolDist)
distController.setContinuous(False)
self.distController = distController
self.distController.disable()
'''PID for Angle'''
self.TolAngle = 2 #degrees
[kP, kI, kD, kF] = [0.025, 0.00, 0.01, 0.00]
if RobotBase.isSimulation():
[kP, kI, kD, kF] = [0.005, 0.0, 0.01, 0.00]
angleController = PIDController(kP,
kI,
kD,
kF,
source=self.__getAngle__,
output=self.__setAngle__)
angleController.setInputRange(-180, 180) #degrees
angleController.setOutputRange(-0.5, 0.5)
angleController.setAbsoluteTolerance(self.TolAngle)
angleController.setContinuous(True)
self.angleController = angleController
self.angleController.disable()
self.k = 1
self.sensitivity = 1
SmartDashboard.putNumber("K Value", self.k)
SmartDashboard.putNumber("sensitivity", self.sensitivity)
self.prevLeft = 0
self.prevRight = 0
def initialize(self, robot):
self.state = -1
self.robot = robot
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.usingNeo = True
self.frontRetractStart = 0
self.wheelsStart = 0
self.wheelsStart2 = 0
if self.usingNeo:
# NOTE: If using Spark Max in PWM mode to control Neo brushless
# motors you MUST configure the speed controllers manually
# using a USB cable and the Spark Max client software!
self.frontLift: Spark = Spark(map.frontLiftPwm)
self.backLift: Spark = Spark(map.backLiftPwm)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
if map.robotId == map.astroV1:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V1'''
self.backLift = Talon(map.backLift)
self.frontLift = Talon(map.frontLift)
self.frontLift.setInverted(True)
self.backLift.setInverted(True)
self.wheelLeft = Victor(map.wheelLeft)
self.wheelRight = Victor(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(True)
else:
if not self.usingNeo:
'''IDS AND DIRECTIONS FOR V2'''
self.backLift = Talon(map.frontLift)
self.frontLift = Talon(map.backLift)
self.frontLift.setInverted(False)
self.backLift.setInverted(False)
self.backLift.setNeutralMode(2)
self.frontLift.setNeutralMode(2)
self.wheelLeft = Talon(map.wheelLeft)
self.wheelRight = Talon(map.wheelRight)
self.wheelRight.setInverted(True)
self.wheelLeft.setInverted(False)
if not self.usingNeo:
for motor in [self.backLift, self.frontLift]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.configContinuousCurrentLimit(30, 0) #Amps per motor
motor.enableCurrentLimit(True)
motor.configVoltageCompSaturation(10,
0) #Sets saturation value
motor.enableVoltageCompensation(
True) #Compensates for lower voltages
for motor in [self.wheelLeft, self.wheelRight]:
motor.clearStickyFaults(0)
motor.setSafetyEnabled(False)
motor.setNeutralMode(2)
self.backSwitch = DigitalInput(map.backFloor)
self.frontSwitch = DigitalInput(map.frontFloor)
self.switchTopBack = DigitalInput(map.backTopSensor)
self.switchTopFront = DigitalInput(map.frontTopSensor)
self.switchBottomBack = DigitalInput(map.backBottomSensor)
self.switchBottomFront = DigitalInput(map.frontBottomSensor)
self.MAX_ANGLE = 2 #degrees
self.TARGET_ANGLE = -1 #degrees
self.climbSpeed = 0.9
self.wheelSpeed = 0.9
self.backHold = -0.10 #holds back stationary if extended ADJUST**
self.frontHold = -0.10 #holds front stationary if extended
self.kP = 0.35 #proportional gain for angle to power
self.autoClimbIndicator = False
'''
NEGATIVE POWER TO ELEVATOR LIFTS ROBOT, LOWERS LEGS
POSITIVE POWER TO ELEVATOR LOWERS ROBOT, LIFTS LEGS
NEGATIVE POWER TO WHEELS MOVES ROBOT BACKWARDS
POSITIVE POWER TO WHEELS MOVES ROBOT FORWARD
'''
self.started = False
def robotInit(self):
self.maxVolts = 10
self.joystick = Joystick(0)
self.motor: Talon = Talon(31)
self.motor.configClearPositionOnLimitF(True)
self.motor.configVoltageCompSaturation(self.maxVolts)
def __init__(self, robot):
self.robot = robot
# Initialize all controllers
self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master'])
self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave'])
self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master'])
self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave'])
wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster",
self.driveLeftMaster)
wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster",
self.driveRightMaster)
# Connect the slaves to the masters on each side
self.driveLeftSlave.follow(self.driveLeftMaster)
self.driveRightSlave.follow(self.driveRightMaster)
# Makes sure both sides' controllers show green and use positive
# values to move the bot forward.
self.driveLeftSlave.setInverted(False)
self.driveLeftMaster.setInverted(False)
self.driveRightSlave.setInverted(True)
self.driveRightMaster.setInverted(True)
"""
Initializes the count for toggling which side of the
robot will be considered the front when driving.
"""
self.robotFrontToggleCount = 2
# Configures each master to use the attached Mag Encoders
self.driveLeftMaster.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
0)
self.driveRightMaster.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
0)
# Reverses the encoder direction so forward movement always
# results in a positive increase in the encoder ticks.
self.driveLeftMaster.setSensorPhase(True)
self.driveRightMaster.setSensorPhase(True)
# these supposedly aren't part of the WPI_TalonSRX class
# self.driveLeftMaster.setSelectedSensorPostion(0, 0, 10)
# self.driveRightMaster.setSelectedSensorPosition(0, 0, 10)
# Throw data on the SmartDashboard so we can work with it.
# SD.putNumber(
# 'Left Quad Pos.',
# self.driveLeftMaster.getQuadraturePosition())
# SD.putNumber(
# 'Right Quad Pos.',
# self.driveRightMaster.getQuadraturePosition())
self.leftVel = None
self.leftPos = None
self.rightVel = None
self.rightPos = None
self.leftMaxVel = 0
self.rightMaxVel = 0
self.leftMinVel = 0
self.rightMinVel = 0
# self.driveLeftMaster.config_kP(0, .3, 10)
self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
self.driveControllerRight.setInverted(True)
self.drive = DifferentialDrive(self.driveControllerLeft,
self.driveControllerRight)
super().__init__()
def initialize(self):
timeout = 15
self.lastMode = "unknown"
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.1
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.05
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
self.motor = Talon(map.intake)
self.motor.configContinuousCurrentLimit(20,timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(30,timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(100,timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.intakeSpeed = 0.9
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#choose sensor
self.wrist.configSelectedFeedbackSensor(
Talon.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,)
self.wrist.setSensorPhase(False) #!!!!!
# Set relevant frame periods to be at least as fast as periodic rate
self.wrist.setStatusFramePeriod(
Talon.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs)
self.wrist.setStatusFramePeriod(
Talon.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs)
# set the peak and nominal outputs
self.wrist.configNominalOutputForward(0, self.kTimeoutMs)
self.wrist.configNominalOutputReverse(0, self.kTimeoutMs)
self.wrist.configPeakOutputForward(0.6, self.kTimeoutMs)
self.wrist.configPeakOutputReverse(-0.25, self.kTimeoutMs)
self.kF = self.getFeedForward(self.getNumber("Wrist F Gain" , 0))
self.kP = self.getNumber("Wrist kP" , 0)
self.kI = self.getNumber("Wrist kI" , 0)
self.kD = self.getNumber("Wrist kD" , 0)
# set closed loop gains in slot0 - see documentation */
self.wrist.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.wrist.config_kF(0, self.kF, self.kTimeoutMs)
self.wrist.config_kP(0, self.kP, self.kTimeoutMs)
self.wrist.config_kI(0, 0, self.kTimeoutMs)
self.wrist.config_kD(0, 0, self.kTimeoutMs)
# set acceleration and vcruise velocity - see documentation
self.wrist.configMotionCruiseVelocity(400, self.kTimeoutMs) #!!!!!
self.wrist.configMotionAcceleration(500, self.kTimeoutMs) #!!!!!
# zero the sensor
self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def robotInit(self):
self.joystick = Joystick(0)
self.motor = Talon(10)
self.motor.configClearPositionOnLimitF(True)
def initialize(self):
timeout = 15
SmartDashboard.putNumber("Wrist Power Set", 0)
SmartDashboard.putNumber("Gravity Power", 0)
self.sb = []
self.targetPosUp = -300 #!!!!!
self.targetPosDown = -1500 #!!!!!
self.maxVolts = 10
self.simpleGain = 0.57
self.wristUpVolts = 5
self.wristDownVolts = 2
self.simpleGainGravity = 0.07
self.xbox = oi.getJoystick(2)
self.joystick0 = oi.getJoystick(0)
#below is the talon on the intake
self.motor = Talon(map.intake)
self.input = 0
self.input2 = 0
self.lastCargoCommand = "unknown"
self.motor.configContinuousCurrentLimit(20,
timeout) #15 Amps per motor
self.motor.configPeakCurrentLimit(
30, timeout) #20 Amps during Peak Duration
self.motor.configPeakCurrentDuration(
100, timeout) #Peak Current for max 100 ms
self.motor.enableCurrentLimit(True)
#Talon motor object created
self.wrist = Talon(map.wrist)
if not wpilib.RobotBase.isSimulation():
self.wrist.configFactoryDefault()
self.wrist.configVoltageCompSaturation(self.maxVolts)
self.wrist.setInverted(True)
self.wrist.setNeutralMode(2)
self.motor.setNeutralMode(2)
self.motor.configVoltageCompSaturation(self.maxVolts)
self.wrist.configClearPositionOnLimitF(True)
#MOTION MAGIC CONFIG
self.loops = 0
self.timesInMotionMagic = 0
#self.wrist.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
self.F = 0
#should be 0.4
SmartDashboard.putNumber("F Gain", self.F)
[self.kP, self.kI, self.kD, self.kF] = [0, 0, 0, 0]
cargoController = PIDController(self.kP, self.kI, self.kD, self.kF,
self, self)
self.cargoController = cargoController
self.cargoController.disable()
self.pidValuesForMode = {
"resting": [-50, self.kP, self.kI, self.kD, 0.15 / -50],
"cargoship": [-28, self.kP, self.kI, self.kD, 0.0],
"intake": [50, self.kP, self.kI, self.kD, 0.0],
"rocket": [5, self.kP, self.kI, self.kD, 0.19 / 5],
}
