Competition 2025

.vscode/tasks.json

@@ -68,7 +68,20 @@
                 "cwd": "${workspaceRoot}/PolySTAR-Taproot-project"
             }
         },
-                {
+        {
+            "label": ".Build Hero",
+            "type": "shell",
+            "presentation": {
+                "panel": "new"
+            },
+            "command": "pipenv run scons build robot=TARGET_HERO",
+            "group": "build",
+            "problemMatcher": [],
+            "options": {
+                "cwd": "${workspaceRoot}/PolySTAR-Taproot-project"
+            }
+        },
+        {
             "label": ".Program ICRA",
             "type": "shell",
             "presentation": {
@@ -120,6 +133,19 @@
                 "cwd": "${workspaceRoot}/PolySTAR-Taproot-project"
             }
         },
+        {
+            "label": ".Program Hero",
+            "type": "shell",
+            "presentation": {
+                "panel": "new"
+            },
+            "command": "pipenv run scons run robot=TARGET_HERO",
+            "group": "build",
+            "problemMatcher": [],
+            "options": {
+                "cwd": "${workspaceRoot}/PolySTAR-Taproot-project"
+            }
+        },
         {
             "label": "Program - Release",
             "type": "shell",

PolySTAR-Taproot-project/src/control/hero/hero_control.cpp

@@ -0,0 +1,145 @@
+#ifdef TARGET_HERO
+
+#include "tap/control/command_mapper.hpp"
+#include "tap/control/hold_command_mapping.hpp"
+#include "tap/control/hold_repeat_command_mapping.hpp"
+#include "tap/control/toggle_command_mapping.hpp"
+#include "control/safe_disconnect.hpp"
+
+// Chassis includes
+#include "subsystems/chassis/chassis_subsystem.hpp"
+#include "subsystems/chassis/chassis_drive_command.hpp"
+#include "subsystems/chassis/chassis_keyboard_drive_command.hpp"
+#include "subsystems/chassis/chassis_calibrate_IMU_command.hpp"
+
+// Turret includes
+#include "subsystems/turret/turret_subsystem.hpp"
+#include "subsystems/turret/turret_manual_aim_command.hpp"
+#include "subsystems/turret/turret_mouse_aim_command.hpp"
+#include "subsystems/turret/turret_test_bottomleft_command.hpp"
+#include "subsystems/turret/turret_test_topright_command.hpp"
+
+// Feeder includes
+#include "subsystems/feeder/feeder_position_subsystem.hpp"
+#include "subsystems/feeder/feeder_move_unjam_command.hpp"
+#include "subsystems/feeder/feeder_move_command.hpp"
+
+//Flywheel includes
+#include "subsystems/flywheel/flywheel_dji_subsystem.hpp"
+#include "subsystems/flywheel/flywheel_fire_dji_command.hpp"
+
+// Motor includes
+#include "control/motor_control.hpp"
+
+using src::control::RemoteSafeDisconnectFunction;
+using tap::communication::serial::Remote;
+using tap::control::CommandMapper;
+using tap::control::HoldCommandMapping;
+using tap::control::HoldRepeatCommandMapping;
+using tap::control::ToggleCommandMapping;
+using tap::control::RemoteMapState;
+
+/*
+ * NOTE: We are using the DoNotUse_getDrivers() function here
+ *      because this file defines all subsystems and command
+ *      and thus we must pass in the single statically allocated
+ *      Drivers class to all of these objects.
+ */
+
+namespace control
+{
+/* define subsystems --------------------------------------------------------*/
+chassis::ChassisSubsystem theChassis(drivers());
+turret::TurretSubsystem theTurret(drivers(), &yawMotor);
+feeder::FeederPositionSubsystem theFeeder(drivers());
+flywheel::FlywheelDjiSubsystem theFlywheel(drivers());
+
+/* define commands ----------------------------------------------------------*/
+chassis::ChassisDriveCommand chassisDrive(&theChassis, drivers());
+chassis::ChassisKeyboardDriveCommand chassisKeyboardDrive(&theChassis, drivers());
+chassis::ChassisCalibrateImuCommand chassisImuCalibrate(&theChassis, drivers());
+
+turret::TurretManualAimCommand turretManualAim(&theTurret, drivers());
+turret::TurretMouseAimCommand turretMouseAim(&theTurret, drivers());
+turret::TurretTestBottomLeftCommand turretLeftAim(&theTurret, drivers()); // Used for tuning
+turret::TurretTestTopRightCommand turretRightAim(&theTurret, drivers()); // Used for tuning
+
+feeder::FeederMoveUnjamCommand feederMoveUnjam(&theFeeder, drivers());
+
+flywheel::FlywheelFireDjiCommand flywheelStart(&theFlywheel, drivers());
+
+/* safe disconnect function -------------------------------------------------*/
+RemoteSafeDisconnectFunction remoteSafeDisconnectFunction(drivers());
+
+/* define command mappings --------------------------------------------------*/
+/* Controller mappings */
+HoldRepeatCommandMapping feedFeeder(drivers(), {&feederMoveUnjam}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::UP), true);
+ToggleCommandMapping startFlywheel(drivers(), {&flywheelStart}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::DOWN));
+
+/* Keyboard mappings */
+HoldRepeatCommandMapping mouseFeedFeeder(drivers(), {&feederMoveUnjam}, RemoteMapState(RemoteMapState::MouseButton::LEFT), true);
+ToggleCommandMapping mouseStartFlywheel(drivers(), {&flywheelStart}, RemoteMapState(RemoteMapState::MouseButton::RIGHT));
+ToggleCommandMapping toggleClientAiming(drivers(), {&chassisKeyboardDrive, &turretMouseAim}, RemoteMapState({Remote::Key::G}));
+
+// ToggleCommandMapping toggleChassisDrive(drivers(), {&chassisKeyboardDrive}, RemoteMapState({Remote::Key::G}));
+// ToggleCommandMapping turretMouseAimToggle(drivers(), {&turretMouseAim}, RemoteMapState({Remote::Key::B}));
+
+/*-Only used for calibration-*/
+// HoldCommandMapping rightAimTurret(drivers(), {&turretRightAim}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP));
+// HoldCommandMapping leftAimTurret(drivers(), {&turretLeftAim}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::DOWN));
+
+/* register subsystems here -------------------------------------------------*/
+void registerStandardSubsystems(src::Drivers *drivers) {
+    drivers->commandScheduler.registerSubsystem(&theChassis);
+    drivers->commandScheduler.registerSubsystem(&theTurret);
+    drivers->commandScheduler.registerSubsystem(&theFeeder);
+    drivers->commandScheduler.registerSubsystem(&theFlywheel);
+}
+
+/* initialize subsystems ----------------------------------------------------*/
+void initializeSubsystems() {
+    theChassis.initialize();
+    theTurret.initialize();
+    theFeeder.initialize();
+    theFlywheel.initialize();
+}
+
+/* set any default commands to subsystems here ------------------------------*/
+void setDefaultStandardCommands(src::Drivers *) {
+    theChassis.setDefaultCommand(&chassisDrive);
+    theTurret.setDefaultCommand(&turretManualAim);
+    // theFlywheel.setDefaultCommand(&flywheelStart);
+}
+
+/* add any starting commands to the scheduler here --------------------------*/
+void startStandardCommands(src::Drivers *drivers) {
+    drivers->commandScheduler.addCommand(&chassisImuCalibrate);
+}
+
+/* register io mappings here ------------------------------------------------*/
+void registerStandardIoMappings(src::Drivers *drivers) {
+    drivers->commandMapper.addMap(&feedFeeder);
+    drivers->commandMapper.addMap(&startFlywheel);
+    drivers->commandMapper.addMap(&mouseFeedFeeder);
+    drivers->commandMapper.addMap(&mouseStartFlywheel);
+    drivers->commandMapper.addMap(&toggleClientAiming);
+    // drivers->commandMapper.addMap(&leftAimTurret);
+    // drivers->commandMapper.addMap(&rightAimTurret);
+}
+
+void initSubsystemCommands(src::Drivers *drivers)
+{
+    drivers->commandScheduler.setSafeDisconnectFunction(&remoteSafeDisconnectFunction);
+    initializeSubsystems();
+    registerStandardSubsystems(drivers);
+    setDefaultStandardCommands(drivers);
+    startStandardCommands(drivers);
+    registerStandardIoMappings(drivers);
+    char buffer[50];
+    int nBytes = sprintf(buffer,"Initializing Standard\n");
+    drivers->uart.write(tap::communication::serial::Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1);
+}
+
+}  // namespace control
+
+#endif  // TARGET_STANDARD

