11/26 edits

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java

@@ -0,0 +1,203 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
+
+
+import androidx.annotation.NonNull;
+
+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.acmerobotics.roadrunner.ParallelAction;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.SequentialAction;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.ftc.Actions;
+
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@Autonomous
+public class redDaniel extends LinearOpMode {
+
+    Robot robot;
+
+    MultipleTelemetry TELE;
+
+    MecanumDrive drive;
+
+    AprilTag aprilTag;
+
+    public Action initShooter(int velocity){
+        return new Action(){
+            double velo = 0.0;
+            double initPos = 0.0;
+            double stamp = 0.0;
+            double powPID = 0.0;
+            final int maxVel = 4500;
+            double ticker = 0.0;
+            public boolean run(@NonNull TelemetryPacket telemetryPacket){
+                velo = -60 * ((((double) robot.shooter1.getCurrentPosition() / 2048) - initPos) / (getRuntime() - stamp));
+                stamp = getRuntime();
+                initPos = (double) robot.shooter1.getCurrentPosition() / 2048;
+                if (Math.abs(velocity - velo) > 3 * velTolerance) {
+                    powPID = (double) velocity / maxVel;
+                    ticker = getRuntime();
+                } else if (velocity - velTolerance > velo) {
+                    powPID = powPID + 0.0001;
+                    ticker = getRuntime();
+                } else if (velocity + velTolerance < velo) {
+                    powPID = powPID - 0.0001;
+                    ticker  = getRuntime();
+                }
+                robot.shooter1.setPower(powPID);
+                robot.shooter2.setPower(powPID);
+                robot.transfer.setPower((powPID / 2) + 0.5);
+
+                return getRuntime() - ticker < 0.5;
+            }
+        };
+    }
+
+    public Action Shoot(double spindexer){
+        return new Action() {
+            boolean transfer = false;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                robot.spin1.setPosition(spindexer);
+                robot.spin2.setPosition(1-spindexer);
+                if (robot.spin1Pos.getVoltage() / 3.3 == spindexer){
+                    robot.transferServo.setPosition(transferServo_in);
+                    transfer = true;
+                }
+                if (robot.transferServoPos.getVoltage() / 3.3 == transferServo_in && transfer){
+                    robot.transferServo.setPosition(transferServo_out);
+                    return false;
+                }
+                return true;
+            }
+        };
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        drive = new MecanumDrive(hardwareMap, new Pose2d(
+                0, 0, 0
+        ));
+
+        aprilTag = new AprilTag(robot, TELE);
+
+        TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .turnTo(Math.toRadians(h2))
+                .strafeToLinearHeading(new Vector2d(x2, y2), h2);
+
+        TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+
+                .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b)
+
+                .strafeToLinearHeading(new Vector2d(x3, y3), h3);
+
+        TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .strafeToLinearHeading(new Vector2d(x1, y1 + 30), h1);
+
+        while (opModeInInit()) {
+
+            if (gamepad2.dpadUpWasPressed()) {
+                hoodDefault -= 0.01;
+            }
+
+            if (gamepad2.dpadDownWasPressed()) {
+                hoodDefault += 0.01;
+            }
+
+            robot.hood.setPosition(hoodDefault);
+
+            robot.turr1.setPosition(turret_red);
+            robot.turr2.setPosition(1 - turret_red);
+
+            robot.transferServo.setPosition(transferServo_out);
+
+            aprilTag.initTelemetry();
+
+            aprilTag.update();
+
+            TELE.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        if (opModeIsActive()) {
+
+            robot.hood.setPosition(hoodDefault);
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            shoot0.build()
+                    )
+            );
+
+
+
+            Actions.runBlocking(
+                    pickup1.build()
+            );
+
+            Actions.runBlocking(
+                    shoot1.build()
+            );
+
+
+            Actions.runBlocking(
+                    pickup2.build()
+            );
+
+            Actions.runBlocking(
+                    shoot2.build()
+            );
+
+
+            Actions.runBlocking(
+                    park.build()
+            );
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            TELE.addLine("finished");
+
+            TELE.update();
+
+            sleep(2000);
+
+        }
+
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java

@@ -15,12 +15,12 @@ public class Poses {
 
     public static double x1 = 50, y1 = 0, h1 = 0;
 
-    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135);
+    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140);
 
