// Called just before this Command runs the first time
 protected void initialize() {
   if (SmartDashboard.getNumber("centerAge", 1) < 0.5)
     angle = SmartDashboard.getNumber("angleToCenter", 0);
   else angle = 0;
   super.initialize();
   //        System.out.println("Angle to Center: " + angle);
 }
  public DriveAtom(double _dist) {

    p = SmartDashboard.getNumber("Drive P: ");
    i = SmartDashboard.getNumber("Drive I: ");
    d = SmartDashboard.getNumber("Drive D: ");

    distance = _dist;
    incrementer = Definitions.DRIVE_ATOM_INCREMENTER;
    threshold = 75; // we can be within approx. half an inch
  }
 @Override
 public void sendToSmartDash() {
   SmartDashboard.putNumber(this.getName() + " kP", kP);
   SmartDashboard.putNumber(this.getName() + " kI", kI);
   SmartDashboard.putNumber(this.getName() + " kD", kD);
   SmartDashboard.putNumber(this.getName() + " ERROR", this.previousError);
   kP = SmartDashboard.getNumber(this.getName() + " kP", kP);
   kI = SmartDashboard.getNumber(this.getName() + " kI", kI);
   kD = SmartDashboard.getNumber(this.getName() + " kD", kD);
 }
 // Called just before this Command runs the first time
 protected void initialize() {
   System.out.println("Initialize timedPullBackHammer");
   setTimeout(SmartDashboard.getNumber("Timed Hammer Pullback"));
   if (!Robot.hammer.isHammerAtLatch()) {
     System.out.println(
         "Pulling back hammer for " + SmartDashboard.getNumber("Timed Hammer Pullback") + " sec");
     if (Robot.hammer.isLatchReady()) Robot.hammer.releaseLatch();
     Robot.hammer.pullBackHammer((float) SmartDashboard.getNumber("Hammer Motor Speed"));
   } else {
     System.out.println("Error: hammer is at latch");
   }
 }
 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());
 }
  /*
   * Code to support update of PIDF constants from a button on the SmartDashboard
   */
  public void updatePIDF() {

    // Assign
    double p = SmartDashboard.getNumber(m_name + " P");
    double i = SmartDashboard.getNumber(m_name + " I");
    double d = SmartDashboard.getNumber(m_name + " D");
    if (m_hasFeedForward) {
      double f = SmartDashboard.getNumber(m_name + " F");
      setPID(p, i, d, f);
    } else setPID(p, i, d);

    // Update Tolerance
    setTolerance(SmartDashboard.getNumber(m_name + " Tolerance"));
  }
 public Double getDouble(String key) {
   try {
     return Double.valueOf(SmartDashboard.getNumber(key));
   } catch (TableKeyNotDefinedException e) {
     return null;
   }
 }
 // Called just before this Command runs the first time
 protected void initialize() {
   if (system == System.DRIVEDISTANCE) {
     target = SmartDashboard.getNumber("PID/DriveDistance TestTarget");
     Robot.drivetrain.setAutoTarget(target, 0, 0);
   } else if (system == System.DRIVESLIDE) {
     target = SmartDashboard.getNumber("PID/DriveSlide TestTarget");
     Robot.drivetrain.setAutoTarget(0, target, 0);
   } else if (system == System.DRIVEANGLE) {
     target = SmartDashboard.getNumber("PID/DriveAngle TestTarget");
     Robot.drivetrain.setAutoTarget(0, 0, target);
   } else if (system == System.ELEVATOR) {
     target = SmartDashboard.getNumber("PID/Elevator TestTarget");
     Robot.elevator.setAutoTarget(target);
     //		} else if (system == System.ARM) {
     //			target = SmartDashboard.getNumber("PID/Arm TestTarget");
     //			SlideWinder.arm.setArmHeight(target);
   }
 }
  // Called repeatedly when this Command is scheduled to run
  protected void execute() {

    deadbandY = SmartDashboard.getNumber("DeadbandY");
    deadbandX = SmartDashboard.getNumber("DeadbandX");
    deadbandZ = SmartDashboard.getNumber("DeadbandZ");
    power = (int) SmartDashboard.getNumber("DelinPower");

    double leftspeed = getLeft();
    double rightspeed = getRight();

    if (currentControlMode == CANTalon.TalonControlMode.Speed.getValue()) {
      Robot.driveTrain.setLeftSpeedPercentage(leftspeed);
      Robot.driveTrain.setRightSpeedPercentage(rightspeed);
    } else {
      Robot.driveTrain.setLeftPercentVBus(leftspeed);
      Robot.driveTrain.setRightPercentVBus(rightspeed);
    }

    Robot.driveTrain.writeDashboardDebugValues();
  }
