class CargoMech():
kSlotIdx = 0
kPIDLoopIdx = 0
kTimeoutMs = 10
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 intake(self, mode):
''' Intake/Outtake/Stop Intake the cargo (turn wheels inward)'''
if mode == "intake": self.motor.set(self.intakeSpeed)
elif mode == "outtake": self.motor.set(-1 * self.intakeSpeed)
elif mode == "stop": self.motor.set(0)
def moveWrist(self,mode):
'''move wrist in and out of robot'''
if mode == "up": self.wrist.set(self.getPowerSimple("up"))
elif mode == "down": self.wrist.set(-1 * self.getPowerSimple("down"))
elif mode == "upVolts": self.wrist.set(self.wristUpVolts/self.maxVolts)
elif mode == "downVolts": self.wrist.set(-1 * self.wristDownVolts/ self.maxVolts)
elif mode == "upMagic":
if self.lastMode != mode:
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)
# Motion Magic - 4096 ticks/rev * 10 Rotations in either direction
self.wrist.set(Talon.ControlMode.MotionMagic, self.targetPosUp)
# append more signals to print when in speed mode.
self.sb.append("\terr: %s" % self.wrist.getClosedLoopError(self.kPIDLoopIdx))
self.sb.append("\ttrg: %.3f" % self.targetPosUp)
elif mode == "downMagic":
if self.lastMode != mode:
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)
# Motion Magic - 4096 ticks/rev * 10 Rotations in either direction
self.wrist.set(Talon.ControlMode.MotionMagic, self.targetPosDown)
# append more signals to print when in speed mode.
self.sb.append("\terr: %s" % self.wrist.getClosedLoopError(self.kPIDLoopIdx))
self.sb.append("\ttrg: %.3f" % self.targetPosDown)
elif mode == "stop":
self.wrist.set(0)
else:
self.wrist.set(self.getGravitySimple())
self.lastMode = mode
def periodic(self):
deadband = 0.4
if self.xbox.getRawAxis(map.intakeCargo)>deadband: self.intake("intake")
elif self.xbox.getRawAxis(map.outtakeCargo)>deadband: self.intake("outtake")
else:
self.intake("stop")
if self.xbox.getRawButton(map.wristUp): self.moveWrist("up")
elif self.xbox.getRawButton(map.wristDown): self.moveWrist("down")
elif self.joystick0.getRawButton(map.wristUpVolts): self.moveWrist("upVolts")
elif self.joystick0.getRawButton(map.wristDownVolts): self.moveWrist("downVolts")
elif self.joystick0.getRawButton(map.wristUpMagic): self.moveWrist("upMagic")
elif self.joystick0.getRawButton(map.wristDownMagic): self.moveWrist("downMagic")
else:
self.moveWrist("gravity")
# calculate the percent motor output
motorOutput = self.wrist.getMotorOutputPercent()
self.sb = []
# prepare line to print
self.sb.append("\tOut%%: %.3f" % motorOutput)
self.sb.append("\tVel: %.3f" % self.wrist.getSelectedSensorVelocity(self.kPIDLoopIdx))
# instrumentation
self.processInstrumentation(self.wrist, self.sb)
def disable(self): self.intake("stop")
def processInstrumentation(self, tal, sb):
# smart dash plots
wpilib.SmartDashboard.putNumber("SensorVel", tal.getSelectedSensorVelocity(self.kPIDLoopIdx))
wpilib.SmartDashboard.putNumber("SensorPos", tal.getSelectedSensorPosition(self.kPIDLoopIdx))
wpilib.SmartDashboard.putNumber("MotorOutputPercent", tal.getMotorOutputPercent())
wpilib.SmartDashboard.putNumber("ClosedLoopError", tal.getClosedLoopError(self.kPIDLoopIdx))
# check if we are motion-magic-ing
if tal.getControlMode() == Talon.ControlMode.MotionMagic:
self.timesInMotionMagic += 1
else:
self.timesInMotionMagic = 0
if self.timesInMotionMagic > 10:
# print the Active Trajectory Point Motion Magic is servoing towards
wpilib.SmartDashboard.putNumber(
"ClosedLoopTarget", tal.getClosedLoopTarget(self.kPIDLoopIdx)
)
if not wpilib.RobotBase.isSimulation():
wpilib.SmartDashboard.putNumber(
"ActTrajVelocity", tal.getActiveTrajectoryVelocity()
)
wpilib.SmartDashboard.putNumber(
"ActTrajPosition", tal.getActiveTrajectoryPosition()
)
wpilib.SmartDashboard.putNumber(
"ActTrajHeading", tal.getActiveTrajectoryHeading()
)
# periodically print to console
self.loops += 1
if self.loops >= 10:
self.loops = 0
print(" ".join(self.sb))
# clear line cache
self.sb.clear()
def getAngle(self):
pos = self.getPosition()
angle = abs(pos * 115/self.targetPosDown)
return angle - 25
def getPosition(self):
return self.wrist.getQuadraturePosition()
def getFeedForward(self, gain):
angle = self.getAngle()
return angle*gain
def getPowerSimple(self, direction):
'''true direction is up into robot
false direction is down out of robot'''
angle = self.getAngle()
power = abs(self.simpleGainGravity * math.sin(math.radians(angle)) + self.simpleGain)
if angle > 80:
if direction == "down":
power = 0
if angle < -20:
if direction == "up":
power = 0
return power
def getGravitySimple(self):
angle = self.getAngle()
power = self.simpleGainGravity * math.sin(math.radians(angle))
return power
def getNumber(self, key, defVal):
val = SmartDashboard.getNumber(key, None)
if val == None:
val = defVal
SmartDashboard.putNumber(key, val)
return val
def dashboardInit(self):
pass
def dashboardPeriodic(self):
self.wristUp = self.getNumber("WristUpSpeed" , 0.5)
self.wristDown = self.getNumber("WristDownSpeed" , 0.2)
self.wristUpVolts = self.getNumber("WristUpVoltage" , 5)
self.wristDownVolts = self.getNumber("WristDownVoltage" , 2)
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)
self.simpleGain = self.getNumber("Wrist Simple Gain", 0.5)
self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", 0.07)
SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition())
SmartDashboard.putNumber("Wrist Angle" , self.getAngle())
SmartDashboard.putNumber("Wrist Power Up" , self.getPowerSimple("up"))
SmartDashboard.putNumber("Wrist Power Down" , self.getPowerSimple("down"))
class Robot(wpilib.IterativeRobot):
#: Which PID slot to pull gains from. Starting 2018, you can choose from
#: 0,1,2 or 3. Only the first two (0,1) are visible in web-based
#: configuration.
kSlotIdx = 0
#: Talon SRX/ Victor SPX will supported multiple (cascaded) PID loops. For
#: now we just want the primary one.
kPIDLoopIdx = 0
#: set to zero to skip waiting for confirmation, set to nonzero to wait and
#: report to DS if action fails.
kTimeoutMs = 10
def robotInit(self):
self.talon = WPI_TalonSRX(3)
self.joy = wpilib.Joystick(0)
self.loops = 0
self.timesInMotionMagic = 0
# first choose the sensor
self.talon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.talon.setSensorPhase(True)
self.talon.setInverted(False)
# Set relevant frame periods to be at least as fast as periodic rate
self.talon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10, self.kTimeoutMs
)
self.talon.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10, self.kTimeoutMs
)
# set the peak and nominal outputs
self.talon.configNominalOutputForward(0, self.kTimeoutMs)
self.talon.configNominalOutputReverse(0, self.kTimeoutMs)
self.talon.configPeakOutputForward(1, self.kTimeoutMs)
self.talon.configPeakOutputReverse(-1, self.kTimeoutMs)
# set closed loop gains in slot0 - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0.2, self.kTimeoutMs)
self.talon.config_kP(0, 0.2, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, self.kTimeoutMs)
# set acceleration and vcruise velocity - see documentation
self.talon.configMotionCruiseVelocity(15000, self.kTimeoutMs)
self.talon.configMotionAcceleration(6000, self.kTimeoutMs)
# zero the sensor
self.talon.setSelectedSensorPosition(0, self.kPIDLoopIdx, self.kTimeoutMs)
def teleopPeriodic(self):
"""
This function is called periodically during operator control
"""
# get gamepad axis - forward stick is positive
leftYstick = -1.0 * self.joy.getY()
# calculate the percent motor output
motorOutput = self.talon.getMotorOutputPercent()
# prepare line to print
sb = []
sb.append("\tOut%%: %.3f" % motorOutput)
sb.append(
"\tVel: %.3f" % self.talon.getSelectedSensorVelocity(self.kPIDLoopIdx)
)
if self.joy.getRawButton(1):
# Motion Magic - 4096 ticks/rev * 10 Rotations in either direction
targetPos = leftYstick * 4096 * 10.0
self.talon.set(WPI_TalonSRX.ControlMode.MotionMagic, targetPos)
# append more signals to print when in speed mode.
sb.append("\terr: %s" % self.talon.getClosedLoopError(self.kPIDLoopIdx))
sb.append("\ttrg: %.3f" % targetPos)
else:
# Percent voltage mode
self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftYstick)
# instrumentation
self.processInstrumentation(self.talon, sb)
def processInstrumentation(self, tal, sb):
# smart dash plots
wpilib.SmartDashboard.putNumber(
"SensorVel", tal.getSelectedSensorVelocity(self.kPIDLoopIdx)
)
wpilib.SmartDashboard.putNumber(
"SensorPos", tal.getSelectedSensorPosition(self.kPIDLoopIdx)
)
wpilib.SmartDashboard.putNumber(
"MotorOutputPercent", tal.getMotorOutputPercent()
)
wpilib.SmartDashboard.putNumber(
"ClosedLoopError", tal.getClosedLoopError(self.kPIDLoopIdx)
)
# check if we are motion-magic-ing
if tal.getControlMode() == WPI_TalonSRX.ControlMode.MotionMagic:
self.timesInMotionMagic += 1
else:
self.timesInMotionMagic = 0
if self.timesInMotionMagic > 10:
# print the Active Trajectory Point Motion Magic is servoing towards
wpilib.SmartDashboard.putNumber(
"ClosedLoopTarget", tal.getClosedLoopTarget(self.kPIDLoopIdx)
)
if not self.isSimulation():
wpilib.SmartDashboard.putNumber(
"ActTrajVelocity", tal.getActiveTrajectoryVelocity()
)
wpilib.SmartDashboard.putNumber(
"ActTrajPosition", tal.getActiveTrajectoryPosition()
)
wpilib.SmartDashboard.putNumber(
"ActTrajHeading", tal.getActiveTrajectoryHeading()
)
# periodically print to console
self.loops += 1
if self.loops >= 10:
self.loops = 0
print(" ".join(sb))
# clear line cache
sb.clear()
class DriveSubsystem(Subsystem):
def __init__(self, robot):
super().__init__("Drive")
self.robot = robot
self.left_master = WPI_TalonSRX(robotmap.DRIVELEFTMASTER)
self.right_master = WPI_TalonSRX(robotmap.DRIVERIGHTMASTER)
self.left_slave = WPI_TalonSRX(robotmap.DRIVELEFTSLAVE)
self.right_slave = WPI_TalonSRX(robotmap.DRIVERIGHTSLAVE)
self.shifter_high = Solenoid(robotmap.PCM1_CANID,
robotmap.SHIFTER_HIGH)
self.shifter_low = Solenoid(robotmap.PCM1_CANID, robotmap.SHIFTER_LOW)
self.differential_drive = DifferentialDrive(self.left_master,
self.right_master)
self.TalonConfig()
self.shiftLow()
def teleDrive(self, xSpeed, zRotation):
if self.robot.oi.getController1().B().get():
scale = SmartDashboard.getNumber("creep_mult", 0.3)
xSpeed = xSpeed * scale
zRotation = zRotation * scale
self.differential_drive.arcadeDrive(xSpeed, zRotation, False)
def getLeftPosition(self):
return self.left_master.getSelectedSensorPosition(0)
def getRightPosition(self):
return self.right_master.getSelectedSensorPosition(0)
def getRightSpeed(self):
return self.right_master.getSelectedSensorVelocity(0)
def getLeftSpeed(self):
return self.unitsToInches(
self.left_master.getSelectedSensorVelocity(0))
def getErrorLeft(self):
return self.left_master.getClosedLoopError(0)
def getErrorRight(self):
return self.right_master.getClosedLoopError(0)
def isLeftSensorConnected(self):
return self.left_master.getSensorCollection(
).getPulseWidthRiseToRiseUs() != 0
def isRightSensorConnected(self):
return self.right_master.getSensorCollection(
).getPulseWidthRiseToRiseUs() != 0
def resetEncoders(self):
self.left_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS)
self.right_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS)
self.left_master.getSensorCollection().setQuadraturePosition(
0, robotmap.TIMEOUT_MS)
self.right_master.getSensorCollection().setQuadraturePosition(
0, robotmap.TIMEOUT_MS)
def setMotionMagicLeft(self, setpoint):
SmartDashboard.putNumber("setpoint_left_units",
self.inchesToUnits(setpoint))
self.left_master.set(ControlMode.MotionMagic,
self.inchesToUnits(setpoint))
def setMotionMagicRight(self, setpoint):
self.right_master.set(ControlMode.MotionMagic,
self.inchesToUnits(-setpoint))
def isMotionMagicNearTarget(self):
retval = False
if self.left_master.getActiveTrajectoryPosition(
) == self.left_master.getClosedLoopTarget(0):
if self.right_master.getActiveTrajectoryPosition(
) == self.right_master.getClosedLoopTarget(0):
if abs(self.left_master.getClosedLoopError(0)) < 300:
if abs(self.right_master.getClosedLoopError(0)) < 300:
retval = True
return retval
def shiftHigh(self):
self.shifter_high.set(True)
self.shifter_low.set(False)
def shiftLow(self):
self.shifter_high.set(False)
self.shifter_low.set(True)
def stop(self):
self.left_master.set(ControlMode.PercentOutput, 0.0)
self.right_master.set(ControlMode.PercentOutput, 0.0)
def unitsToInches(self, units):
return units * robotmap.DRIVE_WHEEL_CIRCUMFERENCE / robotmap.DRIVE_ENCODER_PPR
def inchesToUnits(self, inches):
return inches * robotmap.DRIVE_ENCODER_PPR / robotmap.DRIVE_WHEEL_CIRCUMFERENCE
def autoInit(self):
self.resetEncoders()
self.left_master.setNeutralMode(NeutralMode.Brake)
self.left_slave.setNeutralMode(NeutralMode.Brake)
self.right_master.setNeutralMode(NeutralMode.Brake)
self.right_slave.setNeutralMode(NeutralMode.Brake)
self.differential_drive.setSafetyEnabled(False)
self.shiftLow()
def teleInit(self):
self.left_master.setNeutralMode(NeutralMode.Coast)
self.left_slave.setNeutralMode(NeutralMode.Coast)
self.right_master.setNeutralMode(NeutralMode.Coast)
self.right_slave.setNeutralMode(NeutralMode.Coast)
self.differential_drive.setSafetyEnabled(True)
self.shiftLow()
def TalonConfig(self):
self.left_master.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotmap.TIMEOUT_MS)
self.right_master.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0, robotmap.TIMEOUT_MS)
self.left_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS)
self.right_master.setSelectedSensorPosition(0, 0, robotmap.TIMEOUT_MS)
self.left_master.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10,
robotmap.TIMEOUT_MS)
self.left_master.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10,
robotmap.TIMEOUT_MS)
self.right_master.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_13_Base_PIDF0, 10,
robotmap.TIMEOUT_MS)
self.right_master.setStatusFramePeriod(
WPI_TalonSRX.StatusFrameEnhanced.Status_10_MotionMagic, 10,
robotmap.TIMEOUT_MS)
self.left_master.setSensorPhase(False)
self.right_master.setSensorPhase(False)
self.left_master.setInverted(True)
self.left_slave.setInverted(True)
self.right_master.setInverted(True)
self.right_slave.setInverted(True)
#Makes talons force zero voltage across when zero output to resist motion
self.left_master.setNeutralMode(NeutralMode.Brake)
self.left_slave.setNeutralMode(NeutralMode.Brake)
self.right_master.setNeutralMode(NeutralMode.Brake)
self.right_slave.setNeutralMode(NeutralMode.Brake)
self.left_slave.set(ControlMode.Follower, robotmap.DRIVELEFTMASTER)
self.right_slave.set(ControlMode.Follower, robotmap.DRIVERIGHTMASTER)
#Closed Loop Voltage Limits
self.left_master.configNominalOutputForward(0.0, robotmap.TIMEOUT_MS)
self.right_master.configNominalOutputForward(0.0, robotmap.TIMEOUT_MS)
self.left_master.configNominalOutputReverse(-0.0, robotmap.TIMEOUT_MS)
self.right_master.configNominalOutputReverse(-0.0, robotmap.TIMEOUT_MS)
self.left_master.configPeakOutputForward(1.0, robotmap.TIMEOUT_MS)
self.right_master.configPeakOutputForward(1.0, robotmap.TIMEOUT_MS)
self.left_master.configPeakOutputReverse(-1.0, robotmap.TIMEOUT_MS)
self.right_master.configPeakOutputReverse(-1.0, robotmap.TIMEOUT_MS)
def configGains(self, f, p, i, d, izone, cruise, accel):
self.left_master.selectProfileSlot(0, 0)
self.left_master.config_kF(0, f, robotmap.TIMEOUT_MS)
self.left_master.config_kP(0, p, robotmap.TIMEOUT_MS)
self.left_master.config_kI(0, i, robotmap.TIMEOUT_MS)
self.left_master.config_kD(0, d, robotmap.TIMEOUT_MS)
self.left_master.config_IntegralZone(0, izone, robotmap.TIMEOUT_MS)
self.right_master.selectProfileSlot(0, 0)
self.right_master.config_kF(0, f, robotmap.TIMEOUT_MS)
self.right_master.config_kP(0, p, robotmap.TIMEOUT_MS)
self.right_master.config_kI(0, i, robotmap.TIMEOUT_MS)
self.right_master.config_kD(0, d, robotmap.TIMEOUT_MS)
self.right_master.config_IntegralZone(0, izone, robotmap.TIMEOUT_MS)
self.left_master.configMotionCruiseVelocity(cruise,
robotmap.TIMEOUT_MS)
self.left_master.configMotionAcceleration(accel, robotmap.TIMEOUT_MS)
self.right_master.configMotionCruiseVelocity(cruise,
robotmap.TIMEOUT_MS)
self.right_master.configMotionAcceleration(accel, robotmap.TIMEOUT_MS)