PolySTAR-Taproot-project/src/control/sentry/sentry_control.cpp

@@ -25,13 +25,18 @@
 #include "subsystems/turret/turret_test_auto_aim_command.hpp"
 
 // Feeder includes
-#include "subsystems/feeder/double_feeder_subsystem.hpp"
-#include "subsystems/feeder/double_feeder_auto_feed.hpp"
-#include "subsystems/feeder/double_feeder_auto_feed_test_command.hpp"
+#include "subsystems/feeder/feeder_velocity_subsystem.hpp"
+#include "subsystems/feeder/feeder_auto_feed_command.hpp"
+#include "subsystems/feeder/feeder_auto_feed_test_command.hpp"
+
+#include "subsystems/feeder/feeder_position_subsystem.hpp" // Test with controller
+#include "subsystems/feeder/feeder_move_unjam_command.hpp"
+#include "subsystems/feeder/feeder_move_command.hpp"
 
 // Flywheel includes
-#include "subsystems/flywheel/flywheel_fire_command.hpp"
-#include "subsystems/flywheel/flywheel_subsystem.hpp"
+#include "subsystems/flywheel/flywheel_auto_fire_dji_command.hpp"
+#include "subsystems/flywheel/flywheel_fire_dji_command.hpp"
+#include "subsystems/flywheel/flywheel_dji_subsystem.hpp"
 
 // Motor includes
 #include "control/motor_control.hpp"
