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 LifterComponent(Events):
class EVENTS:
on_control_move = "on_control_move"
on_manual_move = "on_manual move"
# RPM Multipliers
# ELEVATOR_MULTIPLIER = 1635.15 / 6317.67
ELEVATOR_MULTIPLIER = 2102.35 / 3631.33
CARRIAGE_MULTIPLIER = 1.0 - ELEVATOR_MULTIPLIER
# Max heights of each stage.
ELEVATOR_MAX_HEIGHT = 40
CARRIAGE_MAX_HEIGHT = 40
# Conversion factors (counts to inches)
# CHANGE THESE VALUES
ELEVATOR_CONV_FACTOR = 0.00134
CARRIAGE_CONV_FACTOR = 0.000977
el_down = -1
el_up = 1
carriage_down = -1
carriage_up = 1
MAX_SPEED = 0.5
ELEVATOR_ZERO = 0.0
CARRIAGE_ZERO = 0.0
TIMEOUT_MS = 0
ELEVATOR_kF = 0.0
ELEVATOR_kP = 0.1
ELEVATOR_kI = 0.0
ELEVATOR_kD = 0.0
# ALLOWABLE_ERROR = 2
CARRIAGE_ALLOWABLE_ERROR = int(2 / CARRIAGE_CONV_FACTOR)
ELEVATOR_ALLOWABLE_ERROR = int(2 / ELEVATOR_CONV_FACTOR)
# positions = {
# "floor": 2.0,
# "portal": 34.0,
# "scale_low": 48.0,
# "scale_mid": 60.0,
# "scale_high": 72.0,
# "max_height": 84.0
# }
positions = {
"floor": 0.5,
"portal": 34.0,
"scale_low": 52.0,
"scale_mid": 68.0,
"scale_high": 78.0
}
def __init__(self):
Events.__init__(self)
self.elevator_motor = WPI_TalonSRX(5)
self.elevator_bottom_switch = DigitalInput(9)
self.carriage_motor = WPI_TalonSRX(3)
self.carriage_bottom_switch = DigitalInput(1)
self.carriage_top_switch = DigitalInput(2)
self._create_events([
LifterComponent.EVENTS.on_control_move,
LifterComponent.EVENTS.on_manual_move
])
self._is_reset = False
# configure elevator motor and encoder
self.elevator_motor.setNeutralMode(NeutralMode.Brake)
self.elevator_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSensorPhase(True)
self.elevator_motor.setInverted(True)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configAllowableClosedloopError(
0, LifterComponent.ELEVATOR_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.configForwardSoftLimitThreshold(
int(LifterComponent.ELEVATOR_MAX_HEIGHT /
LifterComponent.ELEVATOR_CONV_FACTOR), 0)
self.elevator_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.elevator_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.elevator_motor.setCurrentLimit()
# self.elevator_motor.EnableCurrentLimit()
# self.elevator_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
# configure carriage motor and encoder
self.carriage_motor.setNeutralMode(NeutralMode.Brake)
self.carriage_motor.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, 0,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.setSensorPhase(True)
self.carriage_motor.setInverted(True)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputForward(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configNominalOutputReverse(
LifterComponent.ELEVATOR_ZERO, LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputForward(LifterComponent.el_up,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configPeakOutputReverse(LifterComponent.el_down,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configAllowableClosedloopError(
0, LifterComponent.CARRIAGE_ALLOWABLE_ERROR,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.configForwardSoftLimitThreshold(
int(LifterComponent.CARRIAGE_MAX_HEIGHT /
LifterComponent.CARRIAGE_CONV_FACTOR), 0)
self.carriage_motor.config_kF(0, LifterComponent.ELEVATOR_kF,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kP(0, LifterComponent.ELEVATOR_kP,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kI(0, LifterComponent.ELEVATOR_kI,
LifterComponent.TIMEOUT_MS)
self.carriage_motor.config_kD(0, LifterComponent.ELEVATOR_kD,
LifterComponent.TIMEOUT_MS)
# self.carriage_motor.setCurrentLimit()
# self.carriage_motor.EnableCurrentLimit()
# self.carriage_motor.configVoltageCompSaturation(4, LifterComponent.TIMEOUT_MS)
def set_elevator_speed(self, speed):
if (speed > 0 and self.current_elevator_position >= LifterComponent.ELEVATOR_MAX_HEIGHT - 2) \
or (speed < 0 and self.elevator_bottom_switch.get()):
self.elevator_motor.set(0)
else:
self.elevator_motor.set(speed)
self.trigger_event(LifterComponent.EVENTS.on_manual_move)
def set_carriage_speed(self, speed):
if (speed > 0 and self.carriage_top_switch.get()) \
or (speed < 0 and self.carriage_bottom_switch.get()):
self.carriage_motor.set(0)
else:
self.carriage_motor.set(speed)
self.trigger_event(LifterComponent.EVENTS.on_manual_move)
def reset_sensors(self):
self.carriage_motor.setSelectedSensorPosition(
0, 0, LifterComponent.TIMEOUT_MS)
self.elevator_motor.setSelectedSensorPosition(
0, 0, LifterComponent.TIMEOUT_MS)
self._is_reset = True
@property
def current_elevator_position(self) -> float:
return self.elevator_motor.getSelectedSensorPosition(
0) * LifterComponent.ELEVATOR_CONV_FACTOR
@property
def current_carriage_position(self) -> float:
return self.carriage_motor.getSelectedSensorPosition(
0) * LifterComponent.CARRIAGE_CONV_FACTOR
@property
def current_position(self) -> float:
return self.current_elevator_position + self.current_carriage_position
def stop_lift(self):
self.elevator_motor.stopMotor()
self.carriage_motor.stopMotor()
def elevator_to_target_position(self, inches: float):
if self._is_reset:
self.elevator_motor.set(
WPI_TalonSRX.ControlMode.Position,
inches / LifterComponent.ELEVATOR_CONV_FACTOR)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def carriage_to_target_position(self, inches: float):
if self._is_reset:
self.carriage_motor.set(
WPI_TalonSRX.ControlMode.Position,
inches / LifterComponent.CARRIAGE_CONV_FACTOR)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def lift_to_distance(self, inches):
i = inches + 6
elevator = min(i * LifterComponent.ELEVATOR_MULTIPLIER,
LifterComponent.ELEVATOR_MAX_HEIGHT)
carriage = i - elevator
print("lift_to_distance carriage" + str(carriage))
print("lift_to_distance elevate" + str(elevator))
print("lift_to_distance lifter" + str(carriage + elevator))
self.elevator_to_target_position(elevator)
self.carriage_to_target_position(carriage)
self.trigger_event(LifterComponent.EVENTS.on_control_move)
def is_at_position(self, position: str) -> bool:
return self.is_at_distance(LifterComponent.positions[position])
def is_at_distance(self, inches: float) -> bool:
return inches - 5 < self.current_position < inches + 5
def closest_position(self) -> tuple:
# returns the key for the position closest to the current position
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
return min(
positions,
key=lambda position: abs(self.current_position - position[1]))
def next_position(self) -> str:
position = self.closest_position()
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
index = positions.index(position)
if index == len(positions) - 1:
return position[0]
return positions[index + 1][0]
def prev_position(self) -> str:
position = self.closest_position()
positions = [(key, position)
for key, position in LifterComponent.positions.items()]
index = positions.index(position)
if index == 0:
return position[0]
return positions[index - 1][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 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.lastButton1 = False
self.targetPos = 0
# choose the sensor and sensor direction
self.talon.configSelectedFeedbackSensor(
WPI_TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# choose to ensure sensor is positive when output is positive
self.talon.setSensorPhase(True)
# choose based on what direction you want forward/positive to be.
# This does not affect sensor phase.
self.talon.setInverted(False)
# set the peak and nominal outputs, 12V means full
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 the allowable closed-loop error, Closed-Loop output will be
# neutral within this range. See Table in Section 17.2.1 for native
# units per rotation.
self.talon.configAllowableClosedloopError(0, self.kPIDLoopIdx,
self.kTimeoutMs)
# set closed loop gains in slot0, typically kF stays zero - see documentation */
self.talon.selectProfileSlot(self.kSlotIdx, self.kPIDLoopIdx)
self.talon.config_kF(0, 0, self.kTimeoutMs)
self.talon.config_kP(0, 0.1, self.kTimeoutMs)
self.talon.config_kI(0, 0, self.kTimeoutMs)
self.talon.config_kD(0, 0, 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 = self.joy.getY()
# calculate the percent motor output
motorOutput = self.talon.getMotorOutputPercent()
button1 = self.joy.getRawButton(1)
button2 = self.joy.getRawButton(2)
# deadband gamepad
if abs(leftYstick) < 0.1:
leftYstick = 0
# prepare line to print
sb = []
sb.append("\tOut%%: %.3f" % motorOutput)
sb.append("\tPos: %.3fu" %
self.talon.getSelectedSensorPosition(self.kPIDLoopIdx))
if self.lastButton1 and button1:
# Position mode - button just pressed
# 10 Rotations * 4096 u/rev in either direction
self.targetPos = leftYstick * 4096 * 10.0
self.talon.set(WPI_TalonSRX.ControlMode.Position, self.targetPos)
# on button2 just straight drive
if button2:
# Percent voltage mode
self.talon.set(WPI_TalonSRX.ControlMode.PercentOutput, leftYstick)
if self.talon.getControlMode() == WPI_TalonSRX.ControlMode.Position:
# append more signals to print when in speed mode.
sb.append("\terr: %s" %
self.talon.getClosedLoopError(self.kPIDLoopIdx))
sb.append("\ttrg: %.3f" % self.targetPos)
# periodically print to console
self.loops += 1
if self.loops >= 10:
self.loops = 0
print(" ".join(sb))
# save button state for on press detect
self.lastButton1 = button1
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)
class DriveTrain(Subsystem):
'''
'Tank Drive' system set up with 2 motors per side, one a "master"
with a mag encoder attached and the other "slave" controller set
to follow the "master".
'''
def __init__(self, robot):
self.robot = robot
self.ahrs = AHRS.create_spi()
self.ahrs.reset()
# self.angleAdjustment = self.ahrs.getAngle()
# self.ahrs.setAngleAdjustment(self.angleAdjustment)
# 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)
self.driveLeftMaster.configNominalOutputForward(0, 0)
self.driveLeftMaster.configNominalOutputReverse(0, 0)
self.driveRightMaster.configNominalOutputForward(0, 0)
self.driveRightMaster.configNominalOutputReverse(0, 0)
self.speed = .4
self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)
self.driveRightMaster.configPeakOutputForward(self.speed, 0)
self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)
self.driveLeftMaster.configClosedLoopRamp(.2, 0)
self.driveRightMaster.configClosedLoopRamp(.2, 0)
self.driveLeftMaster.setSafetyEnabled(False)
self.driveRightMaster.setSafetyEnabled(False)
# 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)
self.PID()
"""
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)
self.driveLeftMaster.setSelectedSensorPosition(0, 0, 0)
self.driveRightMaster.setSelectedSensorPosition(0, 0, 0)
# 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.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)
self.drive.setSafetyEnabled(False)
self.previousError = 0
super().__init__()
def autoInit(self):
self.speed = .5
self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)
self.driveRightMaster.configPeakOutputForward(self.speed, 0)
self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)
self.driveLeftMaster.config_kP(0, .115, 0)
self.driveRightMaster.config_kP(0, .115, 0)
# self.driveLeftMaster.config_kP(0, .185, 0)
# self.driveRightMaster.config_kP(0, .185, 0)
# self.driveLeftMaster.config_kP(0, 20, 0)
# self.driveRightMaster.config_kP(0, 20, 0)
self.driveLeftMaster.config_kF(0, 0.0, 0)
self.driveRightMaster.config_kF(0, 0.0, 0)
def teleInit(self):
self.speed = .55
self.driveLeftMaster.configPeakOutputForward(self.speed, 0)
self.driveLeftMaster.configPeakOutputReverse(-self.speed, 0)
self.driveRightMaster.configPeakOutputForward(self.speed, 0)
self.driveRightMaster.configPeakOutputReverse(-self.speed, 0)
self.driveLeftMaster.config_kP(0, 0.0, 0)
self.driveRightMaster.config_kP(0, 0.0, 0)
self.driveLeftMaster.config_kF(0, 0.313, 0)
self.driveRightMaster.config_kF(0, 0.313, 0)
def moveToPosition(self, position):
self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Position,
position)
self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Position,
position)
def stop(self):
self.drive.stopMotor()
def arcade(self, speed, rotation):
# self.updateSD()
if self.robot.dStick.getRawButtonReleased(3):
self.robotFrontToggleCount += 1
"""
This if statement acts as a toggle to change which motors are
inverted, completely changing the "front" of the robot. This is
useful for when we are about to climb.
"""
if self.robotFrontToggleCount % 2 == 0:
self.drive.arcadeDrive(speed, rotation, True)
else:
self.drive.arcadeDrive(-speed, rotation, True)
def arcadeWithRPM(self, speed, rotation, maxRPM):
# self.updateSD()
self.driveLeftMaster.setSafetyEnabled(False)
if self.robot.dStick.getRawButtonReleased(3):
self.robotFrontToggleCount += 1
if self.robotFrontToggleCount % 2 == 0:
XSpeed = wpilib.RobotDrive.limit(speed)
else:
XSpeed = wpilib.RobotDrive.limit(-speed)
XSpeed = self.applyDeadband(XSpeed, .02)
ZRotation = wpilib.RobotDrive.limit(rotation)
ZRotation = self.applyDeadband(ZRotation, .02)
XSpeed = math.copysign(XSpeed * XSpeed, XSpeed)
ZRotation = math.copysign(ZRotation * ZRotation, ZRotation)
maxInput = math.copysign(max(abs(XSpeed), abs(ZRotation)), XSpeed)
if XSpeed >= 0.0:
if ZRotation >= 0.0:
leftMotorSpeed = maxInput
rightMotorSpeed = XSpeed - ZRotation
else:
leftMotorSpeed = XSpeed + ZRotation
rightMotorSpeed = maxInput
else:
if ZRotation >= 0.0:
leftMotorSpeed = XSpeed + ZRotation
rightMotorSpeed = maxInput
else:
leftMotorSpeed = maxInput
rightMotorSpeed = XSpeed - ZRotation
leftMotorSpeed = wpilib.RobotDrive.limit(leftMotorSpeed)
rightMotorSpeed = wpilib.RobotDrive.limit(rightMotorSpeed)
leftMotorRPM = leftMotorSpeed * maxRPM
rightMotorRPM = rightMotorSpeed * maxRPM
self.driveLeftMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity,
leftMotorRPM)
self.driveRightMaster.set(ctre.talonsrx.TalonSRX.ControlMode.Velocity,
rightMotorRPM)
def updateSD(self):
leftVel = self.driveLeftMaster.getSelectedSensorVelocity(0)
leftPos = self.driveLeftMaster.getSelectedSensorPosition(0)
rightVel = self.driveRightMaster.getSelectedSensorVelocity(0)
rightPos = self.driveRightMaster.getSelectedSensorPosition(0)
# calculate side deltas
if self.leftVel:
leftVelDelta = leftVel - self.leftVel
else:
leftVelDelta = 0
if self.leftPos:
leftPosDelta = leftPos - self.leftPos
else:
leftPosDelta = 0
if self.rightVel:
rightVelDelta = rightVel - self.rightVel
else:
rightVelDelta = 0
if self.rightPos:
rightPosDelta = rightPos - self.rightPos
else:
rightPosDelta = 0
# calculate delta of delta
differenceVel = leftVelDelta - rightVelDelta
differencePos = leftPosDelta - rightPosDelta
SD.putNumber("LeftSensorVel", leftVel)
SD.putNumber("LeftSensorPos", leftPos)
SD.putNumber("RightSensorVel", rightVel)
SD.putNumber("RightSensorPos", rightPos)
SD.putNumber('LeftVelDelta', leftVelDelta)
SD.putNumber('LeftPosDelta', leftPosDelta)
SD.putNumber('RightVelDelta', rightVelDelta)
SD.putNumber('RightPosDelta', rightPosDelta)
SD.putNumber('DifferenceVel', differenceVel)
SD.putNumber('DifferencePos', differencePos)
SD.putNumber('Angle', self.ahrs.getAngle())
SD.putNumber('Angle Adjustment', self.ahrs.getAngleAdjustment())
self.leftVel = leftVel
self.leftPos = leftPos
self.rightVel = rightVel
self.rightPos = rightPos
# kP = self.driveLeftMaster.configGetParameter(
# self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)
# SmartDashboard.putNumber('Left Proportional', kP)
# 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))
def applyDeadband(self, value, deadband):
"""Returns 0.0 if the given value is within the specified range around zero. The remaining range
between the deadband and 1.0 is scaled from 0.0 to 1.0.
:param value: value to clip
:param deadband: range around zero
"""
if abs(value) > deadband:
if value < 0.0:
return (value - deadband) / (1.0 - deadband)
else:
return (value + deadband) / (1.0 - deadband)
return 0.0
def setAngle(self, angle, tolerance):
#self.tolerance = tolerance
#self.calculateAdjustedSetpoint(angle)
self.turnController.setSetpoint(angle)
if ((self.ahrs.getYaw() <= (angle + tolerance))
and (self.ahrs.getYaw() >= (angle - tolerance))):
self.turnController.disable()
self.driveLeftMaster.set(0)
self.driveRightMaster.set(0)
else:
self.turnController.enable()
self.drive.arcadeDrive(0, self.output)
#self.leftTurnController.setSetpoint(angle)
def isInGyroPosition(self):
SD.putNumber('Is in gyro position',
((self.ahrs.getYaw() <=
(self.turnController.getSetpoint() +
self.robot.autonomous.ANGLE_TOLERANCE)) and
(self.ahrs.getYaw() >=
(self.turnController.getSetpoint() -
self.robot.autonomous.ANGLE_TOLERANCE))))
return ((self.ahrs.getYaw() <= (self.turnController.getSetpoint() +
self.robot.autonomous.ANGLE_TOLERANCE))
and (self.ahrs.getYaw() >=
(self.turnController.getSetpoint() -
self.robot.autonomous.ANGLE_TOLERANCE)))
def calculateAdjustedSetpoint(self, angle):
self.startingYaw = self.robot.autonomous.startingYaw
adjustedAngle = angle + self.startingYaw
if adjustedAngle < -180:
undershot = adjustedAngle + 180
adjustedAngle = 180 + undershot
elif adjustedAngle > 180:
overshot = adjustedAngle - 180
adjustedAngle = -180 + overshot
self.adjustedSetpoint = adjustedAngle
def PID(self):
self.kP = .045
self.kI = 0.00
self.kD = 0.00
self.kF = 0.00
self.turnController = wpilib.PIDController(self.kP,
self.kI,
self.kD,
self.kF,
self.ahrs,
output=self)
self.turnController.setInputRange(-180, 180)
self.turnController.setOutputRange(-0.55, 0.55)
self.turnController.disable()
def pidWrite(self, output):
self.output = output