示例#1
0
  public void driveToBridge() {
    System.out.println("Val: " + potentiometer.getVoltage());

    if (Autonomousflag) {
      if (potentiometer.getVoltage() < 5) { // line
        shoot();
        Timer.delay(7);
        shoot.set(0);
        collect.set(0);

        driveTrain.tankDrive(-speed, speed);
        Timer.delay(1);
        driveTrain.tankDrive(speed, speed);
        Timer.delay(1.2);

        driveTrain.tankDrive(0, 0);
        Timer.delay(0.9);
        bridge.set(-1);

      } else if (potentiometer.getVoltage() > 5) { // left
        shoot();
      }
      Autonomousflag = false;
    } else {
      driveTrain.tankDrive(0, 0);
    }
  }
  public void autonomousInit() {
    try {
      robot.arm.moveTo(PegPosition.RESET);
    } catch (CANTimeoutException e1) {
      // TODO Auto-generated catch block
      e1.printStackTrace();
    }
    robot.compressor.start();
    Timer.delay(3.5);
    robot.grabber.tiltDown();

    Timer.delay(2);
    robot.grabber.grab();

    robot.lineSensor.setLinePreference(LinePreference.LEFT);
    controller.enable();

    Timer.delay(2.5);
    robot.grabber.tiltUp();
    Timer.delay(2.5);
    try {
      robot.arm.moveTo(PegPosition.MIDDLE_OFFSET);
    } catch (CANTimeoutException e) {
      // TODO Auto-generated catch block
      e.printStackTrace();
    }
  }
 // Called just before this Command runs the first time
 protected void initialize() {
   Robot.driveTrain.setLeftMotors(0.5);
   Robot.driveTrain.setRightMotors(-0.5);
   Timer.delay(1.5);
   Robot.driveTrain.setLeftMotors(0);
   Robot.driveTrain.setRightMotors(-0.8);
   Timer.delay(0.5);
   Robot.driveTrain.setLeftMotors(0.6);
   Robot.driveTrain.setRightMotors(-0.5);
   Timer.delay(1);
 }
示例#4
0
 // Called just before this Command runs the first time
 protected void initialize() {
   Robot.intake.setIntakePosition(true);
   Timer.delay(1);
   Robot.shooter.setShooterPosition(true);
   Robot.intake.setIntakePosition(false);
   Timer.delay(4);
   // Robot.shooter.switchShooterPosition();
   // runTime.reset();
   // runTime.start();
   System.out.println("rock wall");
 }
 protected void initialize() {
   if (!inProgress) {
     inProgress = true;
     Pickup.checkArms();
     Catapult.latch.set(Latch.UNLOCKED);
     Timer.delay(0.5);
     Catapult.setLauncher(RETRACTED);
     Timer.delay(2.0);
     Catapult.latch.set(Latch.LOCKED);
   }
 }
示例#6
0
  /** Displays test pattern of all defined characters. */
  public void displayTestPattern() {
    byte[] byte1 = new byte[10];

    Util.consoleLog();

    // first reset the transmit array to zeros.
    for (int c = 0; c < 10; c++) {
      byte1[c] = (byte) (0b00000000) & 0xFF;
    }

    // put single character data in the array and write to display.
    for (int c = 0; c < 9; c++) {
      byte1[0] = (byte) (0b0000111100001111);
      byte1[2] = charreg[4 * c + 3][0];
      byte1[3] = charreg[4 * c + 3][1];
      byte1[4] = charreg[4 * c + 2][0];
      byte1[5] = charreg[4 * c + 2][1];
      byte1[6] = charreg[4 * c + 1][0];
      byte1[7] = charreg[4 * c + 1][1];
      byte1[8] = charreg[4 * c][0];
      byte1[9] = charreg[4 * c][1];

      // send the array to the board
      i2c.writeBulk(byte1);

      Timer.delay(3);
    }
  }
示例#7
0
  /**
   * start up automatic capture you should see the video stream from the webcam in your FRC PC
   * Dashboard.
   */
  public void operatorControl() {

    while (isOperatorControl() && isEnabled()) {
      /** robot code here! * */
      Timer.delay(0.005); // wait for a motor update time
    }
  }
