Merge pull request #158 from ut-ras/update-taproot

Update taproot to latest develop

ut-robomaster/project.xml

@@ -29,6 +29,22 @@
     <option name="taproot:board:analog_in_pins"></option>
     <option name="taproot:board:pwm_pins">C1,C2,C3,C4,C5,C6,C7,Buzzer,ImuHeater</option>
 
+    <option name="taproot:modm-project.xml:modm_hal_modules">
+      modm:platform:i2c:2
+      modm:platform:rtt
+    </option>
+
+    <option name="taproot:modm-project.xml:modm_hal_options">
+      modm:platform:rtt:buffer.rx 0,00,000,0000
+      modm:platform:rtt:buffer.tx 1024,0b10000000000,0o2000,0x400
+      modm:platform:uart:1:buffer.rx 256
+      modm:platform:uart:1:buffer.tx 256
+      modm:platform:uart:3:buffer.tx 256
+      modm:platform:uart:3:buffer.rx 256
+      modm:platform:uart:6:buffer.rx 256
+      modm:platform:uart:6:buffer.tx 256
+    </option>
+
   </options>
   <modules>
     <module>taproot:core</module>

ut-robomaster/src/main.cpp

@@ -42,6 +42,12 @@ static void updateIo(src::Drivers *drivers)
     drivers->remote.read();
 }
 
+static void updateImu(src::Drivers *drivers)
+{
+    drivers->bmi088.read();
+    drivers->bmi088.periodicIMUUpdate();
+}
+
 src::Drivers drivers;
 RobotControl control{&drivers};
 
@@ -61,7 +67,7 @@ int main()
 
         if (refreshTimer.execute())
         {
-            PROFILE(drivers.profiler, drivers.bmi088.periodicIMUUpdate, ());
+            PROFILE(drivers.profiler, updateImu, (&drivers));
             PROFILE(drivers.profiler, drivers.commandScheduler.run, ());
             PROFILE(drivers.profiler, drivers.djiMotorTxHandler.encodeAndSendCanData, ());
             PROFILE(drivers.profiler, drivers.terminalSerial.update, ());

ut-robomaster/src/subsystems/agitator/agitator_subsystem.cpp

@@ -2,13 +2,9 @@
 
 namespace subsystems::agitator
 {
-
 using tap::algorithms::compareFloatClose;
 using tap::arch::clock::getTimeMilliseconds;
 
-/**
- * AgitatorSubsystem class instantiation
- */
 #if defined(TARGET_STANDARD) || defined(TARGET_SENTRY)
 AgitatorSubsystem::AgitatorSubsystem(
     src::Drivers* drivers,

ut-robomaster/src/subsystems/agitator/agitator_subsystem.hpp

@@ -9,9 +9,7 @@
 
 #include "drivers.hpp"
 
-namespace subsystems
-{
-namespace agitator
+namespace subsystems::agitator
 {
 using flywheel::FlywheelSubsystem;
 using motors::MotorController;
@@ -20,11 +18,7 @@ class AgitatorSubsystem : public tap::control::Subsystem
 {
 public:
     AgitatorSubsystem(src::Drivers *drivers, FlywheelSubsystem *flywheel, MotorConfig motor);
-
-    ~AgitatorSubsystem() = default;
-
     void initialize() override;
-
     void refresh() override;
 
     float getShapedVelocity(float time, float a, float phi, float ballsPerSecond);
@@ -44,6 +38,4 @@ class AgitatorSubsystem : public tap::control::Subsystem
     MotorController feeder;
 #endif
 };
-
-}  // namespace agitator
-}  // namespace subsystems
+}  // namespace subsystems::agitator

ut-robomaster/src/subsystems/chassis/chassis_subsystem.cpp

@@ -4,12 +4,10 @@
 
 #include "robots/robot_constants.hpp"
 
+namespace subsystems::chassis
+{
 using namespace tap::algorithms;
 
-namespace subsystems
-{
-namespace chassis
-{
 ChassisSubsystem::ChassisSubsystem(src::Drivers* drivers)
     : tap::control::Subsystem(drivers),
       drivers(drivers),
@@ -66,11 +64,6 @@ void ChassisSubsystem::limitChassisPower()
     }
 }
 
-void ChassisSubsystem::runHardwareTests()
-{
-    // TODO
-}
-
 void ChassisSubsystem::input(Vector2f move, float spin)
 {
     Vector2f v = move * MAX_LINEAR_VEL;
@@ -124,5 +117,4 @@ Vector3f ChassisSubsystem::measureVelocity()
     // Rotated -90 deg to match our reference frame
     return Vector3f(ya, -xa, wa) * WHEEL_RADIUS / 4.0f * M_TWOPI;
 }
-}  // namespace chassis
-}  // namespace subsystems
+}  // namespace subsystems::chassis

