controller_bb_8_driver/tests/test_odometry.c
void testWheelSpeed( void )
{

    lldControlInit( );
    odometryInit( );
    debug_stream_init( );

    odometrySpeedValue_t    test_wheel_A        = 0;

#ifdef WHEEL_SPEED_MATLAB

    lldControlValue_t       test_delta_prc      = 10;
    lldControlValue_t       test_duty_A_prc     = 0;
    lldControlValue_t       test_duty_B_prc     = 0;
    lldControlValue_t       test_duty_C_prc     = 0;

    odometrySpeedValue_t    test_wheel_A_raw    = 0;

    uint8_t                 matlab_start_flag   = 0;

    int16_t                 matlab_wheel_A      = 0;
    int16_t                 matlab_wheel_A_raw  = 0;
#endif

    systime_t   time = chVTGetSystemTimeX( );
    while( true )
    {
#ifdef WHEEL_SPEED_MATLAB
        test_wheel_A_raw  = odometryGetWheelSpeedRaw( A, REVS_PER_SEC );
#endif
        test_wheel_A      = odometryGetWheelSpeed( A, REVS_PER_SEC );


#ifdef WHEEL_SPEED_MATLAB
        char rc_data         = sdGetTimeout( &SD3, TIME_IMMEDIATE );

        switch( rc_data )
        {
            case 's':   // start
                matlab_start_flag = 1;
                break;

            case 'f':   // increase speed wheel A
                test_duty_A_prc += test_delta_prc;
                break;

            case 'd':   // decrease speed wheel A
                test_duty_A_prc -= test_delta_prc;
                break;

            case 'z':   // increase speed wheel B
                test_duty_B_prc += test_delta_prc;
                break;

            case 'x':   // decrease speed wheel B
                test_duty_B_prc -= test_delta_prc;
                break;

            case 'q':   // increase speed wheel C
                test_duty_C_prc += test_delta_prc;
                break;

            case 'w':   // decrease speed wheel C
                test_duty_C_prc -= test_delta_prc;
                break;

            case ' ':   // stop all wheels
                test_duty_A_prc = 0;
                test_duty_B_prc = 0;
                test_duty_C_prc = 0;
                break;

            default:
                break;
        }

        lldControlSetMotorPower( A, test_duty_A_prc );
        lldControlSetMotorPower( B, test_duty_B_prc );
        lldControlSetMotorPower( C, test_duty_C_prc );

        if( matlab_start_flag )
        {
            matlab_wheel_A      = (int)(test_wheel_A * 100);
            matlab_wheel_A_raw  = (int)(test_wheel_A_raw * 100);
            sdWrite( &SD3, (uint8_t*) &matlab_wheel_A, 2 );
            sdWrite( &SD3, (uint8_t*) &matlab_wheel_A_raw, 2 );
        }
        time = chThdSleepUntilWindowed( time, time + MS2ST( 10 ) );
#else
        dbgprintf( "A: %d\tB: %d\tC: %d\n\r",
                   (int)test_wheel_A,
                   (int)test_wheel_B,
                   (int)test_wheel_C );
        time = chThdSleepUntilWindowed( time, time + MS2ST( 300 ) );
#endif

    }
}
/*
 * @brief   Test wheel speed (A!)
 * @note    Two options are available:
 *              - debug via Terminal (comment   define WHEEL_SPEED_MATLAB)
 *              - debug via Matlab   (uncomment define WHEEL_SPEED_MATLAB)
 *                in Matlab only data about wheel A is sended
 *                Consumption:
 *                  wheel A data is enough
 *                  because all motors (wheels) are the same
 */
