Master for editing

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpmodeConstants.java

@@ -1,39 +1,46 @@
 package org.firstinspires.ftc.teamcode;
 public class OpmodeConstants {
-    public static final int backlineSpeed = 2240;
-    public static final int midSpeed = 1940;
-    public static final int goalSpeed = 1500;
-    public static final int midAngle = 48/360;
-    public static final int goalAngle = 75/360;
-    public static final int backlineAngle = 50/360;
+    public static final int TeleopBacklineSpeed = 2300;
+    public static final int TeleopMidlineSpeed = 1940;
+    public static final int TeleopGoalSpeed = 1500;
+    public static final double TeleopMidAngle = 0.133333; //48/360.0;
+    public static final double TeleopGoalAngle = 0.208333; //75/360.0;
+    public static final double TeleopBacklineAngle = 0.14165; //50/360.0;
 
-    public static final int AutobacklineSpeed = 2240;
-    public static final int AutomidSpeed = 1880;
-    public static final int AutogoalSpeed = 1500;
-    public static final int AutomidAngle = 47;
-    public static final int AutogoalAngle = 75;
-    public static final int AutobacklineAngle = 49;
-    
-    public static final int IntakeRampLaunchPos = 0.61;
-    public static final int IntakeRampIntakePos = 0.84;
+    public static final int AutoBacklineSpeed = 2240;
+    public static final int AutoMidSpeed = 1820;
+    public static final int AutoGoalSpeed = 1500;
+    public static final double AutoMidAngle = 0.133333; //48/360.0;
+    public static final double AutoGoalAngle = 0.202777; //73/360.0;
+    public static final double AutoBacklineAngle = 0.136111; // 49/360.0;
+
+    public static final double IntakeRampLaunchPos = 0.61;
+    public static final double IntakeRampIntakePos = 0.84;
     public static final double FeedTimeSeconds = 0.15;
 
-    public static final double Launcher_P = 203;
-    public static final double Launcher_I = 1.001;
-    public static final double Launcher_D = 0.0015;
-    public static final double Launcher_F = 0.1;
+    public static final double Launcher_P = 80;
+    public static final double Launcher_I = 0.002;
+    public static final double Launcher_D = 0.1;
+    public static final double Launcher_F = 11;
 //hardware map names
-    public static final string IntakeName = "intake";
-    public static final string IntakeRampName = "intake ramp";
-    public static final string IntakeStoperName = "intake stopper";
-    public static final string AimServoName = "left twideler";
-    public static final string LeftFeederName = "left_feeder";
-    public static final string RightFeederName = "right_feeder";
-    public static final string LauncherName = "launcher";
-
-
-
+    public static final String IntakeName = "intake";
+    public static final String IntakeRampName = "intake ramp";
+    public static final String IntakeStopperName = "intake stopper";
+    public static final String AimServoName = "left twideler";
+    public static final String LeftFeederName = "left_feeder";
+    public static final String RightFeederName = "right_feeder";
+    public static final String LauncherName = "launcher";
 
+    public static final String FrontLeftMotor = "front_left_drive";
+    public static final String FrontRightMotor = "front_right_drive";
+    public static final String BackLeftMotor = "back_left_drive";
+    public static final String BackRightMotor = "back_right_drive";
 
+    public static final String IMUName = "imu";
+    public static final String odometryName = "odometry";
+    public static final String PresetLightName = "preset light";
+    public static final String AimLightName = "aim light";
+    public static final String FloodgateName = "floodgate";
+    public static final String WebcamName = "Webcam 1";
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PIDF_Tuning.java

@@ -9,15 +9,18 @@
 import com.qualcomm.robotcore.hardware.DcMotorEx;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
+import java.util.Random;
+
 @Autonomous
-@Configurable
+@Config
 public class PIDF_Tuning  extends OpMode {
     public static double P = 0;
     public static double I = 0;
     public static double D = 0;
     public static double F = 0;
     public static double targetSpeed = 2000;
     public static double switch_time = 10;
+    private Random random = new Random();
 
     private int Switch = 0;
 
@@ -26,27 +29,29 @@ public class PIDF_Tuning  extends OpMode {
     @Override
     public void init() {
         telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
-        launcher = hardwareMap.get(DcMotorEx.class, "launcher");
+        launcher = hardwareMap.get(DcMotorEx.class, OpmodeConstants.LauncherName);
         time.reset();
     }
 
     @Override
     public void loop() {
         launcher.setVelocityPIDFCoefficients(P,I,D,F);
         telemetry.addData("Velocity", launcher.getVelocity());
+        telemetry.addData("target speed", targetSpeed);
         telemetry.addData("Time passed", time.seconds());
         telemetry.addData("P", P);
         telemetry.addData("I", I);
         telemetry.addData("D", D);
         telemetry.addData("F", F);
         if(time.seconds() >= switch_time){
-            if (Switch == 0) {
-                launcher.setVelocity(-targetSpeed);
-                Switch = 1;
-            } else {
-                launcher.setVelocity(targetSpeed);
-                Switch = 0;
-            }
+            launcher.setVelocity(targetSpeed);
+//            if (Switch == 0) {
+//                launcher.setVelocity(-targetSpeed);
+//                Switch = 1;
+//            } else {
+//                launcher.setVelocity(targetSpeed);
+//                Switch = 0;
+//            }
             time.reset();
         }
         telemetry.update();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/StaticCommunism.java

@@ -0,0 +1,7 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.pedropathing.geometry.Pose;
+
+public class StaticCommunism {
+    public static Pose pose = new Pose(72, 9.6, Math.toRadians(90));
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/Back3Blue.java

@@ -5,7 +5,7 @@
 
 @Autonomous(name = "BLUE 3 BACK", group = "Autonomous")
 public class Back3Blue extends Back3Main {
-    int backlineAngle = 110;
+    int backlineAngle = 109;
     public void set_starting_pose(){
         starting_pose_x = 63;
         starting_pose_y = 8;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/Back3Main.java

@@ -15,6 +15,8 @@
 import com.qualcomm.robotcore.hardware.Servo;
 import com.qualcomm.robotcore.util.ElapsedTime;
 
+import org.firstinspires.ftc.teamcode.OpmodeConstants;
+import org.firstinspires.ftc.teamcode.StaticCommunism;
 import org.firstinspires.ftc.teamcode.autos.pedroPathing.Constants;
 
     @Autonomous(name = "donrt run ", group = "Autonomous")
@@ -45,9 +47,9 @@ public abstract class Back3Main extends OpMode {
         private CRServo rightFeeder = null;
         private DcMotorEx launcher = null;
         private DcMotorEx intake = null;
-        public static int targetSpeed = 2380;//launch motor speed
+        public static int targetSpeed = OpmodeConstants.AutoBacklineSpeed;//launch motor speed
         private Servo intake_ramp = null;
-        public static double targetAngle = 0.1444;
+        public static double targetAngle = OpmodeConstants.AutoBacklineAngle;
         public static int INTAKE_SPEED = 1600; //RPM
 
         private TelemetryManager panelsTelemetry; // Panels Telemetry instance
@@ -66,28 +68,28 @@ public abstract class Back3Main extends OpMode {
         private static final double START_TOLERANCE_INCHES = 6.0;
 
         private void initialize_launcher() {
-            launcher = hardwareMap.get(DcMotorEx.class, "launcher");
-            double p = 203;
-            double i = 1.001;
-            double d = 0.0015;
-            double f = 0.1;
+            launcher = hardwareMap.get(DcMotorEx.class, OpmodeConstants.LauncherName);
+            double p = OpmodeConstants.Launcher_P;
+            double i = OpmodeConstants.Launcher_I;
+            double d = OpmodeConstants.Launcher_D;
+            double f = OpmodeConstants.Launcher_F;
             launcher.setVelocityPIDFCoefficients(p, i, d, f);
             launcher.setDirection(DcMotorSimple.Direction.REVERSE);
-            LEFT_LAUNCH_SERVO = hardwareMap.get(Servo.class, "left twideler");
+            LEFT_LAUNCH_SERVO = hardwareMap.get(Servo.class, OpmodeConstants.AimServoName);
         }
 
         private void initialize_feeder() {
-            leftFeeder = hardwareMap.get(CRServo.class, "left_feeder");
-            rightFeeder = hardwareMap.get(CRServo.class, "right_feeder");
+            leftFeeder = hardwareMap.get(CRServo.class, OpmodeConstants.LeftFeederName);
+            rightFeeder = hardwareMap.get(CRServo.class, OpmodeConstants.RightFeederName);
 
             leftFeeder.setDirection(DcMotorSimple.Direction.FORWARD);//  DIRECTION SETUP
             rightFeeder.setDirection(DcMotorSimple.Direction.REVERSE);
 
         }
 
         private void initialize_intake() {
-            intake = hardwareMap.get(DcMotorEx.class, "intake");
-            intake_ramp = hardwareMap.get(Servo.class, "intake ramp");
+            intake = hardwareMap.get(DcMotorEx.class, OpmodeConstants.IntakeName);
+            intake_ramp = hardwareMap.get(Servo.class, OpmodeConstants.IntakeRampName);
             intake.setDirection(DcMotorSimple.Direction.REVERSE);// DIRECTION SETUP
 
         }
@@ -285,7 +287,7 @@ public void autonomousPathUpdate() {
                         waitingForPath = false;
                         pathState = 100;
                         nextPathState = 2;
-                        targetSpeed = 2380;
+                        targetSpeed = OpmodeConstants.AutoBacklineSpeed;
                     }
                     break;
                 case 2:
@@ -303,8 +305,9 @@ public void autonomousPathUpdate() {
                     if (waitingForPath && !follower.isBusy()) {
                         waitingForPath = false;
                         pathState = 0;
-                        nextPathState = 3;
+                        nextPathState = 0;
                     }
+                    StaticCommunism.pose = follower.getPose();
                 case 100:
                     // TODO use case 100 for aiming to launch
                     pathState = 101;
@@ -316,10 +319,16 @@ public void autonomousPathUpdate() {
                     intake.setVelocity(0);
                     doneLaunching = launch();
                     if (doneLaunching){
-                        pathState = nextPathState;
-                        timesShot = 0;
+                            timesShot = 0;
+                            pathState = 200;
+                            waitTimer.reset();
                     }
                     break;
+                case 200:
+                    if (waitTimer.seconds() >= 0.3) {
+                        pathState = nextPathState;
+                    }
+
 
 
             }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/Back3Red.java

@@ -5,7 +5,7 @@
 
 @Autonomous(name = "RED 3 BACK", group = "Autonomous")
 public class Back3Red extends Back3Main {
-    int backlineAngle = 70; // Mirrored from blue 110 (180 - 110 = 70, adjusted to 67 to match other red autos)
+    int backlineAngle = 73; // Mirrored from blue 110 (180 - 110 = 70, adjusted to 67 to match other red autos)
 
     public void set_starting_pose(){
         // Mirrored from blue: x' = 144 - 63 = 81

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autos/GoalBlue.java

@@ -1,22 +1,27 @@
 package org.firstinspires.ftc.teamcode.autos;
 
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 
-@Autonomous(name = "BLUE GOAL")
-
+@Autonomous(name = "Blue Goal", group = "Autonomous")
 public class GoalBlue extends GoalMain {
-    @Override
-    void move() {
-        double forward  = 1;
-        double strafe = 1;
-        double rotate = 0;
-
-        double denominator = Math.max(Math.abs(forward) + Math.abs(strafe) + Math.abs(rotate), 1);
-
-        leftFrontDrive.setPower((forward - strafe - rotate)/denominator);
-        leftBackDrive.setPower((forward + strafe - rotate)/denominator);
-        rightFrontDrive.setPower((forward + strafe + rotate)/denominator);
-        rightBackDrive.setPower((forward - strafe + rotate)/denominator);
-
+    int GoalAimAngle = 142;
+    public void set_starting_pose(){
+        starting_pose_x = 17;
+        starting_pose_y = 119;
+        starting_pose_heading = 142;
+        //follower.setStartingPose(new Pose(63, 8, Math.toRadians(90)));
+    }
+    public void set_color(){
+        String color = "blue";
+        int tagToAim = 20;
+        paths.shootPreloadStart = new Pose(17, 119);
+        paths.shootPreloadEnd = new Pose(23, 119);
+        paths.parkStart = new Pose(23, 119);
+        paths.parkEnd = new Pose(36, 132);
+        paths.headShootPreload1 = 90;
+        paths.headShootPreload2 = GoalAimAngle;
+        paths.headPark1 = GoalAimAngle;
+        paths.headPark2 = 0;
     }
 }