Merge pull request #53 from hytech-racing/feature/simple-launch-mode

Feature/simple launch mode

lib/interfaces/include/DashboardInterface.h

@@ -8,6 +8,8 @@
 #include "MCU_status.h"
 #include "InverterInterface.h"
 
+#include "TorqueControllers.h"
+
 /*
     Enum for the car's torque limits
     MOVE ME! - ideally into a TorqueControllerDefs.h file
@@ -141,7 +143,8 @@ class DashboardInterface
                 bool drivetrain_error, 
                 TorqueLimit_e torque,
                 float min_cell_voltage,
-                AnalogConversion_s glv_voltage);
+                AnalogConversion_s glv_voltage,
+                int launch_state);
 
     /*!
         getter for the dashboard's current dial position (drive profile)

lib/interfaces/src/DashboardInterface.cpp

@@ -73,7 +73,8 @@ void DashboardInterface::tick10(MCUInterface* mcu,
                                 bool drivetrain_error, 
                                 TorqueLimit_e torque,
                                 float min_cell_voltage,
-                                AnalogConversion_s glv_voltage)
+                                AnalogConversion_s glv_voltage,
+                                int launch_state)
 {
 
     soundBuzzer(buzzer);
@@ -85,6 +86,21 @@ void DashboardInterface::tick10(MCUInterface* mcu,
     setLED(DashLED_e::MC_ERROR_LED, !drivetrain_error ? LEDColors_e::ON : LEDColors_e::RED);
     setLED(DashLED_e::COCKPIT_BRB_LED, mcu->brb_ok_is_high() ? LEDColors_e::ON : LEDColors_e::RED);
 
+    switch(launch_state){
+    case 1:
+        setLED(DashLED_e::LAUNCH_CONTROL_LED, LEDColors_e::RED);
+        break;
+    case 2:
+        setLED(DashLED_e::LAUNCH_CONTROL_LED, LEDColors_e::YELLOW);
+        break;
+    case 3:
+        setLED(DashLED_e::LAUNCH_CONTROL_LED, LEDColors_e::ON);
+        break;
+    default:
+        setLED(DashLED_e::LAUNCH_CONTROL_LED, LEDColors_e::OFF);
+        break;
+    }
+
     switch(torque){
     case TorqueLimit_e::TCMUX_LOW_TORQUE:
         setLED(DashLED_e::MODE_LED, LEDColors_e::OFF);

lib/systems/include/DrivetrainSystem.tpp

@@ -235,7 +235,7 @@ DrivetrainDynamicReport_s DrivetrainSystem<InverterType>::get_current_data()
     {
         auto iq = inv_pointer->get_torque_current(); // iq in A
         auto id = inv_pointer->get_mag_current();    // id in A
-        data.measuredSpeeds[inverter_ind] = (float)inv_pointer->get_speed();
+        data.measuredSpeeds[inverter_ind] = inv_pointer->get_speed();
         data.measuredTorqueCurrents[inverter_ind] = iq;
         data.measuredMagnetizingCurrents[inverter_ind] = id;
 

lib/systems/include/TorqueControllerMux.h

@@ -19,9 +19,9 @@ class TorqueControllerMux
     // Use this to map the dial to TCMUX modes
     std::unordered_map<DialMode_e, TorqueController_e> dialModeMap_ = {
         {DialMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE},
-        {DialMode_e::MODE_1, TorqueController_e::TC_SAFE_MODE},
-        {DialMode_e::MODE_2, TorqueController_e::TC_LOAD_CELL_VECTORING},
-        {DialMode_e::MODE_3, TorqueController_e::TC_NO_CONTROLLER},
+        {DialMode_e::MODE_1, TorqueController_e::TC_LOAD_CELL_VECTORING},
+        {DialMode_e::MODE_2, TorqueController_e::TC_NO_CONTROLLER},
+        {DialMode_e::MODE_3, TorqueController_e::TC_SIMPLE_LAUNCH},
         {DialMode_e::MODE_4, TorqueController_e::TC_NO_CONTROLLER},
         {DialMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER},
     };
@@ -30,11 +30,21 @@ class TorqueControllerMux
         {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0},
         {TorqueLimit_e::TCMUX_FULL_TORQUE, 20.0}
     };
+
+    TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER;
     TorqueControllerOutput_s controllerOutputs_[static_cast<int>(TorqueController_e::TC_NUM_CONTROLLERS)];
+
     TorqueControllerNone torqueControllerNone_;
     TorqueControllerSimple torqueControllerSimple_;
     TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_;
-    TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER;
+    TorqueControllerSimpleLaunch torqueControllerSimpleLaunch_;
+    TorqueControllerBase* controllers[static_cast<int>(TorqueController_e::TC_NUM_CONTROLLERS)] = {
+        static_cast<TorqueControllerBase*>(&torqueControllerNone_),
+        static_cast<TorqueControllerBase*>(&torqueControllerSimple_),
+        static_cast<TorqueControllerBase*>(&torqueControllerLoadCellVectoring_),
+        static_cast<TorqueControllerBase*>(&torqueControllerSimpleLaunch_),
+    };
+
     DrivetrainCommand_s drivetrainCommand_;
     TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE;
     bool torqueLimitButtonPressed_ = false;
@@ -44,7 +54,8 @@ class TorqueControllerMux
     TorqueControllerMux()
     : torqueControllerNone_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_NO_CONTROLLER)])
     , torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)])
-    , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)]) {}
+    , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)])
+    , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)]) {}
 
 
     /// @brief torque controller mux constructor that leaves all other TCs with defaults accept for simple TC
@@ -53,7 +64,8 @@ class TorqueControllerMux
     TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale)
     : torqueControllerNone_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_NO_CONTROLLER)])
     , torqueControllerSimple_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale)
-    , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)]) {}
+    , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_LOAD_CELL_VECTORING)])
+    , torqueControllerSimpleLaunch_(controllerOutputs_[static_cast<int>(TorqueController_e::TC_SIMPLE_LAUNCH)]) {}
 // Functions
     void tick(
         const SysTick_s& tick,
@@ -79,6 +91,17 @@ class TorqueControllerMux
     {
         return torqueLimitMap_[torqueLimit_];
     }
+    TorqueControllerBase* activeController()
+    {
+        // check to make sure that there is actually a controller
+        // at the muxMode_ idx
+        if (controllers[muxMode_] != NULL) {
+            return controllers[muxMode_];
+        } else {
+            return static_cast<TorqueControllerBase*>(&torqueControllerNone_);
+        }
+    }
+
 };
 
 #endif /* __TORQUECTRLMUX_H__ */

lib/systems/include/TorqueControllers.h

@@ -18,9 +18,12 @@ const float AMK_MAX_RPM = 20000;
 // const float AMK_MAX_RPM = (2.235 * METERS_PER_SECOND_TO_RPM); // 5mph
 // const float AMK_MAX_RPM = (.89 * METERS_PER_SECOND_TO_RPM); // 1mph
 // const float
-const float AMK_MAX_TORQUE = 20.0; // TODO: update this with the true value
+const float AMK_MAX_TORQUE = 21.42; // TODO: update this with the true value
 const float MAX_REGEN_TORQUE = 10.0;
 
+const float DEFAULT_LAUNCH_RATE = 11.76;
+const int16_t DEFAULT_LAUNCH_SPEED_TARGET = 1500;
+
 const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = {
     .speeds_rpm = {0.0, 0.0, 0.0, 0.0},
     .torqueSetpoints = {0.0, 0.0, 0.0, 0.0}};
@@ -36,8 +39,17 @@ enum TorqueController_e
     TC_NO_CONTROLLER = 0,
     TC_SAFE_MODE = 1,
     TC_LOAD_CELL_VECTORING = 2,
-    TC_PID_VECTORING = 3,
-    TC_NUM_CONTROLLERS = 4,
+    TC_SIMPLE_LAUNCH = 3,
+    TC_PID_VECTORING = 4,
+    TC_NUM_CONTROLLERS = 5,
+};
+
+enum class LaunchStates_e
+{
+    NO_LAUNCH_MODE,
+    LAUNCH_NOT_READY,
+    LAUNCH_READY,
+    LAUNCHING
 };
 
 /// @brief If a command fed through this function exceeds the specified power limit, all torques will be scaled down equally
@@ -60,8 +72,19 @@ static DrivetrainCommand_s TCTorqueLimit(
     DrivetrainCommand_s command,
     float torqueLimits[NUM_MOTORS]);
 
+/*
+    Base torque controller to allow access to internal torque controller members
+*/
+class TorqueControllerBase
+{
+    public:
+    /* returns the launch state for the purpose of lighting the dahsboard LED. To be overridden in launch torque modes */
+    virtual LaunchStates_e get_launch_state() { return LaunchStates_e::NO_LAUNCH_MODE; }
+
+};
+
 template <TorqueController_e TorqueControllerType>
-class TorqueController
+class TorqueController : public TorqueControllerBase
 {
 protected:
 
@@ -185,6 +208,49 @@ class TorqueControllerLoadCellVectoring : public TorqueController<TC_LOAD_CELL_V
         const AnalogConversion_s &rrLoadCellData);
 };
 
+class TorqueControllerSimpleLaunch : public TorqueController<TC_SIMPLE_LAUNCH>
+{
+private:
+    const float launch_ready_accel_threshold = .1;
+    const float launch_ready_brake_threshold = .2;
+    const float launch_ready_speed_threshold = 5.0 * METERS_PER_SECOND_TO_RPM; // rpm
+    const float launch_go_accel_threshold = .9;
+    const float launch_stop_accel_threshold = .5;
+
+    TorqueControllerOutput_s &writeout_;
+    float launch_rate_target_;
+    int16_t init_speed_target_;
+
+    LaunchStates_e launch_state = LaunchStates_e::LAUNCH_NOT_READY;
+    uint32_t time_of_launch;
+    float launch_speed_target = 0.0;
+public:
+
+    /*!
+        SIMPLE LAUNCH CONTROLLER
+        This launch controller is based off of a specified launch rate and an initial speed target
+        It will ramp up the speed target linearlly over time to accelerate
+        @param launch_rate specified launch rate in m/s^2
+        @param initial_speed_target the initial speed commanded to the wheels
+    */
+    TorqueControllerSimpleLaunch(TorqueControllerOutput_s &writeout, float launch_rate, int16_t initial_speed_target)
+        : writeout_(writeout),
+          launch_rate_target_(launch_rate),
+          init_speed_target_(initial_speed_target)
+    {
+        writeout_.command = TC_COMMAND_NO_TORQUE;
+        writeout_.ready = true;
+    }
+
+    TorqueControllerSimpleLaunch(TorqueControllerOutput_s &writeout) : TorqueControllerSimpleLaunch(writeout, DEFAULT_LAUNCH_RATE, DEFAULT_LAUNCH_SPEED_TARGET) {}
+
+    LaunchStates_e get_launch_state() override { return launch_state; }
+
+    void tick(const SysTick_s & tick, 
+              const PedalsSystemData_s &pedalsData,
+              const float wheel_rpms[]);
+};
+
 class TorqueControllerPIDTV: public TorqueController<TC_PID_VECTORING>
 {
 public: 
@@ -204,4 +270,5 @@ class TorqueControllerPIDTV: public TorqueController<TC_PID_VECTORING>
     PID_TV::ExtU_PID_TV_T pid_input_;
     PID_TV tv_pid_;
 };
+
 #endif /* __TORQUECONTROLLERS_H__ */

lib/systems/src/TorqueControllerMux.cpp

@@ -17,6 +17,7 @@ void TorqueControllerMux::tick(
     // Tick all torque controllers
     torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]);
     torqueControllerLoadCellVectoring_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_], loadFLData, loadFRData, loadRLData, loadRRData);