@@ -56,8 +61,9 @@ namespace control
 /* define subsystems --------------------------------------------------------*/
 chassis::ChassisSubsystem theChassis(drivers());
 turret::TurretSubsystem theTurret(drivers(), &yawMotor);
-flywheel::FlywheelSubsystem theFlywheel(drivers());
-feeder::DoubleFeederSubsystem theDoubleFeeder(drivers());
+flywheel::FlywheelDjiSubsystem theFlywheel(drivers());
+feeder::FeederVelocitySubsystem theFeeder(drivers());
+feeder::FeederPositionSubsystem theTestFeeder(drivers());
 
 
 /* define commands ----------------------------------------------------------*/
@@ -78,28 +84,32 @@ turret::TurretAutoAimCommand turretAutoAim(&theTurret, drivers());
 turret::TurretTestAutoAimCommand turretTestAutoAim(&theTurret, drivers());
 
 /* feeder -------------------------------------------------------------------*/
-feeder::DoubleAutoFeedCommand doubleFeederAutoFeed(&theDoubleFeeder, drivers());
-feeder::DoubleAutoFeedTestCommand doubleFeederAutoFeedTest(&theDoubleFeeder, drivers());
+feeder::FeederMoveUnjamCommand feederMoveUnjam(&theTestFeeder, drivers());
+feeder::FeederAutoFeedCommand feederAutoFeed(&theFeeder, drivers());
+feeder::FeederAutoFeedTestCommand feederAutoFeedTest(&theFeeder, drivers());
 
 
 /* flywheel -----------------------------------------------------------------*/
-flywheel::FlywheelFireCommand flywheelStart(&theFlywheel, drivers());
+// flywheel::FlywheelAutoFireDjiCommand flywheelStartTest(&theFlywheel, drivers());
+flywheel::FlywheelAutoFireDjiCommand flywheelStart(&theFlywheel, drivers());
+flywheel::FlywheelFireDjiCommand flywheelStartManual(&theFlywheel, drivers());
 
 /* safe disconnect function -------------------------------------------------*/
 RemoteSafeDisconnectFunction remoteSafeDisconnectFunction(drivers());
 
 /* define command mappings --------------------------------------------------*/
 /*-Ammo Booster-*/