ut-robomaster/src/subsystems/chassis/chassis_subsystem.hpp

@@ -1,5 +1,4 @@
-#ifndef CHASSIS_SUBSYSTEM_HPP_
-#define CHASSIS_SUBSYSTEM_HPP_
+#pragma once
 
 #include "tap/control/subsystem.hpp"
 
@@ -10,27 +9,21 @@
 
 #include "drivers.hpp"
 
+namespace subsystems::chassis
+{
 using namespace tap::communication::sensors::imu;
 using namespace modm;
 using motors::MotorController;
 
-namespace subsystems
-{
-namespace chassis
-{
 class ChassisSubsystem : public tap::control::Subsystem
 {
 public:
     ChassisSubsystem(src::Drivers* drivers);
-
     void initialize() override;
-
     void refresh() override;
 
     void limitChassisPower();
 
-    void runHardwareTests() override;
-
     /// @brief Update robot motion based on simple input controls. Inputs are scaled and corrected
     /// to avoid over-driving motors. This logic can be adjusted to create various input schemes.
     /// @param move Linear movement (magnitude should be within [0,1])
@@ -41,8 +34,6 @@ class ChassisSubsystem : public tap::control::Subsystem
     /// @return x,y is linear velocity (m/s) and z is angular velocity (rad/s)
     Vector3f measureVelocity();
 
-    const char* getName() override { return "Chassis subsystem"; }
-
 private:
     src::Drivers* drivers;
     power_limiter::PowerLimiter powerLimiter;
@@ -64,7 +55,4 @@ class ChassisSubsystem : public tap::control::Subsystem
     static constexpr float ENERGY_BUFFER_CRIT_THRESHOLD = 30.0f;
 #endif
 };
-}  // namespace chassis
-}  // namespace subsystems
-
-#endif
+}  // namespace subsystems::chassis

ut-robomaster/src/subsystems/flywheel/flywheel_subsystem.hpp

@@ -1,5 +1,4 @@
-#ifndef SUBSYSTEMS_SHOOTER_FLYWHEEL_SUBSYSTEM_HPP_
-#define SUBSYSTEMS_SHOOTER_FLYWHEEL_SUBSYSTEM_HPP_
+#pragma once
 
 #include "tap/control/subsystem.hpp"
 
@@ -8,22 +7,15 @@
 
 #include "drivers.hpp"
 
-namespace subsystems
+namespace subsystems::flywheel
 {
-namespace flywheel
-{
-
 using motors::MotorController;
 
 class FlywheelSubsystem : public tap::control::Subsystem
 {
 public:
     FlywheelSubsystem(src::Drivers* drivers);
-
-    ~FlywheelSubsystem() = default;
-
     void initialize() override;
-
     void refresh() override;
 
     /// @brief Change flywheel velocity.
@@ -37,8 +29,4 @@ class FlywheelSubsystem : public tap::control::Subsystem
     MotorController motors[FLYWHEELS];
     float velocity = 0.0f;
 };
-
-}  // namespace flywheel
-}  // namespace subsystems
-
-#endif  // SUBSYSTEMS_SHOOTER_FLYWHEEL_SUBSYSTEM_HPP_
+}  // namespace subsystems::flywheel

ut-robomaster/src/subsystems/odometry/observer_yaw_world.cpp

