def autonomousPeriodic(self):
time = self.autonTimer.get()
if time <= 1.62:
self.setDrivePower(.793, .9)
SmartDashboard.putNumber("autonTime", time)
else:
self.setDrivePower(0, 0)
def execute(self):
""" Measure how long it takes to run a bunch of iterations. """
SmartDashboard.putNumber("CPU Test Loops", self.loopCnts)
self.timer.reset()
self.timer.start()
self.runTest(self.loopCnts)
self.runSecs = self.timer.get()
def drive_by_ticks(self, ticks, speed=0.25):
offset = ticks - self.get_encoder_ticks()
SmartDashboard.putNumber("Offset", offset)
if abs(offset) > 50:
self.arcade_drive(0, speed)
return False
self.stop()
return True
class mySmartDash:
def __init__(self):
self.sd = SmartDashboard()
def sendNumber(self, key, val):
self.sd.putNumber(key, val)
def sendRandomData(self, upper, lower):
sendNumber(self, "Random", random.randrange(upper, lower))
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 __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 climb(self, speed):
if abs(
speed
) > robotMap.WINCHTHRESHOLD and wpilib.DriverStation.getInstance(
).isOperatorControl():
self.winchL.set(speed)
self.winchR.set(-speed)
else:
self.winchL.set(0)
self.winchR.set(0)
if not self.initInClimb:
from common.oi import oi
self.oi = oi
self.initInClimb = True
SmartDashboard.putNumber("Winch Throttle", self.oi.getWinchSpeed())
def __init__(self):
super().__init__("Drive")
Command.pageDrive = lambda x=0: self
self.frontRight = wpilib.Talon(0)
self.rearRight = wpilib.Talon(1)
self.frontRight.setInverted(True)
self.rearRight.setInverted(True)
self.right = wpilib.SpeedControllerGroup(self.frontRight,
self.rearRight)
self.frontLeft = wpilib.Talon(2)
self.rearLeft = wpilib.Talon(3)
self.left = wpilib.SpeedControllerGroup(self.frontLeft, self.rearLeft)
self.leftEncoder = wpilib.Encoder(8, 9)
self.leftEncoder.setDistancePerPulse(1 / 3 * math.pi /
256) # 4 inch wheels?
self.leftEncoder.setSamplesToAverage(10)
SmartDashboard.putNumber("distance", 0)
SmartDashboard.putString("DriveStyle", "Tank")
def pid_turn_to_angle(self, angle, speed=0.25):
offset = angle - self.get_gyro_angle()
SmartDashboard.putNumber("angle_offset", offset)
if abs(offset) > 2:
self.iErr += offset
turn = (offset * self.turning_P.value) + (self.turning_I.value * self.iErr)
turn = max(min(speed, turn), -speed)
if angle < 0:
#Negative angle, negative turn
turn = abs(turn)
else:
turn = -abs(turn)
SmartDashboard.putNumber('turn_value', turn)
self.arcade_drive(turn, 0)
return False
self.logger.info("Turned to angle " + str(angle))
self.iErr = 0
return True
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.controller = XboxController(0)
self.lmotor = wpilib.CANTalon(0)
self.rmotor = wpilib.CANTalon(1)
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
#self.autonomous_modes = AutonomousModeSelector('autonomous')
# Initialize the smart dashboard display elements
self.sd = SmartDashboard()
self.sd.putNumber("Random", 0) # Send initialization packet
def sendRandomData(self, upper, lower, step):
self.sd.putNumber("Random", random.randrange(upper, lower, step))
def disabled(self):
while self.isDisabled():
wpilib.Timer.delay(0.01) # Wait for 0.01 seconds
def autonomous(self):
self.autonomous_modes.run()
Timer.delay(CONTROL_LOOP_WAIT_TIME)
def operatorControl(self):
wpilib.Timer.delay(CONTROL_LOOP_WAIT_TIME)
while self.isOperatorControl() and self.isEnabled():
self.lmotor.set(self.controller.getLeftY())
self.rmotor.set(self.controller.getRightY())
self.sendRandomData(-10,10,1)
def test(self):
wpilib.LiveWindow.run()
def _update_sd(self):
SmartDashboard.putNumber("Left Encoder Position", self.left_talon0.getEncPosition())
SmartDashboard.putNumber("Right Encoder Position", self.right_talon0.getEncPosition())
SmartDashboard.putNumber("heading", self.get_gyro_angle())
self.reversed = SmartDashboard.getBoolean("Reversed", False)
self.turn_controller.setPID(self.turning_P.value, self.turning_I.value, self.turning_D.value, 0)
def diagnosticsToSmartDash(self):
SmartDashboard.putNumber("Elevator Speed",
self.elevatorMotor1.getMotorOutputPercent())
SmartDashboard.putNumber(
"Elevator Motor 1 Amperage",
self.PDP.getCurrent(robotMap.elevatorPDPPort1))
SmartDashboard.putNumber(
"Elevator Motor 2 Amperage",
self.PDP.getCurrent(robotMap.elevatorPDPPort2))
def setupDashboardControls():
# Only need to set up dashbard drive controls once
global modeChooser
if modeChooser == None:
SmartDashboard.putBoolean("Quick Turn", False)
SmartDashboard.putBoolean("Square Inputs", True)
SmartDashboard.putNumber("Fixed Left", 0.4)
SmartDashboard.putNumber("Fixed Right", 0.4)
SmartDashboard.putNumber("Rotation Gain", 0.5)
SmartDashboard.putNumber("Slow Gain", 0.5)
mc = SendableChooser()
mc.addDefault("Arcade", kModeArcade)
mc.addOption("Tank", kModeTank)
mc.addOption("Curvature", kModeCurvature)
mc.addDefault("Indexed Arcade", kModeIndexedArcade)
mc.addDefault("Indexed Tank", kModeIndexedTank)
mc.addOption("Fixed", kModeFixed)
SmartDashboard.putData("Drive Mode", mc)
modeChooser = mc
class MyRobot(wpilib.SampleRobot):
def robotInit(self):
self.controller = XboxController(0)
self.controller2 = XboxController(1)
#self.lmotor = wpilib.CANTalon(1)
#self.rmotor = wpilib.CANTalon(0)
self.drive = driveTrain(self)
self.robotArm = arm(self)
self.climber = lift(self)
self.pixy = Pixy()
self.drive.reset()
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
# Initialize Components functions
self.components = {
'drive' : self.drive,
'arm' : self.robotArm,
'lift' : self.climber,
'pixy' : self.pixy
}
# Initialize Smart Dashboard
self.dash = SmartDashboard()
self.autonomous_modes = AutonomousModeSelector('autonomous', self.components)
self.potentiometer = ('Arm Potentiometer', 0)
self.dash.putNumber('ControlType', 0)
self.dash.putBoolean('Front Switch', 0)
self.dash.putBoolean('Back Switch', 0)
self.drive.log()
def disabled(self):
self.drive.reset()
#self.drive.disablePIDs()
while self.isDisabled():
wpilib.Timer.delay(0.01) # Wait for 0.01 seconds
def autonomous(self):
self.drive.reset()
self.drive.enablePIDs()
while self.isAutonomous() and self.isEnabled():
# Run the actual autonomous mode
self.potentiometer = ('Arm Potentiometer', self.robotArm.getPOT())
self.drive.log()
self.autonomous_modes.run()
def operatorControl(self):
# Resetting encoders
self.drive.reset()
#self.drive.enablePIDs()
while self.isOperatorControl() and self.isEnabled():
self.drive.xboxTankDrive(self.controller.getLeftY(), self.controller.getRightY(), self.controller.getLeftBumper(), self.controller.getRightBumper(), self.controller.getLeftTrigger(), self.controller.getRightTrigger())
self.robotArm.armUpDown(self.controller2.getLeftTriggerRaw(), self.controller2.getRightTriggerRaw(), self.controller2.getButtonA(), rate=0.5)
self.robotArm.wheelSpin(self.controller2.getLeftY())
self.climber.climbUpDown(self.controller2.getLeftBumper(), self.controller2.getRightBumper())
self.drive.log()
wpilib.Timer.delay(CONTROL_LOOP_WAIT_TIME)
# Send encoder data to the smart dashboard
self.dash.putNumber('Arm Potentiometer', self.robotArm.getPOT())
#self.dash.putBoolean('Back Arm Switch', self.robotArm.getFrontSwitch())
#self.dash.putBoolean('Front Arm Switch', self.robotArm.getBackSwitch())
def test(self):
wpilib.LiveWindow.run()
self.drive.reset()
self.drive.enablePIDs()
while self.isTest() and self.isEnabled():
self.drive.xboxTankDrive(self.controller.getLeftY(), self.controller.getRightY(), self.controller.getLeftBumper(), self.controller.getRightBumper(), self.controller.getRightTrigger())
self.robotArm.armUpDown(self.controller2.getLeftTriggerRaw(), self.controller2.getRightTriggerRaw())
'''
def execute(self):
""" Show accumulated statistics that we care about. """
SmartDashboard.putNumber("Run Last", self.last)
SmartDashboard.putNumber("Run Avg", self.stats.getAvg())
SmartDashboard.putNumber("Run Max", self.stats.getMax())
def execute(self):
yawRaw = self.drive.getAngle()
yaw = yawRaw - self.yawLast
leftDist = self.left.getDistance() - self.leftDistLast
leftCnts = self.left.getCounts() - self.leftCntsLast
leftVel = self.left.getVelocity()
rightDist = self.right.getDistance() - self.rightDistLast
rightCnts = self.right.getCounts() - self.rightCntsLast
rightVel = self.right.getVelocity()
SmartDashboard.putNumber("Yaw NavX", yawRaw)
SmartDashboard.putNumber("Yaw Measured", yaw)
SmartDashboard.putNumber("Left Dist", leftDist)
SmartDashboard.putNumber("Left Cnts", leftCnts)
SmartDashboard.putNumber("Left Vel", leftVel)
SmartDashboard.putNumber("Right Dist", rightDist)
SmartDashboard.putNumber("Right Cnts", rightCnts)
SmartDashboard.putNumber("Right Vel", rightVel)
SmartDashboard.putNumber("Accel X", subsystems.drive.getAccelX())
SmartDashboard.putNumber("Accel Y", subsystems.drive.getAccelY())
SmartDashboard.putBoolean("Bump", subsystems.drive.bumpCheck())
def _update_smartdashboard_sensors(self):
SmartDashboard.putNumber("Drivetrain Left Encoder",
self._left_encoder_count)
SmartDashboard.putNumber("Drivetrain Right Encoder",
self._right_encoder_count)
SmartDashboard.putNumber("Gyro Angle", self._gyro_angle)
def getEncoders(self):
left = self.leftEncoder.getDistance()
#right = self.rightEncoder.getDistance()
SmartDashboard.putNumber("distance", left)
return left
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 _update_smartdashboard(self, speed):
SmartDashboard.putBoolean("Feeder Has Ball", self._has_ball)
SmartDashboard.putNumber("Feeder Speed", speed)
def _update_smartdashboard_tank_drive(self, left, right):
SmartDashboard.putNumber("Drivetrain Left Speed", left)
SmartDashboard.putNumber("Drivetrain Right Speed", right)
def _update_smartdashboard_tank_drive(self, left, right):
SmartDashboard.putNumber("Drivetrain Left Speed", left)
SmartDashboard.putNumber("Drivetrain Right Speed", right)
def diagnosticsToSmartDash(self):
# Add position, velocity, and angle values to the SmartDash.
SmartDashboard.putNumber(
"Left Encoder",
self.getLeftPosition() / robotMap.countsPerRevolution)
SmartDashboard.putNumber(
"Right Encoder",
self.getRightPosition() / robotMap.countsPerRevolution)
# SmartDashboard.putNumber("Left Velocity", self.getLeftVelocity())
# SmartDashboard.putNumber("Right Velocity", self.getRightVelocity())
if RobotBase.isReal():
SmartDashboard.putNumber("Left Speed",
self.l1.getMotorOutputPercent())
SmartDashboard.putNumber("Right Speed",
self.r1.getMotorOutputPercent())
SmartDashboard.putNumber("Gyro Angle", self.gyro.getAngle())
SmartDashboard.putNumber("Barometric Pressure",
self.gyro.getBarometricPressure())
def _update_smartdashboard_arcade_drive(self, linear, turn):
SmartDashboard.putNumber("Drivetrain Linear Speed", linear)
SmartDashboard.putNumber("Drivetrain Turn Speed", turn)
def pid_periodic(self,move, powerMultiplier=1):
#prev 0.023 0.4 0.13
const_kP = 0.028
const_kI = 0.5
const_kFF = 0.17
error = self.pid_calc_error()
if abs(error) > 720:
self.arcadeDrive(0, 0, False, 12)
return
##if error < 5:
# const_kI = 1.66 - (error / 6)
self.integral_accum = self.integral_accum + (min(1, max(-1, error)) * 0.005)
if (self.integral_accum > 0 and error < 0) or (self.integral_accum < 0 and error > 0):
self.integral_accum = 0
kP = min(0.4, max(-0.4, const_kP * error))
kI = const_kI * self.integral_accum
#kI = kI * kI * 1.0 if kI > 0 else -1.0
kFF = const_kFF * (1.0 if error > 0 else -1.0)
SmartDashboard.putNumber("Drive kI", kI)
SmartDashboard.putNumber("Drive kP", kP)
SmartDashboard.putNumber("Drive kFF", kFF)
SmartDashboard.putNumber("Integral Accum", self.integral_accum)
SmartDashboard.putNumber("Drive Pid Error", error)
SmartDashboard.putNumber("Drive Pid Setpoint", self.setpoint)
SmartDashboard.putNumber("Total", -(kP + kI + kFF))
self.arcadeDrive(move, (-(kP + kI)) * powerMultiplier - kFF, False, 12)
def end(self):
hz = 0
if self.runSecs > 0:
hz = self.loopCnts / self.runSecs
SmartDashboard.putNumber("CPU Test Secs", self.runSecs)
SmartDashboard.putNumber("CPU Test Hz", hz)
def periodic(self):
SmartDashboard.putNumber("Velocity", self.getVelocity())
def _update_smartdashboard(self, speed):
SmartDashboard.putNumber("Arm Encoder", self._encoder_value)
SmartDashboard.putNumber("Arm Speed", speed)
def createObjects(self):
#navx
self.navx = AHRS.create_spi()
self.navx.setPIDSourceType(PIDSource.PIDSourceType.kDisplacement)
#Drivetrain
self.left_talon0 = ctre.CANTalon(0)
self.left_talon1 = ctre.CANTalon(1)
self.right_talon0 = ctre.CANTalon(2)
self.right_talon1 = ctre.CANTalon(3)
#Set up talon slaves
self.left_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.left_talon1.set(self.left_talon0.getDeviceID())
self.right_talon1.setControlMode(ctre.CANTalon.ControlMode.Follower)
self.right_talon1.set(self.right_talon0.getDeviceID())
#Set talon feedback device
self.left_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
self.right_talon0.setFeedbackDevice(
ctre.CANTalon.FeedbackDevice.QuadEncoder)
#Set the Ticks per revolution in the talons
self.left_talon0.configEncoderCodesPerRev(1440)
self.right_talon0.configEncoderCodesPerRev(1440)
#Reverse left talon
self.left_talon0.setInverted(True)
self.right_talon0.setInverted(False)
#Climber
self.climber_motor = wpilib.Spark(0)
self.climber_2 = wpilib.Spark(1)
#Sensors
self.left_enc = encoder.Encoder(self.left_talon0)
self.right_enc = encoder.Encoder(self.right_talon0, True)
#Controls
self.left_joystick = wpilib.Joystick(0)
self.right_joystick = wpilib.Joystick(1)
self.climber_joystick = wpilib.Joystick(2)
self.buttons = unifiedjoystick.UnifiedJoystick(
[self.left_joystick, self.right_joystick])
self.last_button_state = False
#Bling
self.leds = ledstrip.LEDStrip()
#Autonomous Placement
self.auto_positions = wpilib.SendableChooser()
self.auto_positions.addDefault("Position 1", 1)
self.auto_positions.addObject("Position 2", 2)
self.auto_positions.addObject("Position 3", 3)
SmartDashboard.putData("auto_position", self.auto_positions)
#SD variables
SmartDashboard.putNumber("Vision/Turn", 0)
SmartDashboard.putBoolean("Reversed", True)
#LiveWindow
LiveWindow.addActuator("Drive", "Left Master Talon", self.left_talon0)
LiveWindow.addActuator("Drive", "Right Master Talon",
self.right_talon0)
def _update_smartdashboard_arcade_drive(self, linear, turn):
SmartDashboard.putNumber("Drivetrain Linear Speed", linear)
SmartDashboard.putNumber("Drivetrain Turn Speed", turn)
def teleopInit(self):
SmartDashboard.putBoolean("time_running", True)
SmartDashboard.putNumber("time_remaining", 215)
def log(self):
self.counter += 1
if self.counter % 10 == 0:
# start keeping track of where the robot is with an x and y position
try:
distance = 0.5 * (self.sparkneo_encoder_1.getPosition() - self.sparkneo_encoder_3.getPosition())
except:
print(f"Problem: encoder position returns {self.sparkneo_encoder_1.getPosition()}")
distance = 0
self.x = self.x + (distance - self.previous_distance) * math.sin(
math.radians(self.robot.navigation.get_angle()))
self.y = self.y + (distance - self.previous_distance) * math.cos(
math.radians(self.robot.navigation.get_angle()))
self.previous_distance = distance
# send values to the dash to make sure encoders are working well
SmartDashboard.putNumber("Robot X", round(self.x, 2))
SmartDashboard.putNumber("Robot Y", round(self.y, 2))
SmartDashboard.putNumber("Position Enc1", round(self.sparkneo_encoder_1.getPosition(), 2))
SmartDashboard.putNumber("Position Enc2", round(self.sparkneo_encoder_2.getPosition(), 2))
SmartDashboard.putNumber("Position Enc3", round(self.sparkneo_encoder_3.getPosition(), 2))
SmartDashboard.putNumber("Position Enc4", round(self.sparkneo_encoder_4.getPosition(), 2))
SmartDashboard.putNumber("Velocity Enc1", round(self.sparkneo_encoder_1.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc2", round(self.sparkneo_encoder_2.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc3", round(self.sparkneo_encoder_3.getVelocity(), 2))
SmartDashboard.putNumber("Velocity Enc4", round(self.sparkneo_encoder_4.getVelocity(), 2))
#SmartDashboard.putNumber("Power M1", round(self.spark_neo_left_front.getAppliedOutput(), 2))
#SmartDashboard.putNumber("Power M3", round(self.spark_neo_right_front.getAppliedOutput(), 2))
SmartDashboard.putNumber("Current M1", round(self.spark_neo_left_front.getOutputCurrent(), 2))
SmartDashboard.putNumber("Current M3", round(self.spark_neo_right_front.getOutputCurrent(), 2))
SmartDashboard.putBoolean('AccLimit', self.is_limited)
if self.counter % 1000 == 0:
self.display_PIDs()
SmartDashboard.putString("alert",
f"Position: ({round(self.x, 1)},{round(self.y, 1)}) Time: {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)}")
SmartDashboard.putString("Controller1 Idle", str(self.spark_neo_left_front.getIdleMode()))
SmartDashboard.putNumber("Enc1 Conversion", self.sparkneo_encoder_1.getPositionConversionFactor())
class Pitchfork(TimedRobot):
def robotInit(self):
"""
Robot-wide initialization code goes here. For the command-based programming framework,
this means creating the subsytem objects and the operator input object. BE SURE TO CREATE
THE SUBSYTEM OBJECTS BEFORE THE OPERATOR INPUT OBJECT (since the operator input object
will almost certainly have subsystem dependencies). For further information on the
command-based programming framework see:
wpilib.screenstepslive.com/s/currentCS/m/java/l/599732-what-is-command-based-programming
"""
# Create the subsytems and the operator interface objects
self.driveTrain = DriveTrain(self)
self.boom = Boom(self)
self.intakeMotors = IntakeMotors(self)
self.intakePneumatics = IntakePneumatics(self)
self.oi = OI(self)
# Create the smartdashboard object
self.smartDashboard = SmartDashboard()
# Create the sendable choosers to get the autonomous preferences
self.scoringElementChooser = SendableChooser()
self.scoreScale = ScoreScale()
self.scoreSwitch = ScoreSwitch()
self.scoringElementChooser.addObject("Score Scale", self.scoreScale)
self.scoringElementChooser.addObject("Score Switch", self.scoreSwitch)
self.scoringElementChooser.addDefault("Score Scale", self.scoreScale)
self.smartDashboard.putData("Score Field Element",
self.scoringElementChooser)
self.crossFieldChooser = SendableChooser()
self.crossFieldEnable = CrossFieldEnable()
self.crossFieldDisable = CrossFieldDisable()
self.crossFieldChooser.addObject("Cross Field Enable",
self.crossFieldEnable)
self.crossFieldChooser.addObject("Cross Field Disable",
self.crossFieldDisable)
self.crossFieldChooser.addDefault("Cross Field Disable",
self.crossFieldDisable)
self.smartDashboard.putData("Cross Field Enable",
self.crossFieldChooser)
self.positionChooser = SendableChooser()
self.startingLeft = StartingLeft()
self.startingRight = StartingRight()
self.startingMiddle = StartingMiddle()
self.positionChooser.addObject("Start Left", self.startingLeft)
self.positionChooser.addObject("Start Right", self.startingRight)
self.positionChooser.addObject("Start Middle", self.startingMiddle)
self.positionChooser.addDefault("Start Middle", self.startingMiddle)
self.smartDashboard.putData("Starting Position", self.positionChooser)
# Create a timer for data logging
self.timer = Timer()
# Create the camera server
CameraServer.launch()
# Boom state start at the scale
self.boomState = BOOM_STATE.Scale
self.autonForward = AutonForward(self)
self.autonMiddleStartLeftSwitch = AutonMiddleStartLeftSwitch(self)
self.autonLeftStartLeftScale = AutonLeftStartLeftScale(self)
# Output debug data to the smartdashboard
if LOGGER_LEVEL == logging.DEBUG:
#=======================================================================================
# self.smartDashboard.putNumber("RightEncPos", 0.0)
# self.smartDashboard.putNumber("RightActPos", 0.0)
# self.smartDashboard.putNumber("RightEncVel", 0.0)
# self.smartDashboard.putNumber("RightActVel", 0.0)
# self.smartDashboard.putNumber("RightPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("RightPrimaryError", 0.0)
# self.smartDashboard.putNumber("TimeStamp", 0.0)
# self.smartDashboard.putNumber("LeftEncPos", 0.0)
# self.smartDashboard.putNumber("LeftActPos", 0.0)
# self.smartDashboard.putNumber("LeftEncVel", 0.0)
# self.smartDashboard.putNumber("LeftActVel", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryTarget", 0.0)
# self.smartDashboard.putNumber("LeftPrimaryError", 0.0)
# self.smartDashboard.putNumber("RightTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftTopBufferCount", 0.0)
# self.smartDashboard.putNumber("LeftBottomBufferCount", 0.0)
# self.smartDashboard.putNumber("RightBottomBufferCount", 0.0)
#=======================================================================================
self.smartDashboard.putNumber("EncPos", 0.0)
self.smartDashboard.putNumber("ActPos", 0.0)
self.smartDashboard.putNumber("EncVel", 0.0)
self.smartDashboard.putNumber("ActVel", 0.0)
self.smartDashboard.putNumber("PrimaryTarget", 0.0)
self.smartDashboard.putNumber("PrimaryError", 0.0)
self.smartDashboard.putNumber("TimeStamp", 0.0)
def disabledInit(self):
"""
Initialization code for disabled mode should go here. This method will be called each
time the robot enters disabled mode.
"""
self.timer.stop()
self.timer.reset()
def disabledPeriodic(self):
"""
Periodic code for disabled mode should go here. This method will be called every 20ms.
"""
if self.timer.running:
self.timer.stop()
self.timer.reset()
def robotPeriodic(self):
pass
def autonomousInit(self):
"""
Initialization code for autonomous mode should go here. This method will be called each
time the robot enters autonomous mode.
"""
self.scheduleAutonomous = True
if not self.timer.running:
self.timer.start()
# Get the prioritized scoring element, robot starting posion, and the alliance
# scale/switch data.
self.selectedCrossFieldEnable = self.crossFieldChooser.getSelected()
self.selectedScoringElement = self.scoringElementChooser.getSelected()
self.selectedStartingPosition = self.positionChooser.getSelected()
self.gameData = DriverStation.getInstance().getGameSpecificMessage()
self.crossFieldEnable = self.selectedCrossFieldEnable.getCrossFieldEnable(
)
self.scoringElement = self.selectedScoringElement.getScoringElement()
self.startingPosition = self.selectedStartingPosition.getStartingPosition(
)
def autonomousPeriodic(self):
"""
Periodic code for autonomous mode should go here. This method will be called every 20ms.
"""
# The game specific data will be a 3-character string representing where the teams switch,
# scale, switch are located. For example, "LRR" means your teams closest switch is on the
# left (as you look out onto the field from the drivers station). The teams scale is on
# the right, and the switch furthest away is also on the right.
if self.scheduleAutonomous:
self.scheduleAutonomous = False
logger.info("Game Data: %s" % (self.gameData))
logger.info("Cross Field Enable: %s" % (self.crossFieldEnable))
logger.info("Scoring Element %s" % (self.scoringElement))
logger.info("Starting Position %s" % (self.startingPosition))
self.autonMiddleStartLeftSwitch.start()
#self.autonLeftStartLeftScale.start()
#self.autonForward.start()
Scheduler.getInstance().run()
def teleopInit(self):
"""
Initialization code for teleop mode should go here. This method will be called each time
the robot enters teleop mode.
"""
if not self.timer.running:
self.timer.start()
def teleopPeriodic(self):
"""
Periodic code for teleop mode should go here. This method will be called every 20ms.
"""
Scheduler.getInstance().run()
def _update_smartdashboard_sensors(self):
SmartDashboard.putNumber("Drivetrain Encoder", self._encoder_count)
SmartDashboard.putNumber("Gyro Angle", self._gyro_angle)
SmartDashboard.putNumber("Pitch Angle", self._pitch_gyro_angle)
def execute(self):
winch.ledPower(True)
from common.oi import oi
winch.climb(oi.getWinchSpeed())
SmartDashboard.putNumber("Winch Throttle", oi.getWinchSpeed())
def update_sd(self):
SmartDashboard.putBoolean("time_running", True)
SmartDashboard.putNumber(
"time_remaining",
DriverStation.getInstance().getMatchTime() - 15)
def log(self):
self.counter += 1
if self.counter % 10 == 0:
if self.connected:
SmartDashboard.putBoolean("IsConnected",
self.navx.isConnected())
SmartDashboard.putNumber("Angle", self.navx.getAngle())
SmartDashboard.putNumber("Pitch", self.navx.getPitch())
SmartDashboard.putNumber("Yaw", self.navx.getYaw())
SmartDashboard.putNumber("Roll", self.navx.getRoll())
SmartDashboard.putNumber("Analog",
round(self.analog.getVoltage(), 3))
SmartDashboard.putNumber("Timestamp",
self.navx.getLastSensorTimestamp())
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
def display_PIDs(self):
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
for key in keys:
SmartDashboard.putNumber(key + '_0', self.PID_multiplier * self.PID_dict_pos[key])
SmartDashboard.putNumber(key + '_1', self.PID_multiplier * self.PID_dict_vel[key])
