Beispiel #1
0
  /** @param armPos the armPos to set */
  public void setArmPos(int armPos) {

    this.armPos = armPos;
    try {
      double p = 0;

      switch (armPos) {
        case 0:
          // target = PIDConstants.armIntake;
          target = preferences.getDouble("intake", 0);
          p = 2;
          break;
        case 1:
          // target = PIDConstants.armFeed;
          target = preferences.getDouble("feed", 0);

          p = 2.8;
          break;
        case 2:
          // target = PIDConstants.armStow;
          target = preferences.getDouble("stow", 0);
          // SmartDashboard.putNumber("armCurrent", arm.getOutputCurrent());
          p = 4;
          break;
      }
      double change = arm.getPosition() - target;
      // SmartDashboard.putNumber("armPosition", arm.getPosition());
      // System.out.println(arm.getPosition());
      // arm.setX(change * p*0.9);

    } catch (CANTimeoutException ex) {
      ex.printStackTrace();
    }
  }
  public boolean quickTurn(double setpoint) {
    // Proportional quickturn algorithm
    Preferences p = Preferences.getInstance();
    final double kP = p.getDouble("DriveTrainQuickTurnP", 0.0);
    final double kDB = p.getDouble("DriveTrainQuickTurnDeadband", 0.0);
    double angle = -getGyro(); // -driveTrain.getGyro() b/c the gyro is upsidedown
    double error = setpoint - angle;
    double twist = error * kP;
    double magnitude = Math.abs(twist);
    if (magnitude < kDB) {
      twist = kDB * (twist > 0.0 ? 1.0 : -1.0);
    } else if (magnitude > 1.0) {
      twist = (twist > 0.0 ? 1.0 : -1.0);
    }

    SmartDashboard.putNumber("DriveTrainGyro", angle);

    leftMotor.set(-twist);
    rightMotor.set(twist);

    // Done once we pass the setpoint
    return Math.abs(angle) >= Math.abs(setpoint);
  } // end quickTurn
 // aligns robot for shooting
 public void alignToShoot(double left, double right) {
   Preferences p = Preferences.getInstance();
   final double kShootLimitVoltage = p.getDouble("DriveTrainShootLimitVoltage", 0.0);
   double maxVoltage = ultraDist.getVoltage();
   for (int i = 0; i < 10; i++) {
     double voltage = ultraDist.getVoltage();
     if (voltage > maxVoltage) {
       maxVoltage = voltage;
     }
   }
   SmartDashboard.putNumber(
       "RangefinderVoltage", maxVoltage); // want to see on competition dashboard!!
   System.out.println(
       "RangefinderVoltage: " + maxVoltage + " kShootLimitVoltage: " + kShootLimitVoltage);
   if (maxVoltage <= kShootLimitVoltage) {
     hasBeenAchieved = true;
   }
   if (left <= 0.0 && hasBeenAchieved) {
     left = 0.0;
   }
   arcadeDrive(left, right);
 } // end alignToShoot
 public boolean driveStraight(double dist) {
   SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
   SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
   Preferences p = Preferences.getInstance();
   L_encoderVal =
       leftEncoder.get()
           / p.getDouble(
               "LeftEncoderRatio",
               0.0); // converts encoder value to inches; encoder ratio should be about 58.76
   R_encoderVal =
       rightEncoder.get()
           / p.getDouble("RightEncoderRatio", 0.0); // converts encoder value to inches
   encoderVal = (R_encoderVal); // only one encoder
   encoderErr = dist - encoderVal;
   distOut =
       encoderErr * p.getDouble("Encoder_kP", 0.0)
           + (Math.abs(encoderErr) / encoderErr)
               * p.getDouble(
                   "OutputMin",
                   0.0); // Encoder kP = 0.5; abs(x)/x returns sign of x; 0.2 is the min. magnitude
   gyroErr = gyro.getAngle(); // Setpoint is always 0
   double angleOut = gyroErr * p.getDouble("Gyro_kP", 0.0) + .3; // * distOut / Math.abs(distOut);
   SmartDashboard.putNumber("encoderErr", encoderErr);
   SmartDashboard.putNumber("distOut", distOut);
   SmartDashboard.putNumber("gyroErr", gyro.getAngle());
   SmartDashboard.putNumber("angleOut", angleOut);
   if (Math.abs(encoderErr) < p.getDouble("DistBuffer", 0.0)
       && Math.abs(gyroErr) < p.getDouble("AngleBuffer", 0.0)) {
     //            leftEncoder.reset();
     //            rightEncoder.reset();
     // gyro.reset();
     // arcadeDrive(0, 0);
     return true; // returns true when robot gets to its goal
   } else {
     arcadeDrive(
         -distOut,
         angleOut); // Gyro kP = 1/18.0; Arcade Drive uses reversed rotate values (neg. goes Left /
     // pos. goes Right)
     return false; // returns false if robot still hasn't reached its goal yet
   }
 } // end driveStraight