ESP8266 Community Forum

- Sun Feb 01, 2015 5:54 pm #8735 Hi, I rewrite servo library for better timing and less cpu usage.

Now only active servo makes interrupts.

code is here : https://github.com/milikiller/nodemcu-firmware/

and binary files are here : http://s000.tinyupload.com/?file_id=668 ... 1696460449

servo module :

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 int servo_pins[4] = {-1,-1,-1,-1};
static int servo_pulse_lenght[4] = {1500,1500,1500,1500};

static unsigned current_servo = 0;
static int next_servo[4] = {1,2,3,0};
static int time_to_next_pulse[4] = {5,5,5,5};

static unsigned run = 0;


/*
char debug_buffer [128];
void debug(char *dataPtr)
{
    while ( *dataPtr )
    {
       platform_uart_send( 0, *dataPtr++ );
    }
}
*/

int find_times_between_pulses(void)
{
   int tmp_time,find,i,ii,finded;

   finded = 0;
   for (i = 0 ; i < 4; i++)
   {
     find = 0;
     tmp_time = 5;
     for (ii = 0 ; ii < 4; ii++)
     {
         if (servo_pins[(ii+i+1)%4] == (-1) && find == 0)
         {
            tmp_time+=5;
         }
         else
         {
            find = 1;
            next_servo[i] = (ii+i+1)%4;
            time_to_next_pulse[i] = tmp_time;
            break;
         }
      }
      finded += find;
   }

/*
   for (i = 0 ; i < 4; i++)
   {
      os_sprintf(debug_buffer, "id %d next servo:%d,time%d\r\n",i,next_servo[i],time_to_next_pulse[i]);
      debug(debug_buffer);
   }
*/

return finded;
}




servo_timer_tick(void) // servo timer function
{
   if (run == 1 && time_to_next_pulse[current_servo] >= 0)
   {
      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, time_to_next_pulse[current_servo], 1); // arm the timer every 5ms and repeat

      if (servo_pins[current_servo] >= 0)
      {
         DIRECT_WRITE_HIGH(servo_pins[current_servo]);   // drive output high
         os_delay_us(servo_pulse_lenght[current_servo]);
         DIRECT_WRITE_LOW(servo_pins[current_servo]);   // drive output low
      }

      //os_sprintf(debug_buffer, "id=%d   time=%d   next=%d \r\n",current_servo,time_to_next_pulse[current_servo],next_servo[current_servo]);
      //debug(debug_buffer);

      current_servo = next_servo[current_servo];
   }
   else
   {
      os_timer_disarm(&servo_timer); // dis_arm the timer
   }
}



// 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 (find_times_between_pulses() > 0)
     {
        run = 1;

        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, time_to_next_pulse[current_servo], 1); // arm the timer every 5ms and repeat
     }
     else
     {
        run = 0;
        os_timer_disarm(&servo_timer); // dis_arm the timer
     }

  }

  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(id)
static int servo_stop( lua_State* L )
{
   unsigned id;

   id = luaL_checkinteger( L, 1 );
   if (id > 3)
      return luaL_error( L, "wrong id, must be 0-3" );

   servo_pins[id] = -1;

   if (find_times_between_pulses() > 0)
   {
     run = 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, time_to_next_pulse[current_servo], 1); // arm the timer every 5ms and repeat
   }
   else
   {
     run = 0;
     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 
}


Please send me a message if everything works correctly , or report bug and I try to solve it.

- Sun Feb 01, 2015 6:57 pm #8739 if you reverted pin_map.c, NODE_VERSION and BUILD_DATE from user_config.h, you could do a pull request to get it merged upstream.

unfortunately my servo's are in another project and new ones haven't arrived yet so i can't test.

- Mon Feb 02, 2015 1:39 am #8747 I'm new on github, I'm not sure if he would be interested in this library in the main branch.

I have idea how to save up to 75% cpu time if you use all 4 servos. But it is complicated to program this.

My idea is use one timer with 20ms period and stop program only once for sending all servo pulses.
I need to write library that sort servo pulses times and calculate delay between pulses like this

I need 4 servos with 1ms 1.25ms 1.75ms and 2ms pulses

If timer overflow, set all four pins for servos High.
wait 1ms (servo 1 time) and write pin for servo 1 Low
wait 250us and write pin for servo 2 Low
wait 500us and write pin for servo 3 Low
wait 250us and write pin for servo 4 Low
and this is end of interrupt..

CPU is stopped only one per period for max 2.5ms

- Mon Feb 02, 2015 7:47 am #8764 I am hard working on new algorithm.

I have first working demo of code, for sorting servo times and delays.

Code: Select all#include <stdio.h>

int servo_pin[6] = {1,2,3,-1,-1,-1};
int servo_time[6] = {1001,1000,1500,0,0,0};

int servo_tmp_time[6] = {0,0,0,0,0,0};
int servo_delay_time[6] = {0,0,0,0,0,0};
int servo_pin_sorted_by_pulse_lenght[6] = {0,0,0,0,0,0};







int main()
{
    //printf("Hello, World!\n");
    int c,d,swap,pin_swap,n,delay_sum;
   
    n = 6;

    // 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;
          }
        }
      }
     
   
    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];
    }


    for ( c = 0 ; c < n ; c++ )
     printf("%d - %d  -  %d\n", servo_tmp_time[c], servo_delay_time[c],servo_pin_sorted_by_pulse_lenght[c]);

    return 0;
}

At night I try to implement it to servo module. I must measure how fast is GPIO operations, because i must compensate time needed to change GPIO value


I am very close to completing a cheap RC telemetry transceiver from ESP8266.