ME-507-Romi-Plus-Plus/balance_8h_source.html

/*

 * balance.h

 *

 *  Created on: Jun 11, 2025

 *      Author: jacka

 */


#ifndef INC_BALANCE_H_

#define INC_BALANCE_H_


#include "BNO055_STM32.h"

#include "time_utils.h"

#include <stdint.h>

#include <algorithm>


extern BNO055_Sensors_t BNO055;


extern volatile bool USER;


class BALANCE

{

// Public methods and members

public:


    typedef enum state

    {

        S0_INIT,

        S1_IDLE,

        S2_RUN

    } state_t;


    BALANCE();


    int32_t get_M1_eff(void);


    int32_t get_M2_eff(void);


    void run(void);


// Private methods and members

private:

    state_t state;


    float angle;

    float level;

    bool balance_on;

    float filtered_angle;

    float alpha;


    int32_t M1_eff;

    int32_t M2_eff;


    // PID controller variables

    float Kp;

    float Ki;

    float Kd;


    float error;

    float prev_error;

    float error_diff;

    float error_sum;

    float ctrl;


    float dt;

    uint32_t now;

    uint32_t last_time;

};


#endif /* INC_BALANCE_H_ */

USER
volatile bool USER
External flag indicating user button press.
Definition main.cpp:100

BNO055
BNO055_Sensors_t BNO055
External reference to BNO055 sensor data structure.
Definition main.cpp:94

BALANCE::get_M2_eff
int32_t get_M2_eff(void)
Returns the motor effort for motor 2.
Definition balance.cpp:29

BALANCE::run
void run(void)
Executes the FSM logic and PID balance control.
Definition balance.cpp:36

BALANCE::BALANCE
BALANCE()
Constructor for the BALANCE class.
Definition balance.cpp:14

BALANCE::state
state
Definition balance.h:43

BALANCE::S0_INIT
@ S0_INIT
Initialization state.
Definition balance.h:44

BALANCE::S2_RUN
@ S2_RUN
Running control loop.
Definition balance.h:46

BALANCE::S1_IDLE
@ S1_IDLE
Idle state.
Definition balance.h:45

BALANCE::get_M1_eff
int32_t get_M1_eff(void)
Returns the motor effort for motor 1.
Definition balance.cpp:23

BNO055_Sensors_t
Definition BNO055_STM32.h:278

time_utils.h
Utility functions for microsecond-level timing.