Skip to content

Instantly share code, notes, and snippets.

@IshanArya

IshanArya/robot_loop.java

Created December 20, 2020 23:43
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save IshanArya/c8b3014f45461b1412b8a6eb0bcd4406 to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save IshanArya/c8b3014f45461b1412b8a6eb0bcd4406 to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. IshanArya created this gist Dec 20, 2020.
    192 changes: 192 additions & 0 deletions robot_loop.java
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,192 @@
    package org.firstinspires.ftc.teamcode;

    import com.acmerobotics.dashboard.FtcDashboard;
    import com.acmerobotics.dashboard.config.Config;
    import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
    import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
    import com.arcrobotics.ftclib.hardware.motors.Motor;
    import com.qualcomm.robotcore.eventloop.opmode.OpMode;
    import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
    import com.qualcomm.robotcore.hardware.DcMotor;
    import com.qualcomm.robotcore.hardware.DcMotorEx;

    import org.firstinspires.ftc.robotcore.external.Telemetry;

    @Config
    class PIDVals {
    public static double kVal = 0;
    public static double kp = 0.05;
    public static double ki = 0;
    public static double kd = 0;
    }

    @TeleOp(name = "FlyWheelTuner")
    public class FlyWheelTuner extends OpMode {
    private Motor flywheel;
    private Motor hitter;
    private Motor intake1, intake2;
    private double speedModifier = 0;
    private double direction = 1;
    private double intakeSpeed = 0;
    private boolean pressedA = false;
    private boolean pressedB = false;
    private boolean pressedX = false;
    private boolean pressedY = false;
    private boolean pressedRB = false;
    private boolean pressedUp = false;
    private boolean pressedDown = false;
    private boolean pressedLeft = false;
    private boolean pressedRight = false;

    private KChoice choice = KChoice.KP;



    FtcDashboard dashboard = FtcDashboard.getInstance();

    private enum KChoice {
    KP,
    KI,
    KD
    }

    @Override
    public void init() {
    telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());

    flywheel = new Motor(hardwareMap, "fw", Motor.GoBILDA.BARE);
    flywheel.setRunMode(Motor.RunMode.VelocityControl);
    flywheel.setVeloCoefficients(PIDVals.kp, PIDVals.ki, PIDVals.kd);

    hitter = new Motor(hardwareMap, "h");
    hitter.setRunMode(Motor.RunMode.PositionControl);
    hitter.setPositionCoefficient(0.004);
    hitter.setPositionTolerance(13.6);

    intake1 = new Motor(hardwareMap, "in1");
    intake2 = new Motor(hardwareMap, "in2");
    intake1.setRunMode(Motor.RunMode.VelocityControl);
    intake2.setRunMode(Motor.RunMode.VelocityControl);
    intake1.setVeloCoefficients(0.05, 0, 0);
    intake2.setVeloCoefficients(0.05, 0, 0);
    }

    @Override
    public void loop() {

    if(gamepad1.a) {
    pressedA = true;
    } else if(pressedA) {
    pressedA = false;
    choice = KChoice.KP;
    } else if(gamepad1.x) {
    pressedX = true;
    } else if(pressedX) {
    pressedX = false;
    choice = KChoice.KD;
    } else if(gamepad1.y) {
    pressedY = true;
    } else if(pressedY) {
    pressedY = false;
    choice = KChoice.KI;
    }

    // switch(choice) {
    // case KD:
    // PIDVals.kVal = PIDVals.kd;
    // break;
    // case KI:
    // PIDVals.kVal = PIDVals.ki;
    // break;
    // case KP:
    // PIDVals.kVal = PIDVals.kp;
    // break;
    // }

    if(gamepad1.b) {
    pressedB = true;
    } else if(pressedB) {
    if(speedModifier != 0) {
    speedModifier = 0;
    } else {
    speedModifier = 0.55;
    }
    pressedB = false;
    }

    if(gamepad1.left_bumper) {
    hitter.setTargetPosition(150);
    }
    else {
    hitter.setTargetPosition(0);
    }
    if(!hitter.atTargetPosition()) {
    hitter.set(0.7);
    } else {
    hitter.set(0);
    }

    if(gamepad1.right_trigger >= 0.1) {
    intakeSpeed = 0.7;
    } else if(gamepad1.left_trigger >= 0.1) {
    intakeSpeed = -0.7;
    } else {
    intakeSpeed = 0;
    }

    if(gamepad1.dpad_up) {
    pressedUp = true;
    } else if(pressedUp) {
    pressedUp = false;
    PIDVals.kVal += 0.001;
    } else if(gamepad1.dpad_down) {
    pressedDown = true;
    } else if(pressedDown) {
    pressedDown = false;
    PIDVals.kVal -= 0.001;
    } else if(gamepad1.dpad_left) {
    pressedLeft = true;
    } else if(pressedLeft) {
    pressedLeft = false;
    PIDVals.kVal -= 0.005;
    } else if(gamepad1.dpad_right) {
    pressedRight = true;
    } else if(pressedRight) {
    pressedRight = false;
    PIDVals.kVal += 0.005;
    }

    // switch(choice) {
    // case KD:
    // PIDVals.kd = PIDVals.kVal;
    // break;
    // case KI:
    // PIDVals.ki = PIDVals.kVal;
    // break;
    // case KP:
    // PIDVals.kp = PIDVals.kVal;
    // break;
    // }

    flywheel.setVeloCoefficients(PIDVals.kp, PIDVals.ki, PIDVals.kd);

    double speed = direction * speedModifier * 1;

    flywheel.set(speed);

    telemetry.addData("Flywheel Speed Modifier", speedModifier);
    telemetry.addData("Flywheel Speed", flywheel.get());
    telemetry.addData("Flywheel Set Speed", speed);
    telemetry.addData("Flywheel Velocity", flywheel.encoder.getRawVelocity());
    telemetry.addData("Flywheel Corrected Velocity", flywheel.getCorrectedVelocity());
    telemetry.addData("Flywheel Raw Velocity", ((DcMotorEx)flywheel.motor).getVelocity());
    telemetry.addData("Flywheel Position", flywheel.getCurrentPosition());
    telemetry.addData("Hitter Position", hitter.getCurrentPosition());
    telemetry.addData("FlyWheel PID", flywheel.getVeloCoefficients());
    telemetry.addData("kP", PIDVals.kp);
    telemetry.addData("kI", PIDVals.ki);
    telemetry.addData("kD", PIDVals.kd);


    }
    }