Created
December 20, 2020 23:44
-
-
Save IshanArya/aa59f9acc43f6c8984ef88bcbdc09ed9 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| 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); | |
| } | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment