nyc scrimmage

drive/DriveConstants.java

@@ -35,7 +35,7 @@ public class DriveConstants {
      */
     public static final boolean RUN_USING_ENCODER = true;
     public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(32, 0, 12,
-            10.8);
+            13.6);
 
     // p : 32, i : 0 d : 8, f : 15
 

drive/SampleMecanumDrive.java

@@ -53,8 +53,8 @@
  */
 @Config
 public class SampleMecanumDrive extends MecanumDrive {
-    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(4, 0, 0);
-    public static PIDCoefficients HEADING_PID = new PIDCoefficients(1, 0, 0);
+    public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(2, 0, 0);
+    public static PIDCoefficients HEADING_PID = new PIDCoefficients(4, 0, 0);
 
     public static double LATERAL_MULTIPLIER = 1.3;
 
@@ -79,7 +79,7 @@ public SampleMecanumDrive(HardwareMap hardwareMap) {
         super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
 
         follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
-                new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0);
+                new Pose2d(0.4, 0.4, Math.toRadians(4.0)), 0.1);
 
 //         follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
 //                        new Pose2d(0.25, 0.25, Math.toRadians(2.5)), 0.06);

drive/TwoWheelTrackingLocalizer.java

@@ -37,14 +37,15 @@ public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
     public static double WHEEL_RADIUS = 0.6889764; // in
     public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
 
-    public static double PARALLEL_X = -3.8; // X is the up and down direction
-    public static double PARALLEL_Y = -StandardTrackingWheelLocalizer.LATERAL_DISTANCE / 2.0; // Y is the strafe direction
+    public static double PARALLEL_X = -5.33; // X is the up and down direction
+    public static double PARALLEL_Y = -3.62; // Y is the strafe direction
 
-    public static double PERPENDICULAR_X = StandardTrackingWheelLocalizer.FORWARD_OFFSET;
-    public static double PERPENDICULAR_Y = 2.9;
+    public static double PERPENDICULAR_X = -3.7;
+    public static double PERPENDICULAR_Y = 2.26;
 
-    public static double X_MULTIPLIER = 1;// Multiplier in the X direction
-    public static double Y_MULTIPLIER = 1;// Multiplier in the Y direction
+    public static double X_MULTIPLIER = 1.01785777727;// Multiplier in the X direction
+    public static double Y_MULTIPLIER = 1.00481560895;// Multiplier in the Y direction
+//    public static double Y_MULTIPLIER = 1.0086;// Multiplier in the Y direction
 
     // Parallel/Perpendicular to the forward axis
     // Parallel wheel is parallel to the forward axis
@@ -65,7 +66,7 @@ public TwoWheelTrackingLocalizer(HardwareMap hardwareMap, SampleMecanumDrive dri
         perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "perpOdo"));
 
         // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
