controller_bb_8_driver/src/robot_odometry.c
void robotOdometryInit( void )
{
    if( isInitialized )
        return;

    // constants calculation
    float ODOM_K  = BASE_WHEEL_DIST_M / TWO_PIR;

    k_A[1] = (-1) / TWO_PIR;
    k_A[2] = ODOM_K;

    k_B[0] = 0.866 / TWO_PIR;
    k_B[1] = 0.5 / TWO_PIR;
    k_B[2] = ODOM_K;

    k_C[0] = (-0.866) / TWO_PIR;
    k_C[1] = 0.5 / TWO_PIR;
    k_C[2] = ODOM_K;
    // *************************************************

    wheelControlInit();
    gyroscopeInit( NORMALPRIO );

    // wheel control system is enabled
    wheelControlSetPermeation();

    chVTObjectInit(&angle_vt);
    chVTSet( &angle_vt, MS2ST( ANGLE_VT_MS ), angle_vt_cb, NULL );

    isInitialized = true;
}
/**
 * @brief       Initialization of robot odometry unit
 */
@brief       Initialization of robot odometry unit
@brief Initialization of robot odometry unit
void robotOdometryInit( void )
{
    if( isInitialized )
        return;
    float ODOM_K  = BASE_WHEEL_DIST_M / TWO_PIR;
    k_A[1] = (-1) / TWO_PIR;
    k_A[2] = ODOM_K;
    k_B[0] = 0.866 / TWO_PIR;
    k_B[1] = 0.5 / TWO_PIR;
    k_B[2] = ODOM_K;
    k_C[0] = (-0.866) / TWO_PIR;
    k_C[1] = 0.5 / TWO_PIR;
    k_C[2] = ODOM_K;
    wheelControlInit();
    gyroscopeInit( NORMALPRIO );
    wheelControlSetPermeation();
    chVTObjectInit(&angle_vt);
    chVTSet( &angle_vt, MS2ST( ANGLE_VT_MS ), angle_vt_cb, NULL );
    isInitialized = true;
}
void robotOdometryInit ( void ) { if ( isInitialized ) return ; float ODOM_K = BASE_WHEEL_DIST_M / TWO_PIR ; k_A [ 1 ] = ( -1 ) / TWO_PIR ; k_A [ 2 ] = ODOM_K ; k_B [ 0 ] = 0.866 / TWO_PIR ; k_B [ 1 ] = 0.5 / TWO_PIR ; k_B [ 2 ] = ODOM_K ; k_C [ 0 ] = ( -0.866 ) / TWO_PIR ; k_C [ 1 ] = 0.5 / TWO_PIR ; k_C [ 2 ] = ODOM_K ; wheelControlInit ( ) ; gyroscopeInit ( NORMALPRIO ) ; wheelControlSetPermeation ( ) ; chVTObjectInit ( & angle_vt ) ; chVTSet ( & angle_vt , MS2ST ( ANGLE_VT_MS ) , angle_vt_cb , NULL ) ; isInitialized = true ; }
@brief       Initialization of robot odometry unit
@brief Initialization of robot odometry unit
// constants calculation // ************************************************* // wheel control system is enabled