@@ -7,7 +7,7 @@ ChassisWorldYawObserver::ChassisWorldYawObserver(src::Drivers* drivers) : driver
 bool ChassisWorldYawObserver::getChassisWorldYaw(float* yaw) const
 {
     bmi088::Bmi088* imu = &drivers->bmi088;
-    *yaw = modm::toRadian(imu->getYaw() - 180.0f);
+    *yaw = modm::toRadian(imu->getYaw());
 
     return imu->getImuState() == ImuInterface::ImuState::IMU_CALIBRATED;
 };

ut-robomaster/src/subsystems/odometry/odometry_subsystem.cpp

@@ -1,9 +1,8 @@
 #include "odometry_subsystem.hpp"
 
 #include "robots/robot_constants.hpp"
-namespace subsystems
-{
-namespace odometry
+
+namespace subsystems::odometry
 {
 OdometrySubsystem::OdometrySubsystem(
     src::Drivers* drivers,
@@ -15,9 +14,9 @@ OdometrySubsystem::OdometrySubsystem(
       turret(turret),
       chassisDisplacement(drivers, chassis),
       chassisYaw(drivers),
-      chassisTracker(&chassisYaw, &chassisDisplacement){};
+      chassisTracker(&chassisYaw, &chassisDisplacement) {};
 
-void OdometrySubsystem::initialize(){};
+void OdometrySubsystem::initialize() {};
 void OdometrySubsystem::refresh() { chassisTracker.update(); }
 
 Vector2f OdometrySubsystem::getPosition()
@@ -31,5 +30,4 @@ float OdometrySubsystem::getChassisAngularVelocity() { return chassis->measureVe
 
 float OdometrySubsystem::getTurretLocalYaw() { return turret->getCurrentLocalPitch(); }
 float OdometrySubsystem::getTurretLocalPitch() { return turret->getCurrentLocalYaw(); }
-};  // namespace odometry
-}  // namespace subsystems
+}  // namespace subsystems::odometry

ut-robomaster/src/subsystems/odometry/odometry_subsystem.hpp

@@ -15,12 +15,10 @@
 
 #include "drivers.hpp"
 
-namespace subsystems
+namespace subsystems::odometry
 {
-namespace odometry
-{
-
 using chassis::ChassisSubsystem;
+using modm::Vector2f;
 using tap::algorithms::odometry::Odometry2DTracker;
 using turret::TurretSubsystem;
 
@@ -30,7 +28,6 @@ class OdometrySubsystem : public tap::control::Subsystem
     OdometrySubsystem(src::Drivers* drivers, ChassisSubsystem* chassis, TurretSubsystem* turret);
     void initialize() override;
     void refresh() override;
-    const char* getName() override { return "Odometry subsystem"; }
 
     Vector2f getPosition();
     Vector2f getLinearVelocity();
@@ -50,5 +47,4 @@ class OdometrySubsystem : public tap::control::Subsystem
     ChassisWorldYawObserver chassisYaw;
     Odometry2DTracker chassisTracker;
 };
-}  // namespace odometry
-}  // namespace subsystems
+}  // namespace subsystems::odometry

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

@@ -1,24 +1,24 @@
 #include "sound_subsystem.hpp"
-namespace subsystems
-{
-namespace sound
+
+#include "tap/communication/sensors/buzzer/buzzer.hpp"
+
+namespace subsystems::sound
 {
+using tap::gpio::Pwm;
+
 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
+}  // namespace subsystems::sound

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

@@ -1,30 +1,22 @@
 #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 subsystems::sound
 {
-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

ut-robomaster/src/subsystems/turret/command_move_turret_aimbot.cpp

@@ -6,6 +6,7 @@ namespace commands
 {
 using namespace tap::algorithms::ballistics;
 using communication::TurretData;
+using modm::Vector3f;
 
 void CommandMoveTurretAimbot::initialize() {}
 
@@ -40,7 +41,7 @@ void CommandMoveTurretAimbot::execute()
         targetVel = rotMat * targetVel;
         targetAcc = rotMat * targetAcc;
 
-        MeasuredKinematicState kinState{targetPos, targetVel, targetAcc};
+        SecondOrderKinematicState kinState{targetPos, targetVel, targetAcc};
 
         float turretPitch = 0.0f;
         float turretYaw = 0.0f;