def execute(self):
self.time_accumulator += 1
if self.time_accumulator == 20:
self.time_accumulator = 0
for system in self.systems:
if system.check_continuous_faults():
DriverStation.getInstance().reportWarning(
f"!!! {system.getName()} is faulted !!!", False)
def __check_device_id(self) -> bool:
raw = list()
if self.__m_i2c.read(self.Register.K_PART_ID.value, 1, raw):
DriverStation.reportError("Could not find REV color sensor", False)
return False
if self.__K_PART_ID != raw[0]:
DriverStation.reportError(
"Unknown device found with same I2C address as REV color sensor",
False)
return False
return True
def robotInit(self):
'''Robot initialization function'''
# object that handles basic drive operations
self.frontRightMotor = wpilib.Victor(0)
self.rearRightMotor = wpilib.Victor(1)
self.frontLeftMotor = wpilib.Victor(2)
self.rearLeftMotor = wpilib.Victor(3)
# object that handles basic intake operations
self.omnom_left_motor = wpilib.Spark(7) # make sure channels are correct
self.omnom_right_motor = wpilib.Spark(8)
# object that handles basic lift operations
self.liftMotor = wpilib.Spark(4) # make sure channel is correct
# object that handles basic climb operations
self.winch1 = wpilib.Spark(5)
self.winch2 = wpilib.Spark(6)
# defining motor groups
self.left = wpilib.SpeedControllerGroup(self.frontLeftMotor, self.rearLeftMotor)
self.right = wpilib.SpeedControllerGroup(self.frontRightMotor, self.rearRightMotor)
# setting up drive group for drive motors
self.drive = DifferentialDrive(self.left, self.right)
self.drive.setExpiration(0.1)
# defining omnom motor groups
self.omnom_left = wpilib.SpeedControllerGroup(self.omnom_left_motor)
self.omnom_right = wpilib.SpeedControllerGroup(self.omnom_right_motor)
# setting up omnom group for omnom motors
self.omnom = DifferentialDrive(self.omnom_left, self.omnom_right)
self.omnom.setExpiration(0.1)
# defines timer for autonomous
self.timer = wpilib.Timer()
# joystick 0 & on the driver station
self.leftStick = wpilib.Joystick(0)
self.rightStick = wpilib.Joystick(1)
self.stick = wpilib.Joystick(2)
# initialization of the hall-effect sensors
self.DigitalInput = wpilib.DigitalInput(1)
# initialization of the FMS
self.DS = DriverStation.getInstance()
self.PS = DriverStation.getInstance()
# initialization of the camera server
wpilib.CameraServer.launch()
def __init__(self) -> None:
super().__init__('vision')
self.counter = 0
self.ballcam_table = NetworkTables.getTable('BallCam')
self.driver_station = DriverStation.getInstance()
self.camera_dict = {'red': {}, 'blue': {}}
for key in self.camera_dict.keys():
self.camera_dict[key].update({
'targets_entry':
self.ballcam_table.getEntry(f"/{key}/targets")
})
self.camera_dict[key].update({
'distance_entry':
self.ballcam_table.getEntry(f"/{key}/distance")
})
self.camera_dict[key].update({
'rotation_entry':
self.ballcam_table.getEntry(f"/{key}/rotation")
})
self.targets = 0
self.distance = 0
self.rotation = 0
# set to red by default
self.team_color = 'red'
def get_message():
global _cached_string
if WEEKZERO:
_cached_string = getGameSpecificMessage_WeekZero()
else:
_cached_string = DriverStation.getInstance().getGameSpecificMessage()
return _cached_string
def __init__(self):
super().__init__('Autonomous Program')
# This gets the alliance color (blue/red) from the driver station
self.color = DriverStation.getInstance().getAlliance()
# This gets the robot station (1-3) from the driver station
self.station = DriverStation.getInstance().getLocation()
# This gets the switch position (left/right) from the driver station
self.switch_position = \
DriverStation.getInstance().getGameSpecificMessage()
# This checks whether a game specific message was actually sent
if len(self.switch_position) > 0:
self.switch_position = self.switch_position[0]
self.choose_autonomous()
def execute(self):
if not self.resetDone:
self.resetDone = True
if robotmap.sensors.hasAHRS:
try:
robotmap.sensors.ahrs.zeroYaw();
print('CMD ResetYawAngle: NavX Yaw Zeroed')
except:
print('CMD ResetYawAngle: Exception caught attempted to zero yaw')
if not DriverStation.getInstance().isFmsAttached():
raise
def initialize(self):
loc_switch = DriverStation.getInstance().getGameSpecificMessage()[0]
loc_scale = DriverStation.getInstance().getGameSpecificMessage()[1]
# TODO Add command chains
if robotmap.auton_target == 0:
if loc_switch == 'L':
if robotmap.auton_location == 1:
self.reportCase(loc_switch, robotmap.auton_location)
elif robotmap.auton_location == 2:
self.reportCase(loc_switch, robotmap.auton_location)
elif robotmap.auton_location == 3:
self.reportCase(loc_switch, robotmap.auton_location)
else:
self.logger.error("Auton failed to start!")
elif loc_switch == 'R':
if robotmap.auton_location == 1:
self.reportCase(loc_switch, robotmap.auton_location)
elif robotmap.auton_location == 2:
self.reportCase(loc_switch, robotmap.auton_location)
elif robotmap.auton_location == 3:
self.reportCase(loc_switch, robotmap.auton_location)
else:
self.logger.error("Auton failed to start!")
else:
self.logger.error("Auton failed to start!")
elif robotmap.auton_target == 1:
if loc_scale == 'L':
if robotmap.auton_location == 1:
self.reportCase(loc_scale, robotmap.auton_location)
elif robotmap.auton_location == 3:
self.reportCase(loc_scale, robotmap.auton_location)
elif loc_scale == 'R':
if robotmap.auton_location == 1:
self.reportCase(loc_scale, robotmap.auton_location)
elif robotmap.auton_location == 3:
self.reportCase(loc_scale, robotmap.auton_location)
else:
self.logger.error("Auton failed to start!")
def showField():
field = NetworkTables.getTable("Field")
ds = DriverStation.getInstance()
color = ds.getAlliance()
if color == ds.Alliance.kRed:
field.putValue("color", "red")
elif color == ds.Alliance.kBlue:
field.putValue("color", "blue")
layout = ds.getGameSpecificMessage()
if layout:
field.putValue("layout", layout)
def robotInit(self):
"""Set up everything we need for a working robot."""
if RobotBase.isSimulation():
import mockdata
DriverStation.getInstance().silenceJoystickConnectionWarning(
True) # Amen!
self.subsystems()
controller.layout.init()
autoconfig.init()
driverhud.init()
self.selectedAuto = autoconfig.getAutoProgram()
self.auto = AutonomousCommandGroup()
from commands.startupcommandgroup import StartUpCommandGroup
StartUpCommandGroup().schedule()
from commands.drivetrain.drivecommand import DriveCommand
def __init__(self):
super().__init__()
ds = DriverStation.getInstance()
msg = ds.getGameSpecificMessage()
self.currentAuto = autoconfig.getAutoProgram()
toRun = self.currentAuto
for var in dir(self): # Identifies the method to setup.
if var.lower() == self.currentAuto:
toRun = var
break
eval("self." + toRun + "()") # Runs the method
def on_enable(self):
super().on_enable()
ds = DriverStation.getInstance()
self.start_location = ds.getLocation()
game_data = ds.getGameSpecificMessage()
self.switch_position = game_data[0]
self.scale_position = game_data[1]
self.driver.set_gear(gear=GearMode.LOW)
self.gripper.set_claw_open_state(False)
self.lifter.manual_control = False
self.gripper.set_position_bottom()
self.driving = False
self.turning = False
def robotInit(self):
Command.getRobot = lambda x = 0: self
self.driverStation = DriverStation.getInstance()
self.motorHelper = motorHelper
self.robotMap = robotMap.RobotMap()
self.createNetworkTables()
self.createControllers()
self.drivetrain = subsystems.driveTrain.DriveTrain()
self.loader = subsystems.loader.Loader()
self.timer = wpilib.Timer()
self.loaderButton = wpilib.buttons.JoystickButton(self.auxController, self.robotMap.controllerMap.auxController['loaderToggleButton'])
self.loaderButton.whenPressed(commands.loaderCommand.LoaderToggle())
self.pdp = wpilib.PowerDistributionPanel()
self.healthMonitor = subsystems.driveTrain.HealthMonitor()
self.lifter=subsystems.lifter.Lifter()
#make robot avaiable to commands
wpilib.CameraServer.launch('vision.py:main')
self.smartDashboard.putString('field position' ,"Enter L, R, or C,N")
self.loader.setLoader(self.loader.State.kOpen)
def robotInit(self):
self.navx = Navx.Navx()
self.climber = Climber.Climber()
self.limelight = Limelight.Limelight()
self.drive = Drive.Drive(self)
self.intake = None
self.lift = Lift.Lift(self)
self.intake = Intake.Intake(self)
self.lift._intake = self.intake
self.autonomous = AutonomousBaseSpline.AutonomousBaseSpline(self)
self.controls = DualRemote(self)
self.compressor = Compressor(COMPRESSOR_PIN)
self.driverStation = DriverStation.getInstance()
self.a0 = AnalogInput(0)
self.a1 = AnalogInput(1)
self.a2 = AnalogInput(2)
self.a3 = AnalogInput(3)
self.runAutoOne = True
def write_log(s):
if config.LOGGING and \
s is not None:
DriverStation.reportError(str(s) + "\n", False)
from wpilib import DriverStation
from grt.core import Sensor
# button/pin pair list
BUTTON_TABLE = [('button1', 1), ('button2', 3), ('button3', 5),
('button4', 7), ('button5', 9), ('button6', 11),
('button7', 13), ('button8', 15),
('l_toggle', 10), ('r_toggle', 12),
('orange_button', 14), ('green_button', 16)]
REGISTER_CLK = 2
REGISTER_D1 = 6
REGISTER_D2 = 8
REGISTER_LOAD = 4
IOBOARD = DriverStation.getInstance().getEnhancedIO()
class ButtonBoard(Sensor):
"""
Sensor wrapper for the HH buttonboard.
Has 8 buttons.
"""
button1 = button2 = button3 = button4 = button5 = button6 = \
button7 = button8 = l_toggle = r_toggle = \
orange_button = green_button = False
def __init__(self):
"""
Constructs a new ButtonBoard. Only one should be instantiated.
# dt_l2.changeControlMode(CANTalon.ControlMode.Follower)
# dt_l3.changeControlMode(CANTalon.ControlMode.Follower)
# dt_r2.set(1)
# dt_r3.set(1)
# dt_l2.set(4)
# dt_l3.set(4)
dt = DriveTrain(dt_left,
dt_right,
left_shifter=dt_shifter,
left_encoder=None,
right_encoder=None)
#Straight macro initialization
navx = NavX()
straight_macro = StraightMacro(dt, navx)
# Drive Controllers and sensor pollers
driver_stick = Attack3Joystick(0)
xbox_controller = XboxJoystick(1)
ac = ArcadeDriveController(dt, driver_stick, straight_macro)
hid_sp = SensorPoller((driver_stick, xbox_controller, navx))
# define MechController
mc = MechController(driver_stick, xbox_controller, pickup, manual_shooter)
# define DriverStation
ds = DriverStation.getInstance()
from wpilib import Talon
from grt.core import SensorPoller
# DT Stuff
left_motor_front = Talon(4)
left_motor_back = Talon(3)
right_motor_front = Talon(1)
right_motor_back = Talon(2)
dt = DriveTrain(left_motor_front, left_motor_back, right_motor_front, right_motor_back,left_shifter=None,
right_shifter=None, left_encoder=None, right_encoder=None)
# initialize driver station
ds = DriverStation.getInstance()
# Motor Assignments
flywheel_motor1 = Talon(9)
flywheel_motor2 = Talon(10)
roller_motor = Talon(7)
raiser_motor = Talon(8)
# belts_motor = Talon(?) not connected
# speeds
speed = 0
belt_speed = 0.5
def write_message(s):
DriverStation.reportError(str(s) + "\n", False)
def write_message(s):
DriverStation.reportError(str(s) + "\n", False)
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 autonomousPeriodic(self):
'''This function is called periodically during autonomous.'''
# gets distance from ultrasonic sensor - voltage
distance = self.AnalogInput_one.getVoltage()
distance1 = self.AnalogInput_two.getVoltage()
'''
# Uses voltage to meter to determine the distance to obstacle
if distance > 0.75:
self.drive.tankDrive(0.575, 0.5)
else:
self.drive.tankDrive(0, 0)
if distance1 > 0.75:
self.drive.tankDrive(0.5, 0.5)
else:
self.drive.tankDrive(0, 0)
'''
# gets randomization of field elements
gameData = DriverStation.getInstance().getGameSpecificMessage()
# gets location of robot on the field
position = DriverStation.getInstance().getLocation()
# basic autonomous functions
def stop_motor():
self.drive.tankDrive(0, 0)
def straight_speed():
self.drive.tankDrive(0.5, 0.55)
def left_turn_speed():
self.drive.tankDrive(0.5, -0.5)
def right_turn_speed():
self.drive.tankDrive(-0.5, 0.5)
##################################################################################
# Autonomous functions
def left_switch():
if self.timer.get() < 4.5:
straight_speed()
elif 4.5 < self.timer.get() < 5.25:
left_turn_speed()
elif self.timer.get() > 5.25:
stop_motor()
else:
stop_motor()
def right_switch():
if self.timer.get() < 4.5:
straight_speed()
elif 4.5 < self.timer.get() < 5.25:
right_turn_speed()
# elif statement activating dropping mechanism after turn completion
elif self.timer.get() > 5.25:
stop_motor()
else:
stop_motor()
def left_scale():
if self.timer.get() < 7.30:
straight_speed()
elif 7.30 < self.timer.get() < 8.05:
left_turn_speed()
elif self.timer.get() > 8.05:
stop_motor()
else:
stop_motor()
def right_scale():
if self.timer.get() < 7.30:
straight_speed()
elif 7.30 < self.timer.get() < 8.05:
right_turn_speed()
elif self.timer.get() > 8.05:
stop_motor()
else:
stop_motor()
def straight():
if self.timer.get() < 5.0:
straight_speed()
elif self.timer.get() > 5.0:
stop_motor()
else:
stop_motor()
def center_straight():
if self.timer.get() < 5.0:
straight_speed()
elif self.timer.get() > 4.0:
stop_motor()
else:
stop_motor()
###############################################################
# L-L-R
if gameData == "LLR" and position == 1:
left_switch()
elif position == 2:
center_straight()
elif gameData == "LLR" and position == 3:
straight()
# L-R-L
elif gameData == "LRL" and position == 1:
left_switch()
elif gameData == "LRL" and position == 2:
center_straight()
elif gameData == "LRL" and position == 3:
straight()
# R-L-L
elif gameData == "RLL" and position == 1:
straight()
elif gameData == "RLL" and position == 2:
center_straight()
elif gameData == "RLL" and position == 3:
right_switch()
# R-L-R
elif gameData == "RLR" and position == 1:
straight()
elif gameData == "RLR" and position == 2:
center_straight()
elif gameData == "RLR" and position == 3:
right_switch()
def write_log(s):
if config.LOGGING and \
s is not None:
DriverStation.reportError(str(s) + "\n", False)