-    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135);
+    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);
 
 
-    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135);
+    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
 
     public static Pose2d teleStart = new Pose2d(x1,-10,0);
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java

@@ -15,12 +15,16 @@ public class ServoPositions {
 
     public static double transferServo_out = 0.13;
 
-    public static double transferServo_in = 0.4;
+    public static double transferServo_in = 0.36;
 
     public static double hoodDefault = 0.35;
 
-    public static double turret_red = 0.33;
+    public static double hoodHigh = 0.21;
-    public static double turret_blue = 0.3;
+
+    public static double hoodLow = 1.0;
+
+    public static double turret_red = 0.43;
+    public static double turret_blue = 0.4;
 
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java

@@ -7,4 +7,6 @@ public class ShooterVars {
     public static double turret_GearRatio = 0.9974;
 
     public static double turret_Range = 355;
+
+    public static int velTolerance = 500;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

@@ -83,7 +83,7 @@ public class ShooterTest extends LinearOpMode {
 
             robot.hood.setPosition(hoodPos);
             robot.turr1.setPosition(turretPos);
-            robot.turr2.setPosition(turretPos);
+            robot.turr2.setPosition(1-turretPos);
 
             velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
             stamp1 = getRuntime();
@@ -91,9 +91,9 @@ public class ShooterTest extends LinearOpMode {
             if (Math.abs(vel - velo1) > 3 * tolerance) {
                 powPID = vel / maxVel;
             } else if (vel - tolerance > velo1) {
-                powPID = powPID + 0.001;
+                powPID = powPID + 0.0001;
             } else if (vel + tolerance < velo1) {
-                powPID = powPID - 0.001;
+                powPID = powPID - 0.0001;
             }
             shooter.setVelocity(powPID);
             robot.transfer.setPower((powPID / 2) + 0.5);

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java

@@ -11,7 +11,7 @@ public class ConfigureColorRangefinder extends LinearOpMode {
 
     public static int LED_Brightness = 50;
 
-    public static int lowerGreen = 100;
+    public static int lowerGreen = 120;
 
     public static int higherGreen = 150;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

@@ -1,6 +1,8 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -8,6 +10,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 @Config
 public class PositionalServoProgrammer extends LinearOpMode {
     Robot robot;
+    MultipleTelemetry TELE;
     public static double spindexPos = 0.501;
     public static double turretPos = 0.501;
     public static double transferPos = 0.501;
@@ -15,6 +18,7 @@ public class PositionalServoProgrammer extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()){
@@ -32,6 +36,11 @@ public class PositionalServoProgrammer extends LinearOpMode {
             if (hoodPos != 0.501){
                 robot.hood.setPosition(hoodPos);
             }
+            TELE.addData("spindexer", 1.111111111*((robot.spin1Pos.getVoltage() / 3.3) - 0.05));
+            TELE.addData("hood", 1.111111111*((robot.hoodPos.getVoltage() / 3.3) - 0.05));
+            TELE.addData("transferServo", 1.111111111*((robot.transferServoPos.getVoltage() / 3.3) - 0.05));
+            TELE.addData("turret", 1.111111111*((robot.turr1Pos.getVoltage() / 3.3) - 0.05));
+            TELE.update();
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -59,6 +59,18 @@ public class Robot {
 
     public AnalogInput analogInput2;
 
+    public AnalogInput spin1Pos;
+
+    public AnalogInput spin2Pos;
+
+    public AnalogInput hoodPos;
+
+    public AnalogInput turr1Pos;
+
+    public AnalogInput turr2Pos;
+
+    public AnalogInput transferServoPos;
+
     public AprilTagProcessor aprilTagProcessor;
 
     public WebcamName webcam;
@@ -92,14 +104,24 @@ public class Robot {
 
         hood = hardwareMap.get(Servo.class, "hood");
 
+        hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
+
         turr1 = hardwareMap.get(Servo.class, "t1");
 
+        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
+
         turr2 = hardwareMap.get(Servo.class, "t2");
 
+        turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
+
         spin1 = hardwareMap.get(Servo.class, "spin1");
 
+        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
+
         spin2 = hardwareMap.get(Servo.class, "spin2");
 
+        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+
         pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
 
         pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
@@ -120,6 +142,8 @@ public class Robot {
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
+        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
+
         transfer.setDirection(DcMotorSimple.Direction.REVERSE);
 
         aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();