Exemple #10
0
  /**
   * Spin wheel at desired feet per second Uses PID loop for consistency
   *
   * @param feetPerSecond double
   */
  public void spin(double feetPerSecond) {
    double desiredSpeed = feetPerSecond;
    pidData.kP = SmartDashboard.getNumber("Right flywheel kP: ");
    pidData.kI = SmartDashboard.getNumber("Right flywheel kI: ");
    pidData.kD = SmartDashboard.getNumber("Right flywheel kD: ");

    if (desiredSpeed == 0) {
      motor.set(0);
      return;
    }
    double currentSpeed = 0;
    double error = desiredSpeed - currentSpeed;
    integral += error;
    derivative = error - previousError;

    if (Math.abs(error) < TOLERANCE_FEET_PER_SECOND) {
      integral = 0;
    }

    output += (error * pidData.kP) + (integral * pidData.kI) + (derivative * pidData.kD);

    if (output > 1) {
      output = 1;
    } else if (output < -1) {
      output = -1;
    }
    motor.set(feetPerSecond);
    for (int i = (errors.length - 1); i > 0; i--) {
      errors[i] = errors[i - 1];
    }
    errors[0] = error;
    previousError = error;
    // System.out.println("Output: " + output);
    // System.out.println("Error: " + error);
    // System.out.println("kP: "+ pidData.kP);
  }
  public void update(GoalAlign alignment) {
    double shooterPower;
    if (SmartDashboard.getBoolean("Flywheel Testing Mode") == false) {
      shooterPower = 1;
    } else {
      shooterPower = SmartDashboard.getNumber("Flywheel Testing Power");
    }
    if (gamePad.buttons.BUTTON_A.isOn()) {
      // shoot(DistanceSensorData);
      // shooter.getMotor().getSpark().set(1);
      // shooter.shoot(100);s
      // shooter.setPower(getPowerFromDistance(alignment.imageHeight, alignment.centerY,
      // alignment.height));
      shooter.setPower(shooterPower);

    } else if (!gamePad.buttons.BUTTON_A.isOn()) {
      shooter.setPower(0);
    }

    SmartDashboard.putBoolean("Shooting", gamePad.buttons.BUTTON_A.isOn());
  }
 public double get() {
   return SmartDashboard.getNumber(key, defaultValue);
 }
 public AutoState getState(IRobot robot) {
   int chooser = (int) SmartDashboard.getNumber("Autonomous", 1);
   boolean backwards = SmartDashboard.getBoolean("Autonomous Backwards", false);
   AutoState chain = new NothingAutonomous();
   switch (chooser) {
       // TODO: Refine and test the state chains.
     case 1:
       if (backwards) {
         chain
             .addNext(new ForwardAutonomous(1000, -speed))
             .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
       } else {
         chain.addNext(new ForwardAutonomous(1000, speed));
       }
       // FieldPosition.Left
       chain
           .addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro()))
           // TODO: Determine if positive means clockwise.
           .addNext(null);
       // TODO: Replace with an Autonomous that drives forward until it reaches the goal.
     case 2:
       if (backwards) {
         chain
             .addNext(new ForwardAutonomous(1000, -speed))
             .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
       } else {
         chain.addNext(new ForwardAutonomous(1000, speed));
       }
       // FieldPosition.Left
       chain
           .addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro()))
           .addNext(null);
     case 3:
       if (backwards) {
         chain
             .addNext(new ForwardAutonomous(1000, -speed))
             .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
       } else {
         chain.addNext(new ForwardAutonomous(1000, speed));
       }
       // FieldPosition.Left
       chain
           .addNext(new AngleAutonomous(90.0, 0.0, robot.getSensorData().getGyro()))
           .addNext(null);
     case 4:
       // This is right in front of the tower. What is our procedure for this?
       if (backwards) {
         chain
             .addNext(new ForwardAutonomous(1000, -speed))
             .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
       } else {
         chain.addNext(new ForwardAutonomous(1000, speed));
       }
       // FieldPosition.Right
       chain
           .addNext(new AngleAutonomous(-90.0, 0.0, robot.getSensorData().getGyro()))
           .addNext(null);
     case 5:
       if (backwards) {
         chain
             .addNext(new ForwardAutonomous(1000, -speed))
             .addNext(new AngleAutonomous(180.0, 0.0, robot.getSensorData().getGyro()));
       } else {
         chain.addNext(new ForwardAutonomous(1000, speed));
       }
       // FieldPosition.Right
       chain
           .addNext(new AngleAutonomous(-90.0, 0.0, robot.getSensorData().getGyro()))
           .addNext(null);
     default:
       // Just nothing
   }
   return chain;
 }
Exemple #14
0
 // returns a double at the position of the key
 public double getNumber(String key) {
   return board.getNumber(key);
 }
// Whoopystick
// dudepiston is pneumatic pusher
public class Shooter {

