AbstractServoTuneOp

Go to Servo Presets to see how to extend this class to tune the servos.

Here is AbstractServoTuneOp, which can be extended to tune servos for any Robot Configuration.

ftc/evlib/opmodes/AbstractServoTuneOp.java

package ftc.evlib.opmodes;

import java.util.ArrayList;
import java.util.List;

import ftc.electronvolts.util.Function;
import ftc.electronvolts.util.Functions;
import ftc.electronvolts.util.Utility;
import ftc.electronvolts.util.files.Logger;
import ftc.electronvolts.util.files.OptionsFile;
import ftc.electronvolts.util.units.Time;
import ftc.evlib.hardware.config.RobotCfg;
import ftc.evlib.hardware.servos.ServoCfg;
import ftc.evlib.hardware.servos.ServoControl;
import ftc.evlib.hardware.servos.ServoName;
import ftc.evlib.util.EVConverters;

/**
 * This file was made by the electronVolts, FTC team 7393
 * Date Created: 9/30/16
 *
 * extends AbstractTeleOp and adds tuning of servo presets with the joysticks.
 *
 * Subclasses of this are very simple since this does most of the work.
 *
 * It allows you to change your servo presets without changing the code and re-deploying it to the
 * phone. This means that you can swap out a servo and re-tune it without having to go into the
 * program and fix magic numbers. Note:  It only works if you use presets everywhere instead of
 * hardcoded values.
 *
 * How to use for your robot:
 * Create a subclass of this (AbstractServoTuneOp).
 * return a new instance of your RobotCfg (it has the servos) in createRobotCfg().
 *
 * Subclass example:
 *
 * <code>
 *
 * \@TeleOp(name = "MyRobot ServoTuneOp")
 * public class MyRobotServoTuneOp extends AbstractServoTuneOp {
 * \@Override protected RobotCfg createRobotCfg() {
 * return new MyRobotCfg(hardwareMap);
 * }
 * }
 * </code>
 *
 * How to operate:
 * Use the dpad up and down to cycle through all the servos
 * Use the dpad left and right to move through the presets for that servo.
 * Use the left and right joystick y values to change the servo position.
 * Press start to save the current preset of the current servo to the current value.
 *
 * The presets are saved in files that are retrieved when you run other opmodes to find the value of each preset.
 *
 * @see ServoControl
 * @see ServoCfg
 */
public abstract class AbstractServoTuneOp extends AbstractTeleOp<RobotCfg> {
    /**
     * The index of the servo in the list
     */
    private int servoIndex = 0;

    /**
     * The index of the preset for the current servo
     */
    private int presetIndex = 0;

    /**
     * records whether or not a new servo has been selected
     */
    private boolean servoIndexChanged = true;

    /**
     * records whether or not a new servo preset has been selected
     */
    private boolean servoPresetIndexChanged = true;

    /**
     * The list of current positions for each servo
     */
    private final List<Double> servoPositions = new ArrayList<>();

    /**
     * The list of servo names
     */
    private List<ServoName> servoNames;

    /**
     * The list of preset names for the current servo
     */
    private List<Enum> presetNames;

    /**
     * The list of preset values for the current servo
     */
    private List<Double> presetValues;

    /**
     * The current servo
     */
    private ServoControl servo;

    /**
     * @return no joystick scaling
     */
    @Override
    protected Function getJoystickScalingFunction() {
        return Functions.none();
    }

    /**
     * @return no match timer
     */
    @Override
    public Time getMatchTime() {
        return null;
    }

    /**
     * @return no logging
     */
    @Override
    protected Logger createLogger() {
        return null;
    }

    @Override
    protected void setup() {
        //get a list of servo names from the RobotCfg
        servoNames = robotCfg.getServos().getServoNames();

        //add servo positions to be the same length as servoNames
        for (ServoName ignored : servoNames) {
            servoPositions.add(0.5);
        }
    }

