int ev3_tacho_init( void )
{
char list[ 256 ], s[ 256 ];
char *p;
uint32_t id;
int port;
uint8_t cnt = 0;
memset( ev3_tacho, 0, sizeof( ev3_tacho ));
if ( !ev3_listdir( "/sys/class/tacho-motor", list, sizeof( list ))) return ( EV3_NONE );
p = strtok( list, " " );
while ( p ) {
if ( get_str_suffix( "tacho-motor", p, &id )) {
if ( get_tacho_port_name( id, s, sizeof( s ))) {
if ( strcmp( s, "out""A") == 0 ) port = OUTPUT_A;
else
if ( strcmp( s, "out""B") == 0 ) port = OUTPUT_B;
else
if ( strcmp( s, "out""C") == 0 ) port = OUTPUT_C;
else
if ( strcmp( s, "out""D") == 0 ) port = OUTPUT_D;
else
port = EV3_NONE;
if ( port != EV3_NONE ) {
ev3_tacho[ port ].connected = true;
ev3_tacho[ port ].id = id;
get_tacho_type( id, s, sizeof( s ));
if ( strcmp( s, "tacho") == 0 ) ev3_tacho[ port ].type_id = 7;
else
if ( strcmp( s, "minitacho") == 0 ) ev3_tacho[ port ].type_id = 8;
else
ev3_tacho[ port ].type_id = 0;
cnt++;
}
}
}
p = strtok( NULL, " " );
}
return ( cnt );
}
int ev3_servo_init( void )
{
char list[ 256 ];
char *p;
uint32_t sn;
int cnt = 0;
memset( ev3_servo, 0, sizeof( ev3_servo ));
if ( !ev3_listdir( "/sys/class/servo-motor", list, sizeof( list ))) return ( -1 );
p = strtok( list, " " );
while ( p ) {
if (( ev3_string_suffix( "motor", &p, &sn ) == 1 ) && ( sn < SERVO_DESC__LIMIT_)) {
get_servo_desc( sn, ev3_servo + sn );
++cnt;
}
p = strtok( NULL, " " );
}
return ( cnt );
}
