controller_bb_8_driver/tests/test_odometry.c
void testLinearSpeedMatlab( void )
{
    wheelControlInit();

    sdStart( &SD6, &sdcfg );
    palSetPadMode( GPIOG, 14, PAL_MODE_ALTERNATE(8) );   // TX
    palSetPadMode( GPIOG, 9,  PAL_MODE_ALTERNATE(8) );   // RX

    odometrySpeedValue_t    odom_wheel_speed_A          = 0;
    odometrySpeedValue_t    odom_wheel_speed_B          = 0;
    odometrySpeedValue_t    odom_wheel_speed_C          = 0;

    int16_t                 matlab_speed_A              = 0;
    int16_t                 matlab_speed_B              = 0;
    int16_t                 matlab_speed_C              = 0;

    int16_t                 matlab_odom_speed_A         = 0;
    int16_t                 matlab_odom_speed_B         = 0;
    int16_t                 matlab_odom_speed_C         = 0;

    systime_t   time = chVTGetSystemTimeX( );
    while( true )
    {
        sdRead( &SD6, (uint8_t*) &matlab_speed_A, 2 );
        sdRead( &SD6, (uint8_t*) &matlab_speed_B, 2 );
        sdRead( &SD6, (uint8_t*) &matlab_speed_C, 2 );

        if( abs(matlab_speed_A) <= 300 )
        {
            wheelControlSetPermeation();

            float send_speed_A = (float)matlab_speed_A / 100;
            wheelControlSetSpeed(
                send_speed_A, A, REVS_PER_SEC
            );
        }
        if( abs(matlab_speed_B) <= 300 )
        {
            wheelControlSetPermeation();

            float send_speed_B = (float)matlab_speed_B / 100;
            wheelControlSetSpeed(
                send_speed_B, B, REVS_PER_SEC
            );
        }
        if( abs(matlab_speed_C) <= 300 )
        {
            wheelControlSetPermeation();

            float send_speed_C = (float)matlab_speed_C / 100;
            wheelControlSetSpeed(
                send_speed_C, C, REVS_PER_SEC
            );
        }

        odom_wheel_speed_A = odometryGetWheelSpeed( A, REVS_PER_SEC );
        odom_wheel_speed_B = odometryGetWheelSpeed( B, REVS_PER_SEC );
        odom_wheel_speed_C = odometryGetWheelSpeed( C, REVS_PER_SEC );

        if( abs(matlab_speed_A) == 500 || abs(matlab_speed_B) == 500 || abs(matlab_speed_C) == 500 )
        {
            palSetLine( LINE_LED3 );

            wheelControlSetSpeed(
                0, A, REVS_PER_SEC
            );

            wheelControlSetSpeed(
                0, B, REVS_PER_SEC
            );

            wheelControlSetSpeed(
                0, C, REVS_PER_SEC
            );
        }
        else
        {
            matlab_odom_speed_A = (int)(odom_wheel_speed_A * 100);
            matlab_odom_speed_B = (int)(odom_wheel_speed_B * 100);
            matlab_odom_speed_C = (int)(odom_wheel_speed_C * 100);

            sdWrite(&SD6, (uint8_t*) &matlab_odom_speed_A, 2);
            sdWrite(&SD6, (uint8_t*) &matlab_odom_speed_B, 2);
            sdWrite(&SD6, (uint8_t*) &matlab_odom_speed_C, 2);
        }

        time = chThdSleepUntilWindowed( time, time + MS2ST( 10 ) );
    }
}
/*
 * @brief   Test linear robot speed calculation in Matlab
 */
@brief   Test linear robot speed calculation in Matlab
@brief Test linear robot speed calculation in Matlab
void testLinearSpeedMatlab( void )
{
    wheelControlInit();
    sdStart( &SD6, &sdcfg );
    palSetPadMode( GPIOG, 14, PAL_MODE_ALTERNATE(8) );   
    palSetPadMode( GPIOG, 9,  PAL_MODE_ALTERNATE(8) );   
    odometrySpeedValue_t    odom_wheel_speed_A          = 0;
    odometrySpeedValue_t    odom_wheel_speed_B          = 0;
    odometrySpeedValue_t    odom_wheel_speed_C          = 0;
    int16_t                 matlab_speed_A              = 0;
    int16_t                 matlab_speed_B              = 0;
    int16_t                 matlab_speed_C              = 0;
    int16_t                 matlab_odom_speed_A         = 0;
    int16_t                 matlab_odom_speed_B         = 0;
    int16_t                 matlab_odom_speed_C         = 0;
    systime_t   time = chVTGetSystemTimeX( );
    while( true )
    {
        sdRead( &SD6, (uint8_t*) &matlab_speed_A, 2 );
        sdRead( &SD6, (uint8_t*) &matlab_speed_B, 2 );
        sdRead( &SD6, (uint8_t*) &matlab_speed_C, 2 );
        if( abs(matlab_speed_A) <= 300 )
        {
            wheelControlSetPermeation();
            float send_speed_A = (float)matlab_speed_A / 100;
            wheelControlSetSpeed(
                send_speed_A, A, REVS_PER_SEC
            );
        }
        if( abs(matlab_speed_B) <= 300 )
        {
            wheelControlSetPermeation();
            float send_speed_B = (float)matlab_speed_B / 100;
            wheelControlSetSpeed(
                send_speed_B, B, REVS_PER_SEC
            );
        }
        if( abs(matlab_speed_C) <= 300 )
        {
            wheelControlSetPermeation();
            float send_speed_C = (float)matlab_speed_C / 100;
            wheelControlSetSpeed(
                send_speed_C, C, REVS_PER_SEC
            );
        }
        odom_wheel_speed_A = odometryGetWheelSpeed( A, REVS_PER_SEC );
        odom_wheel_speed_B = odometryGetWheelSpeed( B, REVS_PER_SEC );
        odom_wheel_speed_C = odometryGetWheelSpeed( C, REVS_PER_SEC );
        if( abs(matlab_speed_A) == 500 || abs(matlab_speed_B) == 500 || abs(matlab_speed_C) == 500 )
        {
            palSetLine( LINE_LED3 );
            wheelControlSetSpeed(
                0, A, REVS_PER_SEC
            );
            wheelControlSetSpeed(
                0, B, REVS_PER_SEC
            );
            wheelControlSetSpeed(
                0, C, REVS_PER_SEC
            );
        }
        else
        {
            matlab_odom_speed_A = (int)(odom_wheel_speed_A * 100);
            matlab_odom_speed_B = (int)(odom_wheel_speed_B * 100);
            matlab_odom_speed_C = (int)(odom_wheel_speed_C * 100);
            sdWrite(&SD6, (uint8_t*) &matlab_odom_speed_A, 2);
            sdWrite(&SD6, (uint8_t*) &matlab_odom_speed_B, 2);
            sdWrite(&SD6, (uint8_t*) &matlab_odom_speed_C, 2);
        }
        time = chThdSleepUntilWindowed( time, time + MS2ST( 10 ) );
    }
}
void testLinearSpeedMatlab ( void ) { wheelControlInit ( ) ; sdStart ( & SD6 , & sdcfg ) ; palSetPadMode ( GPIOG , 14 , PAL_MODE_ALTERNATE ( 8 ) ) ; palSetPadMode ( GPIOG , 9 , PAL_MODE_ALTERNATE ( 8 ) ) ; odometrySpeedValue_t odom_wheel_speed_A = 0 ; odometrySpeedValue_t odom_wheel_speed_B = 0 ; odometrySpeedValue_t odom_wheel_speed_C = 0 ; int16_t matlab_speed_A = 0 ; int16_t matlab_speed_B = 0 ; int16_t matlab_speed_C = 0 ; int16_t matlab_odom_speed_A = 0 ; int16_t matlab_odom_speed_B = 0 ; int16_t matlab_odom_speed_C = 0 ; systime_t time = chVTGetSystemTimeX ( ) ; while ( true ) { sdRead ( & SD6 , ( uint8_t * ) & matlab_speed_A , 2 ) ; sdRead ( & SD6 , ( uint8_t * ) & matlab_speed_B , 2 ) ; sdRead ( & SD6 , ( uint8_t * ) & matlab_speed_C , 2 ) ; if ( abs ( matlab_speed_A ) <= 300 ) { wheelControlSetPermeation ( ) ; float send_speed_A = ( float ) matlab_speed_A / 100 ; wheelControlSetSpeed ( send_speed_A , A , REVS_PER_SEC ) ; } if ( abs ( matlab_speed_B ) <= 300 ) { wheelControlSetPermeation ( ) ; float send_speed_B = ( float ) matlab_speed_B / 100 ; wheelControlSetSpeed ( send_speed_B , B , REVS_PER_SEC ) ; } if ( abs ( matlab_speed_C ) <= 300 ) { wheelControlSetPermeation ( ) ; float send_speed_C = ( float ) matlab_speed_C / 100 ; wheelControlSetSpeed ( send_speed_C , C , REVS_PER_SEC ) ; } odom_wheel_speed_A = odometryGetWheelSpeed ( A , REVS_PER_SEC ) ; odom_wheel_speed_B = odometryGetWheelSpeed ( B , REVS_PER_SEC ) ; odom_wheel_speed_C = odometryGetWheelSpeed ( C , REVS_PER_SEC ) ; if ( abs ( matlab_speed_A ) == 500 || abs ( matlab_speed_B ) == 500 || abs ( matlab_speed_C ) == 500 ) { palSetLine ( LINE_LED3 ) ; wheelControlSetSpeed ( 0 , A , REVS_PER_SEC ) ; wheelControlSetSpeed ( 0 , B , REVS_PER_SEC ) ; wheelControlSetSpeed ( 0 , C , REVS_PER_SEC ) ; } else { matlab_odom_speed_A = ( int ) ( odom_wheel_speed_A * 100 ) ; matlab_odom_speed_B = ( int ) ( odom_wheel_speed_B * 100 ) ; matlab_odom_speed_C = ( int ) ( odom_wheel_speed_C * 100 ) ; sdWrite ( & SD6 , ( uint8_t * ) & matlab_odom_speed_A , 2 ) ; sdWrite ( & SD6 , ( uint8_t * ) & matlab_odom_speed_B , 2 ) ; sdWrite ( & SD6 , ( uint8_t * ) & matlab_odom_speed_C , 2 ) ; } time = chThdSleepUntilWindowed ( time , time + MS2ST ( 10 ) ) ; } }
@brief   Test linear robot speed calculation in Matlab
@brief Test linear robot speed calculation in Matlab
// TX // RX