示例#8
0
 /** Runs the motors with tank steering. */
 public void operatorControl() {
   myRobot.setSafetyEnabled(true);
   while (isOperatorControl() && isEnabled()) {
     myRobot.tankDrive(leftStick, rightStick);
     Timer.delay(0.005); // wait for a motor update time
   }
 }
  public AutoCommandGroup() {

    // STARTING AT YELLOW BIN

    addSequential(new LiftCommand(0.75, 0.5, true));
    Timer.delay(1);
    addSequential(new AutoMove(1, 2.5, 13.5));
    addParallel(new LiftCommand(1, 0.5, true));
    addSequential(new LiftCommand(0.75, 0.5, false));

    // STARTING AT LANDFILL

    // addSequential(new AutoMove (0.6, 2.5, 0.4));
    // Timer.delay(1.5);
    // addSequential(new LiftCommand (0.75,0.5, true));
    // addSequential(new AutoMove (-1, 2.5, 7.8));
    // addSequential (new LiftCommand (0.75, 0.5, false));

    // Add Commands here:
    // e.g. addSequential(new Command1());
    //      addSequential(new Command2());
    // these will run in order.

    // To run multiple commands at the same time,
    // use addParallel()
    // e.g. addParallel(new Command1());
    //      addSequential(new Command2());
    // Command1 and Command2 will run in parallel.

    // A command group will require all of the subsystems that each member
    // would require.
    // e.g. if Command1 requires chassis, and Command2 requires arm,
    // a CommandGroup containing them would require both the chassis and the
    // arm.
  }
示例#10
0
 public void testCounter() {
   double counter = 0.0;
   while (true) {
     SmartDashboard.putNumber("Counter", counter++);
     Timer.delay(.10);
   }
 }
 public void init() {
   try {
     TKOHardware.changeTalonMode(
         TKOHardware.getLeftDrive(), CANTalon.ControlMode.Position, p, i, d);
     TKOHardware.changeTalonMode(
         TKOHardware.getRightDrive(), CANTalon.ControlMode.Position, p, i, d);
     TKOHardware.getLeftDrive().reverseOutput(false);
     TKOHardware.getRightDrive().reverseOutput(true);
     TKOHardware.getLeftDrive().reverseSensor(true);
     TKOHardware.getRightDrive().reverseSensor(false);
     TKOHardware.getLeftDrive().enableBrakeMode(true);
     TKOHardware.getRightDrive().enableBrakeMode(true);
     TKOHardware.getLeftDrive().setPosition(0); // resets encoder
     TKOHardware.getRightDrive().setPosition(0);
     TKOHardware.getLeftDrive().ClearIaccum(); // stops bounce
     TKOHardware.getRightDrive().ClearIaccum();
     Timer.delay(0.1);
     TKOHardware.getLeftDrive().set(TKOHardware.getLeftDrive().getPosition());
     TKOHardware.getRightDrive().set(TKOHardware.getRightDrive().getPosition());
     TKOHardware.getPiston(0).set(Definitions.SHIFTER_LOW);
     TKOHardware.getPiston(2).set(Definitions.WHEELIE_RETRACT);
   } catch (TKOException e) {
     e.printStackTrace();
     System.out.println("Err.... Talons kinda died ");
   }
   System.out.println("Drive atom initialized");
 }
示例#12
0
 protected void end() {
   Timer.delay(1.5);
   if (Catapult.latch.get() == Latch.UNLOCKED) {
     Catapult.latch.set(Latch.LOCKED); // set latch in
   }
   inProgress = false;
 }
  public TargetFinder() {
    System.out.println("TargetFinder() begin " + Timer.getFPGATimestamp());

    cam = AxisCamera.getInstance();
    cam.writeResolution(AxisCamera.ResolutionT.k320x240);
    cam.writeBrightness(camBrightness);
    cam.writeColorLevel(camColor);
    cam.writeWhiteBalance(camWhiteBalance);
    cam.writeExposureControl(camExposure);
    cam.writeMaxFPS(15);
    cam.writeExposurePriority(AxisCamera.ExposurePriorityT.none);
    cam.writeCompression(50);
    // System.out.println("TargetFinder() * " + cam.toString() + "[" + Timer.getFPGATimestamp());
    boxCriteria = new CriteriaCollection();
    inertiaCriteria = new CriteriaCollection();
    boxCriteria.addCriteria(
        NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, bboxWidthMin, false);
    boxCriteria.addCriteria(
        NIVision.MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, bboxHeightMin, false);
    inertiaCriteria.addCriteria(
        NIVision.MeasurementType.IMAQ_MT_NORM_MOMENT_OF_INERTIA_XX, 0, inertiaXMin, true);
    inertiaCriteria.addCriteria(
        NIVision.MeasurementType.IMAQ_MT_NORM_MOMENT_OF_INERTIA_YY, 0, inertiaYMin, true);
    Timer.delay(7);
  }
