diff --git a/src/deploy/autos/paths/aside_home b/src/deploy/autos/paths/aside_home index cddce1b..f4dc4d5 100644 --- a/src/deploy/autos/paths/aside_home +++ b/src/deploy/autos/paths/aside_home @@ -1 +1 @@ -F,kRightSub \ No newline at end of file +P,277.15,37.5,55,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_home_2 b/src/deploy/autos/paths/aside_home_2 new file mode 100644 index 0000000..2af41d9 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_piece_one b/src/deploy/autos/paths/aside_piece_one index 24d8961..e83b5e5 100644 --- a/src/deploy/autos/paths/aside_piece_one +++ b/src/deploy/autos/paths/aside_piece_one @@ -1 +1 @@ -P,274.5,94,0,0 \ No newline at end of file +P,290.5,94,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/aside_piece_two b/src/deploy/autos/paths/aside_piece_two index 1defe69..b12764d 100644 --- a/src/deploy/autos/paths/aside_piece_two +++ b/src/deploy/autos/paths/aside_piece_two @@ -1 +1 @@ -P,292,324,0,0 \ No newline at end of file +P,424,165,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/fp_center_run b/src/deploy/autos/paths/fp_center_run index 4fa6c1b..1871b90 100644 --- a/src/deploy/autos/paths/fp_center_run +++ b/src/deploy/autos/paths/fp_center_run @@ -1 +1 @@ -P,217.5,112.0,0,0 \ No newline at end of file +P,214.5,112.0,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_exit b/src/deploy/autos/paths/sside_exit new file mode 100644 index 0000000..8db1591 --- /dev/null +++ b/src/deploy/autos/paths/sside_exit @@ -0,0 +1,2 @@ +P,150.5,80.5,0,0 +P,150.5,115.5,0,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_home b/src/deploy/autos/paths/sside_home index ea4e02b..0631a84 100644 --- a/src/deploy/autos/paths/sside_home +++ b/src/deploy/autos/paths/sside_home @@ -1 +1 @@ -F,kLeftSub \ No newline at end of file +P,205.15,36.75,-55,0 \ No newline at end of file diff --git a/src/deploy/autos/paths/sside_piece_one b/src/deploy/autos/paths/sside_piece_one index 19d8909..4b23bfd 100644 --- a/src/deploy/autos/paths/sside_piece_one +++ b/src/deploy/autos/paths/sside_piece_one @@ -1 +1 @@ -P,160.5,94,0,0 \ No newline at end of file +P,186.5,95.5,0,0 \ No newline at end of file diff --git a/src/deploy/autos/points.lst b/src/deploy/autos/points.lst index 3125578..8824d81 100644 --- a/src/deploy/autos/points.lst +++ b/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 \ No newline at end of file +N,kRightSub,253.15,57.75,60,0 +N,kLeftSub,222.84,57.75,-60,0 \ No newline at end of file diff --git a/src/deploy/autos/scripts/amp_side_auto b/src/deploy/autos/scripts/amp_side_auto index 58abade..0779e23 100644 --- a/src/deploy/autos/scripts/amp_side_auto +++ b/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 \ No newline at end of file diff --git a/src/deploy/autos/scripts/four_piece_auto b/src/deploy/autos/scripts/four_piece_auto index c2c44c5..99421ec 100644 --- a/src/deploy/autos/scripts/four_piece_auto +++ b/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 \ No newline at end of file +ACT,shoot +PATH,fp_center_run \ No newline at end of file diff --git a/src/deploy/autos/scripts/source_side_auto b/src/deploy/autos/scripts/source_side_auto index e5fbe3e..0819bd5 100644 --- a/src/deploy/autos/scripts/source_side_auto +++ b/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 \ No newline at end of file diff --git a/src/deploy/autos/scripts/source_side_mobility b/src/deploy/autos/scripts/source_side_mobility new file mode 100644 index 0000000..ed99f1d --- /dev/null +++ b/src/deploy/autos/scripts/source_side_mobility @@ -0,0 +1,5 @@ +0,2 +F,kLeftSub +ACT,prep_shoot +ACT,shoot +PATH,sside_exit \ No newline at end of file diff --git a/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc b/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc index 27b14a7..b372b8a 100644 --- a/src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc +++ b/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]); } } diff --git a/src/y2024/cpp/FunkyRobot.cc b/src/y2024/cpp/FunkyRobot.cc index fbaf3be..211480f 100644 --- a/src/y2024/cpp/FunkyRobot.cc +++ b/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..."); } diff --git a/src/y2024/cpp/commands/basic/auto_deploy_intake_command.cc b/src/y2024/cpp/commands/basic/auto_deploy_intake_command.cc index 2b90295..ac41465 100644 --- a/src/y2024/cpp/commands/basic/auto_deploy_intake_command.cc +++ b/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()); -} \ No newline at end of file +bool AutoDeployIntakeCommand::IsFinished() { return true; } \ No newline at end of file diff --git a/src/y2024/cpp/commands/complex/home_during_auto_command.cc b/src/y2024/cpp/commands/complex/home_during_auto_command.cc index 1382317..f25ca0f 100644 --- a/src/y2024/cpp/commands/complex/home_during_auto_command.cc +++ b/src/y2024/cpp/commands/complex/home_during_auto_command.cc @@ -14,12 +14,15 @@ HomeDuringAutoCommand::HomeDuringAutoCommand(RobotContainer& container) : frc846::robot::GenericCommandGroup{ container, "home_during_auto_command", - frc2::SequentialCommandGroup{ - frc2::ParallelDeadlineGroup{ - frc2::WaitUntilCommand{[&] { - return container.pivot_.WithinTolerance( - container.super_structure_.getStowSetpoint().pivot); - }}, - StowCommand{container}}, - WristZeroCommand{container}, - }} {} \ No newline at end of file + 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}}}}} {} \ No newline at end of file diff --git a/src/y2024/cpp/commands/teleop/leds_command.cc b/src/y2024/cpp/commands/teleop/leds_command.cc index e5792a3..b913e5f 100644 --- a/src/y2024/cpp/commands/teleop/leds_command.cc +++ b/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; diff --git a/src/y2024/cpp/subsystems/abstract/control_input.cc b/src/y2024/cpp/subsystems/abstract/control_input.cc index 81dda79..18c14b0 100644 --- a/src/y2024/cpp/subsystems/abstract/control_input.cc +++ b/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 diff --git a/src/y2024/cpp/subsystems/hardware/drivetrain.cc b/src/y2024/cpp/subsystems/hardware/drivetrain.cc index 5b079d9..b846886 100644 --- a/src/y2024/cpp/subsystems/hardware/drivetrain.cc +++ b/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]); diff --git a/src/y2024/include/subsystems/hardware/drivetrain.h b/src/y2024/include/subsystems/hardware/drivetrain.h index 39c8a6a..a879584 100644 --- a/src/y2024/include/subsystems/hardware/drivetrain.h +++ b/src/y2024/include/subsystems/hardware/drivetrain.h @@ -111,7 +111,7 @@ class DrivetrainSubsystem // Closed loop tuned for this frc846::ntinf::Pref auto_max_speed_{ - *this, "auto_max_speed", 11.2_fps}; + *this, "auto_speed", 11.2_fps}; frc846::ntinf::Pref driver_speed_multiplier_{ *this, "driver_speed_multiplier", 1.0}; diff --git a/src/y2024/resources/preferences_backup.nform b/src/y2024/resources/preferences_backup.nform index ea3997d..e56c2e2 100644 --- a/src/y2024/resources/preferences_backup.nform +++ b/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