state_machine.c

/**
 * @file state_machine.c
 * @brief "Main Module" master state machine implementation
 * 
 * @author Irving Wang (irvingw@purdue.edu)
 */

#include "main.h"
#include "pindefs.h"

#include "common/can_library/faults_common.h"
#include "common/can_library/generated/MAIN_MODULE.h"
#include "common/can_library/generated/can_types.h"
#include "common/phal/gpio.h"

// FSAE 2026 EV.9.7.2: The Ready to Drive Sound must be sounded continuously for minimum 1 second and maximum 3 seconds
static constexpr uint32_t MIN_BUZZING_TIME_MS = 2500;
static constexpr uint16_t BRAKE_LIGHT_ON_THRESHOLD = 200; // ~5% of 4095
static constexpr uint16_t BRAKE_LIGHT_OFF_THRESHOLD = 100; // ~2.5% of 4095

void ready2drive_periodic() {
    if (can_data.pedals.is_stale()) {
        g_torque_request.front_right = 0;
        g_torque_request.front_left  = 0;
        g_torque_request.rear_left   = 0;
        g_torque_request.rear_right  = 0;
        return;
    }

    // todo regen
    // todo smarter request scheme
    // todo torque vectoring
    // todo alternative throttle mapping (like S curve)

    // assumes pedals.throttle is in the range [0, 4095]
    float throttle = can_data.pedals.throttle / 4095.0f;
    int16_t torque_req_percent = (int16_t)(throttle * 100);

    int16_t rear_torque = torque_req_percent * 1.5f; // allow 50% over-torque

    // Bias to feel like a 30% - 70% torque split
    int16_t front_torque = (30.0f / 70.0f) * rear_torque;
    
    g_torque_request.front_right = front_torque;
    g_torque_request.front_left  = front_torque;
    g_torque_request.rear_left   = rear_torque;
    g_torque_request.rear_right  = rear_torque;
}

static inline bool is_init_complete() {
    return true;
}

static inline bool is_all_AMKS_running() {
    return g_car.front_right.state == AMK_STATE_RUNNING
        && g_car.front_left.state  == AMK_STATE_RUNNING
        && g_car.rear_left.state   == AMK_STATE_RUNNING
        && g_car.rear_right.state  == AMK_STATE_RUNNING;
}

static inline bool is_start_button_pressed() {
    if (can_data.start_button.is_stale()) {
        return false;
    }

    return can_data.start_button.is_pressed;
}

static inline bool is_buzzing_time_elapsed() {
    return (OS_TICKS - g_car.buzzer_start_time >= MIN_BUZZING_TIME_MS);
}

void update_brake_light() {
    if (can_data.pedals.brake > BRAKE_LIGHT_ON_THRESHOLD) {
        if (!g_car.brake_light) {
            g_car.brake_light = true;
        }
    } else if (can_data.pedals.brake < BRAKE_LIGHT_OFF_THRESHOLD) {
        if (g_car.brake_light) {
            g_car.brake_light = false;
        }
    }
}

void update_tsal() {
    // FSAE 2026 EV.5.11.5
    if (is_latched(FAULT_ID_SDC2_BMS) || is_latched(FAULT_ID_IMD)) { 
        g_car.tsal_green_enable = false;
        g_car.tsal_red_enable = true;
    } else {
        g_car.tsal_green_enable = true;
        g_car.tsal_red_enable = false;
    }
}

void report_telemetry() {
    CAN_SEND_main_hb(g_car.current_state);

    CAN_SEND_wheel_speeds(
        g_car.front_right.crit->AMK_ActualSpeed,
        g_car.front_left.crit->AMK_ActualSpeed,
        g_car.rear_left.crit->AMK_ActualSpeed,
        g_car.rear_right.crit->AMK_ActualSpeed
    );

    CAN_SEND_motor_temps(
        g_car.front_right.temps->AMK_MotorTemp,
        g_car.front_left.temps->AMK_MotorTemp,
        g_car.rear_left.temps->AMK_MotorTemp,
        g_car.rear_right.temps->AMK_MotorTemp
    );

    CAN_SEND_igbt_temps(
        g_car.front_right.temps->AMK_IGBTTemp,
        g_car.front_left.temps->AMK_IGBTTemp,
        g_car.rear_left.temps->AMK_IGBTTemp,
        g_car.rear_right.temps->AMK_IGBTTemp
    );

    CAN_SEND_inva_set_vcan(
        g_car.front_right.set->AMK_Control_bReserve,
        g_car.front_right.set->AMK_Control_bInverterOn,
        g_car.front_right.set->AMK_Control_bDcOn,
        g_car.front_right.set->AMK_Control_bEnable,
        g_car.front_right.set->AMK_Control_bErrorReset,
        g_car.front_right.set->AMK_Control_bReserve2,
        g_car.front_right.set->AMK_TorqueSetpoint,
        g_car.front_right.set->AMK_PositiveTorqueLimit,
        g_car.front_right.set->AMK_NegativeTorqueLimit
    );

    CAN_SEND_invb_set_vcan(
        g_car.front_left.set->AMK_Control_bReserve,
        g_car.front_left.set->AMK_Control_bInverterOn,
        g_car.front_left.set->AMK_Control_bDcOn,
        g_car.front_left.set->AMK_Control_bEnable,
        g_car.front_left.set->AMK_Control_bErrorReset,
        g_car.front_left.set->AMK_Control_bReserve2,
        g_car.front_left.set->AMK_TorqueSetpoint,
        g_car.front_left.set->AMK_PositiveTorqueLimit,
        g_car.front_left.set->AMK_NegativeTorqueLimit
    );

    CAN_SEND_invc_set_vcan(
        g_car.rear_left.set->AMK_Control_bReserve,
        g_car.rear_left.set->AMK_Control_bInverterOn,
        g_car.rear_left.set->AMK_Control_bDcOn,
        g_car.rear_left.set->AMK_Control_bEnable,
        g_car.rear_left.set->AMK_Control_bErrorReset,
        g_car.rear_left.set->AMK_Control_bReserve2,
        g_car.rear_left.set->AMK_TorqueSetpoint,
        g_car.rear_left.set->AMK_PositiveTorqueLimit,
        g_car.rear_left.set->AMK_NegativeTorqueLimit
    );

    CAN_SEND_invd_set_vcan(
        g_car.rear_right.set->AMK_Control_bReserve,
        g_car.rear_right.set->AMK_Control_bInverterOn,
        g_car.rear_right.set->AMK_Control_bDcOn,
        g_car.rear_right.set->AMK_Control_bEnable,
        g_car.rear_right.set->AMK_Control_bErrorReset,
        g_car.rear_right.set->AMK_Control_bReserve2,
        g_car.rear_right.set->AMK_TorqueSetpoint,
        g_car.rear_right.set->AMK_PositiveTorqueLimit,
        g_car.rear_right.set->AMK_NegativeTorqueLimit
    );
}

