示例#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 void storeLoaderAngle(String sPosition) {
   float volts = (float) leftAngleCh.getVoltage();
   float voltsR = (float) rightAngleCh.getVoltage();
   m_preferences.putFloat(sPosition, volts);
   m_preferences.putFloat(sPosition + "R", voltsR);
   System.out.println(sPosition + " = " + volts);
 }
示例#3
0
 // Called just before this Command runs the first time
 protected void initialize() {
   pushTime = Preferences.getInstance().getDouble("PushDiskTime", 0.5);
   pushSpeed = Preferences.getInstance().getDouble("PushDiskSpeed", 0.7);
   returnTime = Preferences.getInstance().getDouble("ReturnTime", 0.7);
   returnSpeed = Preferences.getInstance().getDouble("ReturnSpeed", 0.3);
   step = 0;
 }
  public void goToPosition(String sPosition) {

    m_sPosition = sPosition;
    pidLeft.setSetpoint(m_preferences.getFloat(sPosition, 0));
    pidRight.setSetpoint(m_preferences.getFloat(sPosition + "R", 0));

    System.out.println(sPosition + " Setpoint = " + m_preferences.getFloat(sPosition, 0));
  }
 // basic arcadeDrive: y=forward/backward speed, x=left/right speed
 public void arcadeDrive(double y, double x) {
   Preferences p = Preferences.getInstance();
   final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
   robotDrive.arcadeDrive(y, x + 0.05);
   SmartDashboard.putNumber("DriveTrainGyro", -gyro.getAngle()); // upside down
   SmartDashboard.putNumber("RangefinderVoltage", ultraDist.getVoltage());
 } // end arcadeDrive
 // basic arcadeDrive: y=forward/backward speed, x=left/right speed
 public void arcadeDrive(double y, double x) {
   SmartDashboard.putNumber("RightEncoder", rightEncoder.get());
   SmartDashboard.putNumber("LeftEncoder", leftEncoder.get());
   Preferences p = Preferences.getInstance();
   final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
   robotDrive.arcadeDrive(y, x);
 } // end arcadeDrive
    @Override
    public String getValue() {
      if (Preferences.getInstance().containsKey(mKey)) {
        return Preferences.getInstance().getString(mKey, mDefault);
      }

      sPropertyAdded = true;
      Preferences.getInstance().putString(mKey, mDefault);
      return mDefault;
    }
 public boolean alignStraight() {
   double angleOut = gyro.getAngle() * Preferences.getInstance().getDouble("Gyro_kP", 0.0);
   SmartDashboard.putNumber("gyroErr", gyro.getAngle());
   SmartDashboard.putNumber("angleOut", angleOut);
   robotDrive.arcadeDrive(0, angleOut);
   if (Math.abs(gyro.getAngle()) < Preferences.getInstance().getDouble("AngleBuffer", 100)) {
     arcadeDrive(0, 0);
     return true;
   }
   return false;
 }
 public void tankDrive(double leftSpeed, double rightSpeed) {
   Preferences p = Preferences.getInstance();
   final boolean kReverseDirection = p.getBoolean("DriveTrainReverseDirection", false);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
   //        if (kReverseDirection) {
   //            robotDrive.tankDrive(rightSpeed, leftSpeed);
   //            return;
   //        }
   robotDrive.tankDrive(leftSpeed, rightSpeed);
   SmartDashboard.putNumber("gyroErr", gyro.getAngle());
   SmartDashboard.putNumber("encoder", rightEncoder.get());
 } // end tankDrive
示例#10
0
  public Intake() {
    preferences = Preferences.getInstance();

    leftGate =
        new DoubleSolenoid(
            ElectricalConstants.rightGateForward, ElectricalConstants.rightGateReverse);

    rightGate = new Solenoid(ElectricalConstants.leftGate);
    try {
      arm = new CANJaguar(ElectricalConstants.armJag);
      arm.changeControlMode(CANJaguar.ControlMode.kPercentVbus);
      arm.configNeutralMode(CANJaguar.NeutralMode.kBrake);

      arm.configFaultTime(0.5);
      arm.configPotentiometerTurns(1);

      arm.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
      arm.setPID(0, 0, 0);

      arm.enableControl(0);

    } catch (CANTimeoutException ex) {
      ex.printStackTrace();
    }
    rollers = new Victor(ElectricalConstants.rollerVic);
  }
示例#11
0
 // starts encoders
 public DriveTrain() {
   leftEncoder.start();
   rightEncoder.start();
   gyro.reset();
   Preferences p = Preferences.getInstance();
   if (!p.containsKey("DriveTrainShootLimitVoltage")) {
     p.putDouble("DriveTrainShootLimitVoltage", kDefaultShootLimitVoltage);
   }
   if (!p.containsKey("DriveTrainQuickTurnP")) {
     p.putDouble("DriveTrainQuickTurnP", kDefaultQuickTurnProportion);
   }
   if (!p.containsKey("DriveTrainQuickTurnDeadband")) {
     p.putDouble("DriveTrainQuickTurnDeadband", kDefaultQuickTurnDeadband);
   }
   if (!p.containsKey("DriveTrainReverseDirection")) {
     p.putBoolean("DriveTrainReverseDirection", kDefaultReverseDirection);
   }
 } // end constructor
