diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java index 9014b77..9e05cf3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java @@ -22,8 +22,7 @@ public class Arm { private double armTempPos; private double doorPos; private double wristPos; - public static double ARM_KP = 0.2 - ; + public static double ARM_KP = 0.2; public static double doorOpenPos = 0.36; public static double doorClosePos = 0.61; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java index 3b82485..a5f097c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java @@ -17,7 +17,7 @@ public class Slides { // public static double d = 0; // public static double f = 0.01; //p was 0.0014 - public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.02,0,0.01); + public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.05,0,0.01); public static double pTolerance = 20; private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java index e406779..583d669 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java @@ -28,13 +28,13 @@ public class BlueBackStageAuto extends AutoBase { public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180)); public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90)); - public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180)); - public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.8, 33.5, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53.4, 28.7, Math.toRadians(-180)); + public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.7, 34.5, Math.toRadians(-180)); public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180)); public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187)); - public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, 31.6, Math.toRadians(-187)); + public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.7, 32.6, Math.toRadians(-187)); public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187)); @@ -139,7 +139,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.5))); + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.65))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d())); @@ -224,7 +224,7 @@ public class BlueBackStageAuto extends AutoBase { builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2))); break; case 2: - builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-3))); + builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2.75))); break; case 3: builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d())); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java index 4e9b215..1471f35 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java @@ -139,7 +139,7 @@ public class RedBackStageAuto extends AutoBase { builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180)); switch (teamPropLocation) { case 1: - builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.85))); + builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2))); robot.intake.setpos(STACK5); break; case 2: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java index a577b6c..c3aebb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java @@ -3,10 +3,14 @@ package org.firstinspires.ftc.teamcode.opmode.teleop; import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE; import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN; +import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.roadrunner.geometry.Pose2d; 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.Servo; +import com.qualcomm.robotcore.hardware.VoltageSensor; import org.firstinspires.ftc.teamcode.controller.Controller; import org.firstinspires.ftc.teamcode.hardware.Arm; @@ -29,8 +33,29 @@ public class MainTeleOp extends OpMode { public boolean hang_counter = false; + + private DcMotor[] motors = new DcMotor[8]; + private Servo[] servos = new Servo[7]; + @Override public void init() { + this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide"); + this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor"); + this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight"); + this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight"); + this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft"); + this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft"); + this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide"); + this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang"); + + this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake"); + this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist"); + this.servos[2] = this.hardwareMap.get(Servo.class, "Door"); + this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm"); + this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm"); + this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo"); + this.servos[6] = this.hardwareMap.get(Servo.class, "Drone"); + controller1 = new Controller(gamepad1); controller2 = new Controller(gamepad2); @@ -43,13 +68,35 @@ public class MainTeleOp extends OpMode { // } telemetry.addLine("Initialized"); + this.telemetry = FtcDashboard.getInstance().getTelemetry(); telemetry.update(); } + double getBatteryVoltage() { + double result = Double.POSITIVE_INFINITY; + for (VoltageSensor sensor : hardwareMap.voltageSensor) { + double voltage = sensor.getVoltage(); + if (voltage > 0) { + result = Math.min(result, voltage); + } + } + return result; + } + @Override public void loop() { + for (DcMotor motor : this.motors) { + this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower()); + } + for (Servo servo : this.servos) { + this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition()); + } + + this.telemetry.addData("battery", getBatteryVoltage()); + this.telemetry.update(); + // Driver 1 double x = -gamepad1.left_stick_y; double y = -gamepad1.left_stick_x; @@ -175,5 +222,7 @@ public class MainTeleOp extends OpMode { controller2.update(); telemetry.addLine(robot.getTelemetry()); telemetry.update(); + + } }