示例#14
0
  @Override
  protected void initialize() {
    backLeft = new Talon(ControlsManager.BACK_LEFT_SPEED_CONTROLLER);
    frontLeft = new Talon(ControlsManager.FRONT_LEFT_SPEED_CONTROLLER);
    backRight = new Talon(ControlsManager.BACK_RIGHT_SPEED_CONTROLLER);
    frontRight = new Talon(ControlsManager.FRONT_RIGHT_SPEED_CONTROLLER);

    backLeft.setInverted(true);
    frontLeft.setInverted(true);
    backRight.setInverted(true);
    frontRight.setInverted(true);

    lidar = new LIDAR(Port.kMXP);

    // ultrasonic = new Ultrasonic(RobotMap.ultraPing, RobotMap.ultraEcho);
    // ultrasonic.setEnabled(true); ultrasonic.setAutomaticMode(true);

    maxbotix = new BadUltrasonic(ControlsManager.MAXBOTIX_ULTRASONIC);

    // mxp stuff
    serialPort = new SerialPort(57600, SerialPort.Port.kMXP);

    byte update_rate_hz = 127;
    mxp = new IMU(serialPort, update_rate_hz);
    Timer.delay(0.3);
    mxp.zeroYaw();

    train = new RobotDrive(backLeft, frontLeft, backRight, frontRight);
  }
示例#15
0
 /** Gyro sensitivity is set and mecanum drive is used with the gyro angle as an input. */
 public void operatorControl() {
   gyro.setSensitivity(
       voltsPerDegreePerSecond); // calibrate gyro to have the value equal to degrees
   while (isOperatorControl() && isEnabled()) {
     myRobot.mecanumDrive_Cartesian(
         joystick.getX(), joystick.getY(), joystick.getZ(), gyro.getAngle());
     Timer.delay(0.005); // wait 5ms to avoid hogging CPU cycles
   }
 }
示例#16
0
 public void turnStart(double degrees) {
   turnPID.reset();
   gyro.reset();
   turnPID.setSetpoint(degrees);
   turnPID.enable();
   Timer.delay(.1);
   System.out.println(
       "start turning from " + RobotMap.roundtoTwo(gyro.getAngle()) + " to setpoint " + degrees);
 }
示例#17
0
 protected void execute() {
   if (!inProgress) {
     inProgress = true;
     if (Catapult.latch.get() == Latch.LOCKED) {
       Catapult.latch.set(Latch.UNLOCKED);
       Timer.delay(0.2);
     }
     if (pwmMode) {
       Pickup.checkArms();
       Catapult.setLauncher(EXTENDED);
       Timer.delay(0.2);
       Catapult.setLauncher(RETRACTED);
       Timer.delay(0.5);
     } else {
       Catapult.launchRight.set(EXTENDED);
       Timer.delay(3.0);
       Catapult.launchRight.set(RETRACTED);
       Timer.delay(2.0);
     }
   }
 }
示例#18
0
  /**
   * Enable/Disable character LED blink.
   *
   * @param blink True to blink.
   */
  public void blink(boolean blink) {
    byte[] buffer = new byte[1];

    Util.consoleLog("%b", blink);

    if (blink) buffer[0] = (byte) 0x83;
    else buffer[0] = (byte) 0x81;

    i2c.writeBulk(buffer);

    Timer.delay(.01);
  }
示例#19
0
 // Called repeatedly when this Command is scheduled to run
 protected void execute() {
   if (step == 0) {
     Robot.canon.pushLaunchArm(pushSpeed);
     System.out.print("Pushing for ");
     System.out.println(pushTime);
     Timer.delay(pushTime);
     Robot.canon.returnLaunchArm(returnSpeed);
     System.out.print("Pulling for ");
     System.out.println(returnTime);
     setTimeout(returnTime);
     step = 1;
   }
 }
