Ejemplo n.º 1
0
    def teleopInit(self):
        self.autonomousCommand.cancel()

        self.jsAutoTargetButton = JoystickButton(wpilib.Joystick(2), 8)
        self.autoTargetCommand = AutoTargetCommand(self)
        self.teleopCommand.start()
        self.autoTargetCommand.start()
Ejemplo n.º 2
0
class MyRobot(wpilib.IterativeRobot):
    def __init__(self):
        super().__init__()
        self.pdp = wpilib.PowerDistributionPanel()
        self.subsystems = {}
        self.datalogger = DataLogger(logfile=open("robot.log", mode='a'),
                                     curtime=wpilib.DriverStation.getInstance().getMatchTime)

    def robotInit(self):
        subprocess.Popen("/home/lvuser/grip", shell=True)  # Start GRIP process

        self.subsystems['drive'] = DriveSubsystem()
        self.subsystems['shooter'] = ShooterSubsystem(self)
        self.subsystems['tomahawk'] = TomahawkSubsystem()

        self.teleopCommand = TeleopCommand(self)
        self.autonomousCommand = CommandGroup()
        self.autonomousCommand.setTimeout(15)

        self.autoPositionChooser = wpilib.SendableChooser()
        self.autoPositionChooser.addDefault("Position 1", 1)
        self.autoPositionChooser.addObject("Position 2", 2)
        self.autoPositionChooser.addObject("Position 3", 3)
        self.autoPositionChooser.addObject("Position 4", 4)
        self.autoPositionChooser.addObject("Position 5", 5)

        wpilib.SmartDashboard.putData("PositionChooser", self.autoPositionChooser)

        self.autoDefenseChooser = wpilib.SendableChooser()
        self.autoDefenseChooser.addDefault("Nothing", None)
        self.autoDefenseChooser.addObject("Reach", AutoDriveCommand(self, power=.5, dist=100))
        self.autoDefenseChooser.addObject("Rough Terrain", AutoDriveCommand(self, power=.7, dist=200))
        self.autoDefenseChooser.addObject("Rock Wall", AutoDriveCommand(self, power=.9, dist=200,
                                                                        holdpos=config.articulateAngleHigh))
        self.autoDefenseChooser.addObject("Moat", AutoDriveCommand(self, power=-.9, dist=270))
        self.autoDefenseChooser.addObject("Ramparts", AutoDriveCommand(self, power=-.7,
                                                                       dist=200,
                                                                       holdpos=config.articulateAngleHigh))
        self.autoDefenseChooser.addObject("Low Bar", AutoDriveCommand(self, power=.6,
                                                                      dist=200,
                                                                      holdpos=config.articulateAngleLow))
        self.autoDefenseChooser.addObject("Portcullis", AutoDriveCommand(self, power=.6,
                                                                         dist=200,
                                                                         holdpos=config.articulateAngleLow))
        self.autoDefenseChooser.addObject("Cheval de Frise", AutoChevalDeFriseCommand(self))

        wpilib.SmartDashboard.putData("DefenseChooser", self.autoDefenseChooser)

        self.autoActionChooser = wpilib.SendableChooser()
        self.autoActionChooser.addDefault("Nothing", None)
        self.autoActionChooser.addObject("Aim", AutoTargetCommand(self, shoot=False))
        self.autoActionChooser.addObject("Shoot", AutoTargetCommand(self, shoot=True))

        wpilib.SmartDashboard.putData("ActionChooser", self.autoActionChooser)

        wpilib.SmartDashboard.putNumber("Aim at", 150)

        self.datalogger.ready()

    def teleopPeriodic(self):
        Scheduler.getInstance().run()
        self.datalogger.log_data()

    def autonomousPeriodic(self):
        Scheduler.getInstance().run()
        self.datalogger.log_data()

    def teleopInit(self):
        self.autonomousCommand.cancel()

        self.jsAutoTargetButton = JoystickButton(wpilib.Joystick(2), 8)
        self.autoTargetCommand = AutoTargetCommand(self)
        self.teleopCommand.start()
        self.autoTargetCommand.start()

    def autonomousInit(self):
        defense = self.autoDefenseChooser.getSelected()
        action = self.autoActionChooser.getSelected()
        position = self.autoPositionChooser.getSelected()

        k = []
        if defense is not None:
            k += [defense]
        if action is not None:
            action.setPosition(position)
            k += [action]
        for cmd in k:
            cmd.setParent(None)
        self.autonomousCommand = AutonomousCommand(k)
        self.autonomousCommand.start()

    def autoAimShooter(self):
        shooter = self.subsystems['shooter']
        drive = self.subsystems['drive']

        pitch, yawDelta = shooter.calculateShooterParams()
        drive.driveAngle += yawDelta
        shooter.setArticulateAngle(pitch)