Four autos working

src/deploy/autos/paths/aside_home

@@ -1 +1 @@
-F,kRightSub
+P,277.15,37.5,55,0

src/deploy/autos/paths/aside_home_2

@@ -0,0 +1,2 @@
+P,385.15,-60.5,0,3
+P,354.15,-80.5,50,0

src/deploy/autos/paths/aside_piece_one

@@ -1 +1 @@
-P,274.5,94,0,0
+P,290.5,94,0,0

src/deploy/autos/paths/aside_piece_two

@@ -1 +1 @@
-P,292,324,0,0
+P,424,165,0,0

src/deploy/autos/paths/fp_center_run

@@ -1 +1 @@
-P,217.5,112.0,0,0
+P,214.5,112.0,0,0

src/deploy/autos/paths/sside_exit

@@ -0,0 +1,2 @@
+P,150.5,80.5,0,0
+P,150.5,115.5,0,0

src/deploy/autos/paths/sside_home

@@ -1 +1 @@
-F,kLeftSub
+P,205.15,36.75,-55,0

src/deploy/autos/paths/sside_piece_one

@@ -1 +1 @@
-P,160.5,94,0,0
+P,186.5,95.5,0,0

src/deploy/autos/points.lst

@@ -2,5 +2,5 @@ N,kOrigin,0,0,0,0
 N,kSpeaker,217.5,-4,0,0
 N,kAmp,0,0,90,0
 N,kPointBlank,217.5,49,0,0
-N,kRightSub,253.15,57.75,0,60
-N,kLeftSub,222.84,57.75,0,-60
+N,kRightSub,253.15,57.75,60,0
+N,kLeftSub,222.84,57.75,-60,0

src/deploy/autos/scripts/amp_side_auto

@@ -6,9 +6,4 @@ ACT,deploy_intake
 PATH,aside_piece_one
 ACT,a_prep_shoot
 PATH,aside_home
-ACT,shoot
-ACT,deploy_intake
-PATH,aside_piece_two
-ACT,a_prep_shoot
-PATH,aside_home
 ACT,shoot

src/deploy/autos/scripts/four_piece_auto

@@ -16,4 +16,5 @@ ACT,deploy_intake
 PATH,fp_piece_three
 ACT,a_prep_shoot
 PATH,fp_home_3
-ACT,shoot
+ACT,shoot
+PATH,fp_center_run

src/deploy/autos/scripts/source_side_auto

@@ -5,5 +5,5 @@ ACT,shoot
 ACT,deploy_intake
 PATH,sside_piece_one
 ACT,a_prep_shoot
-PATH,aside_home
+PATH,sside_home
 ACT,shoot

src/deploy/autos/scripts/source_side_mobility

@@ -0,0 +1,5 @@
+0,2
+F,kLeftSub
+ACT,prep_shoot
+ACT,shoot
+PATH,sside_exit

src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc

