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 change_PIDs(self, factor=1, dict_0=None, dict_1=None):
'''Pass a value to the and update the PIDs, probably use 1.5 and 0.67 to see how they change
can also pass it a dictionary {'kP': 0.06, 'kI': 0.0, 'kD': 0, 'kIz': 0, 'kFF': 0} to set
slot 0 (position) or slot 1 (velocity) '''
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
for key in keys:
if dict_0 == None:
self.PID_dict_pos[key] *= factor
else:
self.PID_dict_pos[key] = dict_0[key] / self.PID_multiplier
SmartDashboard.putString("alert",
f"Pos: kP: {self.PID_dict_pos['kP']} kI: {self.PID_dict_pos['kI']} kD: {self.PID_dict_pos['kD']} kIz: {self.PID_dict_pos['kIz']} kFF: {self.PID_dict_pos['kFF']}")
if dict_1 == None:
self.PID_dict_vel[key] *= factor
else:
self.PID_dict_vel[key] = dict_1[key] / self.PID_multiplier
SmartDashboard.putString("alert",
f"Vel: kP: {self.PID_dict_vel['kP']} kI: {self.PID_dict_vel['kI']} kD: {self.PID_dict_vel['kD']} kIz: {self.PID_dict_vel['kIz']} kFF: {self.PID_dict_vel['kFF']}")
self.configure_controllers(pid_only=True)
self.display_PIDs()
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())
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)
class MyRobot(wpilib.IterativeRobot):
def robotInit(self):
self.drive = driveTrain(self)
self.interpret = interpret()
self.controllerOne = XboxController(0)
self.controllerTwo = XboxController(1)
self.speedLimiter = 1 #1 = standard speed, greater than 1 to slow down, less than 1 to speed up
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
self.dash = SmartDashboard()
self.positionChooser = wpilib.SendableChooser()
self.positionChooser.addDefault('Position Chooser', '1')
self.positionChooser.addObject('left', 'left')
self.positionChooser.addObject('right', 'right')
self.positionChooser.addObject('center', 'center')
self.switchLscaleL = wpilib.SendableChooser()
self.switchLscaleL.addDefault('Switch and Scale LEFT', '1')
self.switchLscaleL.addObject('Scale', 'scale')
self.switchLscaleL.addObject('Switch', 'switch')
#switchLscaleL.addObject('PrepScaleScore', '4')
self.switchLscaleL.addDefault(
'Drive', 'drive') ##Default for all sendable Choosers
self.switchLscaleL.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchRscaleR = wpilib.SendableChooser()
self.switchRscaleR.addDefault('Switch and Scale RIGHT', '1')
self.switchRscaleR.addObject('Scale', 'scale')
self.switchRscaleR.addObject('Switch', 'switch')
#switchRscaleR.addObject('PrepScaleScore', '4')
self.switchRscaleR.addDefault('Drive', 'drive')
self.switchRscaleR.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchRscaleL = wpilib.SendableChooser()
self.switchRscaleL.addDefault('Switch RIGHT, Scale LEFT', '1')
self.switchRscaleL.addObject('Scale', 'scale')
self.switchRscaleL.addObject('Switch', 'switch')
#switchRscaleL.addObject('PrepScaleScore', '4')
self.switchRscaleL.addDefault('Drive', 'drive')
self.switchRscaleL.addObject('Two Cube Scale', 'Two Cube Scale')
self.switchLscaleR = wpilib.SendableChooser()
self.switchLscaleR.addDefault('Switch LEFT, Scale RIGHT', '1')
self.switchLscaleR.addObject('Scale', 'scale')
self.switchLscaleR.addObject('Switch', 'switch')
#switchLscaleR.addObject('PrepScaleScore', '4')
self.switchLscaleR.addDefault('Drive', 'drive')
self.switchLscaleR.addObject('Two Cube Scale', 'Two Cube Scale')
#print('Dashboard Test')
wpilib.SmartDashboard.putData('Starting Position',
self.positionChooser)
wpilib.SmartDashboard.putData('Switch and Scale Left',
self.switchLscaleL)
wpilib.SmartDashboard.putData('Switch Right, Scale Left',
self.switchRscaleL)
wpilib.SmartDashboard.putData('Switch and Scale Right',
self.switchRscaleR)
wpilib.SmartDashboard.putData('Switch Left, Scale Right',
self.switchLscaleR)
#self.dash.putData('Switch Left, Scale Right', switchLscaleR)
self.dash.putString('SanityCheck', '1')
self.dashTimer = wpilib.Timer() # Timer for SmartDashboard updating
self.dashTimer.start()
#self.drive.encoderReset()
def interperetDashboard(self):
startingPosition = self.positionChooser.getSelected()
gameData = DriverStation.getInstance().getGameSpecificMessage()
gameData = gameData[:-1]
gameData = gameData.upper()
switchPosition = gameData[0]
scalePosition = gameData[1]
objective = 'drive'
if gameData == 'LL':
objective = self.switchLscaleL.getSelected()
elif gameData == 'LR':
objective = self.switchLscaleR.getSelected()
elif gameData == 'RL':
objective = self.switchRscaleL.getSelected()
elif gameData == 'RR':
objective = self.switchRscaleR.getSelected()
else:
objective = 'drive'
print(gameData)
print(startingPosition)
print(switchPosition)
print(scalePosition)
print(objective)
if objective == 'switch':
if (startingPosition == 'left'
and switchPosition == 'L') or (startingPosition == 'right'
and switchPosition == 'R'):
self.auton = autonNearSwitch(startingPosition, switchPosition,
scalePosition, self.drive)
elif startingPosition == 'center':
self.auton = autonCenterEitherSwitch(startingPosition,
switchPosition,
scalePosition, self.drive)
elif (startingPosition == 'left'
and switchPosition == 'R') or (startingPosition == 'right'
and switchPosition == 'L'):
self.auton = autonFarSwitch(startingPosition, switchPosition,
scalePosition, self.drive)
elif objective == 'scale':
if (startingPosition == 'left'
and scalePosition == 'L') or (startingPosition == 'right'
and scalePosition == 'R'):
self.auton = autonNearScale(startingPosition, switchPosition,
scalePosition, self.drive)
elif (startingPosition == 'left'
and scalePosition == 'R') or (startingPosition == 'right'
and scalePosition == 'L'):
self.auton = autonFarScale(startingPosition, switchPosition,
scalePosition, self.drive)
elif objective == 'Two Cube Scale':
if (startingPosition == 'left'
and scalePosition == 'L') or (startingPosition == 'right'
and scalePosition == 'R'):
self.auton = autonTwoCubeSale(startingPosition, switchPosition,
scalePosition, self.drive)
elif (startingPosition == 'left'
and scalePosition == 'R') or (startingPosition == 'right'
and scalePosition == 'L'):
self.auton = autonFarScale(startingPosition, switchPosition,
scalePosition, self.drive)
elif objective == 'drive':
self.auton = autonDrive(startingPosition, switchPosition,
scalePosition, self.drive)
print(self.auton)
def autonomousInit(self):
self.autonCounter = 0
self.drive.zeroGyro()
self.drive.resetMoveNumber()
self.drive.autonShift('low') #Forces into low gear at start of auton
#print('reset moveNumber')
self.interperetDashboard()
#self.auton = AutonInterpreter(3,3,3,self.drive)
#self.auton = autonTurningTuning('any', 'any', 'any', self.drive)
#self.auton = autonNearSwitch('right', 'R', 'L', self.drive)
#self.auton = autonFarSwitch('left', 'R', 'L', self.drive)
#self.auton = autonCenterEitherSwitch('center', 'L', 'L', self.drive)
#self.auton = autonCenterEitherSwitch('center', 'L', 'R', self.drive)
#self.auton = autonTwoCubeScale('left', 'L', 'L', self.drive)
#self.auton = autonNearScale('left', 'L', 'L', self.drive)
#self.auton = autonDrive('any', 'any', 'any', self.drive)
def autonomousPeriodic(self):
#self.drive.printEncoderPosition()#Prints the position of the encoders
#print(self.drive.getGyroAngle())
if self.autonCounter >= 5:
self.auton.run()
else:
self.autonCounter = self.autonCounter + 1
#self.AutonHandling.readCommandList(None, "square")
#lfEncoderPosition = -(self.drive.lfMotor.getQuadraturePosition())
#rbEncoderPosition = self.drive.rbMotor.getQuadraturePosition()
#averageEncoder = (lfEncoderPosition + rbEncoderPosition) / 2
#wpilib.SmartDashboard.putNumber('Left Encoder Position', lfEncoderPosition)
#wpilib.SmartDashboard.putNumber('Right Encoder Position', rbEncoderPosition)
#wpilib.SmartDashboard.putNumber(' Average Encodes', averageEncoder)
wpilib.SmartDashboard.putNumber('Gyro Angle',
self.drive.getGyroAngle())
#self.drive.printer()
def teleopPeriodic(self):
lfEncoderPosition = -(self.drive.lfMotor.getQuadraturePosition())
rfEncoderPosition = self.drive.rbMotor.getQuadraturePosition()
#self.drive.printEncoderPosition()
#print("Gyro Angle", self.drive.getGyroAngle())
wpilib.SmartDashboard.putNumber('Gyro Angle',
self.drive.getGyroAngle())
#wpilib.SmartDashboard.putNumber('Number of Shits', self.drive.shiftCounterReturn())
#wpilib.SmartDashboard.putString('Gear Mode', self.drive.gearMode())
#self.drive.printer()
self.drive.drivePass(self.controllerOne.getLeftY(),
self.controllerOne.getRightX(),
self.controllerOne.getLeftBumper(),
self.controllerOne.getRightBumper(),
self.controllerOne.getButtonA())
self.drive.operate.operate(self.controllerTwo.getLeftY(),
self.controllerTwo.getLeftX(),
self.controllerTwo.getRightY(),
self.controllerTwo.getRightX(),
self.controllerTwo.getButtonA(),
self.controllerTwo.getButtonB(),
self.controllerTwo.getButtonX(),
self.controllerTwo.getButtonY(),
self.controllerTwo.getRightTrigger(),
self.controllerTwo.getRightBumper(),
self.controllerTwo.getLeftTrigger(),
self.controllerTwo.getLeftBumper(),
self.controllerTwo.getStart(),
self.controllerTwo.getBack())
self.timeOut.time += 1
