Java 类edu.wpi.first.wpilibj.templates.RobotMap 实例源码

/**
 *
 */
public DriveTrain() {
    super("DriveTrain");

    FLeftMotor = new Victor(RobotMap.FLeftMotorPWM);
    FRightMotor = new Victor(RobotMap.FRightMotorPWM);
    BLeftMotor = new Victor(RobotMap.BLeftMotorPWM);
    BRightMotor = new Victor(RobotMap.BRightMotorPWM);
    //MLeftMotor = new Victor(RobotMap.MLeftMotorPWM);
    //MRightMotor = new Victor(RobotMap.MRightMotorPWM);
    drive = new RobotDrive(FLeftMotor, BLeftMotor, FRightMotor, BRightMotor);
    //drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
    //drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);

    GShiftSolDown = new Solenoid(RobotMap.DriveTrainLowGearSolenoid);
    GShiftSolUp = new Solenoid(RobotMap.DriveTrainHighGearSolenoid);
    display = DriverStationLCD.getInstance();
}

public RoboDrive(){

    try {
        //creates the motors
        aLeft = new CANJaguar(RobotMap.LEFT_DRIVE_MOTOR_ALPHA);
        bLeft = new CANJaguar(RobotMap.LEFT_DRIVE_MOTOR_BETA);//, CANJaguar.ControlMode.kSpeed);
        aRight = new CANJaguar(RobotMap.RIGHT_DRIVE_MOTOR_ALPHA);
        bRight = new CANJaguar(RobotMap.RIGHT_DRIVE_MOTOR_BETA);//, CANJaguar.ControlMode.kSpeed);

        //creates the drive train
        roboDrive = new RobotDrive(aLeft, bLeft, aRight, bRight);
        roboDrive.setSafetyEnabled(false);  

    } catch(CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

public Drivetrain()
{
    frontModule = new SwerveModule(RobotMap.frontWheelEncoder, RobotMap.frontModuleEncoder, RobotMap.frontWheelMotor, RobotMap.frontModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, 0, RobotMap.FRONT_BACK_LENGTH/2, "frontModule");
    leftModule = new SwerveModule(RobotMap.leftWheelEncoder, RobotMap.leftModuleEncoder, RobotMap.leftWheelMotor, RobotMap.leftModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, -RobotMap.LEFT_RIGHT_WIDTH/2, 0, "leftModule");
    backModule = new SwerveModule(RobotMap.backWheelEncoder, RobotMap.backModuleEncoder, RobotMap.backWheelMotor, RobotMap.backModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, 0, -RobotMap.FRONT_BACK_LENGTH/2, "backModule");
    rightModule = new SwerveModule(RobotMap.rightWheelEncoder, RobotMap.rightModuleEncoder, RobotMap.rightWheelMotor, RobotMap.rightModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, RobotMap.LEFT_RIGHT_WIDTH/2, 0, "rightModule");

    //We were having occasional errors with the creation of the nav6 object, so we make 5 attempts before allowing the error to go through and being forced to redeploy.
    int count = 0;
    int maxTries = 5;
    while(true) {
        try {
            nav6 = new IMUAdvanced(new BufferingSerialPort(57600));
            if(nav6 != null) break;
        } catch (VisaException e) {
            if (++count == maxTries)
            {
                e.printStackTrace();
                break;
            }
            Timer.delay(.01);
        }
    }

    LiveWindow.addSensor("Drivetrain", "Gyro", nav6);
}

public Autonomous1() {
    System.out.println("Auton");
    addParallel(new SetShooter(RobotMap.preset1), 5.0);
    addSequential(new ChangeElevation(true), 5.0);
    addParallel(new SetShooter(RobotMap.preset1));
    addSequential(new ShooterReady());
    addParallel(new SetShooter(RobotMap.preset1));
    addSequential(new KickHopper());
    addParallel(new SetShooter(RobotMap.preset1));
    addSequential(new ShooterReady());
    addParallel(new SetShooter(RobotMap.preset1));
    addSequential(new KickHopper());
    addParallel(new SetShooter(RobotMap.preset1));
    addSequential(new ShooterReady());
    addParallel(new SetShooter(RobotMap.preset1));
    addSequential(new KickHopper());
    addSequential(new SetShooter(0.0), 1.0);
}

public boolean target() {
    gyro.reset();
    double leftValue = limit((getGyroAngle()-aimRotation)*-RobotMap.rotationKp);
    double rightValue = limit((getGyroAngle()-aimRotation)*RobotMap.rotationKp);

    if (leftValue<0) drive.tankDrive(-2.0+leftValue, 2.0+rightValue);
    else drive.tankDrive(2.0+leftValue, -2.0+rightValue);

    if((getGyroAngle()>=-0.5 && getGyroAngle()<=0.5)) {
        double checkRotation = getImage();
        if ((getGyroAngle()-checkRotation>=-0.5) && (getGyroAngle()-checkRotation<=0.5)) {
            drive.drive(0.0, 0.0);
            return true;
        } else return false;
    } else return false;
}

protected void execute() {

//        for (int i = 0; i < 8; i++){
//            
//            switch(i){
//                
//                case 1:
//                   
//            }
//        }

        RobotMap.shifter_2.set(!RobotMap.shifter.get());
        RobotMap.powerTakeoff_2.set(!RobotMap.powerTakeoff.get());
        RobotMap.frontPusherFirst_2.set(!RobotMap.frontPusherFirst.get());
        RobotMap.frontPusherSecond_2.set(!RobotMap.frontPusherSecond.get());
        RobotMap.rearPusher_2.set(!RobotMap.rearPusher.get());
        RobotMap.popper_2.set(!RobotMap.popper.get());
        RobotMap.popper2_2.set(!RobotMap.popper2.get());


    }

protected void execute() {

        this.determineSetpoint();

        if (lastAngle != RobotMap.desiredShooterAngle) {

            VerticalTurretAxis.resetGyro();      
            lastAngle = RobotMap.desiredShooterAngle;
        }

//        if (RobotMap.desiredShooterAngle != 0.0)    {
//            
//            VerticalTurretAxis.getGyro();
//            this.setSetpoint(RobotMap.desiredShooterAngle);
//            //VerticalTurretAxis = RobotMap.desiredShooterAngle;
//        }
//        
//        if (Math.abs (RobotMap.desiredShooterAngle - VerticalTurretAxis.readGyroDegress()) < 0.5) {
//            RobotMap.desiredShooterAngle = 0.0;
//        }

    }

public ShooterMotorControl() {
    super("ShooterControl", Kp, Ki, Kd);
    Encoder topEncoder = new Encoder(RobotMap.topEncoderChannelA, RobotMap.topEncoderChannelB);
    Encoder bottomEncoder = new Encoder(RobotMap.bottomEncoderChannelA, RobotMap.bottomEncoderChannelB);

    try {
                    shooterJagTop = new CANJaguar(RobotMap.shooterJagTopID);
                    shooterJagBottom = new CANJaguar(RobotMap.shooterJagBottomID);
    } catch (CANTimeoutException e) {
        e.printStackTrace();
    }
    // Use these to get going:
    // setSetpoint() -  Sets where the PID controller should move the system
    //                  to
    // enable() - Enables the PID controller.
    enable();
}

protected void initialize() {
    counter++;
    delayTimer = RobotMap.ShooterDelayTimer;
    delayTimer.start();
    display.println(DriverStationLCD.Line.kUser2, 1, "Pass command " + counter + "                    ");
    display.updateLCD();

}

protected void execute() {
    theDriveTrain.drive(RobotMap.AutonomousSpeed);
    if (time.get() > 1.00) {
        timesup = true;
    }

}

protected void initialize() {
    counter++;
    delayTimer = RobotMap.ShooterDelayTimer;
    delayTimer.start();
    //TODO: use roller subsystem to lower the roller.
    display.println(DriverStationLCD.Line.kUser2, 1, "lauch command " + counter + "                  ");
    display.updateLCD();
}

public void launch() {
    if (launcherSafetySwitch.get() == RobotMap.SafetoFire) {
        launcherLeft.set(RobotMap.LaunchSolenoidValue);
        launcherRight.set(RobotMap.LaunchSolenoidValue);
    } else if (launcherSafetySwitch.get() != RobotMap.SafetoFire) {
        launcherLeft.set(!RobotMap.LaunchSolenoidValue);
        launcherRight.set(!RobotMap.LaunchSolenoidValue);
    }
}

public void pass() {
    if (launcherSafetySwitch.get() == RobotMap.SafetoFire) {
        launcherLeft.set(RobotMap.LaunchSolenoidValue);
        launcherRight.set(!RobotMap.LaunchSolenoidValue);
    } else if (launcherSafetySwitch.get() != RobotMap.SafetoFire) {
        launcherLeft.set(!RobotMap.LaunchSolenoidValue);
        launcherRight.set(!RobotMap.LaunchSolenoidValue);
    }
}

/**
 *
 */
public void shiftLowGear() {
    GShiftSolUp.set(!RobotMap.DriveTrainLowGearSolenoidValue);
    GShiftSolDown.set(RobotMap.DriveTrainLowGearSolenoidValue);
    GShiftSolUp.set(false);
    GShiftSolDown.set(true);
    display.println(Line.kUser1, 1, "Into Low Gear ");
    display.updateLCD();
}

/**
 *
 */
public void shiftHighGear() {
    GShiftSolUp.set(!RobotMap.DriveTrainLowGearSolenoidValue);
    GShiftSolDown.set(RobotMap.DriveTrainLowGearSolenoidValue);
    GShiftSolUp.set(true);
    GShiftSolDown.set(false);

    display.println(Line.kUser1, 1, "Into High Gear ");
    display.updateLCD();
}

public Catapult() {
    try {
        //creates the motors
        Arm2 = new CANJaguar(RobotMap.CATAPULT_MOTOR);//, CANJaguar.ControlMode.kSpeed); 
    } catch (CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

public Arm(){ 
    try {
        //creates the motors
        arm = new CANJaguar(RobotMap.ARM_MOTOR);//, CANJaguar.ControlMode.kSpeed);    

    } catch(CANTimeoutException ex) {
        ex.printStackTrace();
    }
}

public boolean isOverRotated()
{
    return Math.abs(frontModule.getModuleAngle()) > RobotMap.MAX_MODULE_ANGLE 
            || Math.abs(leftModule.getModuleAngle()) > RobotMap.MAX_MODULE_ANGLE 
            || Math.abs(backModule.getModuleAngle()) > RobotMap.MAX_MODULE_ANGLE 
            || Math.abs(rightModule.getModuleAngle()) > RobotMap.MAX_MODULE_ANGLE;
}

protected void execute() {

    switch(state) {

        case 0:
            startTime = System.currentTimeMillis();
            hopper.set(true);
            state = 1;
            break;

        case 1: //On
            if(startTime+RobotMap.hopperKickLength*1000<=System.currentTimeMillis()) state = 2;
            break;

        case 2:
            startTime = System.currentTimeMillis();
            hopper.set(false);
            state = 3;
            break;

        case 3: //Off
            if(startTime+RobotMap.hopperKickReturnLength*1000<=System.currentTimeMillis()) state = 4;
            break;

        case 4: //Check
            if(oi.gunStick.getRawButton(RobotMap.hopperKickerButton)) state = 0;
            break;
    }
}

public Shooter() {
    spinner = new Victor(RobotMap.spinnerVictor);

    spinnerEncoder = new Encoder(RobotMap.spinnerEncoderA, RobotMap.spinnerEncoderB);
    spinnerEncoder.setDistancePerPulse(-RobotMap.spinnerEncoderDistancePerPulse);

    pid = new PIDController(RobotMap.spinnerKp, RobotMap.spinnerKi, RobotMap.spinnerKd, this, this);
    //pid.setPercentTolerance(0.5);
}

public void changeGears(boolean lowGear) {
    boolean isEnabled = leftPID.isEnable() && rightPID.isEnable();

    if(lowGear) {
        leftPID.setPID(RobotMap.driveLowKp, RobotMap.driveLowKi, RobotMap.driveLowKd);
    } else {
        rightPID.setPID(RobotMap.driveHighKp, RobotMap.driveHighKi, RobotMap.driveHighKd);
    }

    if(!isEnabled) {
        leftPID.reset();
        rightPID.reset();
    }
}

protected void usePIDOutput(double output) {
    // Only give the PIDcommand output if the manual control is not on.

    //this.driveTrain.rotate(-output*.55);
    ScraperBike.debugToTable("DriveTrain Targeting", "Speed: " + RobotMap.LatMovOut + ", Rotation: " + output*.63);

    if (Math.abs(RobotMap.LatMovOut) >= .1)
    {
        this.driveTrain.drive(RobotMap.LatMovOut*.3, -output*.63);
    } else if (Math.abs(RobotMap.LatMovOut) < .1)
    {
        this.driveTrain.rotate(-output*.63);
    }
}

/**
 *
 */
public Shoot() {
    super("Shoot");
    shooter = ScraperBike.getShooterController();
    requires(shooter);
    joystick = RobotMap.dStick;
    // Use requires() here to declare subsystem dependencies
    // eg. requires(chassis);
}

/**
 *
 */
public EnableJoystick() {
    // Use requires() here to declare subsystem dependencies
    // eg. requires(chassis);
    super("EnableJoystick");
    RobotMap.enableJoystick();
}

protected void execute() {
    switch (Direction) {
        case RobotMap.realignLeft: RobotMap.cameraXOffset += 1;
                                   break; 

        case RobotMap.realignRight: RobotMap.cameraXOffset -= 1;
                                    break;    

        case RobotMap.realignCenter: RobotMap.cameraXOffset = (double)RobotMap.defaultCameraOffset;
                                     break;   
    }
}

/**
 *
 */
public PauseClimb() {
    // Use requires() here to declare subsystem dependencies
    // eg. requires(chassis);
    super("PauseClimb");
    RobotMap.enablePause();
}

/**
 *
 */
public DisableJoystick() {
    // Use requires() here to declare subsystem dependencies
    // eg. requires(chassis);
    super("DisableJoystick");
    RobotMap.disabledJoystick();
}

/**
 *
 */
public PIDShoot() {

    super("PIDShoot", RobotMap.shooterKp, RobotMap.shooterKi, RobotMap.shooterKd);
    getPIDController().setContinuous(false);
    this.shooter = ScraperBike.getShooterController();
    requires(this.shooter);
    //RobotMap.shootRPM = rpm;
    //this.getPIDController().setSetpoint(RobotMap.shootRPM);
    this.getPIDController().setOutputRange(0.0, 1.0);
    this.getPIDController().setPercentTolerance(2);
}

/**
 *
 * @return
 */
protected double returnPIDInput() {
    // Return your input value for the PID loop
    // e.g. a sensor, like a potentiometer:
    // yourPot.getAverageVoltage() / kYourMaxVoltage;

    ScraperBike.debugToTable("PIDInput", RobotMap.shootEncoder.getRate()*60);

    return RobotMap.shootEncoder.getRate()*60;
}

/**
 *
 */
public PauseCancel() {
    // Use requires() here to declare subsystem dependencies
    // eg. requires(chassis);
    super("PauseCancel");
    RobotMap.disablePause();
}

/** The Pusher constructor is called by the ScraperBike constructor.
 *
 */
public Pusher() {
    frontGripDeployed1 = false;
    frontGripDeployed2 = false;
    rearGripDeployed = false;
    frontGripContacted = false;
    rearGripContacted = false;
    frontSolenoid1 = RobotMap.frontPusherFirst; // Solenoid 3
    frontSolenoid2 = RobotMap.frontPusherSecond; // Solenoid 4
    rearSolenoid = RobotMap.rearPusher; // Solenoid 5
    this.moveFrontPusher(0);
    this.moveRearPusher(0);
}

/** The VerticalTurretAxis constructor is called by the ScraperBike constructor.
 *
 */
public VerticalTurretAxis(){
    super("VerticalTurretAxis");
    verTurretTalon = new Talon(RobotMap.VerTurretMotor);

    gyro = new Gyro(RobotMap.gyroAnalogInput);
}

/** The DriveTrain constructor is called by the ScraperBike constructor.
     *
     */
    public DriveTrain(){
        super("Drive Train");
//        Log = new MetaCommandLog("DriveTrain", "Gyro" , "Left Jaguars,Right Jaguars");
        //gyro1 = new Gyro(RobotMap.AnalogSideCar , RobotMap.DriveTrainGyroInput);
        shifter = RobotMap.shifter; // Solenoid 1
        powerTakeOff = RobotMap.powerTakeoff; // Solenoid 2
        //shifter.setDirection(Relay.Direction.kBoth);

        FrontLeftTalon = new Talon(RobotMap.frontLeftMotor);
        RearLeftTalon = new Talon(RobotMap.rearLeftMotor);
        FrontRightTalon = new Talon(RobotMap.frontRightMotor);
        RearRightTalon = new Talon(RobotMap.rearRightMotor);
        drive = new RobotDrive(FrontLeftTalon, RearLeftTalon, FrontRightTalon, RearRightTalon);
        //drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true);
        //drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
        //drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);
        //drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);

        //lfFrontJag = new Jaguar (3);
        //rtFrontJag = new Jaguar (4);

        //joystick2 = new Joystick(2);
        //sensor1 = new DigitalInput(1);
        //sensor2 = new DigitalInput (2);

    }

public Elevator() {
    super("Elevator", Kp, Ki, Kd);

    motor = new Jaguar(RobotMap.elevatorMotor);
    pot = new AnalogChannel(RobotMap.elevatorPot);

    // Set default position and start PID
    setSetpoint(STOW);
    enable();
}

public Wrist() {
    super("Wrist", Kp, Ki, Kd);
    motor = new Victor(RobotMap.wristMotor);
    pot = new AnalogChannel(RobotMap.wristPot);

    // Set the starting setpoint and have PID start running in the background
    setSetpoint(STOW);
    enable(); // - Enables the PID controller.
}

/**
 *
 */
public Launcher() {
    launcherLeft = new Solenoid(RobotMap.LaunchLeftSolenoid);
    launcherRight = new Solenoid(RobotMap.LaunchRightSolenoid);
    launcherSafetySwitch = new DigitalInput(RobotMap.LauncherSafetyDigitalInput);
}

public void retract() {
    launcherLeft.set(!RobotMap.LaunchSolenoidValue);
    launcherRight.set(!RobotMap.LaunchSolenoidValue);

}

/**
 *
 */
public Roller() {
    rollermotor = new Victor(RobotMap.RollerMotorPWM);
    extendPiston = new Solenoid(RobotMap.RollerExtensionSolenoid);
    retractPiston = new Solenoid(RobotMap.RollerRetractSolenoid);
}

public void setRolleroff() {
    rollermotor.set(RobotMap.RollerOffMotorSpeed);
}

public void setRelease() {
    rollermotor.set(RobotMap.RollerReleaseMotorSpeed);
}