ESP8266 Community Forum

- Thu Jan 29, 2015 5:04 pm #8485 Hi, I competely rewrite my servo driving module. Now is much more friendly to CPU usage.

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

Code: Select allservo.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)

Code: Select allservo.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:

Code: Select allservo.stop(0)
servo.stop(1)
servo.stop(2)
servo.stop(3)
servo.stop(4)
servo.stop(5)

code of module is here:

Code: Select all// 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:

Code: Select allservo.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:

Code: Select all// 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:

Last edited by milikiller on Mon Feb 02, 2015 6:28 pm, edited 1 time in total.

- Thu Jan 29, 2015 5:15 pm #8486 Is there no jitter compared to the existing pwm module?

Your code fires an interrupt every 5ms - I believe the existing pwm code only updates itself when necessary? So this version should have a much higher cpu load.

In this build GPIO has ESP-12 friendly mapping like this:

Apparently everyone has their own idea of what makes a good mapping. I say no mapping at all. :p

- Thu Jan 29, 2015 5:27 pm #8487 where are you putting your servo library files? what changes are there to the nodemcu Makefile?

pan+tilt servo's would be a good use of the two gpio pins on an esp-01.

how much current can the esp8266 gpio's sink (each/total) looks like a measly 12mA?

Last edited by sej7278 on Thu Jan 29, 2015 5:43 pm, edited 2 times in total.

- Thu Jan 29, 2015 5:35 pm #8488 Hi, my code have same jitter problem as PWM.
Now i dont know how to solve it. It is problem in Espresif OS.

Yes, my code fires iterrupts every 5ms. But if you are using only one servo, function called by timer interrupt only increment variable and this is everything.

I am realy bad programmer, my code is not perfect, but have much higher resolution than PWM, and servo pulse have every same length as user set. Network load is no problem. On heavy load, the period between pulses can have diferences.

I dont force you to use it.