void fsm_periodic() {
    // set default states
    g_car.current_state = g_car.next_state;
    g_car.next_state    = g_car.current_state; // explicit self loop
    g_car.brake_light = false;
    g_car.buzzer_enable = false;

    // zero torque request by default
    g_torque_request.front_right = 0;
    g_torque_request.front_left  = 0;
    g_torque_request.rear_left   = 0;
    g_torque_request.rear_right  = 0;

    update_brake_light();
    update_tsal();

    // update precharge status
    bool precharge_pin = !PHAL_readGPIO(PRECHARGE_COMPLETE_PORT, PRECHARGE_COMPLETE_PIN);
    update_fault(FAULT_ID_PRECHARGE_INCOMPLETE, precharge_pin);
    // amks need a bool to point to for precharge status
    g_car.is_precharge_complete = is_clear(FAULT_ID_PRECHARGE_INCOMPLETE);

    // any SDCs latched 1-15 faults will cause a fatal state
    if (is_latched(FAULT_ID_SDC15_REAR_INTERLOCK)) {
        g_car.current_state = CAR_STATE_FATAL;
        g_car.next_state = CAR_STATE_FATAL;
    } else if (is_latched(FAULT_ID_SDC16_TSMS)) { // return to idle if TSMS is opened
        g_car.current_state = CAR_STATE_IDLE;
        g_car.next_state = CAR_STATE_IDLE;
    }

    switch (g_car.current_state) {
        case CAR_STATE_FATAL: {
            // nothing for now

            if (is_clear(FAULT_ID_SDC15_REAR_INTERLOCK)) {
                g_car.next_state = CAR_STATE_IDLE;
            }
            break;
        }
        case CAR_STATE_IDLE: {
            // do nothing for now

            if (is_clear(FAULT_ID_SDC16_TSMS)) { // TSMS is closed
                g_car.next_state = CAR_STATE_PRECHARGING;
            }
            break;
        }
        case CAR_STATE_PRECHARGING: {
            // do nothing for now

            if (is_clear(FAULT_ID_PRECHARGE_INCOMPLETE)) {
                g_car.next_state = CAR_STATE_ENERGIZED;
            }
            break;
        }
        case CAR_STATE_ENERGIZED: {
            // do nothing for now

            if (is_start_button_pressed() && is_all_AMKS_running()) {
                g_car.buzzer_start_time = OS_TICKS;
                g_car.next_state = CAR_STATE_BUZZING;
            }
            break;
        }
        case CAR_STATE_BUZZING: {
            g_car.buzzer_enable = true;

            if (is_buzzing_time_elapsed()) {
                g_car.next_state = CAR_STATE_READY2DRIVE;
            }
            break;
        }
        case CAR_STATE_READY2DRIVE: {
            ready2drive_periodic();

            if (is_start_button_pressed()) {
                g_car.next_state = CAR_STATE_ENERGIZED;
            }
            break;
        }
    }

    AMK_set_torque(&g_car.front_right, g_torque_request.front_right);
    AMK_set_torque(&g_car.front_left,  g_torque_request.front_left);
    AMK_set_torque(&g_car.rear_left,   g_torque_request.rear_left);
    AMK_set_torque(&g_car.rear_right,  g_torque_request.rear_right);

    report_telemetry();

    // flush the internal state
    PHAL_writeGPIO(BRAKE_LIGHT_PORT, BRAKE_LIGHT_PIN, g_car.brake_light);
    PHAL_writeGPIO(TSAL_GREEN_CTRL_PORT, TSAL_GREEN_CTRL_PIN, g_car.tsal_green_enable);
    PHAL_writeGPIO(TSAL_RED_CTRL_PORT, TSAL_RED_CTRL_PIN, g_car.tsal_red_enable);
    PHAL_writeGPIO(BUZZER_PORT, BUZZER_PIN, g_car.buzzer_enable);
}