示例#20
0
 public synchronized void openCamera() {
   if (m_id != -1) return; // Camera is already open
   for (int i = 0; i < 3; i++) {
     try {
       m_id =
           NIVision.IMAQdxOpenCamera(
               m_name, NIVision.IMAQdxCameraControlMode.CameraControlModeController);
     } catch (VisionException e) {
       if (i == 2) throw e;
       delay(2.0);
       continue;
     }
     break;
   }
 }
 // Called just before this Command runs the first time
 protected void initialize() {
   shooter.triggerRunning = true;
   System.out.println("Trigger Forward");
   shooter.setTrigger(-1);
   Timer.delay(runTime);
   System.out.println("Trigger Backward");
   shooter.setTrigger(1);
   Timer.delay(runTime);
   System.out.println("Trigger Finished");
   shooter.setTrigger(0);
   shooter.triggerRunning = false;
   //
   //        while (shooter.triggerPot.getAverageValue() < Shooter.TRIGGER_END) {
   //            shooter.setTrigger(-1);
   //            System.out.println(shooter.triggerPot.getAverageValue());
   //        }
   //        shooter.setTrigger(0);
   //        while (shooter.triggerPot.getAverageValue() > Shooter.TRIGGER_START) {
   //            shooter.setTrigger(.7);
   //
   //            System.out.println(shooter.triggerPot.getAverageValue());
   //        }
   //        shooter.setTrigger(0);
 }
示例#22
0
  public void calibrateAcel() {
    double avgX = 0;
    double avgY = 0;

    for (int i = 0; i < calibrationSamples; i++) {
      avgX += acel.getX();
      avgY += acel.getY();
      Timer.delay(0.005); // calibrationSampleRate / 1000.0);
    }
    avgX /= calibrationSamples;
    avgY /= calibrationSamples;
    acelBias = new Vector2(avgX, avgY);
    lastTimestamp = Timer.getFPGATimestamp();
    // SmartDashboard.putNumber("X Bias", acelBias.x);
    // SmartDashboard.putNumber("Y Bias", acelBias.y);
  }
示例#23
0
  public void teleopPeriodic() {
    Definitions.drivetrain.arcadeDrive(
        Definitions.xbox1.getRawAxis(1), Definitions.xbox1.getRawAxis(4), false);
    DriveShifter.checkGearShift();
    SparkyIntakeBar.loadingRoutine();
    Shooter.firingRoutine(5000);
    ArduinoComm.communicate();

    try {
      if (!backfailed && !frontfailed) {
        if (Definitions.buttonbox.getRawButton(2) && !lastbutton4) {
          if (currsession == front) {
            NIVision.IMAQdxStopAcquisition(currsession);
            currsession = back;
            NIVision.IMAQdxConfigureGrab(currsession);
          } else if (currsession == back) {
            NIVision.IMAQdxStopAcquisition(currsession);
            currsession = front;
            NIVision.IMAQdxConfigureGrab(currsession);
          }
        }
      }
      lastbutton4 = Definitions.buttonbox.getRawButton(2);
      NIVision.IMAQdxGrab(currsession, frame, 1);
      CameraServer.getInstance().setImage(frame);
    } catch (Exception e) {
      //    		System.out.println("Camera problem");
    }
    SmartDashboard.putNumber("Pressure", 250 * (Definitions.pressuretrans.getVoltage() / 5.0) - 25);
    SmartDashboard.putNumber("ballSeater", Definitions.ballholder.get() ? 1 : 0);
    SmartDashboard.putBoolean(
        "catapultReady", (250 * (Definitions.pressuretrans.getVoltage() / 5.0) - 25) > 40);

    if (Definitions.joystick.getRawButton(3) && !lastbutton3) {
      Definitions.flashlightrelay.set(
          Definitions.flashlightrelay.get() == Relay.Value.kOff
              ? Relay.Value.kForward
              : Relay.Value.kOff);
    }
    //    	System.out.println(Definitions.flashlightrelay.get());
    lastbutton3 = Definitions.joystick.getRawButton(3);
    System.out.println(Definitions.pdp.getCurrent(4));

    Timer.delay(0.01);
  }
  /** This function is called once each time the robot enters operator control. */
  public void operatorControl() {
    drivetrain.setSafetyEnabled(true);
    compressor.start();
    while (isOperatorControl() && isEnabled()) {
      // The throttle goes from 1 to -1, bottom to top. This
      // scales it to go 0 to 1, bottom to top.
      double throttle = (-leftStick.getRawAxis(3) + 1) / 2;

      double drive = deadband(leftStick.getY()) * throttle;
      double steer = deadband(-rightStick.getX()) * throttle;
      drivetrain.arcadeDrive(drive, steer);

      boolean shouldSpinWheels = leftStick.getRawButton(1);
      boolean shouldFire = shouldSpinWheels && rightStick.getRawButton(1);
      shooterWheels.set(shouldSpinWheels ? -1 : 0);
      shooter.set(shouldFire);

      Timer.delay(0.005); // wait for a motor update time
    }
  }
