Add sound effects using the built-in buzzer

ut-robomaster/src/robots/hero/hero_constants.hpp

@@ -5,6 +5,7 @@
 #include "tap/motor/dji_motor.hpp"
 
 #include "modm/container/pair.hpp"
+#include "subsystems/sound/sounds/sound_smb_powerup.hpp"
 #include "utils/motor_controller/pid.hpp"
 
 using motor_controller::PidConstants;
@@ -106,4 +107,6 @@ static constexpr float YAW_INPUT_SCALE = 4.0f;
 static constexpr float PITCH_INPUT_SCALE = 5.0f;
 
 static constexpr float MOUSE_SENS_YAW = 0.0045f;
-static constexpr float MOUSE_SENS_PITCH = 0.002f;
+static constexpr float MOUSE_SENS_PITCH = 0.002f;
+
+const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;

ut-robomaster/src/robots/hero/hero_control.cpp

@@ -36,6 +36,10 @@
 #include "subsystems/turret/command_move_turret_mouse.hpp"
 #include "subsystems/turret/turret_subsystem.hpp"
 
+// Sound includes -----------------------------------------
+#include "subsystems/sound/command_play_sound.hpp"
+#include "subsystems/sound/sound_subsystem.hpp"
+
 using namespace tap::control;
 using namespace tap::communication::serial;
 
@@ -44,6 +48,7 @@ using namespace subsystems::agitator;
 using namespace subsystems::flywheel;
 using namespace subsystems::turret;
 using namespace subsystems::odometry;
+using namespace subsystems::sound;
 
 using namespace commands;
 
@@ -66,6 +71,7 @@ AgitatorSubsystem agitator(drivers(), ID_AGITATOR, false);
 FlywheelSubsystem flywheel(drivers());
 TurretSubsystem turret(drivers());
 OdometrySubsystem odometry(drivers(), &chassis, &turret);
+SoundSubsystem sound(drivers());
 
 // Command definitions -----------------------------------------------------------
 CommandMoveChassisJoystick moveChassisCommandJoystick(drivers(), &chassis, &turret);
@@ -88,6 +94,8 @@ CommandMoveTurretJoystick moveTurretCommandJoystick(drivers(), &turret);
 CommandMoveTurretJoystick moveTurretWhenChassisIsTurretRelativeCommandJoystick(drivers(), &turret);
 CommandMoveTurretMouse moveTurretCommandMouse(drivers(), &turret);
 
+CommandPlaySound playStartupSoundCommand(drivers(), &sound, SOUND_STARTUP);
+
 // Keyboard mappings ------------------------------------------------------------
 ToggleCommandMapping keyRToggled(
     drivers(),
@@ -141,6 +149,7 @@ void registerStandardSubsystems(src::Drivers *drivers)
     drivers->commandScheduler.registerSubsystem(&flywheel);
     drivers->commandScheduler.registerSubsystem(&turret);
     drivers->commandScheduler.registerSubsystem(&odometry);
+    drivers->commandScheduler.registerSubsystem(&sound);
 }
 
 // Initialize subsystems here ---------------------------------------------
@@ -151,6 +160,7 @@ void initializeSubsystems()
     flywheel.initialize();
     turret.initialize();
     odometry.initialize();
+    sound.initialize();
 }
 
 // Set default commands here -----------------------------------------------
@@ -161,7 +171,10 @@ void setDefaultCommands(src::Drivers *)
     turret.setDefaultCommand(&moveTurretCommandMouse);
 }
 
-void runStartupCommands(src::Drivers *) {}
+void runStartupCommands(src::Drivers *drivers)
+{
+    drivers->commandScheduler.addCommand(&playStartupSoundCommand);
+}
 
 // Register IO mappings here -----------------------------------------------
 void registerMappings(src::Drivers *drivers)

ut-robomaster/src/robots/sentry/sentry_constants.hpp

@@ -5,6 +5,7 @@
 #include "tap/motor/dji_motor.hpp"
 
 #include "modm/container/pair.hpp"
+#include "subsystems/sound/sounds/sound_smb_powerup.hpp"
 #include "utils/motor_controller/pid.hpp"
 
 using motor_controller::PidConstants;
@@ -116,4 +117,6 @@ static constexpr tap::algorithms::SmoothPidConfig PITCH_PID_CONFIG = {
     .tRProportionalKalman = 0.0f,
     .errDeadzone = 0.0f,
     .errorDerivativeFloor = 0.0f,
-};
+};
+
+const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;

ut-robomaster/src/robots/sentry/sentry_control.cpp

@@ -27,6 +27,10 @@
 #include "subsystems/turret/command_sentry_aim.hpp"
 #include "subsystems/turret/turret_subsystem.hpp"
 
+// Sound includes -----------------------------------------
+#include "subsystems/sound/command_play_sound.hpp"
+#include "subsystems/sound/sound_subsystem.hpp"
+
 using namespace tap::control;
 using namespace tap::communication::serial;
 
