Autons

.DataLogTool/datalogtool.json

@@ -1,6 +1,6 @@
 {
   "download": {
     "localDir": "C:\\Users\\Deep Blue\\Downloads",
-    "serverTeam": "199"
+    "serverTeam": "10.1.99.2"
   }
 }

.vscode/settings.json

@@ -30,6 +30,7 @@
   "java.test.defaultConfig": "WPIlibUnitTests",
   "wpilib.skipTests": true,
   "java.debug.settings.onBuildFailureProceed": true,
+  "java.format.settings.url": "https://raw.githubusercontent.com/wpilibsuite/styleguide/main/ide/eclipse-java-google-style.xml",
   "editor.formatOnSave": true,
   "editor.formatOnSaveMode": "modificationsIfAvailable"
 }

src/main/java/org/carlmontrobotics/Constants.java

@@ -59,7 +59,7 @@ public static final class Effectorc {
 		public static final double[] kV = { 0.122, 0/* 0.065239, 0.077913 */ };
 		public static final double[] kA = { 0, 0/* 0.0062809,0.05289 */ };
 		public static final int INTAKE_PORT = 9; // port
-		public static final int OUTAKE_PORT = 10; // port
+		public static final int OUTTAKE_PORT = 10; // port
 		public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port
 		public static final int OUTAKE_DISTANCE_SENSOR_PORT = 10; // port
 		public static final double DISTANCE_BETWEEN_SENSORS_INCHES = 8.189; // inches
@@ -371,32 +371,39 @@ public static final class Autoc {
 					2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the
 									// angular constraints have no effect.
 		}
-		// #endregion
 	}
 
 	public static final class Limelightc {
 		public static final String INTAKE_LL_NAME = "limelight-intake";
 		public static final String SHOOTER_LL_NAME = "limelight-shooter";
 
-		public static final double ERROR_TOLERANCE_RAD = 0.1;
-		public static final double HORIZONTAL_FOV_DEG = 0;
-		public static final double RESOLUTION_WIDTH_PIX = 640;
-		public static final double MOUNT_ANGLE_DEG_SHOOTER = 42.5; 
-		public static final double MOUNT_ANGLE_DEG_INTAKE = -22; // 23.228
-		public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(11.5); // 16.6
-		public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52); // 16.6
-		public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115;
-		public static final double NOTE_HEIGHT = Units.inchesToMeters(0);
+		public static final int[] VALID_IDS = { 4, 7 };
+
+		public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field
+		public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field
+		public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high
+																// to represent unreliable heading
+		public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity
+
+		public static final double ERROR_TOLERANCE_RAD = 0.1; // unused
+		public static final double HORIZONTAL_FOV_DEG = 0; // unused
+		public static final double RESOLUTION_WIDTH_PIX = 640; // unused
+		public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446;
+		public static final double MOUNT_ANGLE_DEG_INTAKE = -29;
+		public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891);
+		public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = Units.inchesToMeters(52);
+		public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused
+		public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused
 		public static final double MIN_MOVEMENT_METERSPSEC = 0.5;
 		public static final double MIN_MOVEMENT_RADSPSEC = 0.5;
 		public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675);
-		public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(19.5);
+		public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911);
 		public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75);
-		public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65);
+		public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused
 		public static final class Apriltag {
 			public static final int RED_SPEAKER_CENTER_TAG_ID = 4;
 			public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7;
-			public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(56.7); //88.125
+			public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125);
 			public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26);
 			public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875);
 		}

src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java

@@ -6,9 +6,6 @@
 
 import org.carlmontrobotics.subsystems.Arm;
 import org.carlmontrobotics.subsystems.Limelight;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class AimArmSpeaker extends Command {
@@ -41,6 +38,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return arm.armAtSetpoint();
   }
 }

src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.carlmontrobotics.commands;
+
+import org.carlmontrobotics.subsystems.Arm;
+import org.carlmontrobotics.subsystems.Limelight;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AimArmSpeakerMT2 extends Command {
+    private final Arm arm;
+    private final Limelight ll;
+
+    /** Creates a new AimOuttakeSpeaker. */
+    public AimArmSpeakerMT2(Arm arm, Limelight ll) {
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(this.arm = arm);
+        this.ll = ll;
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        double goal = ll.getOptimizedArmAngleRadsMT2();
+        arm.setArmTarget(goal);
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return arm.armAtSetpoint();
+    }
+}

src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java

@@ -5,11 +5,9 @@
 package org.carlmontrobotics.commands;
 
 import static org.carlmontrobotics.Constants.Drivetrainc.*;