示例#12
0
 public void enable() {
   rightEncoder.reset();
   leftEncoder.reset();
   rightEncoder.start();
   leftEncoder.start();
   gyro.reset();
   final boolean kReverseDirection =
       Preferences.getInstance().getBoolean("DriveTrainReverseDirection", false);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, kReverseDirection);
   robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, kReverseDirection);
 } // end enable
  // Initialize your subsystem here
  public LoaderClimberOld2PID() {
    // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
    // super("LoaderClimberOld2PID", 1.5, 0.1, 0.0);
    // setAbsoluteTolerance(0.02);
    // getPIDController().setContinuous(false);

    // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=PID
    m_preferences = Preferences.getInstance();
    if (leftJag != null) { // for practice robot

      // Fix for motors going in opposite directios
      PIDOutput rightOutput =
          new PIDOutput() {
            public void pidWrite(double output) {
              rightJag.pidWrite(-1 * output);
            }
          };

      pidLeft = new PIDController(P, I, D, leftAngleCh, leftJag); // PID Output Interface
      pidRight = new PIDController(P, I, D, rightAngleCh, rightOutput); // PID Output Interface*/

      LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidLeft);
      LiveWindow.addActuator("LoaderClimber", "PIDLeft Angle Controller", pidRight);

      pidLeft.setSetpoint(m_preferences.getFloat("Retract", 2.4f));
      pidRight.setSetpoint(m_preferences.getFloat("RetractR", 2.4f));

      pidLeft.setInputRange(0, 4);
      pidRight.setInputRange(0, 4);

      pidLeft.setPercentTolerance(PID_TOL);
      pidRight.setPercentTolerance(PID_TOL);

      pidLeft.enable(); // - Enables the PID controller.
      pidRight.enable(); // - Enables the PID controller.
      // disable();

    }
  }
示例#14
0
 // 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
示例#15
0
  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
示例#16
0
 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
示例#17
0
 public static double getAutoRotateDefaultTaretDegrees() {
   return Preferences.getInstance().getDouble("AutoRotateDefaultTargetDegrees", 0.0);
 }
示例#18
0
 public static double getAutoRotateOnTargetToleranceDegrees() {
   return Preferences.getInstance().getDouble("AutoRotateOnTargetToleranceDegrees", 2.0);
 }
示例#19
0
 // starts encoders
 public DriveTrain() {
   leftEncoder.start();
   rightEncoder.start();
   gyro.reset();
   Preferences p = Preferences.getInstance();
   if (!p.containsKey("DriveTrainShootLimitVoltage")) {
     p.putDouble("DriveTrainShootLimitVoltage", kDefaultShootLimitVoltage);
   }
   if (!p.containsKey("DriveTrainQuickTurnP")) {
     p.putDouble("DriveTrainQuickTurnP", kDefaultQuickTurnProportion);
   }
   if (!p.containsKey("DriveTrainQuickTurnDeadband")) {
     p.putDouble("DriveTrainQuickTurnDeadband", kDefaultQuickTurnDeadband);
   }
   if (!p.containsKey("DriveTrainReverseDirection")) {
     p.putBoolean("DriveTrainReverseDirection", kDefaultReverseDirection);
   }
   if (!p.containsKey("LeftEncoderRatio")) {
     p.putDouble("LeftEncoderRatio", 4.875);
   }
   if (!p.containsKey("RightEncoderRatio")) {
     p.putDouble("RightEncoderRatio", 4.875);
   }
   if (!p.containsKey("Encoder_kP")) {
     p.putDouble("Encoder_kP", 0.05);
   }
   if (!p.containsKey("OutputMin")) {
     p.putDouble("OutputMin", .2);
   }
   if (!p.containsKey("DistBuffer")) {
     p.putDouble("DistBuffer", 1);
   }
   if (!p.containsKey("Gyro_kP")) {
     p.putDouble("Gyro_kP", 1 / 18);
   }
   if (!p.containsKey("AngleBuffer")) {
     p.putDouble("AngleBuffer", 4.5);
   }
   if (!p.containsKey("AutoDist_0")) {
     p.putDouble("AutoDist_0", 0.0);
   }
   if (!p.containsKey("AutoDist_1")) {
     p.putDouble("AutoDist_1", 0.0);
   }
   if (!p.containsKey("AutoDist_2")) {
     p.putDouble("AutoDist_2", 0.0);
   }
   if (!p.containsKey("AutoDist_3")) {
     p.putDouble("AutoDist_3", 0.0);
   }
   if (!p.containsKey("ShiftWhenTurningThreshold")) {
     p.putDouble("ShiftWhenTurningThreshold", 0.5);
   }
   if (!p.containsKey("ShiftWhenTurningDelay")) {
     p.putDouble("ShiftWhenTurningDelay", 1.5);
   }
 } // end constructor
 //    int errors = 0;
 //    protected void usePIDOutput(double output) {
 //
 //        double outputNeg = output * -1;
 //        double slow = output * .815;
 ////        System.out.println("LoaderClimberOld2PID: usePIDOutput, outputNeg=" + outputNeg + "
 // slow=" + slow + "   errors:" + errors);
 //        try {
 //            leftJag.setX(slow);
 //            rightJag.setX(outputNeg);
 //        } catch (CANTimeoutException ex) {
 ////            ex.printStackTrace();
 //            errors = errors + 1;
 //        }
 //    }
 public void savePreferences() {
   m_preferences.save();
   System.out.println("Preferences saved");
 }