@@ -35,6 +39,7 @@ using namespace subsystems::agitator;
 using namespace subsystems::flywheel;
 using namespace subsystems::turret;
 using namespace subsystems::odometry;
+using namespace subsystems::sound;
 
 using namespace commands;
 
@@ -56,11 +61,14 @@ AgitatorSubsystem agitator2(drivers(), ID_AGITATOR_R, true);
 FlywheelSubsystem flywheel(drivers());
 TurretSubsystem turret(drivers());
 OdometrySubsystem odometry(drivers(), &chassis, &turret);
+SoundSubsystem sound(drivers());
 
 // Command definitions -----------------------------------------------------------
 CommandSentryPosition sentryPositionCommand(drivers(), &chassis);
 CommandSentryAim sentryAimCommand(drivers(), &turret);
 
+CommandPlaySound playStartupSoundCommand(drivers(), &sound, SOUND_STARTUP);
+
 // Register subsystems here -----------------------------------------------
 void registerStandardSubsystems(src::Drivers *drivers)
 {
@@ -70,6 +78,7 @@ void registerStandardSubsystems(src::Drivers *drivers)
     drivers->commandScheduler.registerSubsystem(&flywheel);
     drivers->commandScheduler.registerSubsystem(&turret);
     drivers->commandScheduler.registerSubsystem(&odometry);
+    drivers->commandScheduler.registerSubsystem(&sound);
 }
 
 // Initialize subsystems here ---------------------------------------------
@@ -81,6 +90,7 @@ void initializeSubsystems()
     flywheel.initialize();
     turret.initialize();
     odometry.initialize();
+    sound.initialize();
 }
 
 // Set default commands here -----------------------------------------------
@@ -90,7 +100,10 @@ void setDefaultCommands(src::Drivers *)
     turret.setDefaultCommand(&sentryAimCommand);
 }
 
-void runStartupCommands(src::Drivers *) {}
+void runStartupCommands(src::Drivers *drivers)
+{
+    drivers->commandScheduler.addCommand(&playStartupSoundCommand);
+}
 
 // Register IO mappings here -----------------------------------------------
 void registerMappings(src::Drivers *drivers) {}

ut-robomaster/src/robots/standard/standard_constants.hpp

@@ -5,6 +5,7 @@
 #include "tap/motor/dji_motor.hpp"
 
 #include "modm/container/pair.hpp"
+#include "subsystems/sound/sounds/sound_smb_powerup.hpp"
 #include "utils/motor_controller/pid.hpp"
 
 using motor_controller::PidConstants;
@@ -115,4 +116,6 @@ static constexpr tap::algorithms::SmoothPidConfig PITCH_PID_CONFIG = {
     .tRProportionalKalman = 0.0f,
     .errDeadzone = 0.0f,
     .errorDerivativeFloor = 0.0f,
-};
+};
+
+const Sound SOUND_STARTUP = SOUND_SMB_POWERUP;

ut-robomaster/src/robots/standard/standard_control.cpp

@@ -37,6 +37,10 @@
 #include "subsystems/turret/command_move_turret_mouse.hpp"
 #include "subsystems/turret/turret_subsystem.hpp"
 
+// Sound includes -----------------------------------------
+#include "subsystems/sound/command_play_sound.hpp"
+#include "subsystems/sound/sound_subsystem.hpp"
+
 using namespace tap::control;
 using namespace tap::communication::serial;
 
@@ -45,6 +49,7 @@ using namespace subsystems::agitator;
 using namespace subsystems::flywheel;
 using namespace subsystems::turret;
 using namespace subsystems::odometry;
+using namespace subsystems::sound;
 
 using namespace commands;
 
@@ -68,6 +73,7 @@ AgitatorSubsystem agitator2(drivers(), ID_AGITATOR_R, true);
 FlywheelSubsystem flywheel(drivers());
 TurretSubsystem turret(drivers());
 OdometrySubsystem odometry(drivers(), &chassis, &turret);
+SoundSubsystem sound(drivers());
 
 // Command definitions -----------------------------------------------------------
 CommandMoveChassisJoystick moveChassisCommandJoystick(drivers(), &chassis, &turret);
@@ -93,6 +99,8 @@ CommandMoveTurretJoystick moveTurretWhenChassisIsTurretRelativeCommandJoystick(d
 CommandMoveTurretMouse moveTurretCommandMouse(drivers(), &turret);
 CommandMoveTurretAimbot moveTurretCommandAimbot(drivers(), &turret);
 
+CommandPlaySound playStartupSoundCommand(drivers(), &sound, SOUND_STARTUP);
+
 // Keyboard mappings ------------------------------------------------------------
 ToggleCommandMapping keyRToggled(
     drivers(),
@@ -155,6 +163,7 @@ void registerStandardSubsystems(src::Drivers *drivers)
     drivers->commandScheduler.registerSubsystem(&flywheel);
     drivers->commandScheduler.registerSubsystem(&turret);
     drivers->commandScheduler.registerSubsystem(&odometry);
+    drivers->commandScheduler.registerSubsystem(&sound);
 }
 
 // Initialize subsystems here ---------------------------------------------
@@ -166,6 +175,7 @@ void initializeSubsystems()
     flywheel.initialize();
     turret.initialize();
     odometry.initialize();
+    sound.initialize();
 }
 
 // Set default commands here -----------------------------------------------