示例#25
0
 public void upLittle() {
   robot.elevator.elevatorVics.set(1);
   Timer.delay(0.2);
   robot.elevator.elevatorVics.set(0);
 }
示例#26
0
 public void mecanumDrive(double x, double y, double twist, double gyro) {
   drive.mecanumDrive_Cartesian(x, y, twist, gyro);
   Timer.delay(0.001);
 }
示例#27
0
  public void operatorControl() {
    log("Teleop Called Once!");
    resetError(1);
    resetError(2);
    this.elevator.resetState();
    //        try{
    showUserMessages("TeleOp");
    this.arm.state = 0;

    while (isOperatorControl() && isEnabled()) {
      // robotdrive.setSafetyEnabled(true); //In case the program were to stop, this stops the
      // motors from continuing to run.
      if (!Util.checkButton(this, Constants.robotdrive_break_button)) {
        //                robotdrive.tankDrive(-doRobotdriveScaling(joystick_left.getY()),
        //                        -doRobotdriveScaling(joystick_right.getY()));
        robotdrive.arcadeDrive(
            doRobotdriveScaling(joystick_left.getY()), doRobotdriveScaling(joystick_left.getX()));
      }
      // x, y int := 0, 1;
      //            if (driverstation.getDigitalIn(3)){
      //                this.test_CIM_1.set(driverstation.getAnalogIn(2));
      //            }else{
      //                this.test_CIM_1.set(0-driverstation.getAnalogIn(2));
      //            }
      //
      //            if (driverstation.getDigitalIn(4)){
      //                this.test_CIM_2.set(driverstation.getAnalogIn(3));
      //            }else{
      //                this.test_CIM_2.set(0-driverstation.getAnalogIn(3));
      //            }

      updateSmartDashboard();

      this.shooter.updateStates(this);
      this.shooterMotor.set(this.shooter.calculateShooterSpeed());

      if (this.driverstation.getDigitalIn(4)) {
        this.armMotor.set(this.driverstation.getAnalogIn(3));
      } else {
        this.armMotor.set(-this.driverstation.getAnalogIn(3));
      }

      this.arm.update();
      if (this.arm.getArmSpeed() != 0D) {
        this.armMotor.set(this.arm.getArmSpeed());
      }

      if (this.integ.doElevatorUpdate()) {
        this.elevator.update();
        this.Elevator_motor.set(this.elevator.getElevatorSpeed());
      }

      this.intake.update();
      this.Intake_motor.set(this.intake.calculateIntakeSpeed());
      if (this.intake.state == 1) {
        this.Elevator_intake_motor.set(0.6D);
      } else {
        this.Elevator_intake_motor.set(0D);
      }

      if (this.driverstation.getDigitalIn(3)) {
        this.pullDownJaguar.set(this.driverstation.getAnalogIn(2));
      } else {
        this.pullDownJaguar.set(-this.driverstation.getAnalogIn(2));
      }

      this.integ.update();

      SmartDashboardUpdater.updateSmartDashboard(this);

      if (Util.checkButton(this, 9)) {

        this.elevator.numDiscs = 0;
      }
      if (Util.checkButton(this, 8)) {
        this.elevator.state = 0;
        this.integ.state = 0;
        this.intake.state = 0;
        this.integ.elevator_out_roller.set(0d);
        this.arm.state = 0;
      }

      {
        try {
          SmartDashboard.putString("DEBUG*isConnected", "" + this.server.isConnected());
          SmartDashboard.putString("DEBUG*isServer", "" + this.server.isServer());
          SmartDashboard.putDouble("FPS~!", this.server.getNumber("FPS", -1d));
          SmartDashboard.putDouble("NewDistance~!", this.server.getNumber("NewDistance", -1d));
          // SmartDashboard.putString("NetworkTable Keys", this.server.);

          log("W0RK1NG!");
        } catch (Exception be) {
          error("ERROR UPDATING SmartDashboard dtt STACK TRACE: " + be.getMessage(), 1);
        }
      }

      Timer.delay(0.005); // and make sure we dont overload the cRIO
    }
    //        }catch (Exception be){
    //            error("ERROR IN TELEOP! STACKTRACE: \n "+be.getMessage(), 2);
    //        }
  }
  @Override
  public void execute() {
    System.out.println("Starting execution");
    try {
      if (distance > 0) {
        while (DriverStation.getInstance().isEnabled()
            && TKOHardware.getDriveTalon(0).getSetpoint() < distance) {
          TKOHardware.getDriveTalon(0)
              .set(TKOHardware.getDriveTalon(0).getSetpoint() + incrementer);
          TKOHardware.getDriveTalon(2)
              .set(TKOHardware.getDriveTalon(2).getSetpoint() + incrementer);
          System.out.println(
              "Ncoder Left: "
                  + TKOHardware.getDriveTalon(0).getPosition()
                  + "\t Ncoder Rgith: "
                  + TKOHardware.getDriveTalon(2).getPosition()
                  + "\t Left Setpoint: "
                  + TKOHardware.getDriveTalon(0).getSetpoint());
          TKOLogger.getInstance()
              .addMessage(
                  "Ncoder Left: "
                      + TKOHardware.getDriveTalon(0).getPosition()
                      + "\t Ncoder Rgith: "
                      + TKOHardware.getDriveTalon(2).getPosition()
                      + "\t Left Setpoint: "
                      + TKOHardware.getDriveTalon(0).getSetpoint());
          Timer.delay(0.001);
        }
      } else {
        while (DriverStation.getInstance().isEnabled()
            && TKOHardware.getDriveTalon(0).getSetpoint() > distance) {
          TKOHardware.getDriveTalon(0)
              .set(TKOHardware.getDriveTalon(0).getSetpoint() - incrementer);
          TKOHardware.getDriveTalon(2)
              .set(TKOHardware.getDriveTalon(2).getSetpoint() - incrementer);
          System.out.println(
              "Ncoder Left: "
                  + TKOHardware.getDriveTalon(0).getPosition()
                  + "\t Ncoder Rgith: "
                  + TKOHardware.getDriveTalon(2).getPosition()
                  + "\t Left Setpoint: "
                  + TKOHardware.getDriveTalon(0).getSetpoint());
          TKOLogger.getInstance()
              .addMessage(
                  "Ncoder Left: "
                      + TKOHardware.getDriveTalon(0).getPosition()
                      + "\t Ncoder Rgith: "
                      + TKOHardware.getDriveTalon(2).getPosition()
                      + "\t Left Setpoint: "
                      + TKOHardware.getDriveTalon(0).getSetpoint());
          Timer.delay(0.001);
        }
      }

      TKOHardware.getDriveTalon(0).set(distance);
      TKOHardware.getDriveTalon(2).set(distance);

      while (Math.abs(TKOHardware.getLeftDrive().getPosition() - distance) > threshold
          && DriverStation.getInstance().isEnabled()) {
        // not close enough doe; actually gets stuck here
        TKOLogger.getInstance()
            .addMessage(
                "NOT CLOSE ENOUGH TO TARGET DIST: "
                    + (TKOHardware.getLeftDrive().getPosition() - distance));
        Timer.delay(0.001);
      }

    } catch (TKOException e1) {
      e1.printStackTrace();
      System.out.println("Error at another expected spot, I would assume....");
    }
    System.out.println("Done executing");
  }