-HoldRepeatCommandMapping feedFeeder(drivers(), {&doubleFeederAutoFeedTest}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP),true);
+HoldRepeatCommandMapping feedFeeder(drivers(), {&feederMoveUnjam}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::UP),true);
 /*-Flywheel-*/
-// HoldCommandMapping startFlywheel(drivers(), {&flywheelStart}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP));
+// HoldCommandMapping startFlywheel(drivers(), {&flywheelStartTest}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::DOWN));
+HoldCommandMapping startFlywheelManual(drivers(), {&flywheelStartManual}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP));
 /*-Turret-*/
 ToggleCommandMapping turretMouseAimToggle(drivers(), {&turretMouseAim}, RemoteMapState({Remote::Key::B}));
 /*-Chassis-*/
 ToggleCommandMapping toggleChassisDrive(drivers(), {&chassisKeyboardDrive}, RemoteMapState({Remote::Key::G}));
 /*-Auto commands*/
-HoldCommandMapping toggleAutoCommands(drivers(), {&chassisAutoDrive, &turretAutoAim, &doubleFeederAutoFeed}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::UP));
-HoldCommandMapping toggleAutoTestCommands(drivers(), {&chassisTestAutoDrive, &turretTestAutoAim, &doubleFeederAutoFeedTest}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::DOWN));
+HoldCommandMapping toggleAutoCommands(drivers(), {&chassisAutoDrive, &turretAutoAim, &feederAutoFeed, &flywheelStart}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::DOWN));
+// HoldCommandMapping toggleAutoTestCommands(drivers(), {&chassisTestAutoDrive, &turretTestAutoAim, &feederAutoFeedTest}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::DOWN));
 
 /*-Only used for calibration-*/
 // HoldCommandMapping rightAimTurret(drivers(), {&turretRightAim}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP)); 
@@ -110,7 +120,8 @@ void registerStandardSubsystems(src::Drivers *drivers)
 {
     drivers->commandScheduler.registerSubsystem(&theChassis);
     drivers->commandScheduler.registerSubsystem(&theTurret);
-    drivers->commandScheduler.registerSubsystem(&theDoubleFeeder);
+    drivers->commandScheduler.registerSubsystem(&theTestFeeder);
+    drivers->commandScheduler.registerSubsystem(&theFeeder);
     drivers->commandScheduler.registerSubsystem(&theFlywheel);
 }
 
@@ -119,7 +130,8 @@ void initializeSubsystems()
 {
     theChassis.initialize();
     theTurret.initialize();
-    theDoubleFeeder.initialize();
+    theFeeder.initialize();
+    theTestFeeder.initialize();
     theFlywheel.initialize();
 }
 
@@ -142,16 +154,18 @@ void registerStandardIoMappings(src::Drivers *drivers)
 {
     /*-Ammo Booster-*/
     drivers->commandMapper.addMap(&feedFeeder);
+
     /*-Flywheel-*/
     // drivers->commandMapper.addMap(&startFlywheel);
+    drivers->commandMapper.addMap(&startFlywheelManual);
     /*-Turret-*/
     // drivers->commandMapper.addMap(&leftAimTurret);
     // drivers->commandMapper.addMap(&rightAimTurret);
     drivers->commandMapper.addMap(&turretMouseAimToggle);
     /*-Chassis-*/
     drivers->commandMapper.addMap(&toggleChassisDrive);
-    drivers->commandMapper.addMap(&toggleAutoTestCommands);
-    // drivers->commandMapper.addMap(&toggleAutoCommands);
+    // drivers->commandMapper.addMap(&toggleAutoTestCommands);
+    drivers->commandMapper.addMap(&toggleAutoCommands);
 }
 
 void initSubsystemCommands(src::Drivers *drivers)