Example #1
0
  public void teleopPeriodic() {

    double throttle = (driveStick.getThrottle() + 1) / -2;
    double xAxis = throttle * (Math.abs(driveStick.getX()) > 0.15 ? driveStick.getX() : 0.0);
    double yAxis = throttle * (Math.abs(driveStick.getY()) > 0.15 ? driveStick.getY() : 0.0);
    double rotation =
        throttle * (Math.abs(driveStick.getTwist()) > 0.15 ? driveStick.getTwist() : 0.0);

    // Mecanum drive
    drive.mecanumDrive_Cartesian(xAxis, yAxis, rotation, 0);

    // Elevator control
    elevator.set(driveStick.getRawButton(5) ? -1.0 : driveStick.getRawButton(3) ? 1.0 : 0.0);

    // Intake control
    intakeTensionMotor.set(
        driveStick.getRawButton(2) ? -1.0 : driveStick.getRawButton(7) ? 1.0 : 0.0);
    if (Math.abs(controlStick.getTwist()) > 0) {
      double toteTurnSpeed = -controlStick.getTwist();
      intakeMotors[0].set(
          toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
      intakeMotors[1].set(
          toteTurnSpeed > 0 ? Math.pow(toteTurnSpeed, 2) : -Math.pow(toteTurnSpeed, 2));
    } else {
      double intakeSpeed =
          driveStick.getRawButton(1) ? -1.0 : driveStick.getRawButton(8) ? 0.4 : 0.0;
      intakeMotors[0].set(-intakeSpeed);
      intakeMotors[1].set(intakeSpeed);
    }
  }
Example #2
0
 /** Gyro sensitivity is set and mecanum drive is used with the gyro angle as an input. */
 public void operatorControl() {
   gyro.setSensitivity(
       voltsPerDegreePerSecond); // calibrate gyro to have the value equal to degrees
   while (isOperatorControl() && isEnabled()) {
     myRobot.mecanumDrive_Cartesian(
         joystick.getX(), joystick.getY(), joystick.getZ(), gyro.getAngle());
     Timer.delay(0.005); // wait 5ms to avoid hogging CPU cycles
   }
 }
 /** This function is called periodically during operator control */
 public void teleopPeriodic() {
   double x = joy1.getX();
   double y = -joy1.getY();
   double r = joy1.getZ();
   if (Math.abs(x) < .05) x = 0;
   if (Math.abs(y) < .05) y = 0;
   if (Math.abs(r) < .05) r = 0;
   drive(x / 2, y / 2, r / 2);
   shooter1.set(joy2.getY());
   shooter2.set(joy2.getY());
 }
Example #4
0
  public double[] processDriveJoystick() {
    double xValue;
    double yValue;
    if (((driveJoy.getX() < 0.10) && (driveJoy.getX() > 0))
        || ((driveJoy.getX() > -0.10) && (driveJoy.getX() < 0))) { // Check for dead zones
      xValue = 0;
    } else {
      xValue = driveJoy.getX(); // No dead zone proceded normally
    }

    if (((driveJoy.getY() < 0.10) && (driveJoy.getY() > 0))
        || ((driveJoy.getY() > -0.10) && (driveJoy.getY() < 0))) { // Check for dead zones
      yValue = 0;
    } else {
      yValue = driveJoy.getY(); // No dead zone proceded normally
    }

    double values[] = new double[1];
    values[0] = yValue;
    values[1] = xValue;
    return values;
  }
Example #5
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());
  }
Example #6
0
  public void teleopPeriodic() {
    watchdog.feed(); // feed the watchdog
    // Toggle drive mode
    if (!driveToggle && left.getRawButton(2)) {
      driveMode = (driveMode + 1) % 3;
      driveToggle = true;
    } else if (driveToggle && !left.getRawButton(2)) driveToggle = false;

    // Print drive mode to DS & send values to Jaguars
    switch (driveMode) {
      case 0:
        dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Tank  ");
        red.set(left.getY() + gamePad.getY());
        black.set(-right.getY() - gamePad.getThrottle());
        break;
      case 1:
        dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Arcade");
        red.set(right.getX() - right.getY());
        black.set(right.getX() + right.getY());
        break;
      case 2:
        dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Kaj   ");
        red.set(left.getX() - right.getY());
        black.set(left.getX() + right.getY());
        break;
    } /**/
    dsLCD.println(
        DriverStationLCD.Line.kUser3,
        1,
        "1" + photoreceptorL.get() + "2" + photoreceptorM.get() + "3" + photoreceptorR.get());
    dsLCD.println(DriverStationLCD.Line.kUser4, 1, "Encoder in 4,5: " + leftEncoder.get() + "    ");
    dsLCD.println(
        DriverStationLCD.Line.kUser5, 1, "Encoder in 6,7: " + rightEncoder.get() + "    ");

    dsLCD.updateLCD();
  }
  /** This function is called once each time the robot enters operator control. */
  public void operatorControl() {
    drivetrain.setSafetyEnabled(true);
    compressor.start();
    while (isOperatorControl() && isEnabled()) {
      // The throttle goes from 1 to -1, bottom to top. This
      // scales it to go 0 to 1, bottom to top.
      double throttle = (-leftStick.getRawAxis(3) + 1) / 2;

      double drive = deadband(leftStick.getY()) * throttle;
      double steer = deadband(-rightStick.getX()) * throttle;
      drivetrain.arcadeDrive(drive, steer);

      boolean shouldSpinWheels = leftStick.getRawButton(1);
      boolean shouldFire = shouldSpinWheels && rightStick.getRawButton(1);
      shooterWheels.set(shouldSpinWheels ? -1 : 0);
      shooter.set(shouldFire);

      Timer.delay(0.005); // wait for a motor update time
    }
  }
Example #8
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);
    //        }
  }
Example #9
0
 public double getX(Hand hand) {
   double leftVal = left.getX();
   double rightVal = right.getX();
   return nonZero(leftVal, rightVal);
 }
Example #10
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());
  }
Example #11
0
 public double getRightX() {
   return rightJoy.getX();
 }
Example #12
0
 public double getLeftX() {
   return leftJoy.getX();
 }
Example #13
0
 public double getRightX() {
   return rightStick.getX();
 }
Example #14
0
 public double getLeftX() {
   return leftStick.getX();
 }