示例#29
0
  /**
   * This autonomous (along with the chooser code above) shows how to select between different
   * autonomous modes using the dashboard. The sendable chooser code works with the Java
   * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
   * uncomment the getString line to get the auto name from the text box below the Gyro
   *
   * <p>You can add additional auto modes by adding additional comparisons to the switch structure
   * below with additional strings. If using the SendableChooser make sure to add them to the
   * chooser code above as well.
   */
  public void autonomousInit() {
    autoSelected = (String) chooser.getSelected();
    // autoSelected = SmartDashboard.getString("Auto Selector", defaultAuto);
    System.out.println("Auto selected: " + autoSelected);

    switch (autoSelected) {
      case touch:
        Timer.delay(0.5); // Wait 0.5 sec

        //			shooter.armAuto(0.50);		// Lower arm - run motors at 50% speed for 0.5 sec
        //			Timer.delay(0.5);
        //			shooter.armAuto(0);
        //			Timer.delay(1.5);

        tankDrive.auto(0.53, 0.60); // Move forward - run motors at 60% speed for 2 sec
        Timer.delay(2);

        tankDrive.auto(0, 0); // Stop robot
        break;
      case lowBarCross:
        Timer.delay(0.5); // Wait 0.5 sec

        //			shooter.armAuto(0.50);		// Lower arm - run motors at 50% speed for 0.5 sec
        //			Timer.delay(0.5);
        //			shooter.armAuto(0);
        //			Timer.delay(1.5);

        tankDrive.auto(0.0, 0.60); // Move forward - run motors at 80% speed for 4 sec
        Timer.delay(.1);

        tankDrive.auto(0.53, 0.60); // Move forward - run motors at 60% speed for 4.5 sec
        Timer.delay(4.5);

        tankDrive.auto(0, 0); // Stop robot
        break;
      case rockWallCross:
        Timer.delay(0.5); // Wait 0.5 sec

        //			shooter.armAuto(0.50);		// Lower arm - run motors at 50% speed for 0.5 sec
        //			Timer.delay(0.5);
        //			shooter.armAuto(0);
        //			Timer.delay(1.5);

        tankDrive.auto(0.0, 0.85); // Move forward - run motors at 80% speed for 4 sec
        Timer.delay(.1);

        tankDrive.auto(0.78, 0.85); // Move forward - run motors at 80% speed for 4 sec
        Timer.delay(4.5);

        tankDrive.auto(0, 0); // Stop robot
        break;
      case roughTerrainCross:
        Timer.delay(0.5); // Wait 0.5 sec

        //			shooter.armAuto(0.50);		// Lower arm - run motors at 50% speed for 0.5 sec
        //			Timer.delay(0.5);
        //			shooter.armAuto(0);
        //			Timer.delay(1.5);

        tankDrive.auto(0.0, 0.80); // Move forward - run motors at 80% speed for 4 sec
        Timer.delay(.1);

        tankDrive.auto(0.73, 0.80); // Move forward - run motors at 80% speed for 4 sec
        Timer.delay(4);

        tankDrive.auto(0, 0); // Stop robot
        break;
      default:
        // Nothing ...
        break;
    }
  }
