Esempio n. 1
0
  protected boolean isFinished() {

    if (drivebase.onTarget() || isTimedOut()) {
      // Disable PID controller
      m_pc.disable();
      return true;
    } else return false;
  }
Esempio n. 2
0
  public void robotInit() {
    j1 = new Joystick(1);
    j2 = new Joystick(0);
    shooter = new Victor(2);
    shooter2 = new Victor(4);
    intake = new Victor(3);
    right = new Victor(0);
    left = new Victor(1);
    rd = new RobotDrive(left, right);
    light = new DigitalInput(3);
    count = 0;

    comp = new Compressor();
    inup = new DoubleSolenoid(0, 1);
    inup2 = new DoubleSolenoid(2, 3);

    gyro = new AnalogGyro(0);
    gyro.setSensitivity(.007);
    gyro.calibrate();
    gyro.reset();

    enc = new Encoder(1, 2, true, Encoder.EncodingType.k4X);
    enc.setDistancePerPulse(.11977); // circumference of wheel/200 (PPR)
    enc.setPIDSourceType(PIDSourceType.kDisplacement);
    enc.reset();

    out =
        new PIDOutput() {
          @Override
          public void pidWrite(double out) {
            rd.setLeftRightMotorOutputs(out, out);
          }
        };

    controller = new PIDController(.007, 0, 0, enc, out);
    controller.setAbsoluteTolerance(.25);
    controller.disable();

    gcontroller = new PIDController(kp, ki, kd, gyro, out);
    gcontroller.setAbsoluteTolerance(ktol);
    gcontroller.disable();

    timerStart = -1;
    timeToCancel = -1;
  }
Esempio n. 3
0
  public boolean elevatePIDTuning(double height, double speed) {
    elevatorPID.setOutputRange(-0.5, speed);
    elevatorPID.setSetpoint(height);
    elevatorPID.enable();

    if (elevatorPID.onTarget()) {
      elevatorPID.disable();
      robot.elevator.elevatorVics.set(0);
      return true;
    } else {
      return false;
    }
  }
Esempio n. 4
0
  public boolean elevatePID(double height) {
    elevatorPID.setOutputRange(-1, 1);
    elevatorPID.setSetpoint(-height);
    elevatorPID.enable();

    if (elevatorPID.onTarget()) {
      elevatorPID.disable();
      robot.elevator.elevatorSingle.set(0);
      return true;
    } else {
      return false;
    }
  }
Esempio n. 5
0
 public void teleopPeriodic() {
   armpid.setPID(
       SmartDashboard.getNumber("kP"),
       SmartDashboard.getNumber("kI"),
       SmartDashboard.getNumber("kD"));
   if (joy1.getRawButton(1)) {
     armpid.enable();
   } else {
     armpid.disable();
     arm.set(0);
   }
   SmartDashboard.putBoolean("Checkbox 2", limit.get());
   System.out.println("limit " + limit.get());
 }
Esempio n. 6
0
  public boolean elevatePIDLock(double height) {
    if (robot.elevator.locked) {
      robot.elevator.elevatorAutoRelease();
    }
    elevatorPID.setOutputRange(-1, 1);
    elevatorPID.setSetpoint(height);
    elevatorPID.enable();

    if (elevatorPID.onTarget()) {
      elevatorPID.disable();
      robot.elevator.elevatorVics.set(0);

      if (height > 0) {
        robot.elevator.elevatorLock();
      }
    }
    return elevatorPID.onTarget();
  }
Esempio n. 7
0
  public DriveTrain() {
    // super supplies the PID constants (.05, 0, 0)
    super("DriveTrain", .0070, .0003, 0);

    robotDrive = new RobotDrive(_leftMaster, _rightMaster);

    // Invert the appropriate controllers
    // robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, true);
    robotDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);

    _leftSlave1.changeControlMode(TalonControlMode.Follower);
    _leftSlave1.set(_leftMaster.getDeviceID());
    _leftSlave2.changeControlMode(TalonControlMode.Follower);
    _leftSlave2.set(_leftMaster.getDeviceID());
    _rightSlave1.changeControlMode(TalonControlMode.Follower);
    _rightSlave1.set(_rightMaster.getDeviceID());
    _rightSlave2.changeControlMode(TalonControlMode.Follower);
    _rightSlave2.set(_rightMaster.getDeviceID());

    ahrs = Robot.ahrs;

    // getPIDController() returns the PID Controller that is included with
    // classes that extend PIDSubsystems.
    turnController = getPIDController();
    turnController.disable();
    turnController.setInputRange(-180, 180);
    turnController.setOutputRange(-.7, .7);

    // begin vision test

    // frame = RobotMap.FRAME;
    // session = RobotMap.CAMERA_SESSION;
    // NIVision.IMAQdxConfigureGrab(session);
    // colorTable = new NIVision.RawData();

    // end vision test

  }
  public void autonomousContinuous() {
    long currentTime = System.currentTimeMillis();
    long timeSinceLastT = currentTime - lastTtime;

    // If we're not at a T, continue the loop
    if (!robot.lineSensor.isAtT()) return;

    System.out.println("At a T");

    lastTtime = currentTime;

    // If we're seeing the same T, continue the loop
    if (timeSinceLastT < T_WAIT_TIME) return;

    tCount++;

    // If this isn't the last T, continue the loop
    if (tCount != NUM_TS) return;

    controller.disable();
    robot.grabber.release();
    // Arm goes down
  }
