Post CCC

.gitignore

@@ -1,6 +1,9 @@
 # This gitignore has been specially created by the WPILib team.
 # If you remove items from this file, intellisense might break.
 
+# NetworkTables backups
+/nt_backups
+
 #Developer Id
 *.id
 

build.gradle

@@ -112,16 +112,17 @@ model {
 
 task backupNetworkTables(type: Exec) {
     println "Backing up Networktables..."
-    def timestamp = new Date().format('yyyyMMdd_HHmm')
-    def nt_backup_file = "nt_backups/networktables_${timestamp}.json.bck"
-    def backupFile = "networktables_.json.bck"
+    def timestamp = new Date().format('MM_dd_HH_mm')
+    def nt_backup_file = "nt_backups/networktables_${timestamp}.json"
+    def backupFile = "networktables.json.bck"   
+    doFirst {
+        new File("nt_backups").mkdirs()
+    }
     if (System.getProperty('os.name').toLowerCase().contains('windows')) {
-        commandLine 'cmd', '/c', 'scp', "lvuser@10.8.46.2:/home/lvuser/networktables.json", "./${nt_backup_file}"
-        commandLine 'cmd', '/c', 'scp', "lvuser@10.8.46.2:/home/lvuser/networktables.json", "./${backupFile}"
+        commandLine 'cmd', '/c', "scp lvuser@10.8.46.2:/home/lvuser/networktables.json ./${nt_backup_file} && scp lvuser@10.8.46.2:/home/lvuser/networktables.json ./${backupFile}"
     }
     else {
-        commandLine 'sh', '-c', "scp lvuser@10.8.46.2:/home/lvuser/networktables.json ./${nt_backup_file}"
-        commandLine 'sh', '-c', "scp lvuser@10.8.46.2:/home/lvuser/networktables.json ./${backupFile}"
+        commandLine 'sh', '-c', "scp lvuser@10.8.46.2:/home/lvuser/networktables.json ./${nt_backup_file} && scp lvuser@10.8.46.2:/home/lvuser/networktables.json ./${backupFile}"
     }
     ignoreExitValue = true
 }

networktables.json

@@ -74,31 +74,31 @@
   {
     "name": "/Preferences/SwerveDrivetrain/FR/cancoder_offset_ (deg)",
     "type": "double",
-    "value": -73.740234375,
+    "value": -72.158203125,
     "properties": {
       "persistent": true
     }
   },
   {
     "name": "/Preferences/SwerveDrivetrain/FL/cancoder_offset_ (deg)",
     "type": "double",
-    "value": -128.3203125,
+    "value": -129.462890625,
     "properties": {
       "persistent": true
     }
   },
   {
     "name": "/Preferences/SwerveDrivetrain/BL/cancoder_offset_ (deg)",
     "type": "double",
-    "value": -78.92578125,
+    "value": -80.595703125,
     "properties": {
       "persistent": true
     }
   },
   {
     "name": "/Preferences/SwerveDrivetrain/BR/cancoder_offset_ (deg)",
     "type": "double",
-    "value": -78.310546875,
+    "value": -79.27734375,
     "properties": {
       "persistent": true
     }
@@ -275,7 +275,7 @@
   {
     "name": "/Preferences/SwerveDrivetrain/odom_fudge_factor",
     "type": "double",
-    "value": 0.98,
+    "value": 0.95,
     "properties": {
       "persistent": true
     }
@@ -1843,7 +1843,7 @@
   {
     "name": "/Preferences/coral_ss/setpoints/stow_net/height (in)",
     "type": "double",
-    "value": 40.0,
+    "value": 72.0,
     "properties": {
       "persistent": true
     }
@@ -2547,7 +2547,7 @@
   {
     "name": "/Preferences/algal_end_effector/backspin_constant (fps)",
     "type": "double",
-    "value": -55.0,
+    "value": -50.0,
     "properties": {
       "persistent": true
     }
@@ -2667,7 +2667,7 @@
   {
     "name": "/Preferences/algal_ss/score_dc (fps)",
     "type": "double",
-    "value": -10.0,
+    "value": -60.0,
     "properties": {
       "persistent": true
     }
@@ -2791,5 +2791,13 @@
     "properties": {
       "persistent": true
     }
+  },
+  {
+    "name": "/Preferences/algal_end_effector/max_eject (fps)",
+    "type": "double",
+    "value": -40.0,
+    "properties": {
+      "persistent": true
+    }
   }
 ]

networktables_.json.bck

@@ -2547,7 +2547,7 @@
   {
     "name": "/Preferences/algal_end_effector/backspin_constant (fps)",
     "type": "double",
-    "value": -55.0,
+    "value": -30.0,
     "properties": {
       "persistent": true
     }
@@ -2667,7 +2667,7 @@
   {
     "name": "/Preferences/algal_ss/score_dc (fps)",
     "type": "double",
-    "value": -10.0,
+    "value": -50.0,
     "properties": {
       "persistent": true
     }

src/y2025/cpp/commands/teleop/algal_command.cc

@@ -58,6 +58,9 @@ void AlgalCommand::Periodic() {
   else if (ci_readings.dec_a_wrist)
     container_.algal_ss_.adjustWrist(false);
 
+  algal_target.super_mode =
+      container_.control_input_.GetReadings().override_force;
+
   algal_target.cm = container_.control_input_.GetReadings().level_one;
 
   container_.algal_ss_.SetTarget(algal_target);

src/y2025/cpp/commands/teleop/coral_command.cc

@@ -15,7 +15,7 @@ void CoralCommand::Periodic() {
   if (container_.algal_ss_.algal_end_effector.GetReadings().has_piece_ &&
       container_.control_input_.GetReadings().coral_state ==
           kCoral_StowNoPiece) {
-    if (container_.coral_ss_.coral_end_effector.GetReadings().has_piece_ ||
+    if (container_.coral_ss_.coral_end_effector.GetReadings().has_piece_ &&
         (container_.control_input_.GetReadings().algal_state == kAlgae_Net &&
             container_.control_input_.GetReadings().position_algal))
       coral_target.state = kCoral_StowNet;

src/y2025/cpp/commands/teleop/drive_command.cc

@@ -106,16 +106,8 @@ void DriveCommand::Periodic() {
 
   if (ci_readings_.auto_align) {
     units::degree_t target_angle = 1000_deg;
-    if (ci_readings_.algal_state == kAlgae_Net &&
-        container_.algal_ss_.algal_end_effector.GetReadings().has_piece_) {
-      target_angle = isBlue ? 180_deg : 0_deg;
-    } else if (ci_readings_.algal_state == kAlgae_Processor &&
-               container_.algal_ss_.algal_end_effector.GetReadings()
-                   .has_piece_) {
-      target_angle = isBlue ? -90_deg : 90_deg;
-    } else if (ci_readings_.coral_state == kCoral_StowNoPiece &&
-               !container_.coral_ss_.coral_end_effector.GetReadings()
-                    .has_piece_) {
+    if (ci_readings_.coral_state == kCoral_StowNoPiece &&
+        !container_.coral_ss_.coral_end_effector.GetReadings().has_piece_) {
       if (container_.drivetrain_.GetReadings().estimated_pose.position[0] >
           (frc846::math::FieldPoint::field_size_x / 2))
         target_angle = -54_deg;

src/y2025/cpp/commands/teleop/drive_to_net_command.cc

@@ -27,7 +27,10 @@ std::pair<frc846::math::FieldPoint, bool> DriveToNetCommand::GetTargetPoint() {
       pos[0], frc846::math::FieldPoint::field_size_y / 2 - net_offest};
   frc846::math::FieldPoint target_pos =
       frc846::math::FieldPoint{net_pos, 0_deg, 0_fps};
-  if (pos[1] > frc846::math::FieldPoint::field_size_y / 2) {
+  if ((pos[1] > frc846::math::FieldPoint::field_size_y / 2) &&
+      units::math::abs(frc846::math::CoterminalDifference(
+          drivetrain_->GetReadings().estimated_pose.bearing, 0_deg)) >
+          150_deg) {
     target_pos = target_pos.mirrorOnlyY(true);
   }
 

src/y2025/cpp/subsystems/hardware/algal/algal_end_effector.cc

@@ -34,6 +34,9 @@ AlgalEESubsystem::AlgalEESubsystem()
   RegisterPreference("idle_speed_coral", -5.0_fps);
   RegisterPreference("coral_vel_thresh", 5_fps);
   RegisterPreference("test1", false);
+
+  RegisterPreference("max_eject", -40_fps);
+
   REGISTER_PIDF_CONFIG(0.0001, 0.0, 0.0, 0.0);
 
   RegisterPreference("testthing", 0.4);
@@ -107,8 +110,18 @@ void AlgalEESubsystem::WriteToHardware(AlgalEETarget target) {
   Graph("readings/vel1", esc_helper_1_.GetVelocity());
   Graph("readings/vel2", esc_helper_2_.GetVelocity());
 
-  esc_1_.SetGains(GET_PIDF_GAINS());
-  esc_2_.SetGains(GET_PIDF_GAINS());
+  frc846::control::base::MotorGains gains;
+  gains.kP = GetPreferenceValue_double("gains/_kP");
+  gains.kFF = GetPreferenceValue_double("gains/_kF");
+  gains.kD = 0.0;
+  gains.kI = 0.0;
+
+  pleasefixthis += 1;
+  if (pleasefixthis > 50) { gains.kFF += 0.000003; }
+  if (pleasefixthis == 100) { pleasefixthis = 0; }
+
+  esc_1_.SetGains(gains);
+  esc_2_.SetGains(gains);
 
   target.coral_keep &= target.cmode;
 
@@ -150,16 +163,21 @@ void AlgalEESubsystem::WriteToHardware(AlgalEETarget target) {
 
   if (target.cmode && has_coral_piece_) { target.velocity_ = 0_fps; }
 
-  if (units::math::abs(esc_helper_2_.GetVelocity()) <=
-          GetPreferenceValue_unit_type<units::feet_per_second_t>(
-              "piece_thresh") &&
-      target.velocity_ < 0.0_fps) {
-    target.velocity_ =
-        GetPreferenceValue_unit_type<units::feet_per_second_t>("kick_dc");
-  }
+  // if (units::math::abs(esc_helper_2_.GetVelocity()) <=
+  //         GetPreferenceValue_unit_type<units::feet_per_second_t>(
+  //             "piece_thresh") &&
+  //     target.velocity_ < 0.0_fps) {
+  //   target.velocity_ =
+  //       GetPreferenceValue_unit_type<units::feet_per_second_t>("kick_dc");
+  // }
 
   if (piece_override_) { target.velocity_ = 0.0_fps; }
-  if (target.use_back_spin_) {
+  if (target.super_mode) {
+    esc_helper_1_.WriteVelocityOnController(
+        GetPreferenceValue_unit_type<units::feet_per_second_t>("max_eject"));
+    esc_helper_2_.WriteVelocityOnController(
+        GetPreferenceValue_unit_type<units::feet_per_second_t>("max_eject"));
+  } else if (target.use_back_spin_) {
     esc_helper_1_.WriteVelocityOnController(
         target.velocity_ +
         GetPreferenceValue_unit_type<units::feet_per_second_t>(

src/y2025/cpp/subsystems/hardware/algal/algal_ss.cc

@@ -132,16 +132,27 @@ void AlgalSuperstructure::WriteToHardware(AlgalSSTarget target) {
 
   Graph("is_coral_mode", coral_mode);
 
+  if (target.super_mode) {
+    algal_end_effector.SetTarget(
+        {GetPreferenceValue_unit_type<units::feet_per_second_t>("score_dc"),
+            true, false, false, true});
+  }
+
   if (target.score) {
-    if (coral_mode) algal_end_effector.ClearHasPiece();
-    (coral_mode) ? algal_end_effector.SetTarget(
-                       {GetPreferenceValue_unit_type<units::feet_per_second_t>(
-                            "score_coral_vel"),
-                           false})
-                 : algal_end_effector.SetTarget(
-                       {GetPreferenceValue_unit_type<units::feet_per_second_t>(
-                            "score_dc"),
-                           true});
+    if (coral_mode) {
+      algal_end_effector.ClearHasPiece();
+      algal_end_effector.SetTarget(
+          {GetPreferenceValue_unit_type<units::feet_per_second_t>(
+               "score_coral_vel"),
+              false});
+    } else if (target.state == kAlgae_Processor) {
+      algal_end_effector.SetTarget(
+          {GetPreferenceValue_unit_type<units::feet_per_second_t>("score_dc")});
+    } else {
+      algal_end_effector.SetTarget(
+          {GetPreferenceValue_unit_type<units::feet_per_second_t>("score_dc"),
+              true, false, false, target.super_mode});
+    }
   } else if (coral_mode)
     algal_end_effector.SetTarget(
         {setpoint.ee_vel, false, target.cm, target.state != kAlgae_CoralPick});

src/y2025/cpp/subsystems/hardware/climber.cc

@@ -48,7 +48,12 @@ bool ClimberSubsystem::VerifyHardware() {
 ClimberReadings ClimberSubsystem::ReadFromHardware() {
   Graph("readings/position", esc_helper_.GetPosition());
 
-  return {};
+  return {units::math::abs(esc_helper_.GetPosition() -
+                           GetPreferenceValue_unit_type<units::degree_t>(
+                               "limits/upper_limit")) < 6_deg ||
+          units::math::abs(esc_helper_.GetPosition() -
+                           GetPreferenceValue_unit_type<units::degree_t>(
+                               "limits/lower_limit")) < 6_deg};
 }
 
 void ClimberSubsystem::WriteToHardware(ClimberTarget target) {

src/y2025/cpp/subsystems/hardware/leds.cc

@@ -70,6 +70,9 @@ void LEDsSubsystem::WriteToHardware(TLTGT target) {
     } else if (tgt->state == kLEDsTeleop) {
       SetStrip(ORANGE);
       Flash(SLOW_FLASH);
+    } else if (tgt->state == kLEDsClimbed) {
+      SetStrip(GREEN);
+      Flash(RAPID_FLASH);
     } else if (tgt->state == kLEDsClimbing) {
       SetStrip(LIGHT_BLUE);
       Flash(SLOW_FLASH);

src/y2025/cpp/subsystems/hardware/leds_logic.cc

@@ -68,6 +68,8 @@ void LEDsLogic::UpdateLEDs(RobotContainer* container) {
       target.state = kisLinedUp;
     else
       target.state = kLEDsSequencing;
+  } else if (container->climber_.GetReadings().reached_limit_) {
+    target.state = kLEDsClimbed;
   } else if (container->control_input_.GetReadings().extend_climb ||
              container->control_input_.GetReadings().retract_climb) {
     target.state = kLEDsClimbing;

src/y2025/include/subsystems/hardware/algal/algal_end_effector.h

@@ -22,6 +22,7 @@ struct AlgalEETarget {
   bool use_back_spin_ = false;
   bool cmode = false;
   bool coral_keep = false;
+  bool super_mode = false;
 };
 
 using roller_pos_conv_t = units::unit_t<
@@ -68,6 +69,8 @@ class AlgalEESubsystem
 
   double counter_ = 0;
 
+  int pleasefixthis = 0;
+
   AlgalEEReadings ReadFromHardware() override;
 
   void WriteToHardware(AlgalEETarget target) override;

src/y2025/include/subsystems/hardware/algal/algal_ss.h

@@ -33,6 +33,7 @@ struct AlgalSSTarget {
   AlgalStates state;
   bool score;
   bool cm = false;
+  bool super_mode = false;
 };
 
 class AlgalSuperstructure

src/y2025/include/subsystems/hardware/climber.h

@@ -13,7 +13,9 @@
 #include "frc846/control/HigherMotorController.h"
 #include "frc846/robot/GenericSubsystem.h"
 
-struct ClimberReadings {};
+struct ClimberReadings {
+  bool reached_limit_;
+};
 
 struct ClimberTarget {
   double duty_cycle_;

src/y2025/include/subsystems/hardware/leds.h

@@ -14,6 +14,7 @@ enum LEDsState {
   kLEDsSequencing,
   kLEDsTeleop,
   kLEDsClimbing,
+  kLEDsClimbed,
   kLEDsHavePiece,
   kisLinedUp,
   kisCompletelyLinedUp,