Example sensor and servo code

[MatthewL246]

Example Code

Here you go. This was made with the OnBotJava blocks, so it will probably not compile yet.

package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.TouchSensor;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;

@TeleOp(name = "ServoAndSensorTest")
public class ServoandSensorTest extends LinearOpMode {

  private DistanceSensor distance;
  private Servo servo;

  @Override
  public void runOpMode() {
    distance = hardwareMap.get(DistanceSensor.class, "distance");
    servo = hardwareMap.get(Servo.class, "servo");

    waitForStart();
    if (opModeIsActive()) {
      while (opModeIsActive()) {
        if (distance.getDistance(DistanceUnit.CM) < 10) {
          servo.setPosition(0.2); // Servo is closed
        } else {
          servo.setPosition(0.8); // Servo is open
        }
        telemetry.addData("Distance (cm)", distance.getDistance(DistanceUnit.CM));
        telemetry.addData("Servo Position", servo.getPosition());
        telemetry.update();
      }
    }
  }
}

Things that need to be calibrated:

• The distance the object must be away from the sensor
• The sensor open and closed positions

[MatthewL246]

Here are my ideas for the final program:

• Device does not do anything unless it is activated to prevent issues
• Device automatically closes when the ball is close
• Device stays closed until a button is pressed on the controller

Example pseudocode:

bool isEnabled
bool isClosed

if ControllerButton.isPressed(LeftTrigger):
    toggle isEnabled

if not isEnabled:
    return

if not isClosed and distance < 10cm:
    servo.setPosition(0.2)
    isClosed = true

if isClosed and ControllerButton.isPressed(RightTrigger):
    servo.setPosition(0.8)
    isClosed = false

SendTelemetryInfo()