fly wheel by velocity - in progress

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

@@ -170,16 +170,16 @@ public class Blue extends LinearOpMode {
         while(opModeInInit()) {
 
             if (gamepad2.dpadUpWasPressed()){
-                hoodDefault -= 0.02;
+                hoodDefault -= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()){
-                hoodDefault += 0.02;
+                hoodDefault += 0.01;
             }
 
             robot.hood.setPosition(hoodDefault);
 
-            shooter.setTurretPosition(0.3);
+            shooter.setTurretPosition(turret_blue);
 
             aprilTag.initTelemetry();
 

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

@@ -99,45 +99,45 @@ public class Red extends LinearOpMode {
 
 
 
-        TrajectoryActionBuilder traj1 = drive.actionBuilder(new Pose2d(0, 0, 0))
+        TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
                 .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
 
 
-        TrajectoryActionBuilder traj2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
                 .turnTo(Math.toRadians(135))
                 .strafeToLinearHeading(new Vector2d(x2, y2), h2 );
 
 
-        TrajectoryActionBuilder traj3 = drive.actionBuilder(new Pose2d(x2, y2, h2))
+        TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
                 .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
 
-        TrajectoryActionBuilder traj4 = drive.actionBuilder(new Pose2d(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 traj5 = drive.actionBuilder(new Pose2d(x3, y3, h3))
+        TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
                 .strafeToLinearHeading(new Vector2d(x1, y1), h1 );
 
 
-        TrajectoryActionBuilder traj6 = drive.actionBuilder(new Pose2d(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.02;
+                hoodDefault -= 0.01;
             }
 
             if (gamepad2.dpadDownWasPressed()){
-                hoodDefault += 0.02;
+                hoodDefault += 0.01;
             }
 
             robot.hood.setPosition(hoodDefault);
 
-            shooter.setTurretPosition(0.33);
+            shooter.setTurretPosition(turret_red);
 
             aprilTag.initTelemetry();
 
@@ -165,7 +165,7 @@ public class Red extends LinearOpMode {
 
             Actions.runBlocking(
                     new ParallelAction(
-                            traj1.build()
+                            shoot0.build()
                     )
             );
 
@@ -245,12 +245,12 @@ public class Red extends LinearOpMode {
             robot.intake.setPower(1);
 
             Actions.runBlocking(
-                    traj2.build()
+                    pickup1.build()
             );
 
 
             Actions.runBlocking(
-                    traj3.build()
+                    shoot1.build()
             );
 
             shooter.setManualPower(1);
@@ -329,12 +329,12 @@ public class Red extends LinearOpMode {
             robot.intake.setPower(1);
 
             Actions.runBlocking(
-                    traj4.build()
+                    pickup2.build()
             );
 
 
             Actions.runBlocking(
-                    traj5.build()
+                    shoot2.build()
             );
 
             shooter.setManualPower(1);
@@ -411,7 +411,7 @@ public class Red extends LinearOpMode {
             spindexer.outtake3();
 
             Actions.runBlocking(
-                    traj6.build()
+                    park.build()
             );
 
             drive.updatePoseEstimate();

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

@@ -16,8 +16,12 @@ public class ServoPositions {
     public static double transferServo_out = 0.13;
 
     public static double transferServo_in = 0.4;
+    public static double transferServoPos = 0.5;
 
     public static double hoodDefault = 0.35;
 
+    public static double turret_red = 0.33;
+    public static double turret_blue = 0.3;
+
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.subsystems;
 
+import static java.lang.Runtime.getRuntime;
+
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.controller.PIDController;
@@ -45,6 +47,7 @@ public class Shooter implements Subsystem {
     private double p = 0.0003, i = 0, d = 0.00001;
 
     private PIDController controller;
+    private double pow = 0.0;
 
 
 
@@ -100,8 +103,9 @@ public class Shooter implements Subsystem {
         telemetry.addData("hoodPos", gethoodPosition());
         telemetry.addData("turretPos", getTurretPosition());
         telemetry.addData("PID Coefficients", "P: %.6f, I: %.6f, D: %.6f", p, i, d);
-        telemetry.addData("Current Fly 1", fly1.getCurrent(CurrentUnit.AMPS));
+        telemetry.addData("Power Fly 1", fly1.getPower());
-        telemetry.addData("Current Fly 2", fly2.getCurrent(CurrentUnit.AMPS));
+        telemetry.addData("Pow Fly 2", fly2.getPower());
+        telemetry.addData("Pow", pow);
 
     }
 
@@ -118,7 +122,7 @@ public class Shooter implements Subsystem {
     public void setTurretPosition(double pos) {turretPos = pos;}
 
     public double getVelocity() {
-        return ((double) ((fly1.getVelocity(AngleUnit.DEGREES) + fly2.getVelocity(AngleUnit.DEGREES)) /2));
+        return ((double) ((fly1.getVelocity(AngleUnit.DEGREES))));
     }
 
     public void setVelocity(double vel){velocity = vel;}
@@ -292,13 +296,23 @@ public class Shooter implements Subsystem {
         }
 
         else if (Objects.equals(shooterMode, "VEL")){
-
             fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
             fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+            fly1.setPower(pow);
+            fly2.setPower(pow);
 
-            fly1.setVelocity(velocity);
 
-            fly2.setPower(fly1.getPower());
+            if (velocity == 0.0){
+                fly1.setPower(0);
+                fly2.setPower(0);
+            } else if (-velocity < -Math.abs(fly1.getVelocity())){
+                while (-velocity < -Math.abs(fly1.getVelocity())){
+                    pow += 0.001;
+                    fly1.setPower(pow);
+                    fly2.setPower(pow);
+                }
+            }
+
 
 
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java

@@ -1,7 +1,8 @@
 package org.firstinspires.ftc.teamcode.teleop;
 
-import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
+import static org.firstinspires.ftc.teamcode.constants.Poses.*;
-import static org.firstinspires.ftc.teamcode.constants.ServoPositions.hoodDefault;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
 
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
@@ -215,7 +216,6 @@ public class TeleopV1 extends LinearOpMode {
 
 
 
-
         waitForStart();
 
         if (isStopRequested()) return;
@@ -233,6 +233,7 @@ public class TeleopV1 extends LinearOpMode {
 
 
             TELE.addData("off", offset);
+            robot.transferServo.setPosition(transferServoPos);
 
 
             robot.hood.setPosition(pos);