@@ -176,7 +186,10 @@ void setDefaultCommands(src::Drivers *)
     turret.setDefaultCommand(&moveTurretCommandMouse);
 }
 
-void runStartupCommands(src::Drivers *) {}
+void runStartupCommands(src::Drivers *drivers)
+{
+    drivers->commandScheduler.addCommand(&playStartupSoundCommand);
+}
 
 // Register IO mappings here -----------------------------------------------
 void registerMappings(src::Drivers *drivers)

ut-robomaster/src/subsystems/sound/command_play_sound.cpp

@@ -0,0 +1,35 @@
+#include "command_play_sound.hpp"
+
+namespace commands
+{
+
+void CommandPlaySound::initialize()
+{
+    timer.restart();
+    subsystem->setBuzzerFrequency(sound.notes[0]);
+}
+
+void CommandPlaySound::execute()
+{
+    if (timer.execute() && ++note < sound.note_count)
+    {
+        uint16_t freq = sound.notes[note];
+
+        if (freq != sound.notes[note - 1])  // only update if the note changed
+        {
+            if (freq > 0.0f)
+            {
+                subsystem->setBuzzerFrequency(freq);
+            }
+            else
+            {
+                subsystem->silence();  // silence also kills the duty cycle
+            }
+        }
+    }
+}
+
+void CommandPlaySound::end(bool) { subsystem->silence(); }
+bool CommandPlaySound::isFinished() const { return note >= sound.note_count; }
+
+}  // namespace commands

ut-robomaster/src/subsystems/sound/command_play_sound.hpp

@@ -0,0 +1,47 @@
+#pragma once
+
+#include "tap/architecture/periodic_timer.hpp"
+#include "tap/communication/sensors/buzzer/buzzer.hpp"
+#include "tap/control/command.hpp"
+
+#include "drivers.hpp"
+#include "sound.hpp"
+#include "sound_subsystem.hpp"
+
+namespace commands
+{
+
+using subsystems::sound::SoundSubsystem;
+using tap::arch::PeriodicMilliTimer;
+
+class CommandPlaySound : public tap::control::Command
+{
+public:
+    CommandPlaySound(src::Drivers *drivers, SoundSubsystem *subsystem, Sound sound)
+        : drivers(drivers),
+          subsystem(subsystem),
+          sound(sound),
+          timer(sound.note_interval)
+    {
+        addSubsystemRequirement(subsystem);
+    }
+
+    void initialize() override;
+
+    void execute() override;
+
+    void end(bool interrupted) override;
+
+    bool isFinished() const override;
+
+    const char *getName() const override { return "play sound command"; }
+
+private:
+    src::Drivers *drivers;
+    SoundSubsystem *subsystem;
+    Sound sound;
+
+    uint16_t note = 0;
+    PeriodicMilliTimer timer;
+};  // class CommandPlaySound
+}  // namespace commands

ut-robomaster/src/subsystems/sound/sound.hpp

@@ -0,0 +1,9 @@
+#pragma once
+#include <cstdint>
+
+struct Sound
+{
+    uint16_t note_count;
+    uint16_t note_interval;
+    uint16_t* notes;
+};

ut-robomaster/src/subsystems/sound/sound_subsystem.cpp

@@ -0,0 +1,24 @@
+#include "sound_subsystem.hpp"
+namespace subsystems
+{
+namespace sound
+{
+SoundSubsystem::SoundSubsystem(src::Drivers* drivers)
+    : tap::control::Subsystem(drivers),
+      drivers(drivers)
+{
+}
+
+void SoundSubsystem::initialize() {}
+
+void SoundSubsystem::refresh() {}
+
+void SoundSubsystem::silence() { tap::buzzer::silenceBuzzer(&drivers->pwm); }
+
+void SoundSubsystem::setBuzzerFrequency(int frequency)
+{
+    tap::buzzer::playNote(&drivers->pwm, frequency);
+    // define keys with numerical frequencies
+}
+};  // namespace sound
+}  // namespace subsystems

ut-robomaster/src/subsystems/sound/sound_subsystem.hpp

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "tap/communication/gpio/pwm.hpp"
+#include "tap/communication/sensors/buzzer/buzzer.hpp"
+#include "tap/control/subsystem.hpp"
+
+#include "drivers.hpp"
+
+namespace subsystems
+{
+namespace sound
+{
+using tap::gpio::Pwm;
+
+class SoundSubsystem : public tap::control::Subsystem
+{
+public:
+    SoundSubsystem(src::Drivers* drivers);
+    void initialize() override;
+    void refresh() override;
+    const char* getName() override { return "Sound subsystem"; }
+
+    void setBuzzerFrequency(int frequency);
+    void silence();
+
+private:
+    src::Drivers* drivers;
+};
+}  // namespace sound
+}  // namespace subsystems