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 Robot(wpilib.TimedRobot):
def threshold(self, value, limit):
if (abs(value) < limit):
return 0
else:
return round(value, 2)
def robotInit(self):
self.kSlotIdx = 0
self.kPIDLoopIdx = 0
self.kTimeoutMs = 10
# Sets the speed
self.speed = 0.4
self.ySpeed = 1
self.tSpeed = 0.75
self.baseIntakeSpeed = 5
self.previousAvg = 0
self.currentAvg = 0
# Smart Dashboard
self.sd = NetworkTables.getTable('SmartDashboard')
# joysticks 1 & 2 on the driver station
self.driverJoystick = wpilib.Joystick(0)
# Create a simple timer (docs: https://robotpy.readthedocs.io/projects/wpilib/en/latest/wpilib/Timer.html#wpilib.timer.Timer.get)
self.timer = wpilib.Timer()
# TODO: Fix module number
self.compressor = wpilib.Compressor()
# TODO: Fix module numbers
self.intake = Intake(0, 0, 0, 0, 0, 0, 0)
# Talon CAN devices
# TODO: Fix module numbers
self.frontLeftTalon = WPI_TalonSRX(2)
self.rearLeftTalon = WPI_TalonSRX(0)
self.frontRightTalon = WPI_TalonSRX(3)
self.rearRightTalon = WPI_TalonSRX(1)
self.rightPistonButton = ButtonDebouncer(driverJoystick, 4)
self.leftPistonButton = ButtonDebouncer(driverJoystick, 5)
# Enable auto breaking
self.frontLeftTalon.setNeutralMode(NeutralMode.Brake)
self.rearLeftTalon.setNeutralMode(NeutralMode.Brake)
self.frontRightTalon.setNeutralMode(NeutralMode.Brake)
self.rearRightTalon.setNeutralMode(NeutralMode.Brake)
# Setup encoders
self.frontLeftTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.rearLeftTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.frontRightTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
self.rearRightTalon.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative,
self.kPIDLoopIdx,
self.kTimeoutMs,
)
# Setup encoders
self.leftEncoder = self.rearLeftTalon
self.rightEncoder = self.rearRightTalon
def autonomousInit(self):
"""Called only at the beginning of autonomous mode."""
pass
def autonomousPeriodic(self):
"""Called every 20ms in autonomous mode."""
pass
def teleopInit(self):
self.compressor.start()
self.frontLeftTalon.set(ControlMode.Speed)
self.frontRightTalon.set(ControlMode.Speed)
self.rearLeftTalon.set(ControlMode.Speed)
self.rearRightTalon.set(ControlMode.Speed)
pass
def teleopPeriodic(self):
# Get max speed
self.speed = (-self.driverJoystick.getRawAxis(3) + 1) / 2
self.intake.test(True, self.rightPistonButton.get())
self.intake.test(False, self.leftPistonButton.get())
# Get turn and movement speeds
self.tAxis = self.threshold(self.driverJoystick.getRawAxis(2),
0.05) * self.tSpeed * self.speed
self.yAxis = self.threshold(-self.driverJoystick.getRawAxis(1),
0.05) * self.ySpeed * self.speed
# Calculate right and left speeds
leftSpeed = self.yAxis + self.tAxis
rightSpeed = self.yAxis - self.tAxis
# Update Motors
self.frontLeftTalon.set(ControlMode.PercentOutput, leftSpeed)
self.rearLeftTalon.set(ControlMode.PercentOutput, leftSpeed)
self.frontRightTalon.set(ControlMode.PercentOutput, rightSpeed)
self.rearRightTalon.set(ControlMode.PercentOutput, rightSpeed)
# Update SmartDashboard
self.sd.putNumber(
"Left Encoder",
self.leftEncoder.getSelectedSensorPosition(self.kPIDLoopIdx))
self.sd.putNumber(
"Right Encoder",
self.rightEncoder.getSelectedSensorPosition(self.kPIDLoopIdx))
class MyRobot(wpilib.TimedRobot):
WHEEL_DIAMETER = 6 # 6 inches
WHEEL_CIRCUMFERENCE = math.pi * WHEEL_DIAMETER
CAN_BUS_TIMEOUT_MS = 10
ENCODER_COUNTS_PER_REV = 4096
PIGEON_UNITS_PER_ROTATION = 8192
LEFT_MASTER_CAN_ID = 4
RIGHT_MASTER_CAN_ID = 2
LEFT_SLAVE_CAN_ID = 3
RIGHT_SLAVE_CAN_ID = 1
PIGEON_IMU_CAN_ID = 6
ENCODER_SUM_CAN_ID = 5
PRIMARY_PID_LOOP_GAINS_SLOT = 0
AUX_PID_LOOP_GAINS_SLOT = 1
PRIMARY_PID_LOOP = 0
AUX_PID_LOOP = 1
BASE_TRAJECTORY_PERIOD_MS = 20
VELOCITY_MULTIPLIER = 1
def robotInit(self):
self.timer = wpilib.Timer()
self.timer.start()
self.stick = XboxController(0)
self.dummyTalon = WPI_TalonSRX(self.ENCODER_SUM_CAN_ID)
self.leftTalonMaster = WPI_TalonSRX(self.LEFT_MASTER_CAN_ID)
self.leftTalonSlave = WPI_TalonSRX(self.LEFT_SLAVE_CAN_ID)
self.rightTalonMaster = WPI_TalonSRX(self.RIGHT_MASTER_CAN_ID)
self.rightTalonSlave = WPI_TalonSRX(self.RIGHT_SLAVE_CAN_ID)
self.leftTalonSlave.set(ControlMode.Follower, self.LEFT_MASTER_CAN_ID)
self.rightTalonSlave.set(ControlMode.Follower,
self.RIGHT_MASTER_CAN_ID)
self.drive = wpilib.RobotDrive(self.leftTalonMaster,
self.rightTalonMaster)
#self.pigeon = PigeonIMU(self.PIGEON_IMU_CAN_ID)
#if not self.isSimulation():
#self.pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, self.CAN_BUS_TIMEOUT_MS)
#else:
# print("setStatusFramePeriod() is not implmented in pyfrc simulator")
self.masterTalons = [self.leftTalonMaster, self.rightTalonMaster]
self.slaveTalons = [self.leftTalonSlave, self.rightTalonSlave]
self.leftTalons = [self.leftTalonMaster, self.leftTalonSlave]
self.rightTalons = [self.rightTalonMaster, self.rightTalonSlave]
self.talons = [
self.leftTalonMaster, self.leftTalonSlave, self.rightTalonMaster,
self.rightTalonSlave
]
'''Common configuration items common for all talons'''
for talon in self.talons:
talon.configNominalOutputForward(0.0, self.CAN_BUS_TIMEOUT_MS)
talon.configNominalOutputReverse(0.0, self.CAN_BUS_TIMEOUT_MS)
talon.configPeakOutputForward(1.0, self.CAN_BUS_TIMEOUT_MS)
talon.configPeakOutputReverse(-1.0, self.CAN_BUS_TIMEOUT_MS)
talon.enableVoltageCompensation(True)
talon.configVoltageCompSaturation(11.5, self.CAN_BUS_TIMEOUT_MS)
talon.configOpenLoopRamp(0.125, self.CAN_BUS_TIMEOUT_MS)
self.leftTalonSlave.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
self.leftTalonSlave.configSelectedFeedbackCoefficient(
1.0, self.PRIMARY_PID_LOOP, self.CAN_BUS_TIMEOUT_MS)
self.leftTalonSlave.setSensorPhase(False)
self.leftTalonSlave.setStatusFramePeriod(
StatusFrame.Status_2_Feedback0, 10, self.CAN_BUS_TIMEOUT_MS)
self.rightTalonSlave.configSelectedFeedbackSensor(
FeedbackDevice.CTRE_MagEncoder_Relative, self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
self.rightTalonSlave.configSelectedFeedbackCoefficient(
1.0, self.PRIMARY_PID_LOOP, self.CAN_BUS_TIMEOUT_MS)
self.rightTalonSlave.setSensorPhase(False)
self.rightTalonSlave.setStatusFramePeriod(
StatusFrame.Status_2_Feedback0, 10, self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configRemoteFeedbackFilter(
self.leftTalonSlave.getDeviceID(),
RemoteSensorSource.TalonSRX_SelectedSensor, 0,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configRemoteFeedbackFilter(
self.rightTalonSlave.getDeviceID(),
RemoteSensorSource.TalonSRX_SelectedSensor, 1,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configSensorTerm(0, FeedbackDevice.RemoteSensor0,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configSensorTerm(1, FeedbackDevice.RemoteSensor1,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configSelectedFeedbackSensor(FeedbackDevice.SensorSum,
self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.configSelectedFeedbackCoefficient(
1.0, self.PRIMARY_PID_LOOP, self.CAN_BUS_TIMEOUT_MS)
self.dummyTalon.setStatusFramePeriod(StatusFrame.Status_2_Feedback0,
10, self.CAN_BUS_TIMEOUT_MS)
for talon in self.leftTalons:
talon.setInverted(True)
for talon in self.rightTalons:
talon.setInverted(False)
self.leftTalonMaster.setSensorPhase(True)
if not self.isSimulation():
'''Setup the "sum" sensor as remote sensor 0'''
self.leftTalonMaster.configRemoteFeedbackFilter(
self.leftTalonSlave.getDeviceID(),
RemoteSensorSource.TalonSRX_SelectedSensor, 0,
self.CAN_BUS_TIMEOUT_MS)
'''Setup the pigeon as remote sensor 1'''
#self.leftTalonMaster.configRemoteFeedbackFilter(self.PIGEON_IMU_CAN_ID, RemoteSensorSource.Pigeon_Yaw, 1, self.CAN_BUS_TIMEOUT_MS)
else:
print(
"configRemoteFeedbackFilter() is not implemented in pyfrc simulator"
)
if not self.isSimulation():
'''Setup the "sum" sensor as remote sensor 0'''
self.rightTalonMaster.configRemoteFeedbackFilter(
self.rightTalonSlave.getDeviceID(),
RemoteSensorSource.TalonSRX_SelectedSensor, 0,
self.CAN_BUS_TIMEOUT_MS)
'''Setup the pigeon as remote sensor 1'''
#self.rightTalonMaster.configRemoteFeedbackFilter(self.PIGEON_IMU_CAN_ID, RemoteSensorSource.Pigeon_Yaw, 1, self.CAN_BUS_TIMEOUT_MS)
else:
print(
"configRemoteFeedbackFilter() is not implemented in pyfrc simulator"
)
'''
Now that the sensor sum remote sensor is setup, we can setup the master talons close-loop configuration
'''
for talon in self.masterTalons:
talon.configMotionProfileTrajectoryPeriod(
self.BASE_TRAJECTORY_PERIOD_MS)
'''
This just loads the constants into "slots".
Later we will select the slots to use for the primary and aux PID loops.
'''
talon.config_kP(self.PRIMARY_PID_LOOP_GAINS_SLOT, 0.375,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kI(self.PRIMARY_PID_LOOP_GAINS_SLOT, 0.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kD(self.PRIMARY_PID_LOOP_GAINS_SLOT, 0.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kF(self.PRIMARY_PID_LOOP_GAINS_SLOT, 0.35,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kP(self.AUX_PID_LOOP_GAINS_SLOT, 7.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kI(self.AUX_PID_LOOP_GAINS_SLOT, 0.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kD(self.AUX_PID_LOOP_GAINS_SLOT, 8.0,
self.CAN_BUS_TIMEOUT_MS)
talon.config_kF(self.AUX_PID_LOOP_GAINS_SLOT, 0.0,
self.CAN_BUS_TIMEOUT_MS)
'''
Select the gains to use for the position control loop.
It probably doesn't matter what we select here since each individual trajectory point
has a setting for the primary and aux PID fains to use.
Selecting these gains here would be important if we were using motion magic control.
'''
talon.selectProfileSlot(self.PRIMARY_PID_LOOP_GAINS_SLOT,
self.PRIMARY_PID_LOOP)
'''This says the position control loop is allowed to command full motor output'''
talon.configClosedLoopPeakOutput(self.PRIMARY_PID_LOOP_GAINS_SLOT,
1.0, self.CAN_BUS_TIMEOUT_MS)
'''
Select the gains to use for the heading control loop.
It probably doesn't matter what we select here since each individual trajectory point
has a setting for the primary and aux PID fains to use.
Selecting these gains here would be important if we were using motion magic control.
'''
talon.selectProfileSlot(self.AUX_PID_LOOP_GAINS_SLOT,
self.AUX_PID_LOOP)
'''This says the heading control loop is allowed to command full motor output'''
talon.configClosedLoopPeakOutput(self.AUX_PID_LOOP_GAINS_SLOT, 1.0,
self.CAN_BUS_TIMEOUT_MS)
'''Select remote sensor 0 (the "sum" sensor) as the feedback for the primary PID loop (position control loop)'''
talon.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0,
self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
'''Select remote sensor 1 (the pigeon) as the feedback for the aux PID loop (heading control loop)'''
talon.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor1,
self.AUX_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
'''This sets the scale factor on the feedback sensor. For the encoders it is 1.0, this is really used for the gyro (pigeon) feedback'''
talon.configSelectedFeedbackCoefficient(1.0, self.PRIMARY_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
'''This sets the scale factor on the feedback sensor. Finally we use something other than 1.0'''
talon.configSelectedFeedbackCoefficient(
3600 / self.PIGEON_UNITS_PER_ROTATION, self.AUX_PID_LOOP,
self.CAN_BUS_TIMEOUT_MS)
if not self.isSimulation():
self.leftTalonMaster.configAuxPIDPolarity(False,
self.CAN_BUS_TIMEOUT_MS)
self.rightTalonMaster.configAuxPIDPolarity(False,
self.CAN_BUS_TIMEOUT_MS)
else:
print(
"configAuxPIDPolarity() is not implemented in pyfrc simulator")
#SmartDashboard.putString("DRIVE_VALUE", "DRIVE_VALUES")
SmartDashboard.putString("LEFT_Y_VALUES", "LEFT_Y_VALUES")
SmartDashboard.putNumber("left_y_rate", 0.6)
SmartDashboard.putNumber("left_y_expo", 1.0)
SmartDashboard.putNumber("left_y_deadband", 0.1)
SmartDashboard.putNumber("left_y_power", 1.5)
SmartDashboard.putNumber("left_y_min", -0.5)
SmartDashboard.putNumber("left_y_max", 0.5)
SmartDashboard.putString("LEFT_X_VALUES", "LEFT_X_VALUES")
SmartDashboard.putNumber("left_x_rate", 0.5)
SmartDashboard.putNumber("left_x_expo", 1.0)
SmartDashboard.putNumber("left_x_deadband", 0.1)
SmartDashboard.putNumber("left_x_power", 1.5)
SmartDashboard.putNumber("left_x_min", -0.5)
SmartDashboard.putNumber("left_x_max", 0.5)
SmartDashboard.putString("RIGHT_Y_VALUES", "RIGHT_Y_VALUES")
SmartDashboard.putNumber("right_y_rate", 1.0)
SmartDashboard.putNumber("right_y_expo", 0.0)
SmartDashboard.putNumber("right_y_deadband", 0.1)
SmartDashboard.putNumber("right_y_power", 1.0)
SmartDashboard.putNumber("right_y_min", -1.0)
SmartDashboard.putNumber("right_y_max", 1.0)
SmartDashboard.putString("RIGHT_X_VALUES", "RIGHT_X_VALUES")
SmartDashboard.putNumber("right_x_rate", 0.5)
SmartDashboard.putNumber("right_x_expo", 1.0)
SmartDashboard.putNumber("right_x_deadband", 0.1)
SmartDashboard.putNumber("right_x_power", 1.5)
SmartDashboard.putNumber("right_x_min", -0.5)
SmartDashboard.putNumber("right_x_max", 0.5)
def autonomousInit(self):
#if not self.isSimulation():
# self.pigeon.setYaw(0, self.CAN_BUS_TIMEOUT_MS)
# self.pigeon.setFusedHeading(0, self.CAN_BUS_TIMEOUT_MS)
#else:
# print("pigeon.setYaw() is not implemented in pyfrc simulator")
if not self.isSimulation():
self.leftTalonSlave.setSelectedSensorPosition(
0, 0, self.CAN_BUS_TIMEOUT_MS)
self.leftTalonSlave.getSensorCollection().setQuadraturePosition(
0, self.CAN_BUS_TIMEOUT_MS)
if not self.isSimulation():
self.leftTalonMaster.clearMotionProfileTrajectories()
self.leftTalonMaster.clearMotionProfileHasUnderrun(0)
self.leftTalonMaster.set(ControlMode.MotionProfile, 0)
if not self.isSimulation():
self.rightTalonSlave.setSelectedSensorPosition(
0, 0, self.CAN_BUS_TIMEOUT_MS)
self.rightTalonSlave.getSensorCollection().setQuadraturePosition(
0, self.CAN_BUS_TIMEOUT_MS)
if not self.isSimulation():
self.rightTalonMaster.clearMotionProfileTrajectories()
self.rightTalonMaster.clearMotionProfileHasUnderrun(0)
self.rightTalonMaster.set(ControlMode.MotionProfile, 0)
if not self.isSimulation():
with open("/home/lvuser/traj", "rb") as fp:
trajectory = pickle.load(fp)
else:
with open("/home/ubuntu/traj", "rb") as fp:
trajectory = pickle.load(fp)
print("Length of trajectory: " + str(len(trajectory)))
modifier = pf.modifiers.TankModifier(trajectory).modify(
2.1) #Wheelbase in feet
self.leftTrajectory = modifier.getLeftTrajectory()
self.rightTrajectory = modifier.getRightTrajectory()
for i in range(len(trajectory)):
leftSeg = self.leftTrajectory[i]
rightSeg = self.rightTrajectory[i]
velCorrection = (rightSeg.velocity -
leftSeg.velocity) * self.VELOCITY_MULTIPLIER
lposition = self.inchesToUnits((leftSeg.position) * 12)
rposition = self.inchesToUnits((rightSeg.position) * 12)
aux_position = 0
slot0 = self.PRIMARY_PID_LOOP_GAINS_SLOT
slot1 = self.AUX_PID_LOOP_GAINS_SLOT
timeDur = 0
zeroPos = i == 0
isLastPoint = i == len(trajectory) - 1
lvelocity = self.inchesToUnits(leftSeg.velocity * 12) / 10
rvelocity = self.inchesToUnits(rightSeg.velocity * 12) / 10
'''There was no empty constructor.'''
print(
f'position: {lposition}, aux_position: {aux_position}, lvelcoity: {lvelocity}, rvelocity: {rvelocity}'
)
self.leftTrajectory[i] = TrajectoryPoint(lposition, lvelocity,
aux_position, slot0,
slot1, isLastPoint,
zeroPos, timeDur)
self.rightTrajectory[i] = TrajectoryPoint(rposition, rvelocity,
aux_position, slot0,
slot1, isLastPoint,
zeroPos, timeDur)
if not self.isSimulation():
for point in self.leftTrajectory:
self.leftTalonMaster.pushMotionProfileTrajectory(point)
#print("Top buffer count left: " + str(self.leftTalonMaster.getMotionProfileStatus().topBufferCnt))
if not self.isSimulation():
for point in self.rightTrajectory:
self.rightTalonMaster.pushMotionProfileTrajectory(point)
#print("Top buffer count right: " + str(self.rightTalonMaster.getMotionProfileStatus().topBufferCnt))
self.leftDone = False
self.rightDone = False
self.motionProfileEnabled = False
self.bufferProcessingStartTime = 0
self.executionStartTime = 0
self.executionFinishTime = 0
def autonomousPeriodic(self):
self.leftMPStatus = self.leftTalonMaster.getMotionProfileStatus()
self.rightMPStatus = self.rightTalonMaster.getMotionProfileStatus()
'''
The processMotionProfileBuffer() moves trajectory points from the
"Top" level buffer into the "Bottom" level buffer. This just means
they are moved from a buffer inside the roboRIO into a buffer inside
the Talon firmware itself. The more sophisticated method is to call
this function on a seperate background thread which is running at a rate which
is twice as fast as the duration of the trajectory points. Then in the main thread
check that the bottom buffer count is high enough to trigger the motion
profile execution to begin. That allows the motion profile execution to
begin sooner. For this test we can just wait until the top buffer has
been completely emptied into the bottom buffer.
'''
'''Let's time this to see if it's really worth doing it the more sophisticated way'''
if not self.motionProfileEnabled and self.leftMPStatus.btmBufferCnt == 0 and self.rightMPStatus.btmBufferCnt == 0:
print("Beginning to funnel trajectory points into the Talons")
self.bufferProcessingStartTime = self.timer.getFPGATimestamp()
'''
Don't print anything while funneling points into the Talons.
Printing will slow down execution and we want to measure time
that it took to do this operation.
'''
if self.leftMPStatus.topBufferCnt > 0 and self.leftMPStatus.btmBufferCnt < 50:
self.leftTalonMaster.processMotionProfileBuffer()
if self.rightMPStatus.topBufferCnt > 0 and self.rightMPStatus.btmBufferCnt < 50:
self.rightTalonMaster.processMotionProfileBuffer()
if not self.motionProfileEnabled and \
self.leftMPStatus.btmBufferCnt > 20 and \
self.rightMPStatus.btmBufferCnt > 20:
self.leftTalonMaster.set(ControlMode.MotionProfile, 1)
self.rightTalonMaster.set(ControlMode.MotionProfile, 1)
self.motionProfileEnabled = True
self.executionStartTime = self.timer.getFPGATimestamp()
print("Beginning motion profile execution")
if self.leftMPStatus.isLast and self.leftMPStatus.outputEnable == SetValueMotionProfile.Enable and not self.leftDone:
self.leftTalonMaster.neutralOutput()
self.leftTalonMaster.set(ControlMode.PercentOutput, 0)
self.leftDone = True
print("Left motion profile is finished executing")
if self.rightMPStatus.isLast and self.rightMPStatus.outputEnable == SetValueMotionProfile.Enable and not self.rightDone:
self.rightTalonMaster.neutralOutput()
self.rightTalonMaster.set(ControlMode.PercentOutput, 0)
self.rightDone = True
print("Right motion profile is finished executing")
'''
It's ok to print debug info on every loop here because the execution
is all happening inside the Talon firmware. However... continuosly
calling into the Talons to get profile status, output percent, closed loop error etc
may cause too much loading on the Talon firware or too much traffic on the
CAN bus network... so maybe need to be careful with that.
'''
if (not self.leftDone
or not self.rightDone) and self.timer.hasPeriodPassed(0.25):
if not self.isSimulation():
lTopCount = self.leftMPStatus.topBufferCnt
rTopCount = self.rightMPStatus.topBufferCnt
lBufCount = self.leftMPStatus.btmBufferCnt
rBufCount = self.rightMPStatus.btmBufferCnt
lOutput = self.leftTalonMaster.getMotorOutputPercent()
rOutput = self.rightTalonMaster.getMotorOutputPercent()
lUnderrun = self.leftMPStatus.hasUnderrun
rUnderrun = self.rightMPStatus.hasUnderrun
lTarget = self.leftTalonMaster.getClosedLoopTarget(
self.PRIMARY_PID_LOOP)
rTarget = self.rightTalonMaster.getClosedLoopTarget(
self.PRIMARY_PID_LOOP)
hTarget = self.rightTalonMaster.getClosedLoopTarget(
self.AUX_PID_LOOP)
lPosition = self.leftTalonMaster.getSelectedSensorPosition(
self.PRIMARY_PID_LOOP)
rPosition = self.rightTalonMaster.getSelectedSensorPosition(
self.PRIMARY_PID_LOOP)
lError = self.leftTalonMaster.getClosedLoopError(
self.PRIMARY_PID_LOOP)
rError = self.rightTalonMaster.getClosedLoopError(
self.PRIMARY_PID_LOOP)
hError = self.rightTalonMaster.getClosedLoopError(
self.AUX_PID_LOOP)
else:
lTopCount = 0
rTopCount = 0
lBufCount = 0
rBufCount = 0
lOutput = 0
rOutput = 0
lUnderrun = 0
rUnderrun = 0
lTarget = 0
rTarget = 0
lPosition = 0
rPosition = 0
hTarget = 0
lError = 0
rError = 0
hError = 0
print(f'\
****************************************\n\
Top Count <{lTopCount}, {rTopCount}>\n\
Bottom Count <{lBufCount}, {rBufCount}>\n\
Motor Output <{lOutput}, {rOutput}>\n\
Underrun <{lUnderrun}, {rUnderrun}>\n\
Closed Loop Target <{lTarget}, {rTarget}, {hTarget}>\n\
Closed Loop Position <{lPosition}, {rPosition}>\n\
Closed Loop Error <{lError}, {rError}, {hError}>\n\
****************************************\n\
')
else:
self.executionFinishTime = self.timer.getFPGATimestamp()
#print("Both Left and Right motion profiles are finished executing")
#print(f'Buffer fill time: {self.executionStartTime - self.bufferProcessingStartTime}')
#print(f'Profile exec time: {self.executionFinishTime - self.executionStartTime}')
def teleopInit(self):
print("MODE: teleopInit")
#if not self.isSimulation():
#self.pigeon.setYaw(0, self.CAN_BUS_TIMEOUT_MS)
#self.pigeon.setFusedHeading(0, self.CAN_BUS_TIMEOUT_MS)
#else:
# print("pigeon.setYaw() is not implemented in pyfrc simulator")
def teleopPeriodic(self):
if self.timer.hasPeriodPassed(0.25):
if not self.isSimulation():
ypr = [0, 0, 0]
#ypr = self.pigeon.getYawPitchRoll()
primary_fdbk = self.rightTalonMaster.getSelectedSensorVelocity(
self.PRIMARY_PID_LOOP)
aux_fdbk = self.rightTalonMaster.getSelectedSensorPosition(
self.AUX_PID_LOOP)
lOutput = self.leftTalonMaster.getMotorOutputPercent()
rOutput = self.rightTalonMaster.getMotorOutputPercent()
lTicks = self.leftTalonMaster.getQuadratureVelocity()
rTicks = self.rightTalonMaster.getQuadratureVelocity()
lSpeed = self.unitsToInches(
self.leftTalonSlave.getSelectedSensorVelocity(
self.PRIMARY_PID_LOOP)) * 10
rSpeed = self.unitsToInches(
self.rightTalonSlave.getSelectedSensorVelocity(
self.PRIMARY_PID_LOOP)) * 10
else:
ypr = [0, 0, 0]
primary_fdbk = 0
aux_fdbk = 0
lOutput = 0
rOutput = 0
lTicks = 0
rTicks = 0
lSpeed = 0
rSpeed = 0
print(f'\
*************teleopPeriodic*************\n\
Sticks <{self.stick.LeftStickX()}, {-self.stick.LeftStickY()}>\n\
Motor Output % <{lOutput}, {rOutput}>\n\
Ticks per 100ms <{lTicks}, {rTicks}>\n\
Left/Right Speed (in/sec) <{lSpeed},{rSpeed}>\n\
Primary/Aux Feedback <{primary_fdbk}, {aux_fdbk}>\n\
Yaw/Pitch/Roll <{ypr[0]}, {ypr[1]}, {ypr[2]}>\n\
****************************************\n\
')
#self.leftTalonMaster.set(ControlMode.PercentOutput, -1.0 * self.stick.getRawAxis(1))
#self.rightTalonMaster.set(ControlMode.PercentOutput, -1.0 * self.stick.getRawAxis(5))
self.drive.arcadeDrive(self.stick.LeftStickX(),
-self.stick.LeftStickY())
def disabledInit(self):
print("MODE: disabledInit")
def disabledPeriodic(self):
if self.timer.hasPeriodPassed(1.0):
print("MODE: disabledPeriodic")
self.stick.pulseRumble(5)
def unitsToInches(self, units):
return units * self.WHEEL_CIRCUMFERENCE / self.ENCODER_COUNTS_PER_REV
def inchesToUnits(self, inches):
return inches * self.ENCODER_COUNTS_PER_REV / self.WHEEL_CIRCUMFERENCE
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):
# Initialize all controllers
self.driveLeftMaster = Talon(kDriveTrain.lmId)
self.driveLeftSlave = Talon(kDriveTrain.lsId)
self.driveRightMaster = Talon(kDriveTrain.rmId)
self.driveRightSlave = Talon(kDriveTrain.rsId)
# Connect the slaves to the masters on each side
self.driveLeftSlave.follow(self.driveLeftMaster)
self.driveRightSlave.follow(self.driveRightMaster)
# Makes sure both sides' controllers show green and use positive
# values to move the bot forward.
# CURRENTLY DISABLED WHEN USING WITH DifferentialDrive
# self.driveLeftSlave.setInverted(False)
# self.driveLeftMaster.setInverted(False)
self.driveRightSlave.setInverted(True)
self.driveRightMaster.setInverted(True)
# Configures each master to use the attached Mag Encoders
self.driveLeftMaster.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.driveRightMaster.configSelectedFeedbackSensor(
ctre.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
# Reverses the encoder direction so forward movement always
# results in a positive increase in the encoder ticks.
self.driveLeftMaster.setSensorPhase(True)
self.driveRightMaster.setSensorPhase(True)
# Throw data on the SmartDashboard so we can work with it.
SD.putNumber('Left Quad Pos.',
self.driveLeftMaster.getQuadraturePosition())
SD.putNumber('Right Quad Pos.',
self.driveRightMaster.getQuadraturePosition())
# these may give the derivitive an integral of the PID once
# they are set. For now, they just show 0
SD.putNumber('Left Derivative',
self.driveLeftMaster.getErrorDerivative(0))
SD.putNumber('Left Integral',
self.driveLeftMaster.getIntegralAccumulator(0))
self.leftVel = None
self.leftPos = None
self.rightVel = None
self.rightPos = None
# self.driveLeftMaster.config_kP(0, .3, 10)
# kP = self.driveLeftMaster.configGetParameter(
# self.driveLeftMaster.ParamEnum.eProfileParamSlot_P, 0, 10)
# SmartDashboard.putNumber('Left Proportional', kP)
self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
self.driveControllerRight.setInverted(True)
self.drive = DifferentialDrive(self.driveControllerLeft,
self.driveControllerRight)
def moveToPosition(self, position, side='left'):
if side == 'left':
self.driveLeftMaster.setSafetyEnabled(False)
self.driveLeftMaster.set(Talon.ControlMode.Position, position)
else:
self.driveRightMaster.set(Talon.ControlMode.Position, position)
def stop(self):
# self.driveLeftMaster.set(0)
# self.driveRightMaster.set(0)
self.drive.stopMotor()
def arcade(self, speed, rotation):
self.updateSD()
self.drive.arcadeDrive(speed, rotation, True)
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)
self.leftVel = leftVel
self.leftPos = leftPos
self.rightVel = rightVel
self.rightPos = rightPos
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 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
# Initialize all controllers
self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master'])
self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave'])
self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master'])
self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave'])
wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster",
self.driveLeftMaster)
wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster",
self.driveRightMaster)
# Connect the slaves to the masters on each side
self.driveLeftSlave.follow(self.driveLeftMaster)
self.driveRightSlave.follow(self.driveRightMaster)
# Makes sure both sides' controllers show green and use positive
# values to move the bot forward.
self.driveLeftSlave.setInverted(False)
self.driveLeftMaster.setInverted(False)
self.driveRightSlave.setInverted(True)
self.driveRightMaster.setInverted(True)
"""
Initializes the count for toggling which side of the
robot will be considered the front when driving.
"""
self.robotFrontToggleCount = 2
# Configures each master to use the attached Mag Encoders
self.driveLeftMaster.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
0)
self.driveRightMaster.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0,
0)
# Reverses the encoder direction so forward movement always
# results in a positive increase in the encoder ticks.
self.driveLeftMaster.setSensorPhase(True)
self.driveRightMaster.setSensorPhase(True)
# these supposedly aren't part of the WPI_TalonSRX class
# self.driveLeftMaster.setSelectedSensorPostion(0, 0, 10)
# self.driveRightMaster.setSelectedSensorPosition(0, 0, 10)
# Throw data on the SmartDashboard so we can work with it.
# SD.putNumber(
# 'Left Quad Pos.',
# self.driveLeftMaster.getQuadraturePosition())
# SD.putNumber(
# 'Right Quad Pos.',
# self.driveRightMaster.getQuadraturePosition())
self.leftVel = None
self.leftPos = None
self.rightVel = None
self.rightPos = None
self.leftMaxVel = 0
self.rightMaxVel = 0
self.leftMinVel = 0
self.rightMinVel = 0
# self.driveLeftMaster.config_kP(0, .3, 10)
self.driveControllerLeft = SpeedControllerGroup(self.driveLeftMaster)
self.driveControllerRight = SpeedControllerGroup(self.driveRightMaster)
self.driveControllerRight.setInverted(True)
self.drive = DifferentialDrive(self.driveControllerLeft,
self.driveControllerRight)
super().__init__()
def moveToPosition(self, position, side='left'):
if side == 'left':
self.driveLeftMaster.setSafetyEnabled(False)
self.driveLeftMaster.set(
ctre.talonsrx.TalonSRX.ControlMode.Position, position)
else:
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):
if self.robot.dStick.getRawButtonReleased(3):
self.robotFrontToggleCount += 1
self.driveLeftMaster.setSafetyEnabled(False)
XSpeed = wpilib.RobotDrive.limit(speed)
XSpeed = self.applyDeadband(XSpeed, .02)
ZRotation = wpilib.RobotDrive.limit(rotation)
ZRotation = self.applyDeadband(ZRotation, .02)
if self.robotFrontToggleCount % 2 == 1:
XSpeed = -XSpeed
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)
# keep the biggest velocity values
if self.leftMaxVel < leftVel:
self.leftMaxVel = leftVel
if self.rightMaxVel < rightVel:
self.rightMaxVel = rightVel
# keep the smallest velocity values
if self.leftMinVel > leftVel:
self.leftMinVel = leftVel
if self.rightMinVel > rightVel:
self.rightMinVel = rightVel
# 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('Left Max Vel', self.leftMaxVel)
SD.putNumber('Right Max Vel', self.rightMaxVel)
SD.putNumber('Left Min Vel', self.leftMinVel)
SD.putNumber('Right Min Vel', self.rightMinVel)
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
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
# Initialize all controllers
self.driveLeftMaster = Talon(self.robot.kDriveTrain['left_master'])
self.driveLeftSlave = Talon(self.robot.kDriveTrain['left_slave'])
self.driveRightMaster = Talon(self.robot.kDriveTrain['right_master'])
self.driveRightSlave = Talon(self.robot.kDriveTrain['right_slave'])
wpilib.LiveWindow.addActuator("DriveTrain", "LeftMaster",
self.driveLeftMaster)
wpilib.LiveWindow.addActuator("DriveTrain", "RightMaster",
self.driveRightMaster)
# Connect the slaves to the masters on each side
self.driveLeftSlave.follow(self.driveLeftMaster)
self.driveRightSlave.follow(self.driveRightMaster)
# Makes sure both sides' controllers show green and use positive
# values to move the bot forward.
self.driveLeftSlave.setInverted(False)
self.driveLeftMaster.setInverted(False)
self.driveRightSlave.setInverted(True)
self.driveRightMaster.setInverted(True)
# 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.setQuadraturePosition(0, 0)
self.driveRightMaster.setQuadraturePosition(0, 0)
# Throw data on the SmartDashboard so we can work with it.
# SD.putNumber(
# 'Left Quad Pos.',
# self.driveLeftMaster.getQuadraturePosition())
# SD.putNumber(
# 'Right Quad Pos.',
# self.driveRightMaster.getQuadraturePosition())
self.leftVel = None
self.leftPos = None
self.rightVel = None
self.rightPos = None
self.leftMaxVel = 0
self.rightMaxVel = 0
# self.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 = DifferentialDrive(self.driveLeftMaster,
# self.driveRightMaster)
wpilib.LiveWindow.addActuator("DriveTrain", "Left Master",
self.driveLeftMaster)
wpilib.LiveWindow.addActuator("DriveTrain", "Right Master",
self.driveRightMaster)
#wpilib.LiveWindow.add(self.driveLeftSlave)
#wpilib.LiveWindow.add(self.driveRightSlave)
#wpilib.Sendable.setName(self.drive, 'Drive')
#wpilib.LiveWindow.add(self.drive)
#wpilib.Sendable.setName(self.driveLeftMaster, 'driveLeftMaster')
#wpilib.LiveWindow.add(self.driveRightMaster)
super().__init__()
def moveToPosition(self, position, side='left'):
if side == 'left':
self.driveLeftMaster.setSafetyEnabled(False)
self.driveLeftMaster.set(
ctre.talonsrx.TalonSRX.ControlMode.Position, position)
else:
self.driveRightMaster.set(
ctre.talonsrx.TalonSRX.ControlMode.Position, position)
def stop(self):
self.drive.stopMotor()
def arcade(self, speed, rotation):
self.updateSD()
self.drive.arcadeDrive(speed, rotation, True)
def updateSD(self):
leftVel = self.driveLeftMaster.getSelectedSensorVelocity(0)
leftPos = self.driveLeftMaster.getSelectedSensorPosition(0)
rightVel = self.driveRightMaster.getSelectedSensorVelocity(0)
rightPos = self.driveRightMaster.getSelectedSensorPosition(0)
# keep the biggest velocity values
if self.leftMaxVel < leftVel:
self.leftMaxVel = leftVel
if self.rightMaxVel < rightVel:
self.rightMaxVel = rightVel
# 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('Left Max Vel', self.leftMaxVel)
SD.putNumber('Right Max Vel', self.rightMaxVel)
self.leftVel = leftVel
self.leftPos = leftPos
self.rightVel = rightVel
self.rightPos = rightPos
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
class Elevator(Subsystem):
kSwitch = -20000
kScale = -50000
kStart = -5000
kBottom = 0
def __init__(self, robot):
self.robot = robot
self.elevator = Talon(self.robot.kElevator['elevator_motor'])
self.elevator.setInverted(True)
self.driverTwo = self.robot.cStick
self.elevator.configSelectedFeedbackSensor(
ctre.talonsrx.TalonSRX.FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0)
self.elevator.setSensorPhase(True)
self.elevator.setSelectedSensorPosition(0, 0, 0)
self.elevator.configPeakOutputForward(.2, 0)
self.elevator.configPeakOutputReverse(-.95, 0)
self.elevator.configNominalOutputForward(0, 0)
self.elevator.configNominalOutputReverse(0, 0)
self.elevator.setSafetyEnabled(False)
self.smartToggle = True
self.smartPosition = 1
super().__init__()
def elevatorFunction(self):
# if self.elevator.isRevLimitSwitchClosed():
# self.elevator.setSelectedSensorPosition(0, 0, 0)
#self.elevator.set(self.driverTwo.getRawAxis(1))
self.smartPositioning()
#wpilib.SmartDashboard.putNumber('elevator amperage', self.elevator.getOutputCurrent())
# def updateSD(self):
def setElevatorPosition(self, position):
# if self.elevator.isRevLimitSwitchClosed():
# self.elevator.setSelectedSensorPosition(0, 0, 0)
#
self.elevator.set(ctre.talonsrx.TalonSRX.ControlMode.Position, position)
def smartPositioning(self):
if self.driverTwo.getRawButtonReleased(5):
self.smartPosition -= 1
self.smartToggle = True
if self.smartPosition < 0:
self.smartPosition = 0
elif self.driverTwo.getRawButtonReleased(6):
self.smartPosition += 1
self.smartToggle = True
if self.smartPosition > 2:
self.smartPosition = 2
elif (self.driverTwo.getRawAxis(1) > 0.2) or (self.driverTwo.getRawAxis(1) < -0.2):
self.smartToggle = False
if self.smartToggle:
if self.smartPosition == 2:
self.setElevatorPosition(self.kScale)
elif self.smartPosition == 1:
self.setElevatorPosition(self.kSwitch)
elif self.smartPosition == 0:
self.elevator.set(0)
else:
self.elevator.set(self.driverTwo.getRawAxis(1))
if self.elevator.getSelectedSensorPosition(0) < (self.kSwitch + self.kScale)/2:
self.smartPosition = 2
elif self.elevator.getSelectedSensorPosition(0) < self.kSwitch/2:
self.smartPosition = 1
else:
self.smartPosition = 0
def calibrateBottomAutonomous(self):
if self.elevator.isRevLimitSwitchClosed():
self.elevator.setSelectedSensorPosition(0, 0, 0)
self.elevator.set(0)
# self.calibrateStep = 1
else:
self.elevator.set(-.5)
def getBottomLimit(self):
return self.elevator.isFwdLimitSwitchClosed()
def telemetry(self):
wpilib.SmartDashboard.putNumber('Lift Position', self.elevator.getSelectedSensorPosition(0))
wpilib.SmartDashboard.putBoolean('Forward Limit', self.elevator.isFwdLimitSwitchClosed())
wpilib.SmartDashboard.putBoolean('Reverse Limit', self.elevator.isRevLimitSwitchClosed())
