Every signals is generated in one interrupt. This method save more than 80% cpu time. But I think, previous version has
more accurate timing. But its not big problem. Jitter on timing is less than 10us with heavy network load and 4 PWM run.
Now library have support for 6 servos !
setup(id, pin, servo pulse length) - Start generating servopulses on selected pin
position(id, servo pulse length) - Change servo pulse length at selected ID
stop(id) - Stop timer for servo pulse generator
servo.setup(0,1,1000)
servo.setup(1,2,1200)
servo.setup(2,3,1400)
servo.setup(3,4,1600)
servo.setup(4,5,1800)
servo.setup(5,6,2000)
servo.position(0,2000)
servo.position(1,1900)
servo.position(2,1800)
servo.position(3,1700)
servo.position(4,1600)
servo.position(5,1500)
generator is realy precise as you can see :
and stop servos:
servo.stop(0)
servo.stop(1)
servo.stop(2)
servo.stop(3)
servo.stop(4)
servo.stop(5)
code of module is here:
// Module for RC servo interfacing by Milan Spacek
//#include "lua.h"
#include "lualib.h"
#include "lauxlib.h"
#include "platform.h"
#include "auxmods.h"
#include "lrotable.h"
#include "c_types.h"
#define max_servos 6
#define DIRECT_MODE_OUTPUT(pin) platform_gpio_mode(pin,PLATFORM_GPIO_OUTPUT,PLATFORM_GPIO_PULLUP)
#define DIRECT_WRITE_LOW(pin) (GPIO_OUTPUT_SET(GPIO_ID_PIN(pin_num[pin]), 0))
#define DIRECT_WRITE_HIGH(pin) (GPIO_OUTPUT_SET(GPIO_ID_PIN(pin_num[pin]), 1))
static os_timer_t servo_timer;
unsigned char timer_running = 0;
static signed char servo_pin[max_servos] = {-1,-1,-1,-1,-1,-1};
static int servo_time[max_servos] = {0,0,0,0,0,0};
static int servo_tmp_time[max_servos] = {0,0,0,0,0,0};
static int servo_delay_time[max_servos] = {0,0,0,0,0,0};
static signed char servo_pin_sorted_by_pulse_lenght[max_servos] = {-1,-1,-1,-1,-1,-1};
unsigned char sorting = 0;
/*
char debug_buffer [128];
void debug(char *dataPtr)
{
while ( *dataPtr )
{
platform_uart_send( 0, *dataPtr++ );
}
}
*/
int sort_pulse_time(void)
{
int c,d,swap,pin_swap,n,delay_sum;
n = max_servos;
sorting = 1;
// fill variables for calculation
for ( c = 0 ; c < n ; c++ )
{
servo_pin_sorted_by_pulse_lenght[c] = servo_pin[c];
servo_tmp_time[c] = servo_time[c];
}
// sort servos by servo pulse time and sort pin numbers.
for (c = 0 ; c < ( n - 1 ); c++)
{
for (d = 0 ; d < n - c - 1; d++)
{
if (servo_tmp_time[d] > servo_tmp_time[d+1])
{
swap = servo_tmp_time[d];
servo_tmp_time[d] = servo_tmp_time[d+1];
servo_tmp_time[d+1] = swap;
pin_swap = servo_pin_sorted_by_pulse_lenght[d];
servo_pin_sorted_by_pulse_lenght[d] = servo_pin_sorted_by_pulse_lenght[d+1];
servo_pin_sorted_by_pulse_lenght[d+1] = pin_swap;
}
}
}
// calculate delay offset
delay_sum = 0;
for ( c = 0 ; c < n ; c++ )
{
servo_delay_time[c] = servo_tmp_time[c]-delay_sum;
delay_sum += servo_delay_time[c];
}
sorting = 0;
/*
for ( c = 0 ; c < n ; c++ )
{
os_sprintf(debug_buffer,"%d - %d - %d\r\n", servo_tmp_time[c], servo_delay_time[c],servo_pin_sorted_by_pulse_lenght[c]);
debug(debug_buffer);
}
*/
return 0;
}
servo_timer_tick(void) // servo timer function
{
unsigned char i = 0;
if (sorting == 1)
{
os_timer_disarm(&servo_timer); // dis_arm the timer
os_timer_setfn(&servo_timer, (os_timer_func_t *)servo_timer_tick, NULL); // set the timer function, dot get os_timer_func_t to force function convert
os_timer_arm(&servo_timer, 1, 1); // arm the timer after 1ms and repeat
}
else
{
os_timer_disarm(&servo_timer); // dis_arm the timer
os_timer_setfn(&servo_timer, (os_timer_func_t *)servo_timer_tick, NULL); // set the timer function, dot get os_timer_func_t to force function convert
os_timer_arm(&servo_timer, 20, 1); // arm the timer every 20ms and repeat
for ( i = 0 ; i < max_servos ; i++ )
{
if (servo_pin_sorted_by_pulse_lenght[i] >= 0) {DIRECT_WRITE_HIGH(servo_pin_sorted_by_pulse_lenght[i]);}
}
for ( i = 0 ; i < max_servos ; i++ )
{
if (servo_delay_time[i] > 0) {os_delay_us(servo_delay_time[i]);}
if (servo_pin_sorted_by_pulse_lenght[i] >= 0) {DIRECT_WRITE_LOW(servo_pin_sorted_by_pulse_lenght[i]);}
}
}
}
// Lua: setup( id, pin, default_pulse_lenght )
static int servo_setup( lua_State* L )
{
unsigned id, pin, default_pulse;
id = luaL_checkinteger( L, 1 );
if (id >= max_servos)
return luaL_error( L, "wrong id, must be 0-5" );
pin = luaL_checkinteger( L, 2 );
MOD_CHECK_ID( gpio, pin );
default_pulse = luaL_checkinteger( L, 3 );
if (default_pulse < 500 || default_pulse > 2500 )
return luaL_error( L, "wrong pulse length, must be 500-2500" );
//os_sprintf(debug_buffer, "id=%d pin=%d time=%d",id,pin,default_pulse);
//debug(debug_buffer);
if (id >= 0 && id < max_servos)
{
servo_pin[id] = pin;
servo_time[id] = default_pulse;
if (servo_pin[id] != 0)
{
DIRECT_WRITE_LOW(pin);
DIRECT_MODE_OUTPUT(pin);
}
sort_pulse_time();
if (timer_running != 1)
{
os_timer_disarm(&servo_timer); // dis_arm the timer
os_timer_setfn(&servo_timer, (os_timer_func_t *)servo_timer_tick, NULL); // set the timer function, dot get os_timer_func_t to force function convert
os_timer_arm(&servo_timer, 20, 1); // arm the timer every 5ms and repeat
timer_running = 1;
}
}
return 1;
}
// Lua: position( id, pulse_lenght )
static int servo_position( lua_State* L )
{
unsigned id, pulse_lenght;
id = luaL_checkinteger( L, 1 );
if (id >= max_servos)
return luaL_error( L, "wrong id" );
pulse_lenght = luaL_checkinteger( L, 2 );
if (pulse_lenght < 500 || pulse_lenght > 2500 )
return luaL_error( L, "wrong pulse length" );
//os_sprintf(debug_buffer, "id=%d time=%d",id,pulse_lenght);
//debug(debug_buffer);
if (id >= 0 && id < max_servos)
{
servo_time[id] = pulse_lenght;
sort_pulse_time();
}
return 1;
}
// Lua: stop(id)
static int servo_stop( lua_State* L )
{
unsigned id,i,tmp_active;
id = luaL_checkinteger( L, 1 );
if (id >= max_servos)
return luaL_error( L, "wrong id" );
if (id >= 0 && id < max_servos)
{
servo_pin[id] = -1;
servo_time[id] = -1;
sort_pulse_time();
}
tmp_active = 0;
for ( i = 0 ; i < max_servos ; i++ )
{
if (servo_pin[i] >= 0) {tmp_active += 1;}
}
if (tmp_active == 0)
{
os_timer_disarm(&servo_timer); // dis_arm the timer
timer_running = 0;
}
return 1;
}
// Module function map
#define MIN_OPT_LEVEL 2
#include "lrodefs.h"
const LUA_REG_TYPE servo_map[] =
{
{ LSTRKEY( "setup" ), LFUNCVAL( servo_setup ) },
{ LSTRKEY( "position" ), LFUNCVAL( servo_position ) },
{ LSTRKEY( "stop" ), LFUNCVAL( servo_stop ) },
#if LUA_OPTIMIZE_MEMORY > 0
#endif
{ LNILKEY, LNILVAL }
};
LUALIB_API int luaopen_servo( lua_State *L )
{
#if LUA_OPTIMIZE_MEMORY > 0
return 0;
#else // #if LUA_OPTIMIZE_MEMORY > 0
luaL_register( L, AUXLIB_SERVO, servo_map );
// Add constants
return 1;
#endif // #if LUA_OPTIMIZE_MEMORY > 0
}
OLD POST !!! NOW Obsolote
Hi, I write simple new module for better RC servo driving.
Module have 3 functions (setup, position, stop)
setup(id, pin, servo pulse length) - Start generating servopulses on selected pin
position(id, servo pulse length) - Change servo pulse length at selected ID
stop() - Stop timer for servo pulse generator
Module can drive 4 servos (ID 0-3) with pulse time 500 - 2500us
Period is fixed to 50Hz and resolution is 1us
Simple demo for testing:
servo.setup(0,1,1500)
servo.setup(1,2,2000)
servo.setup(2,3,1000)
servo.setup(3,4,1250)
servo.position(0,2000)
servo.position(1,1000)
servo.stop()
Timing test:
Source is here:
// Module for RC servo interfacing by Milan Spacek
//#include "lua.h"
#include "lualib.h"
#include "lauxlib.h"
#include "platform.h"
#include "auxmods.h"
#include "lrotable.h"
#include "c_types.h"
#define DIRECT_MODE_OUTPUT(pin) platform_gpio_mode(pin,PLATFORM_GPIO_OUTPUT,PLATFORM_GPIO_PULLUP)
#define DIRECT_WRITE_LOW(pin) (GPIO_OUTPUT_SET(GPIO_ID_PIN(pin_num[pin]), 0))
#define DIRECT_WRITE_HIGH(pin) (GPIO_OUTPUT_SET(GPIO_ID_PIN(pin_num[pin]), 1))
static os_timer_t servo_timer;
static unsigned servo_pins[4] = {0,0,0,0};
static unsigned servo_pulse_lenght[4] = {1500,1500,1500,1500};
static unsigned servo_loop_counter = 0;
/*
char debug_buffer [128];
void debug(char *dataPtr)
{
while ( *dataPtr )
{
platform_uart_send( 0, *dataPtr++ );
}
}
*/
servo_timer_tick(void) // servo timer function, called every 5ms for one from 4 servos
{
if (servo_pins[servo_loop_counter] != 0)
{
DIRECT_WRITE_HIGH(servo_pins[servo_loop_counter]); // drive output high
os_delay_us(servo_pulse_lenght[servo_loop_counter]);
DIRECT_WRITE_LOW(servo_pins[servo_loop_counter]);
}
servo_loop_counter++;
if(servo_loop_counter >= 4)
servo_loop_counter = 0;
}
// Lua: setup( id, pin, default_pulse_lenght )
static int servo_setup( lua_State* L )
{
unsigned id, pin, default_pulse;
id = luaL_checkinteger( L, 1 );
if (id > 3)
return luaL_error( L, "wrong id, must be 0-3" );
pin = luaL_checkinteger( L, 2 );
MOD_CHECK_ID( gpio, pin );
default_pulse = luaL_checkinteger( L, 3 );
if (default_pulse < 500 || default_pulse > 2500 )
return luaL_error( L, "wrong pulse length, must be 500-2500" );
//os_sprintf(debug_buffer, "id=%d pin=%d time=%d",id,pin,default_pulse);
//debug(debug_buffer);
if (id >= 0 && id <= 3)
{
servo_pins[id] = pin;
servo_pulse_lenght[id] = default_pulse;
if (servo_pins[id] != 0)
{
DIRECT_WRITE_LOW(pin);
DIRECT_MODE_OUTPUT(pin);
}
os_timer_disarm(&servo_timer); // dis_arm the timer
os_timer_setfn(&servo_timer, (os_timer_func_t *)servo_timer_tick, NULL); // set the timer function, dot get os_timer_func_t to force function convert
os_timer_arm(&servo_timer, 5, 1); // arm the timer every 5ms and repeat
}
return 1;
}
// Lua: position( id, pulse_lenght )
static int servo_position( lua_State* L )
{
unsigned id, pulse_lenght;
id = luaL_checkinteger( L, 1 );
if (id > 3)
return luaL_error( L, "wrong id, must be 0-3" );
pulse_lenght = luaL_checkinteger( L, 2 );
if (pulse_lenght < 500 || pulse_lenght > 2500 )
return luaL_error( L, "wrong pulse length, must be 500-2500" );
//os_sprintf(debug_buffer, "id=%d time=%d",id,pulse_lenght);
//debug(debug_buffer);
if (id >= 0 && id <= 3)
{
servo_pulse_lenght[id] = pulse_lenght;
}
return 1;
}
// Lua: stop()
static int servo_stop( lua_State* L )
{
os_timer_disarm(&servo_timer); // dis_arm the timer
return 1;
}
// Module function map
#define MIN_OPT_LEVEL 2
#include "lrodefs.h"
const LUA_REG_TYPE servo_map[] =
{
{ LSTRKEY( "setup" ), LFUNCVAL( servo_setup ) },
{ LSTRKEY( "position" ), LFUNCVAL( servo_position ) },
{ LSTRKEY( "stop" ), LFUNCVAL( servo_stop ) },
#if LUA_OPTIMIZE_MEMORY > 0
#endif
{ LNILKEY, LNILVAL }
};
LUALIB_API int luaopen_servo( lua_State *L )
{
#if LUA_OPTIMIZE_MEMORY > 0
return 0;
#else // #if LUA_OPTIMIZE_MEMORY > 0
luaL_register( L, AUXLIB_SERVO, servo_map );
// Add constants
return 1;
#endif // #if LUA_OPTIMIZE_MEMORY > 0
}
and you can test it without compiling by downloading binary files (build 2015-01-27) : http://s000.tinyupload.com/?file_id=07461189157201651884
In this build GPIO has ESP-12 friendly mapping like this: