class MyRobot(TimedRobot):
def robotInit(self):
self.joystick = Joystick(0)
self.motor = Talon(10)
self.motor.configClearPositionOnLimitF(True)
def teleopPeriodic(self):
self.y = self.joystick.getRawAxis(1)
if abs(self.y) < 0.1:
self.y = 0
self.motor.set(self.y)
def robotPeriodic(self):
sd.putNumber('Forward closed', self.motor.isFwdLimitSwitchClosed())
sd.putNumber('Back closed', self.motor.isRevLimitSwitchClosed())
sd.putNumber('motor position', self.motor.getQuadraturePosition())
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 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)