diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 5db5be6..20f0f6d 100644 --- a/.vscode/tasks.json +++ b/.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", diff --git a/PolySTAR-Taproot-project/src/control/hero/hero_control.cpp b/PolySTAR-Taproot-project/src/control/hero/hero_control.cpp new file mode 100644 index 0000000..13b8dd1 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/control/sentry/sentry_control.cpp b/PolySTAR-Taproot-project/src/control/sentry/sentry_control.cpp index a3c33c5..405ab25 100644 --- a/PolySTAR-Taproot-project/src/control/sentry/sentry_control.cpp +++ b/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) diff --git a/PolySTAR-Taproot-project/src/control/spin_to_win/spin_to_win_control.cpp b/PolySTAR-Taproot-project/src/control/spin_to_win/spin_to_win_control.cpp index 56df097..4376880 100644 --- a/PolySTAR-Taproot-project/src/control/spin_to_win/spin_to_win_control.cpp +++ b/PolySTAR-Taproot-project/src/control/spin_to_win/spin_to_win_control.cpp @@ -10,16 +10,17 @@ #include "subsystems/chassis/chassis_spin2win_subsystem.hpp" #include "subsystems/chassis/chassis_relative_drive_command.hpp" #include "subsystems/chassis/chassis_spin2win_command.hpp" -#include "subsystems/chassis/chassis_keyboard_drive_command.hpp" +#include "subsystems/chassis/chassis_spin2win_keyboard_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_stable_manual_aim_command.hpp" +#include "subsystems/turret/turret_stable_mouse_aim_command.hpp" #include "subsystems/turret/turret_test_bottomleft_command.hpp" #include "subsystems/turret/turret_test_topright_command.hpp" -#include "subsystems/turret/turret_stable_manual_aim_command.hpp" // Feeder includes #include "subsystems/feeder/feeder_position_subsystem.hpp" @@ -30,6 +31,8 @@ #include "subsystems/flywheel/flywheel_subsystem.hpp" #include "subsystems/flywheel/flywheel_fire_command.hpp" +#include "control/drivers/drivers_singleton.hpp" + using src::control::RemoteSafeDisconnectFunction; using tap::communication::serial::Remote; using tap::control::CommandMapper; @@ -45,50 +48,64 @@ using tap::control::RemoteMapState; * Drivers class to all of these objects. */ + + +using src::DoNotUse_getDrivers; + +static src::driversFunc drivers = src::DoNotUse_getDrivers; namespace control { /* define subsystems --------------------------------------------------------*/ tap::motor::DjiMotor yawMotor(drivers(), tap::motor::MOTOR6, tap::can::CanBus::CAN_BUS1, true, "yaw motor"); chassis::ChassisSpin2WinSubsystem theChassis(drivers()); - + turret::TurretSubsystem theTurret(drivers(), &yawMotor); feeder::FeederPositionSubsystem theFeeder(drivers()); flywheel::FlywheelSubsystem theFlywheel(drivers()); /* define commands ----------------------------------------------------------*/ +/* chassis */ chassis::ChassisRelativeDriveCommand chassisRelativeDrive(&theChassis, drivers(), &yawMotor); chassis::ChassisSpin2winCommand chassisSpinDrive(&theChassis, drivers(), &yawMotor); -// chassis::ChassisKeyboardDriveCommand chassisKeyboardDrive(&theChassis, drivers()); -chassis::ChassisCalibrateImuCommand chassisImuCalibrate(&theChassis, drivers()); +chassis::ChassisSpin2winKeyboardCommand chassisKeyboardDrive(&theChassis, drivers(), &yawMotor); +// chassis::ChassisCalibrateImuCommand chassisImuCalibrate(&theChassis, drivers()); +/* turret */ +turret::TurretManualAimCommand turretManualNoSpin(&theTurret, drivers()); +turret::TurretMouseAimCommand turretMouseNoSpin(&theTurret, drivers()); turret::TurretStableManualAimCommand turretManualAim(&theTurret, &chassisSpinDrive, drivers()); -turret::TurretMouseAimCommand turretMouseAim(&theTurret, drivers()); -turret::TurretTestBottomLeftCommand turretLeftAim(&theTurret, drivers()); // Used for tuning -turret::TurretTestTopRightCommand turretRightAim(&theTurret, drivers()); // Used for tuning +turret::TurretStableMouseAimCommand turretMouseAim(&theTurret, &chassisKeyboardDrive, drivers()); + +// turret::TurretTestBottomLeftCommand turretLeftAim(&theTurret, drivers()); // Used for tuning +// turret::TurretTestTopRightCommand turretRightAim(&theTurret, drivers()); // Used for tuning +/* feeder */ feeder::FeederMoveUnjamCommand feederMoveUnjam(&theFeeder, drivers()); +/* flywheel */ flywheel::FlywheelFireCommand flywheelStart(&theFlywheel, drivers()); /* safe disconnect function -------------------------------------------------*/ RemoteSafeDisconnectFunction remoteSafeDisconnectFunction(drivers()); /* define command mappings --------------------------------------------------*/ -HoldRepeatCommandMapping feedFeeder(drivers(), {&feederMoveUnjam}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::UP),true); -HoldRepeatCommandMapping mouseFeedFeeder(drivers(), {&feederMoveUnjam}, RemoteMapState(RemoteMapState::MouseButton::LEFT),true); -// ToggleCommandMapping toggleClientAiming(drivers(), {&chassisKeyboardDrive,&turretMouseAim}, RemoteMapState({Remote::Key::G})); +/* Controller mappings */ +HoldRepeatCommandMapping feedFeeder(drivers(), {&feederMoveUnjam}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP),true); +HoldCommandMapping startFlywheel(drivers(), {&flywheelStart, &feederMoveUnjam}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::UP)); +HoldCommandMapping toggleChassisSpin(drivers(), {&chassisSpinDrive, &turretManualAim}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::DOWN)); -/*-Testing commands-*/ -HoldCommandMapping toggleChassisSpin(drivers(), {&chassisSpinDrive}, RemoteMapState(Remote::Switch::RIGHT_SWITCH, Remote::SwitchState::DOWN)); -// HoldCommandMapping mouseStartFlywheel(drivers(), {&flywheelStart}, RemoteMapState(RemoteMapState::MouseButton::RIGHT)); -// HoldCommandMapping startFlywheel(drivers(), {&flywheelStart}, RemoteMapState(Remote::Switch::LEFT_SWITCH, Remote::SwitchState::UP)); -// ToggleCommandMapping toggleChassisDrive(drivers(), {&chassisKeyboardDrive}, RemoteMapState({Remote::Key::G})); +/* Mouse mappings */ +ToggleCommandMapping mouseStartFlywheel(drivers(), {&flywheelStart}, RemoteMapState(RemoteMapState::MouseButton::RIGHT)); +HoldRepeatCommandMapping mouseFeedFeeder(drivers(), {&feederMoveUnjam}, RemoteMapState(RemoteMapState::MouseButton::LEFT),true); +// ToggleCommandMapping toggleClientAiming(drivers(), {&turretMouseNoSpin}, RemoteMapState({Remote::Key::F})); +ToggleCommandMapping toggleChassisSpinKey(drivers(), {&chassisKeyboardDrive, &turretMouseAim}, RemoteMapState({Remote::Key::R})); // ToggleCommandMapping turretMouseAimToggle(drivers(), {&turretMouseAim}, RemoteMapState({Remote::Key::B})); +// ToggleCommandMapping toggleChassisDrive(drivers(), {&chassisKeyboardDrive}, RemoteMapState({Remote::Key::G})); /*-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)); +// 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) { @@ -109,22 +126,24 @@ void initializeSubsystems() { /* set any default commands to subsystems here ------------------------------*/ void setDefaultStandardCommands(src::Drivers *) { theChassis.setDefaultCommand(&chassisRelativeDrive); - theTurret.setDefaultCommand(&turretManualAim); + theTurret.setDefaultCommand(&turretManualNoSpin); // theFlywheel.setDefaultCommand(&flywheelStart); } /* add any starting commands to the scheduler here --------------------------*/ void startStandardCommands(src::Drivers *drivers) { - drivers->commandScheduler.addCommand(&chassisImuCalibrate); + // drivers->commandScheduler.addCommand(&chassisImuCalibrate); } /* register io mappings here ------------------------------------------------*/ void registerStandardIoMappings(src::Drivers *drivers) { drivers->commandMapper.addMap(&feedFeeder); + drivers->commandMapper.addMap(&startFlywheel); drivers->commandMapper.addMap(&toggleChassisSpin); + drivers->commandMapper.addMap(&mouseStartFlywheel); drivers->commandMapper.addMap(&mouseFeedFeeder); // drivers->commandMapper.addMap(&toggleClientAiming); - drivers->commandMapper.addMap(&startFlywheel); + drivers->commandMapper.addMap(&toggleChassisSpinKey); // drivers->commandMapper.addMap(&leftAimTurret); // drivers->commandMapper.addMap(&rightAimTurret); } diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_auto_drive_command.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_auto_drive_command.cpp index 8573c27..ef64b56 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_auto_drive_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_auto_drive_command.cpp @@ -1,4 +1,5 @@ #include "chassis_auto_drive_command.hpp" +#include "subsystems/sentry_general_constants.hpp" namespace control { @@ -7,17 +8,25 @@ namespace chassis ChassisAutoDriveCommand::ChassisAutoDriveCommand( ChassisSubsystem *const chassis, src::Drivers *drivers) - : GenericAutoDriveCommand(chassis, drivers) + : GenericAutoDriveCommand(chassis, drivers), + startMatchTimeout(0) { + startMatchTimeout.stop(); } -void ChassisAutoDriveCommand::execute() +void ChassisAutoDriveCommand::initialize() { - if (drivers->refSerial.getGameData().gameStage != tap::communication::serial::RefSerialData::Rx::GameStage::IN_GAME) + startMatchTimeout.restart(START_MATCH_WAIT_TIME); +} + +void ChassisAutoDriveCommand::execute() +{ + if (!startMatchTimeout.isExpired()) { chassis->setTargetOutput(0, 0, 0); return; } + drivers->leds.set(tap::gpio::Leds::A, true); GenericAutoDriveCommand::execute(); } diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_auto_drive_command.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_auto_drive_command.hpp index 27e4030..53c50ff 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_auto_drive_command.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_auto_drive_command.hpp @@ -2,6 +2,7 @@ #define CHASSIS_AUTO_DRIVE_COMMAND_HPP_ #include "generic_auto_drive_command.hpp" +#include "tap/architecture/timeout.hpp" namespace control { @@ -25,8 +26,12 @@ class ChassisAutoDriveCommand : public GenericAutoDriveCommand const char *getName() const { return "chassis auto drive command"; } + void initialize() override; + void execute() override; +private: + tap::arch::MilliTimeout startMatchTimeout; }; // ChassisAutoDriveCommand } // namespace chassis diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp index b33fbac..337f7e9 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_constants.hpp @@ -19,10 +19,20 @@ static constexpr float CHASSIS_TR_PROPORTIONAL_KALMAN = 0.0f; * of the chassis when using the keyboard. */ +#ifdef TARGET_HERO +static constexpr float CHASSIS_DEFAULT_SPEED = 0.25f; + +#elif defined(TARGET_SPIN_TO_WIN) +static constexpr float CHASSIS_DEFAULT_SPEED = 0.4f; + +#else static constexpr float CHASSIS_DEFAULT_SPEED = 0.5f; +#endif + static constexpr float CHASSIS_SHIFT_MULTIPLIER = 1.0f; static constexpr float CHASSIS_CTRL_MULTIPLIER = 0.25f; + /** * Left joystick dead zone size. If the absolute value return by the stick is below * this value, it is considered zero. @@ -62,4 +72,26 @@ static constexpr float W_TO_R = 0.07955; // 1rps = 0.5 on chassis r * Enable UART debug messages for chassis */ static constexpr bool CHASSIS_DEBUG_MESSAGE = false; -static constexpr uint32_t CHASSIS_DEBUG_MESSAGE_DELAY_MS = 100; \ No newline at end of file +static constexpr uint32_t CHASSIS_DEBUG_MESSAGE_DELAY_MS = 100; +/** + * Chassis motors can bus. + */ +#ifdef TARGET_HERO +static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS2; +#endif + +#ifdef TARGET_SENTRY +static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS2; +#endif + +#ifdef TARGET_STANDARD +static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; +#endif + +#ifdef TARGET_ICRA +static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; +#endif + +#ifdef TARGET_SPIN_TO_WIN +static constexpr tap::can::CanBus CHASSIS_CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; +#endif diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_keyboard_drive_command.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_keyboard_drive_command.cpp index 2fce3a7..a5cf633 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_keyboard_drive_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_keyboard_drive_command.cpp @@ -1,4 +1,5 @@ #include "chassis_keyboard_drive_command.hpp" +#include "chassis_constants.hpp" #include "tap/algorithms/math_user_utils.hpp" #include "tap/errors/create_errors.hpp" diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_relative_drive_command.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_relative_drive_command.cpp index c251082..d99fdf1 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_relative_drive_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_relative_drive_command.cpp @@ -1,4 +1,5 @@ #include "chassis_relative_drive_command.hpp" +#include "chassis_constants.hpp" #include "subsystems/turret/turret_constants.hpp" @@ -37,6 +38,28 @@ void ChassisRelativeDriveCommand::execute() float yInput = drivers->controlInterface.getChassisYInput(); float rInput = drivers->controlInterface.getChassisRInput(); + keyboard_input = drivers->controlInterface.getChassisKeyboardInput(); + + float xKeyInput = 0; + float yKeyInput = 0; + float rKeyInput = 0; + + float multiplier = CHASSIS_DEFAULT_SPEED; + + if (keyboard_input["w"]) { xKeyInput += 1; } + if (keyboard_input["s"]) { xKeyInput -= 1; } + if (keyboard_input["d"]) { yKeyInput += 1; } + if (keyboard_input["a"]) { yKeyInput -= 1; } + if (keyboard_input["q"]) { rKeyInput += 1; } + if (keyboard_input["e"]) { rKeyInput -= 1; } + if (keyboard_input["shift"]) { multiplier = CHASSIS_SHIFT_MULTIPLIER; } + if (keyboard_input["ctrl"]) { multiplier = CHASSIS_CTRL_MULTIPLIER; } + if (keyboard_input["shift"] && keyboard_input["ctrl"]) { multiplier = CHASSIS_DEFAULT_SPEED; } + + xInput += xKeyInput * multiplier; + yInput += yKeyInput * multiplier; + rInput += rKeyInput * multiplier; + // Chassis joystick orientation in radians float chassisRad = atan2(yInput, xInput); diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_relative_drive_command.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_relative_drive_command.hpp index 424c178..328db9c 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_relative_drive_command.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_relative_drive_command.hpp @@ -43,6 +43,8 @@ class ChassisRelativeDriveCommand : public tap::control::Command ChassisSpin2WinSubsystem *const chassis; + std::map keyboard_input; + src::Drivers *drivers; }; // ChassisRelativeDriveCommand diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_keyboard_command.cpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_keyboard_command.cpp index 878854d..8222772 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_keyboard_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_keyboard_command.cpp @@ -1,4 +1,5 @@ #include "chassis_spin2win_keyboard_command.hpp" +#include "chassis_constants.hpp" #include "subsystems/turret/turret_constants.hpp" diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp index ec5f264..e4adb5e 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_spin2win_subsystem.hpp @@ -40,10 +40,10 @@ class ChassisSpin2WinSubsystem : public tap::control::Subsystem ChassisSpin2WinSubsystem(src::Drivers *drivers) : tap::control::Subsystem(drivers), drivers(drivers), - frontLeftMotor(drivers, FRONT_LEFT_MOTOR_ID, CAN_BUS_MOTORS, false, "front left motor"), - frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CAN_BUS_MOTORS, true, "front right motor"), - backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CAN_BUS_MOTORS, false, "back left motor"), - backRightMotor(drivers, BACK_RIGHT_MOTOR_ID, CAN_BUS_MOTORS, true, "back right motor"), + frontLeftMotor(drivers, FRONT_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "front left motor"), + frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "front right motor"), + backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "back left motor"), + backRightMotor(drivers, BACK_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "back right motor"), frontLeftPid(pidConfig), frontRightPid(pidConfig), backLeftPid(pidConfig), diff --git a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp index 93be0ee..aa11674 100644 --- a/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/chassis/chassis_subsystem.hpp @@ -40,10 +40,10 @@ class ChassisSubsystem : public tap::control::Subsystem ChassisSubsystem(src::Drivers *drivers) : tap::control::Subsystem(drivers), drivers(drivers), - frontLeftMotor(drivers, FRONT_LEFT_MOTOR_ID, CAN_BUS_MOTORS, false, "front left motor"), - frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CAN_BUS_MOTORS, true, "front right motor"), - backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CAN_BUS_MOTORS, false, "back left motor"), - backRightMotor(drivers, BACK_RIGHT_MOTOR_ID, CAN_BUS_MOTORS, true, "back right motor"), + frontLeftMotor(drivers, FRONT_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "front left motor"), + frontRightMotor(drivers, FRONT_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "front right motor"), + backLeftMotor(drivers, BACK_LEFT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, false, "back left motor"), + backRightMotor(drivers, BACK_RIGHT_MOTOR_ID, CHASSIS_CAN_BUS_MOTORS, true, "back right motor"), frontLeftPid(pidConfig), frontRightPid(pidConfig), backLeftPid(pidConfig), @@ -90,7 +90,6 @@ class ChassisSubsystem : public tap::control::Subsystem static constexpr tap::motor::MotorId FRONT_RIGHT_MOTOR_ID = tap::motor::MOTOR2; static constexpr tap::motor::MotorId BACK_RIGHT_MOTOR_ID = tap::motor::MOTOR3; static constexpr tap::motor::MotorId BACK_LEFT_MOTOR_ID = tap::motor::MOTOR4; - static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; ///< Motors. Use these to interact with any dji style motors. tap::motor::DjiMotor frontLeftMotor; diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/hero_feeder_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/hero_feeder_constants.hpp new file mode 100644 index 0000000..ef280a8 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/hero_feeder_constants.hpp @@ -0,0 +1,58 @@ +#include "algorithms/feed_forward.hpp" +#include "tap/algorithms/smooth_pid.hpp" + +#include "tap/communication/can/can_bus.hpp" + +/** + * Feeder position PID: A PID controller for feeder position. The PID parameters for the + * controller are listed below. + */ + +static constexpr tap::algorithms::SmoothPidConfig FEEDER_PID_CONFIG( + 0.075f, // kP + 0.0f, // kI + -7.5f, // kD + 5000.0f, // Max error sum + 16000.0f, // Max output + 1.0f, // TQ Derivative Kalman + 0.0f, // TR Derivative Kalman + 1.0f, // TQ Proportional Kalman + 0.0f, // TR Proportional Kalman + 0.0f, // Error Deadzone + 0.0f // Error derivative floor +); + +/** + * Turret Position FeedForward: Feed Forward controllers for feeder position. The FF parameters for the + * controller are listed below. + */ + +static constexpr src::algorithms::FeedForwardConfig FEEDER_FF_CONFIG( + 400.0f, // kS + 0.0f, // kV + 0.0f, // kG + 1000.0f // maxVelocity +); + +/** + * The feeder RPM set when the feeder is on +*/ +static constexpr float FEEDER_RPM = 2500; +static constexpr float FEEDER_REVERSE_RPM = -1500; + +static constexpr float DEGREE_TO_TICK = 8192*36/360; // 8192 Ticks per turn, 36:1 gear ratio +static constexpr float UNJAM_MAX_WAIT_TIME_MS = 500; // TO BE DETERMINED +static constexpr float MOVE_DISPLACEMENT_TICK = 45*DEGREE_TO_TICK; // TO BE DETERMINED +static constexpr float UNJAM_CYCLES = 1; // TO BE DETERMINED +static constexpr float UNJAM_DISPLACEMENT_TICK = 45*DEGREE_TO_TICK; // TO BE DETERMINED +static constexpr float PAUSE_AFTER_MOVE_TIME_MS = 100; // TO BE DETERMINED +static constexpr float MOVE_TIME_MS = 300; +static constexpr float SETPOINT_POS_TOLERANCE_TICK = 45*DEGREE_TO_TICK; // TO BE DETERMINED + +static constexpr float JAM_CHECKER_TOLERANCE_TICK = 20*DEGREE_TO_TICK; +static constexpr uint32_t JAM_CHECKER_TOLERANCE_MS = 500; + +static constexpr bool IS_FEEDER_INVERTED = true; + +// CanBus different from other robots since feeder is in chassis +static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS2; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/icra_feeder_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/icra_feeder_constants.hpp index e9d3192..85f2e57 100644 --- a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/icra_feeder_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/icra_feeder_constants.hpp @@ -1,6 +1,8 @@ #include "algorithms/feed_forward.hpp" #include "tap/algorithms/smooth_pid.hpp" +#include "tap/communication/can/can_bus.hpp" + /** * Feeder position PID: A PID controller for feeder position. The PID parameters for the * controller are listed below. @@ -50,4 +52,6 @@ static constexpr float SETPOINT_POS_TOLERANCE_TICK = 1*DEGREE_TO_TICK; // TO BE static constexpr float JAM_CHECKER_TOLERANCE_TICK = 5*DEGREE_TO_TICK; static constexpr uint32_t JAM_CHECKER_TOLERANCE_MS = 500; -static constexpr bool IS_FEEDER_INVERTED = false; \ No newline at end of file +static constexpr bool IS_FEEDER_INVERTED = false; + +static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/sentry_feeder_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/sentry_feeder_constants.hpp index e3d476b..f62d80b 100644 --- a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/sentry_feeder_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/sentry_feeder_constants.hpp @@ -1,6 +1,8 @@ #include "algorithms/feed_forward.hpp" #include "tap/algorithms/smooth_pid.hpp" +#include "tap/communication/can/can_bus.hpp" + /** * Feeder position PID: A PID controller for feeder position. The PID parameters for the * controller are listed below. @@ -50,4 +52,6 @@ static constexpr float SETPOINT_POS_TOLERANCE_TICK = 1*DEGREE_TO_TICK; // TO BE static constexpr float JAM_CHECKER_TOLERANCE_TICK = 5*DEGREE_TO_TICK; static constexpr uint32_t JAM_CHECKER_TOLERANCE_MS = 500; -static constexpr bool IS_FEEDER_INVERTED = true; \ No newline at end of file +static constexpr bool IS_FEEDER_INVERTED = false; + +static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/spin_to_win_feeder_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/spin_to_win_feeder_constants.hpp index f9178fb..e848a11 100644 --- a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/spin_to_win_feeder_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/spin_to_win_feeder_constants.hpp @@ -1,6 +1,8 @@ #include "algorithms/feed_forward.hpp" #include "tap/algorithms/smooth_pid.hpp" +#include "tap/communication/can/can_bus.hpp" + /** * Feeder position PID: A PID controller for feeder position. The PID parameters for the * controller are listed below. @@ -50,4 +52,6 @@ static constexpr float SETPOINT_POS_TOLERANCE_TICK = 45*DEGREE_TO_TICK; // TO BE static constexpr float JAM_CHECKER_TOLERANCE_TICK = 5*DEGREE_TO_TICK; static constexpr uint32_t JAM_CHECKER_TOLERANCE_MS = 500; -static constexpr bool IS_FEEDER_INVERTED = true; \ No newline at end of file +static constexpr bool IS_FEEDER_INVERTED = true; + +static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/standard_feeder_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/standard_feeder_constants.hpp index 1565d54..1b263e4 100644 --- a/PolySTAR-Taproot-project/src/subsystems/feeder/constants/standard_feeder_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/constants/standard_feeder_constants.hpp @@ -1,6 +1,8 @@ #include "algorithms/feed_forward.hpp" #include "tap/algorithms/smooth_pid.hpp" +#include "tap/communication/can/can_bus.hpp" + /** * Feeder position PID: A PID controller for feeder position. The PID parameters for the * controller are listed below. @@ -50,4 +52,6 @@ static constexpr float SETPOINT_POS_TOLERANCE_TICK = 45*DEGREE_TO_TICK; // TO BE static constexpr float JAM_CHECKER_TOLERANCE_TICK = 5*DEGREE_TO_TICK; static constexpr uint32_t JAM_CHECKER_TOLERANCE_MS = 500; -static constexpr bool IS_FEEDER_INVERTED = true; \ No newline at end of file +static constexpr bool IS_FEEDER_INVERTED = true; + +static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_auto_feed_command.cpp b/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_auto_feed_command.cpp index eeb77ef..b138d8b 100644 --- a/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_auto_feed_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_auto_feed_command.cpp @@ -5,6 +5,8 @@ #include "tap/algorithms/math_user_utils.hpp" #include "tap/errors/create_errors.hpp" +#include "subsystems/sentry_general_constants.hpp" + namespace control { namespace feeder @@ -16,17 +18,21 @@ FeederAutoFeedCommand::FeederAutoFeedCommand( { } +void FeederAutoFeedCommand::initialize() +{ + startMatchTimeout.restart(START_MATCH_WAIT_TIME); +} -void FeederAutoFeedCommand::execute() { - - if(drivers->refSerial.getGameData().gameStage != tap::communication::serial::RefSerialData::Rx::GameStage::IN_GAME) { +void FeederAutoFeedCommand::execute() +{ + if(!startMatchTimeout.isExpired()) { feeder->setDesiredOutput(0); return; } + drivers->leds.set(tap::gpio::Leds::B, true); GenericAutoFeedCommand::execute(); } - } // namespace feeder } // namespace control \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_auto_feed_command.hpp b/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_auto_feed_command.hpp index 8260ae4..41b4495 100644 --- a/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_auto_feed_command.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_auto_feed_command.hpp @@ -27,11 +27,14 @@ class FeederAutoFeedCommand : public GenericAutoFeedCommand FeederAutoFeedCommand &operator=(const GenericAutoFeedCommand &other) = delete; - const char *getName() const { return "feeder auto feed command"; } + + void initialize() override; void execute() override; +private: + tap::arch::MilliTimeout startMatchTimeout; }; // FeederAutoFeedCommand } // namespace feeder diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_constants.hpp index db6bedf..ac295ee 100644 --- a/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_constants.hpp @@ -17,6 +17,10 @@ #include "constants/icra_feeder_constants.hpp" #endif +#ifdef TARGET_HERO +#include "constants/hero_feeder_constants.hpp" +#endif + static constexpr float FEEDER_PID_KP = 20.0f; static constexpr float FEEDER_PID_KI = 5.0f; static constexpr float FEEDER_PID_KD = 0.0f; diff --git a/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_position_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_position_subsystem.hpp index a95db0c..b563153 100644 --- a/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_position_subsystem.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/feeder/feeder_position_subsystem.hpp @@ -71,7 +71,6 @@ class FeederPositionSubsystem : public tap::control::setpoint::SetpointSubsystem private: ///< Hardware constants, not specific to any particular feeder. static constexpr tap::motor::MotorId FEEDER_MOTOR_ID = tap::motor::MOTOR8; - static constexpr tap::can::CanBus CAN_BUS_MOTORS = tap::can::CanBus::CAN_BUS1; ///< Motors. Use these to interact with any dji style motors. tap::motor::DjiMotor feederMotor; diff --git a/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_auto_fire_dji_command.cpp b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_auto_fire_dji_command.cpp new file mode 100644 index 0000000..31cee65 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_auto_fire_dji_command.cpp @@ -0,0 +1,78 @@ +#include "flywheel_subsystem.hpp" +#include "flywheel_auto_fire_dji_command.hpp" + +#include "subsystems/sentry_general_constants.hpp" + +namespace control +{ +namespace flywheel +{ + +FlywheelAutoFireDjiCommand::FlywheelAutoFireDjiCommand( + FlywheelDjiSubsystem *const flywheel, + src::Drivers *drivers) + : flywheel(flywheel), + drivers(drivers) +{ + if (flywheel == nullptr) + { + return; + } + this->addSubsystemRequirement(dynamic_cast(flywheel)); +} + +void FlywheelAutoFireDjiCommand::initialize() { + char buffer[50]; + int nBytes = sprintf (buffer, "starting firing\n"); + drivers->uart.write(tap::communication::serial::Uart::Uart8,(uint8_t*) buffer, nBytes+1); + + isKickstartDone = false; + startingTs = tap::arch::clock::getTimeMilliseconds(); + + startMatchTimeout.restart(START_MATCH_WAIT_TIME); +} + +void FlywheelAutoFireDjiCommand::execute() { + // if(drivers->refSerial.getGameData().gameStage != tap::communication::serial::RefSerialData::Rx::GameStage::IN_GAME) { + // flywheel->stopFiring(); + // isKickstartDone = false; + // return; + // } + + if (!startMatchTimeout.isExpired()) { + flywheel->stopFiring(); + isKickstartDone = false; + return; + } + + drivers->leds.set(tap::gpio::Leds::C, true); + + if (!drivers->cvHandler.shouldShoot()) { + flywheel->stopFiring(); + isKickstartDone = false; + return; + } + + uint32_t currentTs = tap::arch::clock::getTimeMilliseconds(); + if (!currentTs - startingTs < KICKSTART_DELAY_MS) { + flywheel->sendStartingBoost(); + } else if (!isKickstartDone) { + flywheel->startFiring(); // will send default speeds to DjiMotors + isKickstartDone = true; + } +} + +void FlywheelAutoFireDjiCommand::end(bool) +{ + flywheel->stopFiring(); +} + +bool FlywheelAutoFireDjiCommand::isFinished() const +{ + return false; +} + +} // namespace flywheel + +} // namespace control + diff --git a/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_auto_fire_dji_command.hpp b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_auto_fire_dji_command.hpp new file mode 100644 index 0000000..8f9c504 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_auto_fire_dji_command.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include "tap/control/command.hpp" +#include "control/drivers/drivers.hpp" +#include "flywheel_dji_subsystem.hpp" + +namespace control +{ +namespace flywheel +{ + +class FlywheelAutoFireDjiCommand : public tap::control::Command +{ +public: + + /** + * Constructs a new Flywheel fire command + * @param[in] flywheel a pointer to the flywheel to be passed in that this + * Command will interact with. + */ + FlywheelAutoFireDjiCommand(FlywheelDjiSubsystem *const flywheel, src::Drivers *drivers); + + FlywheelAutoFireDjiCommand(const FlywheelAutoFireDjiCommand &other) = delete; + + FlywheelAutoFireDjiCommand &operator=(const FlywheelAutoFireDjiCommand &other) = delete; + + const char *getName() const { return "flywheel fire command"; } + + void initialize() override; + + bool isFinished() const override; + + void execute() override; + + void end(bool) override; + +private: + // Hardware constants, not specific to any particular flywheel subsystem. + FlywheelDjiSubsystem *const flywheel; + + src::Drivers *drivers; + + bool isKickstartDone = false; + uint32_t startingTs; + + tap::arch::MilliTimeout startMatchTimeout; +}; // class FlywheelAutoFireDjiCommand + +} // namespace flywheel + +} // namespace control diff --git a/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_constants.hpp index 314a379..c6b6997 100644 --- a/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_constants.hpp @@ -11,14 +11,38 @@ constexpr static float FLYWHEEL_DEFAULT_THROTTLE = 0.4; #endif #ifdef TARGET_SPIN_TO_WIN -constexpr static float FLYWHEEL_DEFAULT_THROTTLE = 0.4; +constexpr static float FLYWHEEL_DEFAULT_THROTTLE = 0.6; #endif #ifdef TARGET_SENTRY constexpr static float FLYWHEEL_DEFAULT_THROTTLE = 0.25; // Desired Pulse width 1250 us #endif +#ifdef TARGET_HERO +constexpr static float FLYWHEEL_DEFAULT_THROTTLE = 0.4; +#endif + // Delay after start of flywheels before feeder start. constexpr static uint32_t FEEDER_DELAY_MS = 300; +// DjiMotor constants +static constexpr tap::motor::MotorId LEFT_MOTOR_ID = tap::motor::MOTOR1; +static constexpr tap::motor::MotorId RIGHT_MOTOR_ID = tap::motor::MOTOR2; +static constexpr tap::can::CanBus CAN_BUS_MOTORS_FLYWHEEL = tap::can::CanBus::CAN_BUS1; + +// Speeds based on rpmScaleFactor of ChassisSubsystem +static constexpr float MOTOR_LOW_SPEED = 500.0f; + +#ifdef TARGET_SENTRY +static constexpr float MOTOR_MEDIUM_SPEED = 2000.0f; +#elif defined(TARGET_HERO) +static constexpr float MOTOR_MEDIUM_SPEED = 1500.0f; +#else +static constexpr float MOTOR_MEDIUM_SPEED = 1500.0f; +#endif + +static constexpr float MOTOR_HIGH_SPEED = 3000.0f; + +static constexpr uint32_t KICKSTART_DELAY_MS = 300; + #endif \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_dji_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_dji_subsystem.cpp new file mode 100644 index 0000000..69119e8 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_dji_subsystem.cpp @@ -0,0 +1,94 @@ +#include "flywheel_dji_subsystem.hpp" + +#include "tap/communication/serial/remote.hpp" +#include "tap/algorithms/math_user_utils.hpp" +#include "control/drivers/drivers.hpp" + +#include + +using namespace tap; +using tap::communication::serial::Uart; + +namespace control +{ +namespace flywheel +{ +void FlywheelDjiSubsystem::initialize() +{ + snailMotor.init(); + leftMotor.initialize(); + rightMotor.initialize(); +} + +void FlywheelDjiSubsystem::refresh() { + /*if (tap::arch::clock::getTimeMilliseconds() - prevMeasureTime < BALLISTIC_MEASURE_DELAY_MS) { + return; + } + prevMeasureTime = tap::arch::clock::getTimeMilliseconds(); + + + // Update buffers + bulletSpeedBuf.push_back(drivers->refSerial.getRobotData().turret.bulletSpeed); + firingFreqBuf.push_back(drivers->refSerial.getRobotData().turret.firingFreq); + + if (bulletSpeedBuf.size() > N_MEASURES) { + bulletSpeedBuf.pop_front(); + } + if (firingFreqBuf.size() > N_MEASURES) { + firingFreqBuf.pop_front(); + } + + if (tap::arch::clock::getTimeMilliseconds() - prevDebugTime > BALLISTIC_DEBUG_DELAY_MS) { + prevDebugTime = tap::arch::clock::getTimeMilliseconds(); + + // Calculate mean + float bulletSpeedMean = static_cast(std::accumulate(bulletSpeedBuf.begin(), bulletSpeedBuf.end(), 0)) + / firingFreqBuf.size(); + float firingFreqMean = static_cast(std::accumulate(firingFreqBuf.begin(), firingFreqBuf.end(), 0)) + / firingFreqBuf.size(); + + char buffer[500]; + + // Front right debug message + int nBytes = sprintf (buffer, "Bullet Speed: %i\t Firing Frequency: %i\n", + (int)bulletSpeedMean, + (int)firingFreqMean); + drivers->uart.write(Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); + }*/ +} + +void FlywheelDjiSubsystem::startFiring() { + snailMotor.setThrottle(currentThrottle); + leftMotor.setDesiredOutput(currentDjiSpeed); + rightMotor.setDesiredOutput(currentDjiSpeed); + // char buffer[50]; + // int nBytes = sprintf(buffer,"Start firing\n"); + // drivers->uart.write(tap::communication::serial::Uart::UartPort::Uart8,(uint8_t*) buffer, nBytes+1); +} + +void FlywheelDjiSubsystem::stopFiring() { + snailMotor.setThrottle(0); + rightMotor.setDesiredOutput(0); + leftMotor.setDesiredOutput(0); +} + +void FlywheelDjiSubsystem::sendStartingBoost() { + rightMotor.setDesiredOutput(MOTOR_MEDIUM_SPEED); + leftMotor.setDesiredOutput(MOTOR_MEDIUM_SPEED); +} + +void FlywheelDjiSubsystem::setThrottle(float throttle) { + currentThrottle = throttle; + + if (firing == false) return; + + startFiring(); +} + +float FlywheelDjiSubsystem::getCurrentThrottle() const { + return currentThrottle; +} + +} // namespace flywheel + +} // namespace control diff --git a/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_dji_subsystem.hpp b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_dji_subsystem.hpp new file mode 100644 index 0000000..b643796 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_dji_subsystem.hpp @@ -0,0 +1,85 @@ +#ifndef FLYWHEEL_DJI_SUBSYSTEM_HPP_ +#define FLYWHEEL_DJI_SUBSYSTEM_HPP_ + +#include "tap/control/subsystem.hpp" +#include "snail_motor.hpp" +#include "tap/util_macros.hpp" +#include "flywheel_constants.hpp" + +#include + +namespace control +{ +namespace flywheel +{ +/** + * A bare bones Subsystem for interacting with a flywheel. + */ +class FlywheelDjiSubsystem : public tap::control::Subsystem +{ +public: + + /** + * Constructs a new FlywheelSubsystem with default parameters specified in + * the private section of this class. + */ + FlywheelDjiSubsystem(tap::Drivers *drivers) + : tap::control::Subsystem(drivers), + snailMotor(drivers, FLYWHEEL_PWM_PIN), + leftMotor(drivers, LEFT_MOTOR_ID, CAN_BUS_MOTORS_FLYWHEEL, false, "left motor"), + rightMotor(drivers, RIGHT_MOTOR_ID, CAN_BUS_MOTORS_FLYWHEEL, true, "right motor"), + currentThrottle(FLYWHEEL_DEFAULT_THROTTLE), + currentDjiSpeed(MOTOR_LOW_SPEED), // TODO: change speed here + firing(false) + { + } + + FlywheelDjiSubsystem(const FlywheelDjiSubsystem &other) = delete; + + FlywheelDjiSubsystem &operator=(const FlywheelDjiSubsystem &other) = delete; + + ~FlywheelDjiSubsystem() = default; + + void initialize() override; + + void refresh() override; + + void startFiring(); + + void stopFiring(); + + void sendStartingBoost(); + + void setThrottle(float throttle); + + float getCurrentThrottle() const; + + const src::motor::SnailMotor &getFlywheelMotor() const { return snailMotor; } + +private: + // Hardware constants, not specific to any particular flywheel subsystem. + static constexpr tap::gpio::Pwm::Pin FLYWHEEL_PWM_PIN = tap::gpio::Pwm::Pin::Z; + + src::motor::SnailMotor snailMotor; + + ///< Motors. Use these to interact with any dji style motors. + tap::motor::DjiMotor leftMotor; + tap::motor::DjiMotor rightMotor; + + float currentThrottle; + float currentDjiSpeed; + + float firing; + + std::deque bulletSpeedBuf; + std::deque firingFreqBuf; + + uint32_t prevDebugTime; + uint32_t prevMeasureTime; +}; // class FlywheelSubsystem + +} // namespace flywheel + +} // namespace control + +#endif // FLYWHEEL_SUBSYSTEM_HPP_ \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_fire_dji_command.cpp b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_fire_dji_command.cpp new file mode 100644 index 0000000..a06fab6 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_fire_dji_command.cpp @@ -0,0 +1,54 @@ +#include "flywheel_subsystem.hpp" +#include "flywheel_fire_dji_command.hpp" + +namespace control +{ +namespace flywheel +{ + +FlywheelFireDjiCommand::FlywheelFireDjiCommand( + FlywheelDjiSubsystem *const flywheel, + src::Drivers *drivers) + : flywheel(flywheel), + drivers(drivers) +{ + if (flywheel == nullptr) + { + return; + } + this->addSubsystemRequirement(dynamic_cast(flywheel)); +} + +void FlywheelFireDjiCommand::initialize() { + char buffer[50]; + int nBytes = sprintf (buffer, "starting firing\n"); + drivers->uart.write(tap::communication::serial::Uart::Uart8,(uint8_t*) buffer, nBytes+1); + flywheel->sendStartingBoost(); // will not modify speed attributes of DjiMotors + + isKickstartDone = false; + startingTs = tap::arch::clock::getTimeMilliseconds(); +} + +void FlywheelFireDjiCommand::execute() { + if (!isKickstartDone && tap::arch::clock::getTimeMilliseconds() - startingTs > KICKSTART_DELAY_MS) { + flywheel->startFiring(); // will send default speeds to DjiMotors + isKickstartDone = true; + } +} + +void FlywheelFireDjiCommand::end(bool) +{ + flywheel->stopFiring(); +} + +bool FlywheelFireDjiCommand::isFinished() const +{ + return false; +} + +} // namespace flywheel + +} // namespace control + + + diff --git a/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_fire_dji_command.hpp b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_fire_dji_command.hpp new file mode 100644 index 0000000..fef9a61 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/flywheel/flywheel_fire_dji_command.hpp @@ -0,0 +1,53 @@ +#ifndef FLYWHEEL_FIRE_DJI_COMMAND_HPP_ +#define FLYWHEEL_FIRE_DJI_COMMAND_HPP_ + +#include "tap/control/command.hpp" +#include "control/drivers/drivers.hpp" +#include "flywheel_dji_subsystem.hpp" + +namespace control +{ +namespace flywheel +{ + +class FlywheelFireDjiCommand : public tap::control::Command +{ +public: + + /** + * Constructs a new Flywheel fire command + * @param[in] flywheel a pointer to the flywheel to be passed in that this + * Command will interact with. + */ + FlywheelFireDjiCommand(FlywheelDjiSubsystem *const flywheel, src::Drivers *drivers); + + FlywheelFireDjiCommand(const FlywheelFireDjiCommand &other) = delete; + + FlywheelFireDjiCommand &operator=(const FlywheelFireDjiCommand &other) = delete; + + const char *getName() const { return "flywheel fire command"; } + + void initialize() override; + + bool isFinished() const override; + + void execute() override; + + void end(bool) override; + +private: + // Hardware constants, not specific to any particular flywheel subsystem. + FlywheelDjiSubsystem *const flywheel; + + src::Drivers *drivers; + + bool isKickstartDone = false; + uint32_t startingTs; + +}; // class FlywheelFireDjiCommand + +} // namespace flywheel + +} // namespace control + +#endif // FLYWHEEL_FIRE_COMMAND_HPP diff --git a/PolySTAR-Taproot-project/src/subsystems/sentry_general_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/sentry_general_constants.hpp new file mode 100644 index 0000000..84fce09 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/sentry_general_constants.hpp @@ -0,0 +1,9 @@ +#pragma once + +#include + +/** + * Time (ms) the sentry will wait before listening to jetson commands + * We do this since we can't read the refSerial data (for some unknown reason) + */ +constexpr uint32_t START_MATCH_WAIT_TIME = 30 * 1000; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/constants/hero_turret_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/constants/hero_turret_constants.hpp new file mode 100644 index 0000000..fe4ab36 --- /dev/null +++ b/PolySTAR-Taproot-project/src/subsystems/turret/constants/hero_turret_constants.hpp @@ -0,0 +1,99 @@ +#include "tap/algorithms/smooth_pid.hpp" + +/** + * Turret Position Controllers: Cascaded PID parameters for turret position (pitch and yaw). + */ + +static constexpr tap::algorithms::SmoothPidConfig PITCH_OUTER_PID_CONFIG( + 0.3f, // kP + 0.0f, // kI + 0.8f, // kD + 20.0f, // Max error sum + 60.0f, // Max output + 1.0f, // TQ Derivative Kalman + 0.0f, // TR Derivative Kalman + 1.0f, // TQ Proportional Kalman + 0.0f, // TR Proportional Kalman + 2.0f, // Error Deadzone + 0.0f // Error derivative floor +); + +static constexpr tap::algorithms::SmoothPidConfig PITCH_INNER_PID_CONFIG( + 260.0f, // kP + 0.8f, // kI + 0.0f, // kD + 5000.0f, // Max error sum + 16000.0f, // Max output + 1.0f, // TQ Derivative Kalman + 0.0f, // TR Derivative Kalman + 1.0f, // TQ Proportional Kalman + 0.0f, // TR Proportional Kalman + 0.0f, // Error Deadzone + 0.0f // Error derivative floor +); + +static constexpr tap::algorithms::SmoothPidConfig YAW_OUTER_PID_CONFIG( + 0.08f, // kP + 0.0f, // kI + 0.45f, // kD + 20.0f, // Max error sum + 60.0f, // Max output + 1.0f, // TQ Derivative Kalman + 0.0f, // TR Derivative Kalman + 1.0f, // TQ Proportional Kalman + 0.0f, // TR Proportional Kalman + 2.0f, // Error Deadzone + 0.0f // Error derivative floor +); + +static constexpr tap::algorithms::SmoothPidConfig YAW_INNER_PID_CONFIG( + 300.0f, // kP + 0.0f, // kI + 0.0f, // kD + 5000.0f, // Max error sum + 16000.0f, // Max output + 1.0f, // TQ Derivative Kalman + 0.0f, // TR Derivative Kalman + 1.0f, // TQ Proportional Kalman + 0.0f, // TR Proportional Kalman + 0.0f, // Error Deadzone + 0.0f // Error derivative floor +); + +/** + * Neutral position values for YAW and PITCH. Corresponds to turret aiming straight ahead, parallel to ground. + */ +static constexpr uint16_t YAW_NEUTRAL_POS = 4072; +static constexpr uint16_t PITCH_NEUTRAL_POS = 5150; + +/** + * Range values for YAW and PITCH. Motion is limited to range [-Range, +Range] from neutral position. + */ +static constexpr float YAW_RANGE_DEGREES = 90; +static constexpr float PITCH_RANGE_DEGREES = 40; + +/** + * Range values in encoder ticks, where 0..8191 is a full revolution + */ +static constexpr uint16_t YAW_RANGE = (uint16_t)(YAW_RANGE_DEGREES * 8192.0f / 360.0f); +static constexpr uint16_t PITCH_RANGE = (uint16_t)(PITCH_RANGE_DEGREES * 8192.0f / 360.0f); + +/** + * Scale factor for converting user inputs into position setpoint deltas. + * In other words, input sensitivity. + */ +static constexpr float YAW_SCALE_FACTOR = 900.0f; +static constexpr float PITCH_SCALE_FACTOR = 400.0f; + +/* + * Mouse sensitivity + */ +static constexpr float TURRET_MOUSE_X_SCALE_FACTOR = 0.05f; +static constexpr float TURRET_MOUSE_Y_SCALE_FACTOR = -0.05f; + +/** + * Inverted directions + */ + +static constexpr float YAW_IS_INVERTED = true; +static constexpr float PITCH_IS_INVERTED = true; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/constants/sentry_turret_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/constants/sentry_turret_constants.hpp index b7cf5f1..b279ead 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/constants/sentry_turret_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/constants/sentry_turret_constants.hpp @@ -20,11 +20,11 @@ static constexpr tap::algorithms::SmoothPidConfig PITCH_OUTER_PID_CONFIG( ); static constexpr tap::algorithms::SmoothPidConfig PITCH_INNER_PID_CONFIG( - 265.0f, // kP + 200.0f, // kP 0.1f, // kI - 0.0f, // kD + 0.1f, // kD 5000.0f, // Max error sum - 16000.0f, // Max output + 18000.0f, // Max output 1.0f, // TQ Derivative Kalman 0.0f, // TR Derivative Kalman 1.0f, // TQ Proportional Kalman @@ -48,7 +48,7 @@ static constexpr tap::algorithms::SmoothPidConfig YAW_OUTER_PID_CONFIG( ); static constexpr tap::algorithms::SmoothPidConfig YAW_INNER_PID_CONFIG( - 350.0f, // kP + 250.0f, // kP 0.17f, // kI 0.0f, // kD 5000.0f, // Max error sum @@ -64,8 +64,8 @@ static constexpr tap::algorithms::SmoothPidConfig YAW_INNER_PID_CONFIG( /** * Neutral position values for YAW and PITCH. Corresponds to turret aiming straight ahead, parallel to ground. */ -static constexpr int64_t YAW_NEUTRAL_POS = 5487; -static constexpr int64_t PITCH_NEUTRAL_POS = 4000; +static constexpr int64_t YAW_NEUTRAL_POS = 6900; +static constexpr int64_t PITCH_NEUTRAL_POS = 3900; /** * Turret Pos PID: PID controllers for turret position (pitch and yaw). The PID parameters for the diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/constants/spin_to_win_turret_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/constants/spin_to_win_turret_constants.hpp index 05b2d0c..7d668a5 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/constants/spin_to_win_turret_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/constants/spin_to_win_turret_constants.hpp @@ -19,8 +19,8 @@ static constexpr tap::algorithms::SmoothPidConfig PITCH_OUTER_PID_CONFIG( ); static constexpr tap::algorithms::SmoothPidConfig PITCH_INNER_PID_CONFIG( - 260.0f, // kP - 0.8f, // kI + 70.0f, // kP + 0.1f, // kI 0.0f, // kD 5000.0f, // Max error sum 16000.0f, // Max output diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_auto_aim_command.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_auto_aim_command.cpp index 6377ae3..7989cd2 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_auto_aim_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_auto_aim_command.cpp @@ -3,6 +3,8 @@ #include "tap/algorithms/math_user_utils.hpp" #include "tap/errors/create_errors.hpp" +#include "subsystems/sentry_general_constants.hpp" + namespace control { namespace turret @@ -14,13 +16,19 @@ TurretAutoAimCommand::TurretAutoAimCommand( { } -void TurretAutoAimCommand::execute() +void TurretAutoAimCommand::initialize() +{ + startMatchTimeout.restart(START_MATCH_WAIT_TIME); +} + +void TurretAutoAimCommand::execute() { - if (drivers->refSerial.getGameData().gameStage != tap::communication::serial::RefSerialData::Rx::GameStage::IN_GAME) + if (!startMatchTimeout.isExpired()) { - turret->setAbsoluteOutputDegrees(turret->getYawNeutralPos(), turret->getPitchNeutralPos()); + turret->setAbsoluteOutputDegrees(0, 0); return; } + drivers->leds.set(tap::gpio::Leds::D, true); // Acquire setpoints received from CV over serial through CVHandler GenericAutoAimCommand::execute(); } diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_auto_aim_command.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_auto_aim_command.hpp index 30249fa..4f2fcc1 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_auto_aim_command.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_auto_aim_command.hpp @@ -3,6 +3,8 @@ #include "generic_auto_aim_command.hpp" +#include "subsystems/sentry_general_constants.hpp" + namespace control { namespace turret @@ -24,9 +26,13 @@ class TurretAutoAimCommand : public GenericAutoAimCommand TurretAutoAimCommand &operator=(const TurretAutoAimCommand &other) = delete; const char *getName() const { return "turret auto aim command"; } + + void initialize() override; void execute() override; +private: + tap::arch::MilliTimeout startMatchTimeout; }; // TurretAutoAimCommand } // namespace turret diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp index c598ac4..71fbae9 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_constants.hpp @@ -18,6 +18,10 @@ using tap::communication::serial::Uart; #ifdef TARGET_SENTRY #include "constants/sentry_turret_constants.hpp" #endif + +#ifdef TARGET_HERO +#include "constants/hero_turret_constants.hpp" +#endif /** * Right joystick dead zone size. If the absolute value returned by the stick is below * this value, it is considered zero. @@ -47,5 +51,5 @@ static constexpr float DEGREE_TO_MILLIRAD = 17.453293; * Spin2win stabilization constants * Represents how much the joystick should have move per ms to stabilize the turret */ -static constexpr float LOW_ROTATION = 0.56; -static constexpr float HIGH_ROTATION = 0.85; \ No newline at end of file +static constexpr float LOW_ROTATION = 0.67; +static constexpr float HIGH_ROTATION = 0.95; \ No newline at end of file diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_manual_aim_command.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_manual_aim_command.cpp index c2ca599..97e7acc 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_manual_aim_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_manual_aim_command.cpp @@ -29,6 +29,12 @@ void TurretManualAimCommand::execute() float xInput = drivers->controlInterface.getTurretXInput(); float yInput = drivers->controlInterface.getTurretYInput(); + float xMouseInput = drivers->controlInterface.getTurretXMouseInput() * TURRET_MOUSE_X_SCALE_FACTOR; + float yMouseInput = drivers->controlInterface.getTurretYMouseInput() * TURRET_MOUSE_Y_SCALE_FACTOR; + + xInput += xMouseInput; + yInput += yMouseInput; + turret->setRelativeOutput( fabs(xInput) >= TURRET_DEAD_ZONE ? xInput : 0.0f, // Inverted Left-Right fabs(yInput) >= TURRET_DEAD_ZONE ? yInput : 0.0f); diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_mouse_aim_command.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_mouse_aim_command.cpp index 5df94d0..5551d81 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_mouse_aim_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_mouse_aim_command.cpp @@ -26,7 +26,7 @@ void TurretMouseAimCommand::initialize() {} void TurretMouseAimCommand::execute() { float xMouseInput = drivers->controlInterface.getTurretXMouseInput() * TURRET_MOUSE_X_SCALE_FACTOR; - float yMouseInput = drivers->controlInterface.getTurretYMouseInput() * TURRET_MOUSE_Y_SCALE_FACTOR ; + float yMouseInput = drivers->controlInterface.getTurretYMouseInput() * TURRET_MOUSE_Y_SCALE_FACTOR; turret->setRelativeOutput(xMouseInput, yMouseInput); } diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_stable_mouse_aim_command.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_stable_mouse_aim_command.cpp index 52213ad..bc73b06 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_stable_mouse_aim_command.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_stable_mouse_aim_command.cpp @@ -14,6 +14,7 @@ TurretStableMouseAimCommand::TurretStableMouseAimCommand( chassis::ChassisSpin2winKeyboardCommand *const chassisCommand, src::Drivers *drivers) : turret(turret), + chassisCommand(chassisCommand), drivers(drivers) { if (turret == nullptr) @@ -23,12 +24,20 @@ TurretStableMouseAimCommand::TurretStableMouseAimCommand( this->addSubsystemRequirement(dynamic_cast(turret)); } -void TurretStableMouseAimCommand::initialize() {} +void TurretStableMouseAimCommand::initialize() { + prevUpdate = tap::arch::clock::getTimeMilliseconds(); +} void TurretStableMouseAimCommand::execute() { float xMouseInput = drivers->controlInterface.getTurretXMouseInput() * TURRET_MOUSE_X_SCALE_FACTOR; float yMouseInput = drivers->controlInterface.getTurretYMouseInput() * TURRET_MOUSE_Y_SCALE_FACTOR ; + uint32_t currentUpdate = tap::arch::clock::getTimeMilliseconds(); + uint32_t timeDelta = currentUpdate - prevUpdate; + prevUpdate = currentUpdate; + + xMouseInput += (chassisCommand->isMoving() ? LOW_ROTATION : HIGH_ROTATION) * timeDelta; + turret->setRelativeOutput(xMouseInput, yMouseInput); } diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_stable_mouse_aim_command.hpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_stable_mouse_aim_command.hpp index b6c0cbc..9a8be61 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_stable_mouse_aim_command.hpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_stable_mouse_aim_command.hpp @@ -42,6 +42,9 @@ class TurretStableMouseAimCommand : public tap::control::Command TurretSubsystem *const turret; src::Drivers *drivers; + + uint32_t prevUpdate; + chassis::ChassisSpin2winKeyboardCommand *const chassisCommand; }; // TurretStableMouseAimCommand } // namespace turret diff --git a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp index a178183..293670e 100644 --- a/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp +++ b/PolySTAR-Taproot-project/src/subsystems/turret/turret_subsystem.cpp @@ -131,13 +131,13 @@ void TurretSubsystem::setRelativeOutput(float yawDelta, float pitchDelta) void TurretSubsystem::sendCVUpdate() { // Get motor encoder positions in body frame (neutral position is straight ahead, parallel to ground) - float currentBodyYawDeg = yawMotor->encoderToDegrees(yawMotor->getEncoderUnwrapped()-YAW_NEUTRAL_POS); - float currentBodyPitchDeg = pitchMotor.encoderToDegrees(pitchMotor.getEncoderWrapped()-PITCH_NEUTRAL_POS); + float currentBodyYawDeg = yawMotor->encoderToDegrees(yawMotor->getEncoderUnwrapped()-YAW_NEUTRAL_POS); + float currentBodyPitchDeg = pitchMotor.encoderToDegrees(pitchMotor.getEncoderWrapped()-PITCH_NEUTRAL_POS); src::communication::cv::CVSerialData::Tx::TurretMessage turretMessage; // CV protocol expects angles in milliradians turretMessage.yaw = static_cast(currentBodyYawDeg*DEGREE_TO_MILLIRAD); - turretMessage.pitch = static_cast(currentBodyPitchDeg*DEGREE_TO_MILLIRAD); + turretMessage.pitch = static_cast(currentBodyPitchDeg*DEGREE_TO_MILLIRAD * -1); drivers->uart.write(Uart::UartPort::Uart7, (uint8_t*)(&turretMessage), sizeof(turretMessage)); }