Exemple #1
0
 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)
Exemple #2
0
 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
Exemple #3
0
    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()
Exemple #4
0
    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'
Exemple #5
0
def get_message():
    global _cached_string
    if WEEKZERO:
        _cached_string = getGameSpecificMessage_WeekZero()
    else:
        _cached_string = DriverStation.getInstance().getGameSpecificMessage()
    return _cached_string
Exemple #6
0
    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()
Exemple #7
0
 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
Exemple #8
0
    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!")
Exemple #9
0
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)
Exemple #10
0
    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
Exemple #11
0
    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
Exemple #12
0
    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
Exemple #13
0
    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)
Exemple #14
0
    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
Exemple #15
0
def write_log(s):
    if config.LOGGING and \
       s is not None:
        DriverStation.reportError(str(s) + "\n", False)
Exemple #16
0
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.
Exemple #17
0
# 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()
Exemple #18
0
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
Exemple #19
0
def write_message(s):
    DriverStation.reportError(str(s) + "\n", False)
Exemple #20
0
def write_message(s):
    DriverStation.reportError(str(s) + "\n", False)
Exemple #21
0
 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)
Exemple #22
0
    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()
Exemple #23
0
def write_log(s):
    if config.LOGGING and \
       s is not None:
        DriverStation.reportError(str(s) + "\n", False)