def config_talon(talon: WPI_TalonSRX, motor_config: TalonMotorConfig):
# when deploying it might be a somewhat good idea to actually uncomment this next line
talon.configFactoryDefault()
talon.enableVoltageCompensation(True)
talon.configVoltageCompSaturation(motor_config.voltage_saturation)
talon.enableCurrentLimit(True)
talon.configPeakCurrentLimit(motor_config.peak_current)
talon.configContinuousCurrentLimit(motor_config.continuous_current)
talon.setNeutralMode(motor_config.default_mode)
talon.configNeutralDeadband(motor_config.deadband)
# talon.configOpenLoopRamp(motor_config.ramp_rate)
talon.setInverted(motor_config.reversed)
def config_talon(talon: WPI_TalonSRX, motor_config: MotorConfig) -> None:
talon.enableVoltageCompensation(True)
talon.configVoltageCompSaturation(motor_config.voltage_saturation)
talon.enableCurrentLimit(True)
talon.configPeakCurrentLimit(motor_config.peak_current)
talon.configContinuousCurrentLimit(motor_config.continuous_current)
talon.setNeutralMode(motor_config.default_mode)
talon.configNeutralDeadband(motor_config.deadband)
talon.configOpenLoopRamp(motor_config.ramp_rate)
class CargoMech():
kSlotIdx = 0
kPIDLoopIdx = 0
kTimeoutMs = 10
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 intake(self, mode):
#Intake/Outtake/Stop, based on the mode it changes the speed of the motor
if mode == "intake": self.motor.set(0.9)
elif mode == "outtake": self.motor.set(-0.9)
elif mode == "stop": self.motor.set(0)
def moveWrist(self, mode):
#move wrist in and out of robot
'''replace modes up and down with pid and set angles'''
if mode == "up":
self.input = (self.getPowerSimple("up")) + self.getGravity()
self.wrist.set(self.getPowerSimple("up") + self.getGravity())
elif mode == "down":
self.input = (-1 * self.getPowerSimple("down")) + self.getGravity()
self.wrist.set((-1 * self.getPowerSimple("down")) +
self.getGravity())
elif mode == "upVolts":
self.input = (self.wristUpVolts /
self.maxVolts) + self.getGravity()
self.wrist.set((self.wristUpVolts / self.maxVolts) +
self.getGravity())
elif mode == "downVolts":
self.input = (-1 * self.wristDownVolts /
self.maxVolts) + self.getGravity()
self.wrist.set((-1 * self.wristDownVolts / self.maxVolts) +
self.getGravity())
elif mode == "stop":
self.input = 0
self.wrist.set(0)
else:
#self.input = self.getGravity#()
#self.input = self.getNumber("#Wrist Power Set", 0)
self.input = 0
self.wrist.set(self.input)
#self.cargoController.setSetpoint(self.getAngle())
#self.cargoController.enable()
#self.wrist.set(self.gPower)
'''replace pid here with kF'''
def periodic(self):
#0.4 as a deadband
if self.xbox.getRawAxis(map.intakeCargo) > 0.4: self.intake("intake")
elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4:
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")
else:
self.moveWrist("gravity")
#disables intake
def disable(self):
self.intake("stop")
#gets the angle, used in other support functions
def getAngle(self):
pos = self.getPosition()
angle = abs(pos * 115 / self.targetPosDown)
return (angle - 25)
'''cargo mech vertical is 0 degrees'''
'''cargo mech down completely is 90 degrees'''
'''cargo mech has range of -25 to 85 degrees'''
def getGravity(self):
'''angle = self.getAngle()
power = 0
if angle < -20:
power = 0
elif angle > 80:
power = 0
else:
power = math.sin(math.radians(angle)) / 2
return power'''
return 0
def getPosition(self):
return self.wrist.getQuadraturePosition()
def getFeedForward(self, gain):
angle = self.getAngle()
return angle * gain
def __setGravity__(self, output):
self.gPower = output
def getPowerSimple(self, direction):
'''true direction is up into robot
false direction is down out of robot'''
angle = self.getAngle()
power = abs(self.simpleGain)
if angle > 80:
if direction == "down":
power = 0
#if angle < -20:
# if direction == "up":
# power = 0
return power
def setWristPower(self, power):
self.wrist.set(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.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain)
#self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity)
self.kP = self.getNumber("Wrist kP", 0)
self.kI = self.getNumber("Wrist kI", 0)
self.kD = self.getNumber("Wrist kD", 0)
SmartDashboard.putNumber("Wrist Position",
self.wrist.getQuadraturePosition())
SmartDashboard.putNumber("Wrist Angle", self.getAngle())
#SmartDashboard.putNumber("Wrist Power", self.input)
SmartDashboard.putNumber("Wrist Power Up", self.getPowerSimple("up"))
SmartDashboard.putNumber("Wrist Power Down",
self.getPowerSimple("down"))
class CargoMech():
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 setIntake(self, mode):
#Intake/Outtake/Stop, based on the mode it changes the speed of the motor
if mode == "intake": self.intake.set(0.9)
elif mode == "outtake": self.intake.set(-0.9)
else: self.intake.set(0)
def setWrist(self, mode):
[setpoint, error] = [10, 5]
mult = abs(
self.getAngle() - 50
) / 100 + 0.5 #increase constant if the arm is not moving enough close to the setpoint
if mode == "rocket" and self.getAngle() < setpoint - error:
self.moveWrist(self.wristDownVolts / self.maxVolts * mult, mode)
elif mode == "rocket" and self.getAngle() > setpoint + error:
self.moveWrist(self.wristUpVolts / self.maxVolts * mult, mode)
elif mode == "up":
self.moveWrist(self.wristUpVolts / self.maxVolts, mode)
elif mode == "down":
self.moveWrist(self.wristDownVolts / self.maxVolts, mode)
elif mode == "rocket" or mode == "gravity":
self.moveWrist(0, mode)
else:
self.moveWrist(-self.getGravity(), "stop")
def moveWrist(self, power, mode):
self.out = self.wristBounds(power + self.getGravity(), mode)
self.wrist.set(self.out)
def wristBounds(self, power, mode):
angle = self.getAngle()
if angle > 80 and (mode == "down" or mode == "gravity"): power = 0
elif angle < -20 and (mode == "up" or mode == "gravity"): power = 0
elif angle < 10 and mode == "gravity":
power = self.F #if operator lets go of buttons while at top, arm goes back to upper limit sqitch
return power
def getGravity(self):
return math.sin(math.radians(self.getAngle())) * self.F
def getAngle(self):
pos = 0
if self.wrist.isRevLimitSwitchClosed():
pos = self.range
self.wrist.setQuadraturePosition(pos)
else:
pos = self.getPosition()
angle = abs(pos * 115 / self.range)
return (angle - 25)
'''
def getAngle(self):
return self.angle
'''
def getPosition(self):
return self.wrist.getQuadraturePosition()
def periodic(self):
if self.xbox.getRawAxis(map.intakeCargo) > 0.4:
self.setIntake("intake")
elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4:
self.setIntake("outtake")
else:
self.setIntake("stop")
if self.xbox.getPOV() >= 0 and self.povPressed: self.povPressed = False
elif self.xbox.getPOV() >= 0 and not self.povPressed:
self.povPressed = True
if self.povPressed: self.setWrist("rocket")
elif self.xbox.getRawButton(map.wristUp): self.setWrist("up")
elif self.xbox.getRawButton(map.wristDown): self.setWrist("down")
else: self.setWrist("gravity")
#disables intake
def disable(self):
self.setIntake("stop")
self.setWrist("stop")
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):
#commented out some values. DON'T DELETE THESE VALUES
#SmartDashboard.putNumber("Wrist Position", self.wrist.getQuadraturePosition())
SmartDashboard.putNumber("Wrist Angle", self.getAngle())
#SmartDashboard.putNumber("Output", self.out)
#self.F = SmartDashboard.getNumber("F Gain", 0)
#self.wristUpVolts = SmartDashboard.getNumber("Wrist Up Volts", 0)
#self.wristDownVolts = SmartDashboard.getNumber("Wrist Down Volts", 0)
SmartDashboard.putBoolean("Limit Switch",
self.wrist.isFwdLimitSwitchClosed())
SmartDashboard.putBoolean("Limit Switch Reverse",
self.wrist.isRevLimitSwitchClosed())
class HatchMech(Subsystem):
def __init__(self, Robot):
#Create all physical parts used by subsystem.
super().__init__('Hatch')
#self.debug = True
self.robot = Robot
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 periodic(self):
self.updateHatch()
self.currKick = self.xbox.getRawButton(map.kickHatch)
self.currSlide = self.xbox.getRawButton(map.toggleHatch)
#if the variable is true and it does not equal the lastkick/slide boolean, sets it to the opposite of what it currently is
if self.currKick and (self.currKick != self.lastKick): self.kick("toggle")
if self.currSlide and (self.currSlide != self.lastSlide): self.slide("toggle")
#after the if statement, sets the lastkick/slide to the currkick/slide
self.lastKick = self.currKick
self.lastSlide = self.currSlide
self.setWheels()
# kick function to activate kicker solenoid
def kick(self, mode):
#out mode sets kicker solenoid to true
if mode == "out": self.setKick(True)
#in mode sets kicker solenoid to false
elif mode == "in": self.setKick(False)
#if neither of them, makes the kicker solenoid to the opposite of what it is
else: self.setKick(not self.kicker.get())
# slide function to activate slide solenoid
def slide(self, mode):
# out mode sets slider solenoid to true
if mode == "out": self.slider.set(True)
# in mode sets slider solenoid to false
elif mode == "in": self.slider.set(False)
#if neither of them, makes the slider solenoid to the opposite of what it is
else: self.slider.set(not self.slider.get())
def setKick(self, state):
self.kicker.set(state)
if state and self.slider.get() and self.hasHatch: self.hasHatch = False
def setWheels(self):
if self.kicker.get() and self.slider.get() and self.hasHatch:
self.wheels.set(self.powerOut)
self.outPower = self.powerOut
elif not self.kicker.get() and self.slider.get() and not self.hasHatch:
#should be "and not self.hasHatch"
self.wheels.set(self.powerIn)
self.outPower = self.powerIn
#elif self.joystick1.getRawButton(1):
# self.wheels.set(self.powerIn)
# self.outPower = self.powerIn
else:
self.wheels.set(0)
self.outPower = 0
def updateHatch(self):
#only checks current to possibly set false to true for hasHatch
threshold = 10 #10 amp current separates freely spinning and stalled
if self.slider.get() and self.wheels.getOutputCurrent()>threshold:
self.hasHatch = True
if self.joystick0.getRawButton(3) or self.joystick0.getRawButton(4) or self.joystick1.getRawButton(3) or self.joystick1.getRawButton(4):
self.hasHatch = True
#disable function runs kick function on in
def disable(self): self.kick("in")
def dashboardInit(self): pass
def dashboardPeriodic(self):
#commented out some values. DON'T DELETE THESE VALUES
#SmartDashboard.putNumber("Hatch Current", self.wheels.getOutputCurrent())
#SmartDashboard.putNumber("Power", self.outPower)
SmartDashboard.putBoolean("Has Hatch", self.hasHatch)
SmartDashboard.putBoolean("Slider Out", self.slider.get())
SmartDashboard.putBoolean("Kicker Out", self.kicker.get())
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.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 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 == "stop":
self.wrist.set(0)
else:
self.wrist.set(self.getGravitySimple())
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")
else:
self.moveWrist("gravity")
def disable(self): self.intake("stop")
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.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain)
self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity)
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 CargoMech():
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)
def setIntake(self, mode):
#Intake/Outtake/Stop, based on the mode it changes the speed of the motor
if mode == "intake": self.intake.set(0.9)
elif mode == "outtake": self.intake.set(-0.9)
else: self.intake.set(0)
def setWrist(self, mode):
if mode == "up":
self.out = self.wristUpVolts / self.maxVolts
elif mode == "down":
self.out = self.wristDownVolts / self.maxVolts
else:
self.out = 0
self.setWristPower(self.out)
def setWristPower(power):
self.wrist.set(power)
def periodic(self):
if self.xbox.getRawAxis(map.intakeCargo) > 0.4:
self.setIntake("intake")
elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4:
self.setIntake("outtake")
else:
self.setIntake("stop")
if self.xbox.getRawButton(map.wristUp): self.setWrist("up")
elif self.xbox.getRawButton(map.wristDown): self.setWrist("down")
else: self.setWrist("stop")
#disables intake
def disable(self):
self.setIntake("stop")
self.setWrist("stop")
def dashboardInit(self):
pass
def dashboardPeriodic(self):
SmartDashboard.putNumber("Cargomech Wrist Output", self.out)
self.wristPowerSet = SmartDashboard.getNumber("Wrist Power Set", 0)
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 CargoMech():
kSlotIdx = 0
kPIDLoopIdx = 0
kTimeoutMs = 10
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],
}
def intake(self, mode):
#Intake/Outtake/Stop, based on the mode it changes the speed of the motor
if mode == "intake": self.motor.set(0.9)
elif mode == "outtake": self.motor.set(-0.9)
elif mode == "stop": self.motor.set(0)
def pidWrite(self, output):
maxPower = 0.3
if output < -maxPower:
output = -maxPower
elif output > maxPower:
output = maxPower
self.wrist.set(output)
self.input = output
def pidGet(self):
return self.getAngle()
def setPIDSourceType(self):
pass
def getPIDSourceType(self):
return 0
def moveWrist(self, mode):
if mode != self.lastCargoCommand:
self.lastCargoCommand = mode
if mode in self.pidValuesForMode:
array = self.pidValuesForMode[mode]
self.cargoController.setP(array[1])
self.cargoController.setI(array[2])
self.cargoController.setD(array[3])
self.cargoController.setF(
math.sin(self.getAngle() / 180 * math.pi) * self.F)
self.cargoController.setSetpoint(array[0])
self.cargoController.enable()
else:
self.cargoController.disable()
elif (mode not in self.pidValuesForMode):
self.wrist.set(math.sin(self.getAngle() / 180 * math.pi) * self.F)
self.input2 = math.sin(self.getAngle() / 180 * math.pi) * self.F
def periodic(self):
#0.4 as a deadband
if self.xbox.getRawAxis(map.intakeCargo) > 0.4: self.intake("intake")
elif self.xbox.getRawAxis(map.outtakeCargo) > 0.4:
self.intake("outtake")
else:
self.intake("stop")
if self.xbox.getPOV() == 0:
self.moveWrist("resting")
self.lastCargoCommand = "resting"
elif self.xbox.getPOV() == 90:
self.moveWrist("cargoship")
self.lastCargoCommand = "cargoship"
elif self.xbox.getPOV() == 180:
self.moveWrist("intake")
self.lastCargoCommand = "intake"
elif self.xbox.getPOV() == 270:
self.moveWrist("rocket")
self.lastCargoCommand = "rocket"
else:
self.moveWrist(self.lastCargoCommand)
#disables intake
def disable(self):
self.intake("stop")
#gets the angle, used in other support functions
def getAngle(self):
pos = self.getPosition()
angle = abs(pos * 115 / self.targetPosDown)
return (angle - 20)
def getPosition(self):
return self.wrist.getQuadraturePosition()
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.simpleGain = self.getNumber("Wrist Simple Gain", self.simpleGain)
#self.simpleGainGravity = self.getNumber("Wrist Simple Gain Gravity", self.simpleGainGravity)
SmartDashboard.putNumber("Wrist Position",
self.wrist.getQuadraturePosition())
SmartDashboard.putData("PID Controller", self.cargoController)
SmartDashboard.putNumber("Wrist Angle", self.getAngle())
SmartDashboard.putNumber("Wrist Power", self.input)
SmartDashboard.putNumber("Wrist Power2", self.input2)
SmartDashboard.putString("Last Cargo Command", self.lastCargoCommand)
SmartDashboard.putBoolean("Wrist PinState Quad A",
self.wrist.getPinStateQuadA())
SmartDashboard.putBoolean("Wrist PinState Quad B",
self.wrist.getPinStateQuadB())
self.F = SmartDashboard.getNumber("F Gain", 0)