+    torqueControllerSimpleLaunch_.tick(tick, pedalsData, drivetrainData.measuredSpeeds);
     // Tick torque button logic at 50hz
     if (tick.triggers.trigger50)
     {
@@ -43,8 +44,9 @@ void TorqueControllerMux::tick(
         {
             // Check if speed permits mode change
             bool speedPreventsModeChange = false;
-            for (int i = 0; i < NUM_MOTORS; i++)
+            for (int i = 0; i < NUM_MOTORS; i++) {
                 speedPreventsModeChange |= drivetrainData.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND >= MAX_SPEED_FOR_MODE_CHANGE;
+            }
 
             // Check if torque delta permits mode change
             bool torqueDeltaPreventsModeChange = false;

lib/systems/src/TorqueControllers.cpp

@@ -156,6 +156,114 @@ void TorqueControllerLoadCellVectoring::tick(
     }
 }
 
+void TorqueControllerSimpleLaunch::tick(
+    const SysTick_s &tick,
+    const PedalsSystemData_s &pedalsData,
+    const float wheel_rpms[])
+{
+
+    int16_t brake_torque_req = pedalsData.regenPercent * MAX_REGEN_TORQUE;
+
+    float max_speed = 0;
+    for(int i = 0; i < 4; i++){
+        max_speed = std::max(max_speed, abs(wheel_rpms[i]));
+    }
+
+    writeout_.ready = true;
+
+    switch(launch_state){
+        case LaunchStates_e::LAUNCH_NOT_READY:
+            // set torques and speed to 0
+            writeout_.command.speeds_rpm[FL] = 0.0;
+            writeout_.command.speeds_rpm[FR] = 0.0;
+            writeout_.command.speeds_rpm[RL] = 0.0;
+            writeout_.command.speeds_rpm[RR] = 0.0;
+
+            writeout_.command.torqueSetpoints[FL] = brake_torque_req;
+            writeout_.command.torqueSetpoints[FR] = brake_torque_req;
+            writeout_.command.torqueSetpoints[RL] = brake_torque_req;
+            writeout_.command.torqueSetpoints[RR] = brake_torque_req;
+
+            //init launch vars
+            launch_speed_target = 0;
+            time_of_launch = tick.millis;
+            // check speed is 0 and pedals not pressed
+            if((pedalsData.accelPercent < launch_ready_accel_threshold)
+               && (pedalsData.brakePercent < launch_ready_brake_threshold)
+               && (max_speed < launch_ready_speed_threshold))
+            {
+                launch_state = LaunchStates_e::LAUNCH_READY;
+            }
+
+            break;
+        case LaunchStates_e::LAUNCH_READY:
+            // set torques and speed to 0
+            writeout_.command.speeds_rpm[FL] = 0.0;
+            writeout_.command.speeds_rpm[FR] = 0.0;
+            writeout_.command.speeds_rpm[RL] = 0.0;
+            writeout_.command.speeds_rpm[RR] = 0.0;
+
+            writeout_.command.torqueSetpoints[FL] = brake_torque_req;
+            writeout_.command.torqueSetpoints[FR] = brake_torque_req;
+            writeout_.command.torqueSetpoints[RL] = brake_torque_req;
+            writeout_.command.torqueSetpoints[RR] = brake_torque_req;
+
+            //init launch vars
+            launch_speed_target = 0;
+            time_of_launch = tick.millis;
+
+            //check speed is 0 and brake not pressed
+            if ((pedalsData.brakePercent >= launch_ready_brake_threshold)
+                || (max_speed >= launch_ready_speed_threshold))
+            {
+                launch_state = LaunchStates_e::LAUNCH_NOT_READY;
+            } else if(pedalsData.accelPercent >= launch_go_accel_threshold){
+                launch_state = LaunchStates_e::LAUNCHING;
+            }
+
+            //check accel above launch threshold and launch
+            break;
+        case LaunchStates_e::LAUNCHING:
+            { // use brackets to ignore 'cross initialization' of secs_since_launch
+            //check accel below launch threshold and brake above
+            if((pedalsData.accelPercent <= launch_stop_accel_threshold)
+               || (pedalsData.brakePercent >= launch_ready_brake_threshold))
+            {
+                launch_state = LaunchStates_e::LAUNCH_NOT_READY;
+            }
+
+            /*
+            Stolen launch algo from HT07. This ramps up the speed target over time.
+            launch rate target is m/s^2 and is the target acceleration rate
+            secs_since_launch takes the milliseconds since launch started and converts to sec
+            This is then converted to RPM for a speed target
+            There is an initial speed target that is your iitial instant acceleration on the wheels
+            */
+            float secs_since_launch = (float)(tick.millis - time_of_launch) / 1000.0;
+            launch_speed_target = (int16_t)((float) secs_since_launch * launch_rate_target_ * METERS_PER_SECOND_TO_RPM);
+            launch_speed_target += init_speed_target_;
+            launch_speed_target = std::min((int)AMK_MAX_RPM, std::max(0, (int)launch_speed_target));
+
+            writeout_.command.speeds_rpm[FL] = launch_speed_target;
+            writeout_.command.speeds_rpm[FR] = launch_speed_target;
+            writeout_.command.speeds_rpm[RL] = launch_speed_target;
+            writeout_.command.speeds_rpm[RR] = launch_speed_target;
+
+            writeout_.command.torqueSetpoints[FL] = AMK_MAX_TORQUE;
+            writeout_.command.torqueSetpoints[FR] = AMK_MAX_TORQUE;
+            writeout_.command.torqueSetpoints[RL] = AMK_MAX_TORQUE;
+            writeout_.command.torqueSetpoints[RR] = AMK_MAX_TORQUE;
+
+            }
+            break;
+        default:
+            break;
+
+
+
+    }
+}
+
 void TorqueControllerPIDTV::tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float vx_b, float wheel_angle_rad, float yaw_rate)
 {
 

src/main.cpp

@@ -247,7 +247,8 @@ void tick_all_interfaces(const SysTick_s &current_system_tick)
                         drivetrain.drivetrain_error_occured(), 
                         torque_controller_mux.getTorqueLimit(),
                         ams_interface.get_filtered_min_cell_voltage(),
-                        telem_interface.get_glv_voltage(a1.get()));
+                        telem_interface.get_glv_voltage(a1.get()),
+                        static_cast<int>(torque_controller_mux.activeController()->get_launch_state()));
 
         main_ecu.tick(static_cast<int>(fsm.get_state()),
                       drivetrain.drivetrain_error_occured(),