@brief   Test wheel speed (A!)
@note    Two options are available:
- debug via Terminal (comment   define WHEEL_SPEED_MATLAB)
- debug via Matlab   (uncomment define WHEEL_SPEED_MATLAB)
in Matlab only data about wheel A is sended
Consumption:
wheel A data is enough
because all motors (wheels) are the same
@brief Test wheel speed ( A! ) @note Two options are available : - debug via Terminal ( comment define WHEEL_SPEED_MATLAB ) - debug via Matlab ( uncomment define WHEEL_SPEED_MATLAB ) in Matlab only data about wheel A is sended Consumption : wheel A data is enough because all motors ( wheels ) are the same
void testWheelSpeed( void )
{
    lldControlInit( );
    odometryInit( );
    debug_stream_init( );
    odometrySpeedValue_t    test_wheel_A        = 0;
#ifdef WHEEL_SPEED_MATLAB
    lldControlValue_t       test_delta_prc      = 10;
    lldControlValue_t       test_duty_A_prc     = 0;
    lldControlValue_t       test_duty_B_prc     = 0;
    lldControlValue_t       test_duty_C_prc     = 0;
    odometrySpeedValue_t    test_wheel_A_raw    = 0;
    uint8_t                 matlab_start_flag   = 0;
    int16_t                 matlab_wheel_A      = 0;
    int16_t                 matlab_wheel_A_raw  = 0;
#endif
    systime_t   time = chVTGetSystemTimeX( );
    while( true )
    {
#ifdef WHEEL_SPEED_MATLAB
        test_wheel_A_raw  = odometryGetWheelSpeedRaw( A, REVS_PER_SEC );
#endif
        test_wheel_A      = odometryGetWheelSpeed( A, REVS_PER_SEC );
#ifdef WHEEL_SPEED_MATLAB
        char rc_data         = sdGetTimeout( &SD3, TIME_IMMEDIATE );
        switch( rc_data )
        {
            case 's':   
                matlab_start_flag = 1;
                break;
            case 'f':   
                test_duty_A_prc += test_delta_prc;
                break;
            case 'd':   
                test_duty_A_prc -= test_delta_prc;
                break;
            case 'z':   
                test_duty_B_prc += test_delta_prc;
                break;
            case 'x':   
                test_duty_B_prc -= test_delta_prc;
                break;
            case 'q':   
                test_duty_C_prc += test_delta_prc;
                break;
            case 'w':   
                test_duty_C_prc -= test_delta_prc;
                break;
            case ' ':   
                test_duty_A_prc = 0;
                test_duty_B_prc = 0;
                test_duty_C_prc = 0;
                break;
            default:
                break;
        }
        lldControlSetMotorPower( A, test_duty_A_prc );
        lldControlSetMotorPower( B, test_duty_B_prc );
        lldControlSetMotorPower( C, test_duty_C_prc );
        if( matlab_start_flag )
        {
            matlab_wheel_A      = (int)(test_wheel_A * 100);
            matlab_wheel_A_raw  = (int)(test_wheel_A_raw * 100);
            sdWrite( &SD3, (uint8_t*) &matlab_wheel_A, 2 );
            sdWrite( &SD3, (uint8_t*) &matlab_wheel_A_raw, 2 );
        }
        time = chThdSleepUntilWindowed( time, time + MS2ST( 10 ) );
#else
        dbgprintf( "A: %d\tB: %d\tC: %d\n\r",
                   (int)test_wheel_A,
                   (int)test_wheel_B,
                   (int)test_wheel_C );
        time = chThdSleepUntilWindowed( time, time + MS2ST( 300 ) );
#endif
    }
}
void testWheelSpeed ( void ) { lldControlInit ( ) ; odometryInit ( ) ; debug_stream_init ( ) ; odometrySpeedValue_t test_wheel_A = 0 ; #ifdef WHEEL_SPEED_MATLAB lldControlValue_t test_delta_prc = 10 ; lldControlValue_t test_duty_A_prc = 0 ; lldControlValue_t test_duty_B_prc = 0 ; lldControlValue_t test_duty_C_prc = 0 ; odometrySpeedValue_t test_wheel_A_raw = 0 ; uint8_t matlab_start_flag = 0 ; int16_t matlab_wheel_A = 0 ; int16_t matlab_wheel_A_raw = 0 ; #endif systime_t time = chVTGetSystemTimeX ( ) ; while ( true ) { #ifdef WHEEL_SPEED_MATLAB test_wheel_A_raw = odometryGetWheelSpeedRaw ( A , REVS_PER_SEC ) ; #endif test_wheel_A = odometryGetWheelSpeed ( A , REVS_PER_SEC ) ; #ifdef WHEEL_SPEED_MATLAB char rc_data = sdGetTimeout ( & SD3 , TIME_IMMEDIATE ) ; switch ( rc_data ) { case ' ' : matlab_start_flag = 1 ; break ; case ' ' : test_duty_A_prc += test_delta_prc ; break ; case ' ' : test_duty_A_prc -= test_delta_prc ; break ; case ' ' : test_duty_B_prc += test_delta_prc ; break ; case ' ' : test_duty_B_prc -= test_delta_prc ; break ; case ' ' : test_duty_C_prc += test_delta_prc ; break ; case ' ' : test_duty_C_prc -= test_delta_prc ; break ; case ' ' : test_duty_A_prc = 0 ; test_duty_B_prc = 0 ; test_duty_C_prc = 0 ; break ; default : break ; } lldControlSetMotorPower ( A , test_duty_A_prc ) ; lldControlSetMotorPower ( B , test_duty_B_prc ) ; lldControlSetMotorPower ( C , test_duty_C_prc ) ; if ( matlab_start_flag ) { matlab_wheel_A = ( int ) ( test_wheel_A * 100 ) ; matlab_wheel_A_raw = ( int ) ( test_wheel_A_raw * 100 ) ; sdWrite ( & SD3 , ( uint8_t * ) & matlab_wheel_A , 2 ) ; sdWrite ( & SD3 , ( uint8_t * ) & matlab_wheel_A_raw , 2 ) ; } time = chThdSleepUntilWindowed ( time , time + MS2ST ( 10 ) ) ; #else dbgprintf ( " \t \t \n \r " , ( int ) test_wheel_A , ( int ) test_wheel_B , ( int ) test_wheel_C ) ; time = chThdSleepUntilWindowed ( time , time + MS2ST ( 300 ) ) ; #endif } }
@brief   Test wheel speed (A!)
@brief Test wheel speed ( A! )
// start // increase speed wheel A // decrease speed wheel A // increase speed wheel B // decrease speed wheel B // increase speed wheel C // decrease speed wheel C // stop all wheels