fixed robot starting state (#278)

Co-authored-by: FRCTeam3255-Shared <170778697+FRCTeam3255-Shared@users.noreply.github.com>
FRCTeam3255 · Feb 11, 2025 · 5401ec4 · 5401ec4
1 parent 108adb5
commit 5401ec4
Show file tree

Hide file tree

Showing 8 changed files with 34 additions and 13 deletions.
diff --git a/src/main/deploy/pathplanner/paths/P-F.path b/src/main/deploy/pathplanner/paths/P-F.path
@@ -37,6 +37,12 @@
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [
+    {
+      "name": "GetCoralStationPiece",
+      "waypointRelativePos": 0.0,
+      "endWaypointRelativePos": null,
+      "command": null
+    },
     {
       "name": "PrepPlace",
       "waypointRelativePos": 0.6953086419753075,

diff --git a/src/main/deploy/pathplanner/paths/P-I.path b/src/main/deploy/pathplanner/paths/P-I.path
@@ -8,8 +8,8 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.022485930490561,
-        "y": 5.861963767575542
+        "x": 6.821792763157894,
+        "y": 6.0359375
       },
       "isLocked": false,
       "linkedName": null
@@ -32,6 +32,12 @@
   "constraintZones": [],
   "pointTowardsZones": [],
   "eventMarkers": [
+    {
+      "name": "GetCoralStationPiece",
+      "waypointRelativePos": 0,
+      "endWaypointRelativePos": null,
+      "command": null
+    },
     {
       "name": "PrepPlace",
       "waypointRelativePos": 0.8149253731343287,

diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
@@ -16,8 +16,8 @@
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 115.0,
-  "robotMOI": 6.883,
+  "robotMass": 143.0,
+  "robotMOI": 6.0,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.049,
   "driveGearing": 6.75,

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -227,9 +227,9 @@ public static class AUTO {
           constDrivetrain.AUTO.AUTO_STEER_I,
           constDrivetrain.AUTO.AUTO_STEER_D);
 
-      public static final double MASS = 115;
+      public static final double MASS = 143;
       // TODO: Calcuate the real vaule
-      public static final double MOI = 6.8;
+      public static final double MOI = 6.0;
       public static final double WHEEL_COF = 1.0;
       public static final DCMotor DRIVE_MOTOR = DCMotor.getKrakenX60(1);
       public static final ModuleConfig MODULE_CONFIG = new ModuleConfig(WHEEL_RADIUS, OBSERVED_DRIVE_SPEED, WHEEL_COF,

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -78,6 +78,7 @@ public void disabledPeriodic() {
   @Override
   public void disabledExit() {
     m_robotContainer.checkForManualZeroing().cancel();
+    m_robotContainer.checkForCoral();
   }
 
   @Override

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -51,7 +51,7 @@ public class RobotContainer {
   private final StateMachine subStateMachine = new StateMachine(subAlgaeIntake, subClimber, subCoralOuttake,
       subDrivetrain, subElevator, subHopper, subLED, conOperator);
   private final IntakeCoralHopper comIntakeCoralHopper = new IntakeCoralHopper(subStateMachine, subHopper,
-      subCoralOuttake, subLED, subElevator);
+      subCoralOuttake, subLED, subElevator, subAlgaeIntake);
   private final ClimberDeploying comClimb = new ClimberDeploying(subStateMachine, subClimber, subElevator,
       subAlgaeIntake, subLED);
   private final ScoringCoral comScoringCoral = new ScoringCoral(subCoralOuttake, subStateMachine, subElevator, subLED,
@@ -156,10 +156,7 @@ public RobotContainer() {
 
     subDrivetrain.resetModulesToAbsolute();
 
-    if (subCoralOuttake.sensorSeesCoral()) {
-      subStateMachine.setRobotState(RobotState.HAS_CORAL);
-      subCoralOuttake.setHasCoral(true);
-    }
+    checkForCoral();
   }
 
   public void setMegaTag2(boolean setMegaTag2) {
@@ -380,4 +377,11 @@ public boolean allZeroed() {
   public static boolean isPracticeBot() {
     return !isPracticeBot.get();
   }
+
+  public void checkForCoral() {
+    if (subCoralOuttake.sensorSeesCoral()) {
+      subStateMachine.setRobotState(RobotState.HAS_CORAL);
+      subCoralOuttake.setHasCoral(true);
+    }
+  }
 }
diff --git a/src/main/java/frc/robot/commands/states/IntakeCoralHopper.java b/src/main/java/frc/robot/commands/states/IntakeCoralHopper.java
@@ -16,24 +16,27 @@ public class IntakeCoralHopper extends Command {
   CoralOuttake globalCoralOuttake;
   Elevator globalElevator;
   LED globalLED;
+  AlgaeIntake globalAlgaeIntake;
   boolean hasSeenCoral = false;
 
   /** Creates a new IntakeCoralHopper. */
   public IntakeCoralHopper(StateMachine subStateMachine, Hopper subHopper, CoralOuttake subCoralOuttake,
-      LED subLED, Elevator subElevator) {
+      LED subLED, Elevator subElevator, AlgaeIntake subAlgaeIntake) {
     // Use addRequirements() here to declare subsystem dependencies.
     globalStateMachine = subStateMachine;
     this.globalHopper = subHopper;
     this.globalCoralOuttake = subCoralOuttake;
     globalLED = subLED;
     globalElevator = subElevator;
+    globalAlgaeIntake = subAlgaeIntake;
     addRequirements(globalStateMachine);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     globalStateMachine.setRobotState(StateMachine.RobotState.INTAKING_CORAL_HOPPER);
+    globalAlgaeIntake.setAlgaePivotAngle(Constants.constAlgaeIntake.PREP_ALGAE_ZERO_PIVOT_POSITION);
     globalCoralOuttake.setCoralOuttake(Constants.constCoralOuttake.CORAL_INTAKE_SPEED);
     globalHopper.runHopper(Constants.constHopper.HOPPER_SPEED);
     globalLED.setLED(constLED.LED_INTAKE_CORAL_HOPPER);

diff --git a/src/main/java/frc/robot/subsystems/StateMachine.java b/src/main/java/frc/robot/subsystems/StateMachine.java
@@ -90,7 +90,8 @@ public Command tryState(RobotState desiredState) {
       case INTAKING_CORAL_HOPPER:
         switch (currentRobotState) {
           case NONE:
-            return new IntakeCoralHopper(subStateMachine, subHopper, subCoralOuttake, subLED, subElevator);
+            return new IntakeCoralHopper(subStateMachine, subHopper, subCoralOuttake, subLED, subElevator,
+                subAlgaeIntake);
         }
         break;