-import static org.carlmontrobotics.Constants.Limelightc.*;
 
 import org.carlmontrobotics.subsystems.Drivetrain;
 import org.carlmontrobotics.subsystems.Limelight;
-import org.carlmontrobotics.subsystems.LimelightHelpers;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
@@ -33,7 +31,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) {
 
         rotationPID.enableContinuousInput(-180, 180);
         Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
-                .minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg()));
+                .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
         rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
         rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
         SendableRegistry.addChild(this, rotationPID);
@@ -43,7 +41,7 @@ public AlignToApriltag(Drivetrain drivetrain, Limelight limelight) {
     @Override
     public void execute() {
         Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
-                .minus(Rotation2d.fromDegrees(limelight.getRotateAngleDeg()));
+                .minus(Rotation2d.fromRadians(limelight.getRotateAngleRad()));
         rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
         if (teleopDrive == null)
             drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));

src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.carlmontrobotics.commands;
+
+import static org.carlmontrobotics.Constants.Drivetrainc.*;
+
+import org.carlmontrobotics.subsystems.Drivetrain;
+import org.carlmontrobotics.subsystems.Limelight;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AlignToApriltagMegatag2 extends Command {
+
+    public final TeleopDrive teleopDrive;
+    public final Drivetrain drivetrain;
+    private Limelight limelight;
+
+    public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1],
+            thetaPIDController[2]);
+
+    public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) {
+        this.limelight = limelight;
+        this.drivetrain = drivetrain;
+        this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand();
+
+        rotationPID.enableContinuousInput(-180, 180);
+        Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
+                .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2()));
+        rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
+        rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
+        SendableRegistry.addChild(this, rotationPID);
+        addRequirements(drivetrain);
+    }
+
+    @Override
+    public void execute() {
+        Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
+                .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2()));
+        rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
+        if (teleopDrive == null)
+            drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));
+        else {
+            double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds();
+            drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1],
+                    rotationPID.calculate(drivetrain.getHeading()));
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+        // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint());
+        // SmartDashboard.putNumber("Error", rotationPID.getPositionError());
+        // return rotationPID.atSetpoint();
+    }
+}

src/main/java/org/carlmontrobotics/commands/IntakeNEO.java