示例#30
0
  public void run() {

    stop = false;
    boolean stream_response_received = false;
    double last_valid_packet_time = 0.0;
    int partial_binary_packet_count = 0;
    int stream_response_receive_count = 0;
    int timeout_count = 0;
    int discarded_bytes_count = 0;
    int port_reset_count = 0;
    double last_stream_command_sent_timestamp = 0.0;
    int updates_in_last_second = 0;
    double last_second_start_time = 0;
    try {
      serial_port.setReadBufferSize(512);
      serial_port.setTimeout(1.0);
      serial_port.enableTermination('\n');
      serial_port.flush();
      serial_port.reset();
    } catch (RuntimeException ex) {
      ex.printStackTrace();
    }

    IMUProtocol.StreamResponse response = new IMUProtocol.StreamResponse();

    byte[] stream_command = new byte[256];

    int cmd_packet_length =
        IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
    try {
      serial_port.reset();
      serial_port.write(stream_command, cmd_packet_length);
      serial_port.flush();
      port_reset_count++;
      // SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
      last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
    } catch (RuntimeException ex) {
      ex.printStackTrace();
    }

    while (!stop) {
      try {

        // Wait, with delays to conserve CPU resources, until
        // bytes have arrived.

        while (!stop && (serial_port.getBytesReceived() < 1)) {
          Timer.delay(1.0 / update_rate_hz);
        }

        int packets_received = 0;
        byte[] received_data = serial_port.read(256);
        int bytes_read = received_data.length;
        if (bytes_read > 0) {
          byte_count += bytes_read;
          int i = 0;
          // Scan the buffer looking for valid packets
          while (i < bytes_read) {

            // Attempt to decode a packet

            int bytes_remaining = bytes_read - i;

            if (received_data[i] != IMUProtocol.PACKET_START_CHAR) {
              /* Skip over received bytes until a packet start is detected. */
              i++;
              discarded_bytes_count++;
              // SmartDashboard.putNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
              continue;
            } else {
              if ((bytes_remaining > 2) && (received_data[i + 1] == (byte) '#')) {
                /* Binary packet received; next byte is packet length-2 */
                byte total_expected_binary_data_bytes = received_data[i + 2];
                total_expected_binary_data_bytes += 2;
                while (bytes_remaining < total_expected_binary_data_bytes) {

                  /* This binary packet contains an embedded     */
                  /* end-of-line character.  Continue to receive */
                  /* more data until entire packet is received.  */
                  byte[] additional_received_data = serial_port.read(256);
                  byte_count += additional_received_data.length;
                  bytes_remaining += additional_received_data.length;

                  /* Resize array to hold existing and new data */
                  byte[] c = new byte[received_data.length + additional_received_data.length];
                  if (c.length > 0) {
                    System.arraycopy(received_data, 0, c, 0, received_data.length);
                    System.arraycopy(
                        additional_received_data,
                        0,
                        c,
                        received_data.length,
                        additional_received_data.length);
                    received_data = c;
                  } else {
                    /* Timeout waiting for remainder of binary packet */
                    i++;
                    bytes_remaining--;
                    partial_binary_packet_count++;
                    // SmartDashboard.putNumber("nav6 Partial Binary Packets",
                    // (double)partial_binary_packet_count);
                    continue;
                  }
                }
              }
            }

            int packet_length = decodePacketHandler(received_data, i, bytes_remaining);
            if (packet_length > 0) {
              packets_received++;
              update_count++;
              last_valid_packet_time = Timer.getFPGATimestamp();
              updates_in_last_second++;
              if ((last_valid_packet_time - last_second_start_time) > 1.0) {
                // SmartDashboard.putNumber("nav6 UpdatesPerSec", (double)updates_in_last_second);
                updates_in_last_second = 0;
                last_second_start_time = last_valid_packet_time;
              }
              i += packet_length;
            } else {
              packet_length =
                  IMUProtocol.decodeStreamResponse(received_data, i, bytes_remaining, response);
              if (packet_length > 0) {
                packets_received++;
                setStreamResponse(response);
                stream_response_received = true;
                i += packet_length;
                stream_response_receive_count++;
                // SmartDashboard.putNumber("nav6 Stream Responses",
                // (double)stream_response_receive_count);
              } else {
                // current index is not the start of a valid packet; increment
                i++;
              }
            }
          }

          if ((packets_received == 0) && (bytes_read == 256)) {
            // Workaround for issue found in SerialPort implementation:
            // No packets received and 256 bytes received; this
            // condition occurs in the SerialPort.  In this case,
            // reset the serial port.
            serial_port.reset();
            port_reset_count++;
            // SmartDashboard.putNumber("nav6_PortResets", (double)port_reset_count);
          }

          // If a stream configuration response has not been received within three seconds
          // of operation, (re)send a stream configuration request

          if (!stream_response_received
              && ((Timer.getFPGATimestamp() - last_stream_command_sent_timestamp) > 3.0)) {
            cmd_packet_length =
                IMUProtocol.encodeStreamCommand(stream_command, update_type, update_rate_hz);
            try {
              last_stream_command_sent_timestamp = Timer.getFPGATimestamp();
              serial_port.write(stream_command, cmd_packet_length);
              serial_port.flush();
            } catch (RuntimeException ex2) {
              ex2.printStackTrace();
            }
          } else {
            // If no bytes remain in the buffer, and not awaiting a response, sleep a bit
            if (stream_response_received && (serial_port.getBytesReceived() == 0)) {
              Timer.delay(1.0 / update_rate_hz);
            }
          }

          /* If receiving data, but no valid packets have been received in the last second */
          /* the navX MXP may have been reset, but no exception has been detected.         */
          /* In this case , trigger transmission of a new stream_command, to ensure the    */
          /* streaming packet type is configured correctly.                                */

          if ((Timer.getFPGATimestamp() - last_valid_packet_time) > 1.0) {
            stream_response_received = false;
          }
        }
      } catch (RuntimeException ex) {
        // This exception typically indicates a Timeout
        stream_response_received = false;
        timeout_count++;
        // SmartDashboard.putNumber("nav6 Serial Port Timeouts", (double)timeout_count);
        // SmartDashboard.putString("LastNavExceptionBacktrace",ex.getStackTrace().toString());
        // SmartDashboard.putString("LastNavException", ex.getMessage() + "; " + ex.toString());
      }
    }
  }