-        parallelEncoder.setDirection(Encoder.Direction.REVERSE);
+        perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);
     }
 
     public static double encoderTicksToInches(double ticks) {

hardware/Arm.java

@@ -13,7 +13,7 @@ public class Arm extends Mechanism {
     private Servo leftArm;
     private Servo rightArm;
 
-    public static double INTAKE_POS = 0.84; // 0.18
+    public static double INTAKE_POS = 0.865; // 0.18
     public static double AUTO_INTAKE_POS = 0.18;
     public static double AUTO_SCORE_POS = 0.28;
     public static double AUTO_CONE_STACK_POS = 0.57;

hardware/MXSensor.java

@@ -6,7 +6,9 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.AnalogInput;
 import com.qualcomm.robotcore.hardware.AnalogInputController;
+import com.qualcomm.robotcore.hardware.DigitalChannelImpl;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.PwmControl;
 import com.stuyfission.fissionlib.util.Mechanism;
 
 import org.firstinspires.ftc.robotcore.external.Telemetry;
@@ -33,6 +35,8 @@ public enum Side {
     private AnalogInput mxSensorLeft;
     private AnalogInput mxSensorRight;
 
+
+
     // rev hubs supply 3.3 volts to analog ports
     double SUPPLIED_VOLTAGE = 3.3;
 
@@ -89,8 +93,17 @@ public double getLowPassMM(Side side) {
     @Override
     public void telemetry(Telemetry telemetry) {
         telemetry.addData("MM", getDistanceMM(Side.ALL));
-        telemetry.addData("LowPass MM:", getLowPassMM(Side.ALL));
+        telemetry.addData("LowPass MM", getLowPassMM(Side.ALL));
         telemetry.addData("LowPass Offset", Math.abs(getLowPassMM(Side.ALL) - getDistanceMM(Side.ALL)));
+
+        telemetry.addData("Left MM", getDistanceMM(Side.LEFT));
+        telemetry.addData("LowPass Left MM", getLowPassMM(Side.LEFT));
+
+        telemetry.addData("Right MM", getDistanceMM(Side.RIGHT));
+        telemetry.addData("LowPass Right MM", getLowPassMM(Side.RIGHT));
+
+        telemetry.addData("left voltage", mxSensorLeft.getVoltage());
+        telemetry.addData("right voltage", mxSensorRight.getVoltage());
     }
 
 }

hardware/ScoringFSM.java

@@ -46,6 +46,8 @@ public void init(HardwareMap hwMap) {
 
         time = new ElapsedTime();
         clampOverride = false;
+
+        arm.intakePos();
     }
 
     public enum SlidesState {

hardware/SlidesMotors.java

@@ -22,19 +22,20 @@ public class SlidesMotors extends Mechanism {
 
     public static double MAX_VEL = 100;
     public static double MAX_ACCEL = 100;
-    public static double RETRACTION_MULTIPLIER = 1.25;
+    public static double RETRACTION_MULTIPLIER = 1;
 
-    public static double kP = 0.27;
+    public static double kP = 0.26;
     public static double kI = 0;
     public static double kD = 0;
     public static double kF = 0;
 
+    public static double POS_REST = -1;
     public static double POS_GROUND = 0;
     public static double POS_PREP_ARM = 15;
     public static double POS_LOW = 20.2;
     public static double POS_MEDIUM = 39;
     public static double POS_HIGH = 58.4;
-    public static double POS_HIGH_AUTO = 59;
+    public static double POS_HIGH_AUTO = 58.9;
 
     public static double TELE_DROP_AMT = 4;
     private static double TELE_REST_POS = 0;
@@ -63,8 +64,8 @@ public void init(HardwareMap hwMap) {
     }
 
     public void rest() {
-        leftSlideMotor.setTargetPosition(0);
-        rightSlideMotor.setTargetPosition(0);
+        leftSlideMotor.setTargetPosition(POS_REST);
+        rightSlideMotor.setTargetPosition(POS_REST);
     }
 
     public void setTeleRestPos(double pos) {

opmode/auton/AutoConstants.java

@@ -15,7 +15,7 @@ public class AutoConstants {
 
     public static final double DELAY_SCORING = 0.05; // 0.15
 
-    public static final double[] SLIDE_EXTEND_POSITIONS = {12, 9.7, 6.7, 4.2, 1.2, 0.4, 0, 0};
+    public static final double[] SLIDE_EXTEND_POSITIONS = {12, 9.2, 6.3, 3.8, 0.9, 0.2, 0, 0};
     public static final double[] ARM_CONE_STACK_POSITIONS = {0.82, 0.82, 0.82, 0.82, 0.82, 0.84, 0.9};
 
     /** ==== END CONSTANTS FOR CONE AUTOS ==== **/
@@ -72,26 +72,30 @@ public class AutoConstants {
     public static final double RR_ODO_HIGH_GOAL_HEADING = Math.toRadians(135);
     public static final double RR_ODO_CONE_STACK_TANGENT = Math.toRadians(315);
     public static final double RR_ODO_CONE_STACK_HEADING = Math.toRadians(0);
-    public static final double RR_ODO_MIDDLE_PARK_HEADING = Math.toRadians(270);
+    public static final double RR_ODO_MIDDLE_PARK_HEADING = Math.toRadians(295);
+    public static final double RR_ODO_SIDE_PARK_HEADING = Math.toRadians(90);
 
-    public static final double RR_ODO_CONE_STACK_X = 60;
-    public static final double RR_ODO_CONE_STACK_Y = -12;
+    public static final double RR_ODO_CONE_STACK_X = 61.5;
+    public static final double RR_ODO_CONE_STACK_Y = -12.9;
     public static final Vector2d RR_ODO_CONE_STACK_VECTOR = new Vector2d(RR_ODO_CONE_STACK_X, RR_ODO_CONE_STACK_Y);
     public static final Pose2d RR_ODO_PRELOAD_CONE_STACK_POSE = new Pose2d(RR_CENTER_X, RR_ODO_CONE_STACK_Y, RR_HEADING);
 
 
-    public static final double RR_ODO_HIGH_GOAL_X = 23.5;
-    public static final double RR_ODO_HIGH_GOAL_Y = 0;
+    public static final double RR_ODO_HIGH_GOAL_X = 26.75;
+    public static final double RR_ODO_HIGH_GOAL_Y = -2.69;
+    public static final double RR_ODO_PRELOAD_HIGH_GOAL_X = 28;
+    public static final double RR_ODO_PRELOAD_HIGH_GOAL_Y = 0.2;
     public static final Vector2d RR_ODO_HIGH_GOAL_VECTOR = new Vector2d(RR_ODO_HIGH_GOAL_X, RR_ODO_HIGH_GOAL_Y);
-    public static final Pose2d RR_ODO_PRELOAD_HIGH_GOAL_POSE = new Pose2d(RR_CENTER_X, RR_ODO_HIGH_GOAL_Y, RR_HEADING);
+    public static final Vector2d RR_ODO_PRELOAD_HIGH_GOAL_VECTOR = new Vector2d(RR_ODO_PRELOAD_HIGH_GOAL_X, RR_ODO_PRELOAD_HIGH_GOAL_Y);
+    public static final Pose2d RR_ODO_PRELOAD_HIGH_GOAL_POSE = new Pose2d(RR_CENTER_X, RR_ODO_PRELOAD_HIGH_GOAL_Y, RR_HEADING);
 
-    public static final Vector2d RR_ODO_MIDDLE_PARK_VECTOR = new Vector2d(RR_CENTER_X, RR_ODO_CONE_STACK_Y);
+    public static final Vector2d RR_ODO_MIDDLE_PARK_VECTOR = new Vector2d(RR_CENTER_X, RR_ODO_CONE_STACK_Y - 0.75);
 
-    public static final double RR_ODO_LEFT_PARK_X = 11;
-    public static final Vector2d RR_ODO_LEFT_PARK_VECTOR = new Vector2d(RR_ODO_LEFT_PARK_X, RR_ODO_CONE_STACK_Y);
+    public static final double RR_ODO_LEFT_PARK_X = 13.5;
+    public static final Pose2d RR_ODO_LEFT_PARK_POSE = new Pose2d(RR_ODO_LEFT_PARK_X, RR_ODO_CONE_STACK_Y - 2.5, RR_ODO_SIDE_PARK_HEADING);
 
-    public static final double RR_ODO_RIGHT_PARK_X = 58;
-    public static final Vector2d RR_ODO_RIGHT_PARK_VECTOR = new Vector2d(RR_ODO_RIGHT_PARK_X, RR_ODO_CONE_STACK_Y);
+    public static final double RR_ODO_RIGHT_PARK_X = 59.5;
+    public static final Pose2d RR_ODO_RIGHT_PARK_POSE = new Pose2d(RR_ODO_RIGHT_PARK_X, RR_ODO_CONE_STACK_Y - 2.5, RR_ODO_SIDE_PARK_HEADING);
     // END ODO CONSTANTS //
 
     /** ======= END CONSTANTS FOR RED RIGHT ======= **/

opmode/auton/right/FiveConeAuto.java

@@ -26,7 +26,6 @@ public class FiveConeAuto extends LinearOpMode {
     private Arm arm;
     private SlidesMotors slides;
     private SignalSleeveWebcam signalSleeveWebcam = new SignalSleeveWebcam(this, "rightWebcam", SignalSleeveWebcam.ROBOT_SIDE.CONTROL_HUB);
-    private ConeSensor coneSensor;
 
     private SignalSleeveWebcam.Side parkSide = SignalSleeveWebcam.Side.ONE;
 
@@ -106,10 +105,8 @@ public void runOpMode() throws InterruptedException {
         clamp = new Clamp(this);
         arm = new Arm(this);
         slides = new SlidesMotors(this);
-        coneSensor = new ConeSensor(this);
 
         signalSleeveWebcam.init(hardwareMap);
-        coneSensor.init(hardwareMap);
         clamp.init(hardwareMap);
         arm.init(hardwareMap);
         slides.init(hardwareMap);
@@ -179,7 +176,6 @@ public void runOpMode() throws InterruptedException {
 
         while (opModeIsActive() && !isStopRequested()) {
             telemetry.addData("cones scored", conesScored);
-            coneSensor.telemetry(telemetry);
             telemetry.update();
             drive.update();
             slides.update();

opmode/auton/right/midgoal/SixConeAuto.java

@@ -24,7 +24,6 @@ public class SixConeAuto extends LinearOpMode {
     private Arm arm;
     private SlidesMotors slides;
     private SignalSleeveWebcam signalSleeveWebcam = new SignalSleeveWebcam(this, "rightWebcam", SignalSleeveWebcam.ROBOT_SIDE.CONTROL_HUB);
-    private ConeSensor coneSensor;
 
     private SignalSleeveWebcam.Side parkSide = SignalSleeveWebcam.Side.ONE;