@@ -10,51 +10,37 @@
 
 public class IntakeNEO extends Command {
   // intake until sees game peice or 4sec has passed
-  private Timer timer = new Timer();
   private final IntakeShooter intake;
-  double increaseAmount = 0.05;
-  int index = 0;
-  public int speed;
 
   public IntakeNEO(IntakeShooter intake) {
     addRequirements(this.intake = intake);
-    SmartDashboard.putNumber("Intake RPM", speed);
-
   }
 
   @Override
   public void initialize() {
-    //TODO: Adjust speed or add in an index
-    timer.reset();
+    // TODO: Adjust speed or add in an index;
     // if (intake.intakeDetectsNote()) {
     // return;
     // }
-    intake.motorSetIntake(SmartDashboard.getNumber("Intake RPM", speed));
+    intake.motorSetIntake(.5); // Fast intake speed for initial intake
 
     intake.resetCurrentLimit();
-    index=0; 
 
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    intake.motorSetIntake(SmartDashboard.getNumber("Intake RPM", speed));
     // Intake Led
     if((intake.intakeDetectsNote())) {
-      timer.start();
-    } else {
-      timer.stop();
-      timer.reset();
+      intake.motorSetIntake(.1); // Slower intake speed triggered after intake ds sees note
     }
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
     intake.stopIntake();
-    timer.stop();
-    index = 0;
     //intake.resetCurrentLimit();
   }
 
@@ -63,6 +49,6 @@ public void end(boolean interrupted) {
   public boolean isFinished() {
     // return intake.intakeDetectsNote() && timer.get()>0.1;
     // || //timer.hasElapsed(MAX_SECONDS_OVERLOAD);
-    return intake.intakeDetectsNote();
+    return intake.outtakeDetectsNote();
   }
 }

src/main/java/org/carlmontrobotics/subsystems/Arm.java

@@ -175,6 +175,9 @@ public void setBooleanDrive(boolean climb) {
 
     @Override
     public void periodic() {
+
+        SmartDashboard.putNumber("arm angle", getArmPos()); // for limelight testing
+
         babyMode = SmartDashboard.getBoolean("babymode", false);
 
 

src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java

@@ -1004,5 +1004,8 @@ public void keepRotateMotorsAtDegrees(int angle) {
         }
     }
 
+    public double getGyroRate() {
+        return gyro.getRate();
+    }
     // #endregion
 }

src/main/java/org/carlmontrobotics/subsystems/IntakeShooter.java

@@ -27,7 +27,8 @@ public class IntakeShooter extends SubsystemBase {
     private final CANSparkMax intakeMotor = MotorControllerFactory.createSparkMax(INTAKE_PORT, MotorConfig.NEO);
     // private final CANSparkMax outakeMotor =
     // MotorControllerFactory.createSparkMax(10, MotorConfig.NEO_550);
-    private final CANSparkFlex outtakeMotorVortex = new CANSparkFlex(10, MotorType.kBrushless);
+    private final CANSparkFlex outtakeMotorVortex =
+            new CANSparkFlex(OUTTAKE_PORT, MotorType.kBrushless);
     private final RelativeEncoder outtakeEncoder = outtakeMotorVortex.getEncoder();
     private final RelativeEncoder intakeEncoder = intakeMotor.getEncoder();
     private final SparkPIDController pidControllerOutake = outtakeMotorVortex.getPIDController();
@@ -36,7 +37,6 @@ public class IntakeShooter extends SubsystemBase {
     private Timer intakeTOFTimer = new Timer();
     private Timer outtakeTOFTimer = new Timer();
     private int count = 0;
-    private StringLogEntry tofLogEntry;
     private SimpleMotorFeedforward intakeFeedforward = new SimpleMotorFeedforward(kS[INTAKE], kV[INTAKE],
             kA[INTAKE]);
     private final SimpleMotorFeedforward outtakeFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE],
@@ -61,8 +61,8 @@ public IntakeShooter() {
         pidControllerIntake.setI(kI[INTAKE]);
         pidControllerIntake.setD(kD[INTAKE]);
         SmartDashboard.putData("Intake Shooter", this);
-        SmartDashboard.putNumber("Intake Ks", kS[INTAKE]);
-        SmartDashboard.putNumber("Intake Kv", kV[INTAKE]);
+        // SmartDashboard.putNumber("Intake Ks", kS[INTAKE]);
+        // SmartDashboard.putNumber("Intake Kv", kV[INTAKE]);
         intakeEncoder.setAverageDepth(4);
         intakeEncoder.setMeasurementPeriod(8);
         // SmartDashboard.putNumber("intake volts", 0);
@@ -71,7 +71,8 @@ public IntakeShooter() {
         intakeDistanceSensor.setRangingMode(RangingMode.Short, 24);// 24 ms is the minimum sample time acc to docs
         outtakeDistanceSensor.setRangingMode(RangingMode.Short, 24);
         outtakeMotorVortex.setSmartCurrentLimit(60);
-        SmartDashboard.putNumber("Intake target RPM", 0);
+        // SmartDashboard.putNumber("Intake target RPM", 0);
+        // SmartDashboard.putNumber("Vortex volts", 0);
     }
 
     public boolean intakeIsOverTemp() {
@@ -141,12 +142,12 @@ public void updateValues() {
     @Override
     public void periodic() {
         updateValues();
-        double newKS = SmartDashboard.getNumber("Intake Ks", kS[INTAKE]);
-        double newKV = SmartDashboard.getNumber("Intake Kv", kV[INTAKE]);
+        // double newKS = SmartDashboard.getNumber("Intake Ks", kS[INTAKE]);
+        /// double newKV = SmartDashboard.getNumber("Intake Kv", kV[INTAKE]);
 
-        if (newKS != intakeFeedforward.ks || newKV != intakeFeedforward.kv) {
-            intakeFeedforward = new SimpleMotorFeedforward(newKS, newKV);
-        }
+        // if (newKS != intakeFeedforward.ks || newKV != intakeFeedforward.kv) {
+        // intakeFeedforward = new SimpleMotorFeedforward(newKS, newKV);
+        // }
         SmartDashboard.putBoolean("instake ds sees", intakeDetectsNote());
         SmartDashboard.putBoolean("outtake ds sees", outtakeDetectsNote());
         SmartDashboard.putNumber("intake sample rate", intakeDistanceSensor.getSampleTime());
@@ -174,10 +175,6 @@ else if (rangingModeOuttake == TimeOfFlight.RangingMode.Medium)
         // outakeMotor.set(SmartDashboard.getNumber("intake volts", 0));
 
         // count++;
-
-        // double volts = SmartDashboard.getNumber("Vortex volts", 0);
-        // outakeMotorVortex.set(volts);
-
         // setMaxOutake();
 
         SmartDashboard.putNumber("Intake amps", intakeMotor.getOutputCurrent());