Esempio n. 9
0
 public static void pidDisable() {
   controller.disable();
 }
 public void disabledInit() {
   controller.disable();
 }
Esempio n. 11
0
 // Called once after isFinished returns true
 protected void end() {
   // Stop PID and the wheels
   pid.disable();
   Robot.drivetrain.drive(0, 0);
 }
Esempio n. 12
0
  public void teleopPeriodic() {
    /*SmartDashboard.getNumber("Accel x: ", accel.getX());
    SmartDashboard.getNumber("Accel Y: ", accel.getY());
    SmartDashboard.getNumber("Accel Z: ", accel.getZ());*/
    if (controller.isEnabled()) {
      if (((Timer.getFPGATimestamp() - timerStart) > timeToCancel) || controller.onTarget()) {
        controller.disable();
        timerStart = -1;
        timeToCancel = -1;
      }
    }

    if (j1.getRawButton(2)) {
      enc.reset();
      controller.setSetpoint(120); // destination 24 inches -> NO!! Trying to figure out this value

      timerStart = Timer.getFPGATimestamp();
      timeToCancel = 10; // timeout after 10 seconds
      controller.enable();
    } else if (j1.getRawButton(
        1)) { // this button stops the robot if the button 2 command goes crazy
      controller.disable();
      timerStart = -1;
      timeToCancel = -1;
    } else { // if time out or distance, end
      controller.disable();
      timerStart = -1;
      timeToCancel = -1;
    }

    if (!controller.isEnabled()) {
      rd.arcadeDrive(-j1.getY(), -j1.getX()); // normal arcade drive
    }

    if (j2.getRawAxis(2) != 0) { // set shooter values to the left trigger value
      shooter.set(-j2.getRawAxis(2));
      shooter2.set(-j2.getRawAxis(2));
      SmartDashboard.putNumber("Shooter: ", shooter.get());
    } else { // stop shooter
      shooter.set(0);
      shooter2.set(0);
    }
    /*if(j2.getRawButton(8)){ //runs intake, waits, runs shooter
    	shooter.set(-1);
    	intake.set(.5);
    	Timer.delay(.50);
    	intake.set(0);
    	Timer.delay(1.5);
    	intake.set(-1);
    	Timer.delay(1);
    	shooter.set(0);
    	intake.set(0);
    }*/

    // Need to mess around with sensitivity of light sensor
    if (j2.getRawAxis(3) != 0 && !light.get()) { // run intake at speed of right trigger
      intake.set(-1 * j2.getRawAxis(3));
    } else if (j2.getRawButton(5) && !light.get()) { // run intake into robot
      intake.set(-1);
    } else if (j2.getRawButton(6)) { // run intake out of robot
      intake.set(1);
    } else { // stop intake
      intake.set(0);
    }

    if (j2.getRawButton(2)) { // lift intake
      inup.set(DoubleSolenoid.Value.kForward);
      inup2.set(DoubleSolenoid.Value.kForward);
    } else if (j2.getRawButton(3)) { // drop intake
      inup.set(DoubleSolenoid.Value.kReverse);
      inup2.set(DoubleSolenoid.Value.kReverse);
    } else { // solenoids off
      inup.set(DoubleSolenoid.Value.kOff);
      inup2.set(DoubleSolenoid.Value.kOff);
    }

    // reading values
    SmartDashboard.putNumber("Encoder Dist: ", enc.getDistance()); // Distance is in inches
    SmartDashboard.putNumber("Encoder: ", enc.get());
    SmartDashboard.putNumber("Encoder Rate: ", enc.getRate());
    SmartDashboard.putNumber("Gyro: ", gyro.getAngle());
    SmartDashboard.putNumber("Encoder Raw", enc.getRaw());
    SmartDashboard.putNumber("Controller: ", controller.getSetpoint());
    SmartDashboard.putBoolean("Light Sensor: ", light.get());
  }
 protected void end() {
   pidControllerDistance.disable();
   drivetrainStrafe.mecanumPolarStrafe(0, 0);
   drivetrainRotation.mecanumPolarRotate(0);
   timer.stop();
 }
 // Called once after isFinished returns true
 protected void end() {
   // Disable PID output and stop robot to be safe
   leftPID.disable();
   rightPID.disable();
   Robot.driveTrain.drive(0, 0);
 }
 public void stopBalancing() {
   enabled = false;
   controller.disable();
 }