    @Override
    protected void setup_act() {

    }

    @Override
    protected void go() {

    }

    @Override
    protected void act() {


        //if dpad up is pressed
        if (driver1.dpad_up.justPressed() || driver2.dpad_up.justPressed()) {
            servoIndex += 1; //move to the next servo
            //wrap around if the index is too large
            if (servoIndex > servoNames.size() - 1) servoIndex = 0;
            servoIndexChanged = true; //signal that the index changed
        }

        //if dpad down is pressed
        if (driver1.dpad_down.justPressed() || driver2.dpad_down.justPressed()) {
            servoIndex -= 1; //move to the previous servo
            //wrap around if the index is too small
            if (servoIndex < 0) servoIndex = servoNames.size() - 1;
            servoIndexChanged = true; //signal that the index changed
        }

        //if a different servo was selected
        if (servoIndexChanged) {
            servoIndexChanged = false;

            servo = robotCfg.getServo(servoNames.get(servoIndex));//get the servo
            presetNames = new ArrayList<>(servo.getPresets().keySet()); //get the preset names from the servo
            presetValues = new ArrayList<>(servo.getPresets().values()); //get the presets from the servo

            presetIndex = 0; //start at the first preset for the new servo
            servoPresetIndexChanged = true; //signal to reload the servo preset
        }

        //get the servo position
        double servoPosition = servoPositions.get(servoIndex);

        //if the dpad left was just pressed
        if (driver1.dpad_left.justPressed() || driver2.dpad_left.justPressed()) {
            presetIndex -= 1; //select the previous servo preset
            //wrap around if the index is too small
            if (presetIndex < 0) presetIndex = presetValues.size() - 1;
            servoPresetIndexChanged = true; //signal that the index changed
        }

        //if the dpad right was just pressed
        if (driver1.dpad_right.justPressed() || driver2.dpad_right.justPressed()) {
            presetIndex += 1; //select the next servo preset
            //wrap around if the index is too large
            if (presetIndex > presetValues.size() - 1) presetIndex = 0;
            servoPresetIndexChanged = true; //signal that the index changed
        }

        //is the servo preset index changed
        if (servoPresetIndexChanged) {
            servoPresetIndexChanged = false;
            servoPosition = presetValues.get(presetIndex); //set the servo to the preset position
        }

        telemetry.addData("Press start to set the current preset to the current value", "");
        //if start is pressed, save the current preset to a file
        if (driver1.start.justPressed() || driver2.start.justPressed()) {
            //set the current selected preset to the current servo position
            servo.getPresets().put(presetNames.get(presetIndex), servoPosition);
            presetValues.set(presetIndex, servoPosition);

            OptionsFile optionsFile = new OptionsFile(EVConverters.getInstance()); //create an OptionsFile

            //put the preset names and presets into the OptionsFile
            for (int i = 0; i < presetNames.size(); i++) {
                optionsFile.set(presetNames.get(i).name(), presetValues.get(i).toString());
            }

            optionsFile.writeToFile(ServoCfg.getServoFile(servoNames.get(servoIndex))); //store the OptionsFile to a file
        }

        //modify the servo position using the joysticks
        servoPosition += 2e-4 * matchTimer.getDeltaTime() * (driver1.left_stick_y.getValue() + 0.1 * driver1.right_stick_y.getValue() + driver2.left_stick_y.getValue() + 0.1 * driver2.right_stick_y.getValue());

        //limit the position
        servoPosition = Utility.servoLimit(servoPosition);

        //set the servo to the position
        servo.setPosition(servoPosition);

        //store the position
        servoPositions.set(servoIndex, servoPosition);

        //display telemetry about the servo
        telemetry.addData("Servo Name", servoNames.get(servoIndex));
        telemetry.addData("Servo Preset Name", presetNames.get(presetIndex));
        telemetry.addData("Servo Preset Value", servoPosition);
    }

    @Override
    protected void end() {

    }
}