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 setup(self):
self.left = 0
self.right = 0
self.sd = NetworkTable.getTable("/SmartDashboard")
#Turn to angle PI values
self.turning_P = self.sd.getAutoUpdateValue("TurnToAngle/P", 1)
self.turning_I = self.sd.getAutoUpdateValue("TurnToAngle/I", 0)
self.turning_D = self.sd.getAutoUpdateValue("TurnToAngle/D", 0)
self.turn_controller = wpilib.PIDController(Kp=self.turning_P.value, Ki=self.turning_I.value, Kd=self.turning_D.value, source=self, output=self)
self.turn_controller.setInputRange(-180, 180)
self.turn_controller.setOutputRange(-1, 1)
self.turn_controller.setContinuous(True)
self.pid_value = 0
self.drive_constant = self.sd.getAutoUpdateValue("Drive/Drive Constant", 0.0001)
self.drive_multiplier = self.sd.getAutoUpdateValue("Drive/Drive Multiplier", 0.75)
SmartDashboard.putBoolean("Reversed", False)
self.reversed = False
self.logger = logging.getLogger("drive")
self.iErr = 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 turn_right(self, state_tm, initial_call):
if initial_call:
self.drive.reset_encoders()
if SmartDashboard.getDouble("angle_offset") < (self.turn_right_angle /
2):
SmartDashboard.putBoolean("run_vision", True)
if self.drive.turn_to_angle(self.turn_right_angle):
self.next_state('approach')
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 turn(self):
station = SmartDashboard.getString("auto_position/selected",
"Position 1")
SmartDashboard.putBoolean("run_vision", False)
if station is "Position 1":
self.next_state('turn_right')
elif station is "Position 3":
self.next_state('turn_left')
else:
self.next_state('approach')
def robotInit(self):
""" Initalizes all subsystems and user controls. """
if not self.bareBones:
# Set up subsystems
subsystems.initialize()
# Set up user controls
oi.initialize()
if self.debug:
self.performance = Performance()
SmartDashboard.putData("Measure Performance", self.performance)
def turn_to_angle(self, angle, speed=0.25):
offset = angle - self.get_gyro_angle()
SmartDashboard.putDouble("angle_offset", offset)
if abs(offset) > 0.1:
if offset > 0:
turn_speed = speed
else:
turn_speed = -speed
self.arcade_drive(turn_speed, speed)
return False
return True
def _create_smartdashboard_buttons(self):
self._auto_program_chooser = SendableChooser()
#self._auto_program_chooser.addDefault("Cross Line", 1)
#self._auto_program_chooser.addObject("Hang Gear", 2)
self._auto_program_chooser.addObject("Do Nothing", 3)
SmartDashboard.putData("Autonomous", self._auto_program_chooser)
self._starting_chooser = SendableChooser()
self._starting_chooser.addDefault("Left", 1)
self._starting_chooser.addObject("Center", 2)
self._starting_chooser.addObject("Right", 3)
SmartDashboard.putData("Starting_Position", self._starting_chooser)
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 teleopPeriodic(self):
self.update_sd()
self.drive.tankdrive(self.left_joystick.getRawAxis(1),
self.right_joystick.getRawAxis(1))
if self.climber_joystick.getRawButton(3):
self.climber.enable()
self.climber.set(self.climber_joystick.getRawAxis(1))
if self.buttons.getButton(11) and self.last_button_state is False:
self.last_button_state = True
SmartDashboard.putBoolean("Reversed", not self.drive.reversed)
if not self.buttons.getButton(11) and self.last_button_state is True:
self.last_button_state = False
def forwards(self, initial_call, state_tm):
self.found = False
SmartDashboard.putBoolean("Found", False)
if initial_call:
self.drive.reset_encoders()
self.logger.info(
SmartDashboard.getString("auto_position/selected"))
if SmartDashboard.getString("auto_position/selected",
"Position 1") is "Position 2":
distance = self.middle_peg_distance
else:
distance = self.initial_distance
if self.drive.drive_distance(distance, speed=0.25):
self.drive.stop()
self.next_state('turn')
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 initialize(self):
self.register_sd_var('initial_distance', 60, vmin=0, vmax=150)
self.register_sd_var("middle_peg_distance", 24, vmin=1, vmax=100)
self.register_sd_var('turning_time', 1.5, vmin=1, vmax=5)
self.register_sd_var('turning_speed', 0.25)
self.register_sd_var('turn_to_peg_P', 0.075)
self.register_sd_var("wait_to_look", 0.5, vmin=0, vmax=3)
self.register_sd_var("turn_left_angle", 300, vmin=0, vmax=360)
self.register_sd_var("turn_right_angle", 60, vmin=0, vmax=360)
self.register_sd_var("forward_time", 3, vmin=1, vmax=10)
self.turnt = False
self.found = False
SmartDashboard.putBoolean("run_vision", False)
self.logger = logging.Logger("Vision Auto")
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 approach(self, state_tm):
if SmartDashboard.getBoolean("Vision/Found Hook"):
self.next_state('found_hook')
else:
if state_tm < self.forward_time:
self.drive.arcade_drive(0, 0.25)
else:
self.next_state('stop')
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 __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 initialize(self):
if self.initialized:
return
# initializing auto chooser
self.preferredElement = wpilib.SendableChooser()
self.preferredElement.addObject('crossLine', 0)
self.preferredElement.addObject('switch', 1)
self.preferredElement.addObject('scale', 2)
self.startingPosition = wpilib.SendableChooser()
self.startingPosition.addObject('left', 0)
self.startingPosition.addObject('middle', 1)
self.startingPosition.addObject('right', 2)
SmartDashboard.putData("Preferred Element", self.preferredElement)
SmartDashboard.putData("Starting Position", self.startingPosition)
print("AutoManager: Initialized")
self.initialized = True
def __init__(self, robot):
self.robot = robot
# update OI with logs
Event_Manager.add_listeners({
'auto.periodic' : self.log,
'teleop.periodic' : self.log
})
# notify listeners when SmartDashboard is updated
sd = NetworkTable.getTable('SmartDashboard')
sd.addTableListener(self.dashboard_listener, True)
# add autonomous mode chooser to smartdashboard
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Auto Do Nothing', Autonomous.Modes.DO_NOTHING)
self.auto_choose.addObject('Auto Move Forward', Autonomous.Modes.MOVE_FORWARD)
self.auto_choose.addObject('Auto One Object', Autonomous.Modes.ONE_OBJECT)
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# joystick events
self.joystick = Joystick(0)
l_bumper = Joystick_Button(self.joystick, lc.L_BUMPER)
l_trigger = Joystick_Button(self.joystick, lc.L_TRIGGER)
r_bumper = Joystick_Button(self.joystick, lc.R_BUMPER)
r_trigger = Joystick_Button(self.joystick, lc.R_TRIGGER)
btn_one = Joystick_Button(self.joystick, 1)
btn_two = Joystick_Button(self.joystick, 2)
# update the joysick axis periodically
Event_Manager.add_listener('teleop.periodic', self.update_axis)
l_bumper.while_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.while_pressed'))
l_trigger.while_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.while_pressed'))
l_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.when_pressed'))
l_trigger.when_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.when_pressed'))
l_bumper.when_released(lambda: Event_Manager.trigger('joystick.l_bumper.when_released'))
l_trigger.when_released(lambda: Event_Manager.trigger('joystick.l_trigger.when_released'))
r_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.r_bumper.when_pressed'))
btn_two.when_pressed(lambda: Event_Manager.trigger('joystick.btn_two.when_pressed'))
btn_one.when_pressed(lambda: Event_Manager.trigger('joystick.btn_one.when_pressed'))
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 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 initialize(self):
"""Called just before this Command runs the first time."""
strip_name = lambda x: str(x)[1 + str(x).rfind('.'):-2]
print(
"\n" +
f"** Started {self.name} with input {self.factor} at {round(Timer.getFPGATimestamp() - self.robot.enabled_time, 1)} s **"
)
keys = ['kP', 'kI', 'kD', 'kIz', 'kFF']
if self.from_dashboard == 'position':
dict_0 = {}
for key in keys:
value = float(SmartDashboard.getNumber(str(key) + '_0', 0))
dict_0.update({key: value})
self.robot.drivetrain.change_PIDs(factor=1, dict_0=dict_0)
if self.from_dashboard == 'velocity':
dict_1 = {}
for key in keys:
value = float(SmartDashboard.getNumber(str(key) + '_1', 0))
dict_1.update({key: value})
self.robot.drivetrain.change_PIDs(factor=1, dict_1=dict_1)
else:
self.robot.drivetrain.change_PIDs(self.factor)
def LeftStickX(self):
creep_mode = SmartDashboard.getBoolean("Creep", False)
if creep_mode:
return self.conditonAxis(
self.getRawAxis(robotmap.XBOX_LEFT_X_AXIS),
self.creep_x_deadband, self.creep_x_rate, self.creep_x_expo,
self.creep_x_power, self.creep_x_min, self.creep_x_max)
else:
return self.conditonAxis(
self.getRawAxis(robotmap.XBOX_LEFT_X_AXIS),
self.left_x_deadband, self.left_x_rate, self.left_x_expo,
self.left_x_power, self.left_x_min, self.left_x_max)
def loopFunc(self):
"""This version of loopFunc passes the event loop to all init() and periodic() functions."""
if self.isDisabled():
if self.last_mode is not self.Mode.kDisabled:
LiveWindow.setEnabled(False)
self._flush_commands()
self.disabledInit()
self.last_mode = self.Mode.kDisabled
hal.observeUserProgramDisabled()
self.disabledPeriodic()
elif self.isAutonomous():
if self.last_mode is not self.Mode.kAutonomous:
LiveWindow.setEnabled(False)
self._flush_commands()
self.autonomousInit()
self.last_mode = self.Mode.kAutonomous
hal.observeUserProgramAutonomous()
self.autonomousPeriodic()
elif self.isOperatorControl():
if self.last_mode is not self.Mode.kTeleop:
LiveWindow.setEnabled(False)
self._flush_commands()
self.teleopInit()
self.last_mode = self.Mode.kTeleop
hal.observeUserProgramTeleop()
self.teleopPeriodic()
else:
if self.last_mode is not self.Mode.kTest:
LiveWindow.setEnabled(True)
self._flush_commands()
self.testInit()
self.last_mode = self.Mode.kTest
hal.observeUserProgramTest()
self.testPeriodic()
self.robotPeriodic()
self._run_commands()
SmartDashboard.updateValues()
LiveWindow.updateValues()
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 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 __init__(self, default=ledstrip.LEDStrip.Command.RED):
'''
Constructor
'''
self.command = default
self.chooser = wpilib.SendableChooser()
self.chooser.addDefault("Red", ledstrip.LEDStrip.Command.RED)
self.chooser.addObject("Blue", ledstrip.LEDStrip.Command.BLUE)
self.chooser.addObject("Green", ledstrip.LEDStrip.Command.GREEN)
self.chooser.addObject("Rainbow", ledstrip.LEDStrip.Command.RAINBOW)
self.chooser.addObject("Theater Blue",
ledstrip.LEDStrip.Command.THEATER_BLUE)
self.chooser.addObject("Theater Red",
ledstrip.LEDStrip.Command.THEATER_RED)
self.chooser.addObject("Theater Rainbow",
ledstrip.LEDStrip.Command.THEATER_RAINBOW)
self.chooser.addObject("Off", ledstrip.LEDStrip.Command.OFF)
SmartDashboard.putData("LEDs", self.chooser)
self.r = 0
self.g = 0
self.b = 0
def initialize(self):
""" Read in and apply current drive choices. """
DriveHuman.setupDashboardControls()
global modeChooser
self.mode = modeChooser.getSelected()
self.quickTurn = SmartDashboard.getBoolean("Quick Turn", self.quickTurn)
self.squareInputs = SmartDashboard.getBoolean("Square Inputs", self.squareInputs)
self.fixedLeft = SmartDashboard.getNumber("Fixed Left", self.fixedLeft)
self.fixedRight = SmartDashboard.getNumber("Fixed Right", self.fixedRight)
self.rotationGain = SmartDashboard.getNumber("Rotation Gain", self.rotationGain)
self.slowGain = SmartDashboard.getNumber("Slow Gain", self.slowGain)
def robotInit(self):
""" Initalizes all subsystems and user controls. """
if self.numSubsystems > 0:
# Create drive subsystems
pwmOfs: int = 0
if self.enableDrive:
leftMotors = SpeedControllerGroup(wpilib.VictorSP(0), wpilib.VictorSP(1))
rightMotors = SpeedControllerGroup(wpilib.VictorSP(2), wpilib.VictorSP(3))
gamePad: GenericHID = XboxController(0)
drive: Drive = Drive(99, leftMotors, rightMotors)
drive.setDefaultCommand(DriveArcade(drive, leftMotors, rightMotors, gamePad))
pwmOfs += 4
for i in range(0, self.numSubsystems):
pwm: int = pwmOfs + i * 2
leftMotor: SpeedController = wpilib.VictorSP(pwm)
rightMotor: SpeedController = wpilib.VictorSP(pwm + 1)
drive = Drive(i, leftMotor, rightMotor)
SmartDashboard.putData("Forward " + str(i), DrivePower(drive, 0.2, 0.2))
SmartDashboard.putData("Backward " + str(i), DrivePower(drive, -0.2, -0.2))
# Add command to dashboard to track time for one periodic pass
self.performance = Performance()
SmartDashboard.putData("Measure Performance", self.performance)
def _update_smartdashboard_tank_drive(self, left, right):
SmartDashboard.putNumber("Drivetrain Left Speed", left)
SmartDashboard.putNumber("Drivetrain Right Speed", right)
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 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 __init__(self):
self.sd = SmartDashboard()
def _update_smartdashboard(self, speed):
SmartDashboard.putBoolean("Feeder Has Ball", self._has_ball)
SmartDashboard.putNumber("Feeder Speed", speed)
def reverse(self):
SmartDashboard.putBoolean("Reversed", not SmartDashboard.getBoolean("Reversed", False))
def __init__(self, robot):
self.robot = robot
# Controllers
# Xbox
self.xbox_controller_1 = controls.xbox_controller.Xbox_Controller(0)
self.xbox_controller_2 = controls.xbox_controller.Xbox_Controller(1)
## COMMANDS ##
##Controller 1 Commands
# DRIVE COMMANDS
self.drive_command = commands.drivetrain.Drive_With_Tank_Values(
self.robot,
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_RIGHT_X,
inverted=INVERT_XBOX_RIGHT_X),
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_LEFT_Y,
inverted=INVERT_XBOX_LEFT_Y),
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_LEFT_X,
inverted=INVERT_XBOX_LEFT_X),
)
self.robot.drive.setDefaultCommand(self.drive_command)
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Do Nothing',
commands.autonomous.Do_Nothing(self.robot))
self.auto_choose.addDefault(
'Cross Baseline', commands.autonomous.Cross_Baseline(self.robot))
self.auto_choose.addObject(
'Center Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_CENTER_GEAR))
self.auto_choose.addDefault(
'Center Gear No Camera',
commands.autonomous.Center_Gear_Without_Camera(self.robot))
self.auto_choose.addObject(
'Right Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_RIGHT_GEAR))
self.auto_choose.addObject(
'Right Gear And Shoot Red',
commands.autonomous.Move_To_Gear_And_Shoot(self.robot,
const.ID_AUTO_RED_SIDE))
self.auto_choose.addObject(
'Left Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_LEFT_GEAR))
self.auto_choose.addObject(
'Left Gear And Shoot Blue',
commands.autonomous.Move_To_Gear_And_Shoot(
self.robot, const.ID_AUTO_BLUE_SIDE))
self.auto_choose.addObject(
'Shoot From Hopper Red',
commands.autonomous.Shoot_From_Hopper(self.robot,
const.ID_AUTO_RED_SIDE))
self.auto_choose.addObject(
'Shoot From Hopper Blue',
commands.autonomous.Shoot_From_Hopper(self.robot,
const.ID_AUTO_BLUE_SIDE))
self.auto_choose.addObject(
'Shoot From Start',
commands.autonomous.Shoot_From_Start(self.robot))
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# Toggle Drive State
self.button_drive_shift = Xbox_Trigger(self.xbox_controller_1,
XBOX_BTN_LEFT_TRIGGER)
self.button_drive_shift.whenPressed(
commands.drivetrain.Toggle_Drive_State(self.robot))
# Toggle drivetrain gear
if not const.DEMO_MODE:
self.button_gear_shift = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_LEFT_BUMPER)
self.button_gear_shift.whenPressed(
commands.drivetrain.Toggle_Gear_State(self.robot))
# Slow speed for driving in Y direction so that we slow down before the gear peg
self.button_slow_driving = Xbox_Trigger(self.xbox_controller_1,
XBOX_BTN_RIGHT_TRIGGER)
self.button_slow_driving.whenPressed(
commands.drivetrain.Set_Sensitivity_Low(self.robot))
self.button_slow_driving.whenReleased(
commands.drivetrain.Set_Sensitivity_High(self.robot))
# Drives backwards small distance, to align better for gear feeding
self.button_drive_gear_feeder_reverse = Xbox_Button(
self.xbox_controller_1, XBOX_BTN_X)
self.button_drive_gear_feeder_reverse.whenPressed(
commands.drivetrain.Back_Up(self.robot))
if not const.DEMO_MODE:
# For testing
self.button_rotate_to_boiler = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_Y)
self.button_rotate_to_boiler.whenPressed(
commands.drivetrain.Rotate_To_Boiler(self.robot))
# For testing
self.rotate = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_RIGHT_BUMPER)
self.rotate.whenPressed(
commands.drivetrain.Rotate_To_Angle(self.robot, -52))
self.rotate_to_gear = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_A)
self.rotate_to_gear.whenPressed(
commands.drivetrain.Rotate_To_Gear(self.robot))
## Controller 2 Commands
# Run Shooter
if const.DEMO_MODE:
# Lower top speed on shooter when in demo mode
self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_RIGHT_TRIGGER)
self.button_run_shooter.whenPressed(
commands.shooter.Run_Shooter_Wheel(self.robot))
self.button_run_shooter.whenReleased(
commands.shooter.Stop_Shooter_Wheel(self.robot))
else:
self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_RIGHT_TRIGGER)
self.button_run_shooter.whenPressed(
commands.shooter.Run_Shooter_Wheel_Full_Speed(self.robot))
self.button_run_shooter.whenReleased(
commands.shooter.Stop_Shooter_Wheel(self.robot))
# Run Agitator
self.button_run_agitator = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_LEFT_TRIGGER)
self.button_run_agitator.whenPressed(
commands.agitator.Run_Agitator(self.robot, 0.8))
self.button_run_agitator.whenReleased(
commands.agitator.Stop_Agitator(self.robot))
# FEEDER COMMANDS
# Toggles feeder on/off when pressed
self.button_run_feeder_forward = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_RIGHT_BUMPER)
self.button_run_feeder_forward.whenPressed(
commands.feeder.Run_Feeder(self.robot, 1.0))
self.button_run_feeder_forward.whenReleased(
commands.feeder.Stop_Feeder(self.robot))
self.button_run_feeder_backward = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_LEFT_BUMPER)
self.button_run_feeder_backward.whenPressed(
commands.feeder.Run_Feeder(self.robot, -1.0))
self.button_run_feeder_backward.whenReleased(
commands.feeder.Stop_Feeder(self.robot))
if not const.DEMO_MODE:
# GILLOTUINNEE
self.button_toggle_gear_lexan = Xbox_Button(
self.xbox_controller_2, XBOX_BTN_Y)
self.button_toggle_gear_lexan.whenPressed(
commands.gear_lexan.Toggle_Gear_Lexan_State(self.robot))
#self.button_toggle_gear_lexan.whenPressed( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_OPEN ) )
#self.button_toggle_gear_lexan.whenReleased( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_CLOSED) )
# Release gear
self.button_toggle_gear_release = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_A)
self.button_toggle_gear_release.whenPressed(
commands.gear_claw.Open_Container(self.robot))
self.button_toggle_gear_release.whenReleased(
commands.gear_claw.Close_Container(self.robot))
# Toggle funnel
self.button_toggle_gear_funnel = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_B)
self.button_toggle_gear_funnel.whenPressed(
commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot))
self.button_toggle_gear_funnel.whenPressed(
commands.gear_lexan.Set_Gear_Lexan_State(
self.robot, const.ID_GEAR_LEXAN_CLOSED))
self.button_toggle_gear_funnel.whenReleased(
commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot))
# CLIMBER COMMANDS
self.button_run_climber_up = Joystick_POV(
self.xbox_controller_2, controls.joystick_pov.JOY_POV_UP)
self.button_run_climber_up.whenPressed(
commands.climber.Run_Climber(self.robot, 1.0))
self.button_run_climber_up.whenReleased(
commands.climber.Stop_Climber(self.robot))
#self.button_run_climber_down = Joystick_POV( self.xbox_controller_1, controls.joystick_pov.JOY_POV_DOWN )
#self.button_run_climber_down.whenPressed( commands.climber.Run_Climber( self.robot, -0.2 ) )
#self.button_run_climber_down.whenReleased( commands.climber.Stop_Climber( self.robot ) )
#self.button_change_gear_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_BACK )
#self.button_change_gear_camera_type.whenPressed( commands.gear_camera.Toggle_Gear_Camera_Mode( self.robot ) )
#self.button_change_shooter_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_START )
#self.button_change_shooter_camera_type.whenPressed( commands.shooter_camera.Toggle_Shooter_Camera_Mode( self.robot ) )
#smart dashboard changing PID values
dp_trigger = SmartDashboard_Update_Trigger('Drive P: ',
self.robot.drive.default_kP)
dp_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(p=dp_trigger.
get_key_value())))
di_trigger = SmartDashboard_Update_Trigger('Drive I: ',
self.robot.drive.default_kI)
di_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(i=di_trigger.
get_key_value())))
dd_trigger = SmartDashboard_Update_Trigger('Drive D: ',
self.robot.drive.default_kD)
dd_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(d=dd_trigger.
get_key_value())))
#smart dashboard changing Shooter PID values
sp_trigger = SmartDashboard_Update_Trigger(
'Shooter P: ', self.robot.shooter.default_kP)
sp_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
p=sp_trigger.get_key_value())))
si_trigger = SmartDashboard_Update_Trigger(
'Shooter I: ', self.robot.shooter.default_kI)
si_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
i=si_trigger.get_key_value())))
sd_trigger = SmartDashboard_Update_Trigger(
'Shooter D: ', self.robot.shooter.default_kD)
sd_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
d=sd_trigger.get_key_value())))
sf_trigger = SmartDashboard_Update_Trigger(
'Shooter F: ', self.robot.shooter.default_kF)
sf_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
f=sf_trigger.get_key_value())))
enc_trigger = SmartDashboard_Update_Trigger(
'ENC Autocorrect constant ',
const.DRIVE_CORRECTION_PROPORTION_FORWARD_ENC)
enc_trigger.whenActive(
Command_Call(
lambda: const.update_enc_const(enc_trigger.get_key_value())))
drive_corr_trigger = SmartDashboard_Update_Trigger(
'Drive Correction Enbaled ', const.DRIVE_CORRECTION_ENABLED)
drive_corr_trigger.whenActive(
Command_Call(lambda: const.update_auto_correct(drive_corr_trigger.
get_key_value())))
shooter_trigger = SmartDashboard_Update_Trigger(
'Shooter Speed ', const.SHOOTER_SPEED)
shooter_trigger.whenActive(
Command_Call(lambda: const.update_shooter_speed(shooter_trigger.
get_key_value())))
# Trigger for changing cameras exposure
cam_exp_trigger = SmartDashboard_Update_Trigger(
'Cam Exposure ', const.DEFAULT_CAMERA_EXPOSURE)
cam_exp_trigger.whenActive(
Command_Call(lambda: self._update_cameras_exposure(
cam_exp_trigger.get_key_value())))
def _update_smartdashboard(self, speed):
SmartDashboard.putNumber("Arm Encoder", self._encoder_value)
SmartDashboard.putNumber("Arm Speed", speed)
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 _update_smartdashboard_arcade_drive(self, linear, turn):
SmartDashboard.putNumber("Drivetrain Linear Speed", linear)
SmartDashboard.putNumber("Drivetrain Turn Speed", turn)