  public int shootingMotor = 6;
  public int shootingMotor2 = 2;
  public Victor motorOne = new Victor(6);
  public Victor motorTwo = new Victor(2);
  double maxspeed;
  double angle;
  double speeddif;
  double speed = 0;
  double testValueOne = SmartDashboard.getNumber("testValueOne");
  double testValueTwo = SmartDashboard.getNumber("testValueTwo");
  OI oi;
  CANJaguar Shrek;
  double speedmultOne = 0; // goal side wheel
  double speedmultTwo = 0; // feeder side wheel
  public CANJaguar tester1;
  public CANJaguar tester2;
  public CANJaguar tester3;
  public CANJaguar tester4;
  public double magOne;
  public double magTwo;
  public double speedOne;
  public double speedTwo;
  public double tspeedTwo;
  public double speedThree;
  public double speedFour;
  public double deadZone = .1;
  CANJaguar.ControlMode controlMode = CANJaguar.ControlMode.kVoltage; // Voltage drive
  CANJaguar.NeutralMode neutralMode2 = CANJaguar.NeutralMode.kBrake;
  CANJaguar.NeutralMode neutralMode = CANJaguar.NeutralMode.kCoast;
  CANJaguar.ControlMode controlModew = CANJaguar.ControlMode.kSpeed;
  // NetworkTable table = NetworkTable.getTable("camera");
  public double distance;

  public Shooter(OI oi) {
    this.oi = oi;
    initializeOne();
    initializeTwo();
    // NetworkTable.setTeam(1259);
    // NetworkTable.setIPAddress("10.12.59.2");
    //       distance = table.getDouble("distance");

  }

  public void initializeOne() {
    try {
      tester1 = new CANJaguar(shootingMotor, controlMode);
      tester1.configNeutralMode(neutralMode);
      // FrontLeftJJ.setPID(kP, kI, kD);
      //            FrontLeftJJ.setVoltageRampRate(rampRate);
      //            BackLeftJJ.setVoltageRampRate(rampRate);
      //            FrontRightJJ.setVoltageRampRate(rampRate);
      //            BackRightJJ.setVoltageRampRate(rampRate);
    } catch (CANTimeoutException ex) {
      ex.printStackTrace();
    }
  }

  public void initializeTwo() {
    try {
      tester2 = new CANJaguar(shootingMotor2, controlMode);
      tester2.configNeutralMode(neutralMode);
      // FrontLeftJJ.setPID(kP, kI, kD);
      //            FrontLeftJJ.setVoltageRampRate(rampRate);
      //            BackLeftJJ.setVoltageRampRate(rampRate);
      //            FrontRightJJ.setVoltageRampRate(rampRate);
      //            BackRightJJ.setVoltageRampRate(rampRate);
    } catch (CANTimeoutException ex) {
      ex.printStackTrace();
    }
  }

  public void voltageChange() {

    // 3 point goal
    if (this.oi.getY()) { // shoot mid
      speedmultOne = 10.6; // 10.1 8.7
      speedmultTwo = 8.2; // 2.16
    } // 2 point goal
    else if (this.oi.getX()) {
      speedmultOne = 10.4; // 10.7 8.2
      speedmultTwo = 7.9;
    } // pooter speed
    else if (this.oi.getB()) {
      speedmultOne = 4.0;
      speedmultTwo = 7.5;
    } else if (this.oi.getLJoystick().getRawButton(10)) {
      speedmultOne = testValueOne;
      speedmultTwo = testValueTwo;
    }
  }

  public void autoShootThree() {
    try {
      tester1.setX(10.8); // 9.5 8.2 2 pt
      tester2.setX(8.3);
    } catch (CANTimeoutException ex) {
      ex.printStackTrace();
    }
  }

  public void autoShoot() {
    try {
      tester1.setX(10.3); // 9.5 8.2 2 pt
      tester2.setX(7.8);
    } catch (CANTimeoutException ex) {
      ex.printStackTrace();
    }
  }

  public void autoShootMid() { // shoot middle
    try {
      tester1.setX(10.5);
      tester2.setX(8.0); // 9.2

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

  public void autoShootThreeSide() { // shoot middle
    try {
      tester1.setX(10.6);
      tester2.setX(8.5);

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

  public void drive() {
    try {
      magOne = this.oi.getXJoystick().getY();
      if (Math.abs(magOne) < deadZone) {
        magOne = 0;
      }
      if (Math.abs(magTwo) < deadZone) {
        magTwo = 0;
      }
      speedOne = magOne * speedmultOne; // 4500
      speedTwo = magTwo * speedmultTwo; // 14000
      tspeedTwo = magOne * speedmultTwo;
      tester1.setX(-speedOne);
      tester2.setX(-tspeedTwo);
    } catch (CANTimeoutException ex) {
      ex.printStackTrace();
    }
  }

  public void testValue() {
    if (oi.getLJoystick().getTrigger()) {

      boolean distcheck = NetworkTable.getTable("camera").containsKey("distance");
      NetworkTable.getTable("rob").putNumber("x", 8008135);
      System.out.println(distcheck);
      if (distcheck) {
        distance = NetworkTable.getTable("camera").getNumber("distance");
      }
      System.out.println("Distance:" + " " + distance);
    }
  }
}