Ejemplo n.º 1
0
 public void autoDrive() {
   double speed = 0;
   if (distPID.isEnable()) {
     speed = distPID.get();
   }
   double turn = 0;
   if (turnPID.isEnable()) {
     turn = turnPID.get();
   }
   if (speed > 0) {
     drive.drive(speed, turn);
   } else {
     if (turn > 0) {
       // turn right
       tankDrive(turn, -turn);
     } else if (turn < 0) {
       tankDrive(-turn, turn);
     }
   }
   SmartDashboard.putDouble("left pwm", leftJag.get());
   SmartDashboard.putDouble("right pwm", rightJag.get());
   SmartDashboard.putDouble("speed", RobotMap.roundtoTwo(speed));
   SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(turn));
   SmartDashboard.putDouble("turn error", turnPID.getError());
   // System.out.println("driving " + speed + " " + turn);
 }
Ejemplo n.º 2
0
  public void updateSmartDashboard() {
    if (joystick_left.getRawButton(2) && dev_ < 100) {
      dev_++;

    } else if (joystick_left.getRawButton(3) && dev_ > 0) {
      dev_--;
    }

    SmartDashboard.putDouble("X", joystick_left.getX());
    SmartDashboard.putDouble("Y", joystick_left.getY());
    // SmartDashboard.putDouble("Optical Sensor", Optical_Sensor.getVoltage());
    SmartDashboard.putDouble("Battery Voltage", DriverStation.getInstance().getBatteryVoltage());
    SmartDashboard.putInt("dev_", dev_);

    SmartDashboard.putDouble(
        "Elevator Sensor Value (.getAverageVoltage())",
        this.elevator.ana_chana.getAverageVoltage());

    String state = "ROBOT STATE INFORMATION:";
    if (this.elevator.state == 0) {
      state = state + "\nElevator Stopped.";
    }
    if (this.elevator.state == 1) {
      state = state + "\nElevator Moving.";
    }
    if (this.elevator.state == 2) {
      state = state + "\nElevator E-Stopped.";
    }

    //            state = state+"\nShooter Power Level (Raw):
    // "+this.shooter.calculateShooterSpeed();
    //            state = state+"\nShooter Power Level (Setting): "+this.shooter.shooterSpeedState;

    // if (this.integ.globalState==1){
    //    state = state+"\nMoving Shooter Up To Shoot.";
    // }if (this.integ.globalState==2){
    //    state = state+"\nShooting!";
    // }if (this.integ.globalState2LOCK){
    //     state = state+"\nREALLY SHOOTING!.";
    // }

    state = state + "\nNumber Of Discs: " + this.elevator.numDiscs;

    state =
        state
            + "\n Louis + Bobni Are Amazing (Hiding This Message Is ILLEGAL! IT WILL STOP THE ROBOT!)";

    SmartDashboard.putString("State", state);
    SmartDashboard.putBoolean("Intake", this.intake.toggle.get());
  }
 // Called repeatedly when this Command is scheduled to run
 protected void execute() {
   // implement manual jog //
   double y = oi.getNESStick().getY();
   if (Math.abs(y) > 0.1) {
     double val = this.camera.getPitch() - y * PITCH_JOG;
     if (val > MAX_PITCH) val = MAX_PITCH;
     if (val < MIN_PITCH) val = MIN_PITCH;
     this.camera.setPitch(val);
   }
   double x = oi.getNESStick().getX();
   if (Math.abs(x) > 0.1) {
     double val = this.camera.getYaw() + x * YAW_JOG;
     if (val > MAX_YAW) val = MAX_YAW;
     if (val < MIN_YAW) val = MIN_YAW;
     this.camera.setYaw(val);
   }
   // preset positions //
   if (oi.getNESStick().getAButton().get()) this.setPosition(pFORWARD);
   else if (oi.getNESStick().getBButton().get()) this.setPosition(pDOWN);
   else if (oi.getNESStick().getStartButton().get()) this.setPosition(pDRIVE);
   SmartDashboard.putDouble("camera pitch", this.camera.getPitch());
   SmartDashboard.putDouble("camera yaw", this.camera.getYaw());
 }
Ejemplo n.º 4
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);
    //        }
  }
Ejemplo n.º 5
0
 public void tankDrive(double left, double right) {
   drive.tankDrive(left, right);
   SmartDashboard.putDouble("left pwm", left);
   SmartDashboard.putDouble("right pwm", right);
 }
Ejemplo n.º 6
0
 public void sendPeriodicData() {
   // SmartDashboard output stuff.
   SmartDashboard.putDouble("Force Sensor Value", CommandBase.elev.getPressure());
   SmartDashboard.putDouble("Yaw position", CommandBase.dt.yawGyro.getAngle());
   SmartDashboard.putDouble("Target1 X", CommandBase.cam.tracker.pidGet());
   SmartDashboard.putDouble("Cam servo", CommandBase.cam.getPos());
   SmartDashboard.putDouble("Shooter rate", CommandBase.shooter.getRate());
   SmartDashboard.putDouble("DT left", CommandBase.dt.getLeftPos());
   SmartDashboard.putDouble("DT right", CommandBase.dt.getRightPos());
   SmartDashboard.putDouble("Cam pos", CommandBase.cam.getPos());
   SmartDashboard.putDouble("Shooter power", CommandBase.shooter.getPower());
   SmartDashboard.putDouble("Throttle", CommandBase.oi.getDriveThrottle());
   SmartDashboard.putDouble("Wheel", CommandBase.oi.getDriveWheel());
   SmartDashboard.putDouble("Cam delta", CommandBase.oi.getCamDelta());
 }