Merge remote-tracking branch 'origin/add-tilt' into cowtown-work

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

@@ -28,9 +28,13 @@ public class TeleopV4 extends LinearOpMode {
     Turret turret;
     Flywheel flywheel;
 
+    ParkTilter parkTilter;
+
     @Override
     public void runOpMode() throws InterruptedException {
 
+        Robot.resetInstance();
+
         robot = Robot.getInstance(hardwareMap);
 
         TELE = new MultipleTelemetry(
@@ -45,6 +49,8 @@ public class TeleopV4 extends LinearOpMode {
         flywheel = new Flywheel(robot);
         turret = new Turret(robot);
 
+        parkTilter = new ParkTilter(robot);
+
 
         shooter = new Shooter(robot, TELE, follower, Color.redAlliance, turret, flywheel);
         shooter.setState(Shooter.ShooterState.TRACK_GOAL);
@@ -66,12 +72,14 @@ public class TeleopV4 extends LinearOpMode {
                     gamepad1.left_stick_x
             );
 
+            follower.update();
+
             shooter.update();
             spindexerTransferIntake.update();
 
             SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState();
 
-            if (gamepad1.xWasPressed() &&
+            if (gamepad1.leftBumperWasPressed() &&
                     (state == SpindexerTransferIntake.RapidMode.INTAKE ||
                             state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF ||
                             state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT ||
@@ -82,7 +90,7 @@ public class TeleopV4 extends LinearOpMode {
                 spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE);
             }
 
-            if (gamepad1.aWasPressed() &&
+            if (gamepad1.right_trigger > 0.5 &&
                     (state == SpindexerTransferIntake.RapidMode.INTAKE ||
                             state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) {
 
@@ -90,8 +98,7 @@ public class TeleopV4 extends LinearOpMode {
                         SpindexerTransferIntake.RapidMode.HOLD_BALLS
                 );
             }
-
-            if (gamepad1.yWasPressed()
+            if (gamepad1.rightBumperWasPressed()
                     && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) {
 
                 spindexerTransferIntake.setRapidMode(
@@ -99,6 +106,11 @@ public class TeleopV4 extends LinearOpMode {
                 );
             }
 
+            if (gamepad1.dpad_down){
+                parkTilter.park();
+            } else if (gamepad1.dpad_up) {
+                parkTilter.unpark();
+            }
 
             TELE.update();
         }

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

@@ -130,7 +130,7 @@ public class Hardware_Tester extends LinearOpMode {
 //            TELE.addData("Beam Break 3?", robot.beam3.isPressed());
 
             NormalizedRGBA revColor = robot.revSensor.getNormalizedColors();
-            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.MM));
+            TELE.addData("REV Distance", robot.revSensor.getDistance(DistanceUnit.CM));
             TELE.addData("REV Green", revColor.green / (revColor.red + revColor.blue + revColor.green));
 
             TELE.addData("Voltage Sensor", robot.voltage.getVoltage());

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

@@ -3,6 +3,8 @@ package org.firstinspires.ftc.teamcode.tests;
 import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -20,7 +22,9 @@ public class NewShooterTest extends LinearOpMode {
     Robot robot;
     Flywheel flywheel;
     Turret turret;
+    Shooter shooter;
     MultipleTelemetry TELE;
+    Follower follower;
 
 
     public static boolean intake = true;
@@ -52,14 +56,16 @@ public class NewShooterTest extends LinearOpMode {
 
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
+        follower = Constants.createFollower(hardwareMap);
+        follower.setStartingPose(new Pose(72, 72, 0));
 
         flywheel = new Flywheel(robot);
         turret = new Turret(robot);
 
-        Shooter shooter = new Shooter(
+        shooter = new Shooter(
                 robot,
                 TELE,
-                Constants.createFollower(hardwareMap),
+                follower,
                 true,
                 turret,
                 flywheel
@@ -73,6 +79,8 @@ public class NewShooterTest extends LinearOpMode {
 
         while (opModeIsActive()) {
 
+            follower.update();
+
             robot.setHoodPos(hoodPos);
             shooter.setTurretPosition(turretPos);
             shooter.setFlywheelVelocity(flywheel_velo);
@@ -136,9 +144,10 @@ public class NewShooterTest extends LinearOpMode {
 
             TELE.addData("Flywheel Velocity1", (robot.shooter1.getVelocity() * 60) / 28);
             TELE.addData("Flywheel Velocity2", (robot.shooter2.getVelocity() * 60) / 28);
-            TELE.addData("Flywheel Averag Velocity", flywheel.getAverageVelocity());
+            TELE.addData("Flywheel Average Velocity", flywheel.getAverageVelocity());
             TELE.addData("PIDF Coefficients", Flywheel.shooterPIDF);
             TELE.addData("Power", flywheel.getShooterPower());
+            TELE.addData("Distance", shooter.getDistance());
             TELE.update();
 
             shooter.update();

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java

@@ -128,5 +128,7 @@ public class Flywheel {
 
         steady = (Math.abs(commandedVelocity - averageVelocity) < 50);
     }
+
+
     public double getShooterPower(){return power;}
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/ParkTilter.java

@@ -0,0 +1,22 @@
+package org.firstinspires.ftc.teamcode.utilsv2;
+
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.firstinspires.ftc.teamcode.constants.ServoPositions;
+
+public class ParkTilter {
+    Robot robot;
+    public ParkTilter (Robot rob) {
+        this.robot = rob;
+    }
+
+    public void park() {
+        robot.setTilt1Pos(ServoPositions.tilt1_down);
+        robot.setTilt2Pos(ServoPositions.tilt2_down);
+    }
+
+    public void unpark() {
+        robot.setTilt1Pos(ServoPositions.tilt1_up);
+        robot.setTilt2Pos(ServoPositions.tilt2_up);
+    }
+}

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

@@ -74,20 +74,30 @@ public class Shooter {
         return turr.getObeliskID();
     }
 
-
+    private final double shooterDistFromCenter = 1.545;
     public void update() {
 
         switch (state) {
             case NOTHING:
                 break;
             case MANUAL:
+
+                commander.getVeloPredictive(
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
+                        follow.getVelocity().getXComponent(),
+                        follow.getAcceleration().getXComponent(),
+                        follow.getVelocity().getYComponent(),
+                        follow.getAcceleration().getYComponent()
+                );
+
                 fly.manageFlywheel(flywheelVelocity);
                 turr.manual(turretPosition);
                 break;
             case TRACK_GOAL:
                 turr.trackGoal(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -97,8 +107,8 @@ public class Shooter {
                 );
 
                 flywheelVelocity = commander.getVeloPredictive(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
@@ -109,14 +119,14 @@ public class Shooter {
                 break;
             case READ_OBELISK:
                 turr.trackObelisk(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (obeliskX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (obeliskY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading()
                 );
 
                 flywheelVelocity = commander.getVeloPredictive(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
@@ -129,8 +139,8 @@ public class Shooter {
             case MANUAL_TURRET_TRACK_FLY:
                 turr.manual(turretPosition);
                 flywheelVelocity = commander.getVeloPredictive(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getVelocity().getXComponent(),
                         follow.getAcceleration().getXComponent(),
                         follow.getVelocity().getYComponent(),
@@ -142,8 +152,8 @@ public class Shooter {
 
             case MANUAL_FLYWHEEL_TRACK_TURR:
                 turr.trackGoal(
-                        (follow.getPose().getX() - goalX),
-                        (follow.getPose().getY() - goalY),
+                        (goalX - follow.getPose().getX() - shooterDistFromCenter*Math.cos(follow.getHeading())),
+                        (goalY - follow.getPose().getY() - shooterDistFromCenter*Math.sin(follow.getHeading())),
                         follow.getHeading(),
                         follow.getAngularVelocity(),
                         follow.getVelocity().getXComponent(),
@@ -158,4 +168,7 @@ public class Shooter {
 
     }
 
+    public double getDistance(){return commander.getDistance();}
+
+
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java

@@ -13,8 +13,8 @@ public class SpindexerTransferIntake {
         this.robot = rob;
     }
 
-    private final double sensorDistanceThreshold = 4.0;
-    private final long pulseTime = 50; // ms
+    private final double sensorDistanceThreshold = 6.0;
+    private final long pulseTime = 100; // ms
 
     public enum SpindexerMode {
         RAPID,
@@ -74,7 +74,7 @@ public class SpindexerTransferIntake {
                     case INTAKE:
 
                         robot.setIntakePower(1);
-                        robot.setTransferPower(1);
+                        robot.setTransferPower(-0.7);
                         robot.setRapidFireBlockerPos(
                                 ServoPositions.rapidFireBlocker_Closed
                         );
@@ -95,7 +95,7 @@ public class SpindexerTransferIntake {
 
                     case TRANSFER_OFF:
 
-                        robot.setTransferPower(0.3);
+                        robot.setTransferPower(-0.7);
 
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
@@ -145,9 +145,6 @@ public class SpindexerTransferIntake {
 
                             robot.setIntakePower(1);
                         }
-
-
-
                         break;
 
                     case OPEN_GATE:

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java

@@ -83,7 +83,7 @@ public class Turret {
         robot.setTurretPos(pos);
 
     }
-
+    // 1.545
 
     public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) {
         // dx, dy, dz is target - robot

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java

@@ -12,23 +12,25 @@ public class VelocityCommander {
     }
 
     private double distToRPM (double dist){
-        return Math.sqrt(dist*dist + goalH*goalH);
+        return Math.sqrt(dist*dist + goalH*goalH) * 20;
         //TODO: Add regression here using goalH
     }
-
+    // 27
     public double getVeloStationary (double distance){
         return distToRPM(distance);
     }
 
+    private final double goalHeight = 28;
+    double predictedDist = 0;
     public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) {
 
         double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot
         double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK);  // Negative bc dy = target - robot
 
-        double predictedDist = Math.sqrt(dx*dx + dy*dy);
+        predictedDist = Math.sqrt(predictedDx*predictedDx + predictedDy*predictedDy + goalHeight*goalHeight);
 
         return distToRPM(predictedDist);
     }
 
-
+    public double getDistance(){return predictedDist;}
 }