Beispiel #1
0
  /** Handles periodic operations. Should be called periodically. */
  public void loop() {

    ds.setDigitalOut(8, limitSwitch.get());

    final int tolerance =
        10; // the number of degrees, in either direction, of tolerance for motor movement

    if (!isAccepting) {
      if (topTarget > topEncoder.get() && Math.abs(topTarget - topEncoder.get()) > tolerance) {
        int error = Math.abs(topTarget - topEncoder.get());

        topMotor.set(-limit(error / ERROR_CONVERSION));
        // SmartDashboard.log("Backwards", "Gripper Top Motor");
      } else if (topTarget < topEncoder.get()
          && Math.abs(topTarget - topEncoder.get()) > tolerance) {
        double error = Math.abs(topTarget - topEncoder.get());

        topMotor.set(limit(error / ERROR_CONVERSION));
        // SmartDashboard.log("Forward", "Gripper Top Motor");
      } else {
        topMotor.set(0);
      }

      if (bottomTarget > bottomEncoder.get()
          && Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
        double error = Math.abs(bottomTarget - bottomEncoder.get());
        bottomMotor.set(-limit(error / ERROR_CONVERSION));
        // SmartDashboard.log("Backwards", "Gripper Bottom Motor");
      } else if (bottomTarget < bottomEncoder.get()
          && Math.abs(bottomTarget - bottomEncoder.get()) > tolerance) {
        double error = Math.abs(bottomTarget - bottomEncoder.get());
        bottomMotor.set(limit(error / ERROR_CONVERSION));
        // SmartDashboard.log("Forward", "Gripper Bottom Motor");
      } else {
        bottomMotor.set(0);
        // SmartDashboard.log("Stopped", "Gripper Bottom Motor");
      }
    }

    // if the gripper is in accept mode, it needs to stop when the limit switch is pressed
    if (isAccepting && !limitSwitch.get()) {
      // and also by simply stopping them
      topMotor.set(0);
      bottomMotor.set(0);
      isAccepting = false;
      // stop the motors by making the target positions equal to the current positions
      topTarget = topEncoder.get();
      bottomTarget = bottomEncoder.get();
    } else if (isAccepting && limitSwitch.get()) {
      // if it is accepting and the limit switch is not pressed
      topMotor.set(1);
      bottomMotor.set(-1);
    }
  }
Beispiel #2
0
 public double doRobotdriveScaling(double value) {
   if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel)) {
     if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel2)) {
       driverstation.setDigitalOut(2, true);
       driverstation.setDigitalOut(1, false);
       driverstation.setDigitalOut(3, false);
       if (driverstation.getAnalogIn(Constants.driverstation_scale_channel) < 1) {
         return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel);
       }
       driverstation.setDigitalOut(3, true);
       return 0;
     }
     driverstation.setDigitalOut(3, false);
     driverstation.setDigitalOut(1, true);
     driverstation.setDigitalOut(2, false);
     return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel) * 0.2;
   }
   driverstation.setDigitalOut(3, false);
   driverstation.setDigitalOut(1, false);
   driverstation.setDigitalOut(2, false);
   return value * Constants.robotdrive_scale_hi;
 }