@@ -18,7 +18,7 @@ void FollowTrajectoryCommand::OnInit() {
                              container_.drivetrain_.max_acceleration_.value(),
                              container_.drivetrain_.max_deceleration_.value(),
                              container_.drivetrain_.max_speed_.value(),
-                             100_fps_sq, 100_fps_sq, 3_in, 20_ms});
+                             100_fps_sq, 100_fps_sq, 5_in, 20_ms});
 
   Log("Starting Trajectory");
   Log("Initial pose x{}, y{}, Bearing {}",
@@ -56,6 +56,9 @@ void FollowTrajectoryCommand::OnInit() {
   if (path_points_.size() < 2) {
     Error("Trajectory size ({}) is less than 2 - ending!", path_points_.size());
     is_done_ = true;
+  } else {
+    Log("First target point x{} y{}", path_points_[target_idx_].point[0],
+        path_points_[target_idx_].point[1]);
   }
 }
 

src/y2024/cpp/FunkyRobot.cc

@@ -31,6 +31,8 @@
 FunkyRobot::FunkyRobot() : GenericRobot{&container_} {}
 
 void FunkyRobot::OnInitialize() {
+  container_.leds_.SetDefaultCommand(LEDsCommand{container_});
+
   Field::Setup();
 
   for (auto x : Field::getAllAutoData()) {
@@ -74,6 +76,8 @@ void FunkyRobot::OnInitialize() {
 }
 
 void FunkyRobot::OnDisable() {
+  container_.leds_.SetDefaultCommand(LEDsCommand{container_});
+
   container_.pivot_.Brake();
   container_.wrist_.Brake();
   container_.telescope_.Brake();
@@ -102,6 +106,7 @@ void FunkyRobot::OnPeriodic() {
 
   if (!homing_switch_.Get()) {
     container_.super_structure_.ZeroSubsystem();
+    frc846::util::ShareTables::SetBoolean("zero sequence", true);
     Log("Zeroing subsystems...");
   }
 

src/y2024/cpp/commands/basic/auto_deploy_intake_command.cc

@@ -7,7 +7,7 @@ AutoDeployIntakeCommand::AutoDeployIntakeCommand(RobotContainer& container)
                    &container_.super_structure_});
 }
 
-void AutoDeployIntakeCommand::OnInit() {}
+void AutoDeployIntakeCommand::OnInit() { Periodic(); }
 
 void AutoDeployIntakeCommand::Periodic() {
   container_.intake_.SetTarget({kIntake});
@@ -18,7 +18,4 @@ void AutoDeployIntakeCommand::Periodic() {
 
 void AutoDeployIntakeCommand::OnEnd(bool interrupted) {}
 
-bool AutoDeployIntakeCommand::IsFinished() {
-  return container_.super_structure_.hasReachedSetpoint(
-      container_.super_structure_.getIntakeSetpoint());
-}
+bool AutoDeployIntakeCommand::IsFinished() { return true; }

src/y2024/cpp/commands/complex/home_during_auto_command.cc

@@ -14,12 +14,15 @@ HomeDuringAutoCommand::HomeDuringAutoCommand(RobotContainer& container)
     : frc846::robot::GenericCommandGroup<RobotContainer, HomeDuringAutoCommand,
                                          frc2::SequentialCommandGroup>{
           container, "home_during_auto_command",
-          frc2::SequentialCommandGroup{
-              frc2::ParallelDeadlineGroup{
-                  frc2::WaitUntilCommand{[&] {
-                    return container.pivot_.WithinTolerance(
-                        container.super_structure_.getStowSetpoint().pivot);
-                  }},
-                  StowCommand{container}},
-              WristZeroCommand{container},
-          }} {}
+          frc2::SequentialCommandGroup{frc2::ParallelDeadlineGroup{
+              frc2::WaitUntilCommand{[&] {
+                return container_.super_structure_.wrist_->GetHasZeroed();
+              }},
+              frc2::SequentialCommandGroup{
+                  frc2::ParallelDeadlineGroup{
+                      frc2::WaitUntilCommand{[&] {
+                        return container.pivot_.WithinTolerance(
+                            container.super_structure_.getStowSetpoint().pivot);
+                      }},
+                      StowCommand{container}},
+                  WristZeroCommand{container}}}}} {}

src/y2024/cpp/commands/teleop/leds_command.cc

@@ -17,7 +17,7 @@ void LEDsCommand::Periodic() {
     lstate = LEDsState::kLEDSAutonomous;
   } else if (frc846::util::ShareTables::GetBoolean("zero sequence")) {
     lstate = LEDsState::kLEDSZeroing;
-  } else if (container_.super_structure_.GetHasZeroed()) {
+  } else if (!container_.super_structure_.GetHasZeroed()) {
     lstate = LEDsState::kLEDSNotReady;
   } else if (frc846::util::ShareTables::GetString("mode") == "disabled") {
     lstate = LEDsState::kLEDSDisabled;

src/y2024/cpp/subsystems/abstract/control_input.cc

@@ -167,10 +167,9 @@ ControlInputReadings ControlInputSubsystem::UpdateWithInput() {
   // PASS
   ci_readings_.running_pass = op_readings.x_button;
 
-  // SHOOT
+  // SHOOTING
   ci_readings_.shooting =
-      ((ci_readings_.running_prep_shoot || ci_readings_.running_super_shoot ||
-        ci_readings_.running_pass) &&
+      ((ci_readings_.running_prep_shoot || ci_readings_.running_super_shoot) &&
        dr_readings.right_bumper) ||
       op_readings.pov == frc846::XboxPOV::kRight;
   // EJECT

src/y2024/cpp/subsystems/hardware/drivetrain.cc

@@ -227,7 +227,7 @@ DrivetrainReadings DrivetrainSubsystem::ReadFromHardware() {
   readings.pose = frc846::math::FieldPoint(odometry_.position(), bearing,
                                            unfiltered_velocity);
 
-    pose_x_graph_.Graph(odometry_.position()[0]);
+  pose_x_graph_.Graph(odometry_.position()[0]);
   pose_y_graph_.Graph(odometry_.position()[1]);
   // pose_bearing_graph.Graph(odometry_.position().bearing);
   v_x_graph_.Graph(readings.pose.velocity[0]);

src/y2024/include/subsystems/hardware/drivetrain.h

@@ -111,7 +111,7 @@ class DrivetrainSubsystem
 
   // Closed loop tuned for this
   frc846::ntinf::Pref<units::feet_per_second_t> auto_max_speed_{
-      *this, "auto_max_speed", 11.2_fps};
+      *this, "auto_speed", 11.2_fps};
 
   frc846::ntinf::Pref<double> driver_speed_multiplier_{
       *this, "driver_speed_multiplier", 1.0};

src/y2024/resources/preferences_backup.nform

@@ -13,7 +13,7 @@ Preferences/MotionTargets/Telescope/trapscore_position (in).double | 3.000000
 Preferences/MotionTargets/Wrist/amp_position (deg).double | 100.000000
 Preferences/MotionTargets/Wrist/auto_shoot_position (deg).double | 42.500000
 Preferences/MotionTargets/Wrist/intake_position (deg).double | 32.000000
-Preferences/MotionTargets/Wrist/pass_position (deg).double | 80.000000
+Preferences/MotionTargets/Wrist/pass_position (deg).double | 86.000000
 Preferences/MotionTargets/Wrist/preclimb_position (deg).double | -30.000000
 Preferences/MotionTargets/Wrist/prescore_position (deg).double | 40.000000
 Preferences/MotionTargets/Wrist/shoot_position (deg).double | 41.000000
@@ -27,7 +27,7 @@ SuperStructure/shooting/auto_shooter_height (in).double | 23.000000
 SuperStructure/shooting/auto_shooter_x (in).double | 15.000000
 SuperStructure/shooting/teleop_shooter_height (in).double | 23.650000
 SuperStructure/shooting/teleop_shooter_x (in).double | 25.000000
-drivetrain/auto_max_speed (fps).double | 13.000000
+drivetrain/auto_max_speed (fps).double | 3.000000
 drivetrain/bearing_gains/d.double | -13.500000
 drivetrain/bearing_gains/p.double | 2.000000
 drivetrain/drive_esc/Configs/CurrentLimiting/peak_time_threshold (ms).double | 1000.000000