i did some tests with our soft PWM servo code. i would like to have the possibility to run the servos with more then 50Hz .. because for what i know the most digital servos works well with up to 300Hz.
so i changed the servo ISR to work in a chain order. it works well as it is .. and it cleans also the compiled code inside of it. but to have the flexibility of the used "from - to servo selection" which i like! i had to re sort them to have a clean serie (servo 1-4 instead of 1-2 and 4-5). and this is the part im not happy with .. to do it with the compiler i ended up with a very big #if block..
i hope that there are smarter ways that i dont know
some measurements (with SERVO_FAST_PWM)
- 1 servo = ~280Hz (the frequency changes with the servos position .. so it is between 320-240Hz..)
- 2 servos = ~250Hz
- 3 servos = ~216Hz
- 4 servos = ~163Hz
- 6 servos = ~110Hz
- 8 servos = ~ 82Hz
without SERVO_FAST_PWM it varies between 49-60Hz
my test code..
the #if block in def.h (to add under the servo from-to defines)
Code: Select all
/********************** Sort the Servos for the moust ideal SW PWM ************************/
// this define block sorts the above slected servos to be in a simple order from 1 - (count of total servos)
// its pretty fat but its the best way i found to get less compiled code and max speed in the ISR without loosing its flexibility
#if (PRI_SERVO_FROM == 1) || (SEC_SERVO_FROM == 1)
#define LAST_LOW SERVO_1_PIN_LOW
#define SERVO_1_HIGH SERVO_1_PIN_HIGH
#define SERVO_1_LOW SERVO_1_PIN_LOW
#define SERVO_1_ARR_POS 0
#endif
#if (PRI_SERVO_FROM <= 2 && PRI_SERVO_TO >= 2) || (SEC_SERVO_FROM <= 2 && SEC_SERVO_TO >= 2)
#undef LAST_LOW
#define LAST_LOW SERVO_2_PIN_LOW
#if !defined(SERVO_1_HIGH)
#define SERVO_1_HIGH SERVO_2_PIN_HIGH
#define SERVO_1_LOW SERVO_2_PIN_LOW
#define SERVO_1_ARR_POS 1
#else
#define SERVO_2_HIGH SERVO_2_PIN_HIGH
#define SERVO_2_LOW SERVO_2_PIN_LOW
#define SERVO_2_ARR_POS 1
#endif
#endif
#if (PRI_SERVO_FROM <= 3 && PRI_SERVO_TO >= 3) || (SEC_SERVO_FROM <= 3 && SEC_SERVO_TO >= 3)
#undef LAST_LOW
#define LAST_LOW SERVO_3_PIN_LOW
#if !defined(SERVO_1_HIGH)
#define SERVO_1_HIGH SERVO_3_PIN_HIGH
#define SERVO_1_LOW SERVO_3_PIN_LOW
#define SERVO_1_ARR_POS 2
#elif !defined(SERVO_2_HIGH)
#define SERVO_2_HIGH SERVO_3_PIN_HIGH
#define SERVO_2_LOW SERVO_3_PIN_LOW
#define SERVO_2_ARR_POS 2
#else
#define SERVO_3_HIGH SERVO_3_PIN_HIGH
#define SERVO_3_LOW SERVO_3_PIN_LOW
#define SERVO_3_ARR_POS 2
#endif
#endif
#if (PRI_SERVO_FROM <= 4 && PRI_SERVO_TO >= 4) || (SEC_SERVO_FROM <= 4 && SEC_SERVO_TO >= 4)
#undef LAST_LOW
#define LAST_LOW SERVO_4_PIN_LOW
#if !defined(SERVO_1_HIGH)
#define SERVO_1_HIGH SERVO_4_PIN_HIGH
#define SERVO_1_LOW SERVO_4_PIN_LOW
#define SERVO_1_ARR_POS 3
#elif !defined(SERVO_2_HIGH)
#define SERVO_2_HIGH SERVO_4_PIN_HIGH
#define SERVO_2_LOW SERVO_4_PIN_LOW
#define SERVO_2_ARR_POS 3
#elif !defined(SERVO_3_HIGH)
#define SERVO_3_HIGH SERVO_4_PIN_HIGH
#define SERVO_3_LOW SERVO_4_PIN_LOW
#define SERVO_3_ARR_POS 3
#else
#define SERVO_4_HIGH SERVO_4_PIN_HIGH
#define SERVO_4_LOW SERVO_4_PIN_LOW
#define SERVO_4_ARR_POS 3
#endif
#endif
#if (PRI_SERVO_FROM <= 5 && PRI_SERVO_TO >= 5) || (SEC_SERVO_FROM <= 5 && SEC_SERVO_TO >= 5)
#undef LAST_LOW
#define LAST_LOW SERVO_5_PIN_LOW
#if !defined(SERVO_1_HIGH)
#define SERVO_1_HIGH SERVO_5_PIN_HIGH
#define SERVO_1_LOW SERVO_5_PIN_LOW
#define SERVO_1_ARR_POS 4
#elif !defined(SERVO_2_HIGH)
#define SERVO_2_HIGH SERVO_5_PIN_HIGH
#define SERVO_2_LOW SERVO_5_PIN_LOW
#define SERVO_2_ARR_POS 4
#elif !defined(SERVO_3_HIGH)
#define SERVO_3_HIGH SERVO_5_PIN_HIGH
#define SERVO_3_LOW SERVO_5_PIN_LOW
#define SERVO_3_ARR_POS 4
#elif !defined(SERVO_4_HIGH)
#define SERVO_4_HIGH SERVO_5_PIN_HIGH
#define SERVO_4_LOW SERVO_5_PIN_LOW
#define SERVO_4_ARR_POS 4
#else
#define SERVO_5_HIGH SERVO_5_PIN_HIGH
#define SERVO_5_LOW SERVO_5_PIN_LOW
#define SERVO_5_ARR_POS 4
#endif
#endif
#if (PRI_SERVO_FROM <= 6 && PRI_SERVO_TO >= 6) || (SEC_SERVO_FROM <= 6 && SEC_SERVO_TO >= 6)
#undef LAST_LOW
#define LAST_LOW SERVO_6_PIN_LOW
#if !defined(SERVO_1_HIGH)
#define SERVO_1_HIGH SERVO_6_PIN_HIGH
#define SERVO_1_LOW SERVO_6_PIN_LOW
#define SERVO_1_ARR_POS 5
#elif !defined(SERVO_2_HIGH)
#define SERVO_2_HIGH SERVO_6_PIN_HIGH
#define SERVO_2_LOW SERVO_6_PIN_LOW
#define SERVO_2_ARR_POS 5
#elif !defined(SERVO_3_HIGH)
#define SERVO_3_HIGH SERVO_6_PIN_HIGH
#define SERVO_3_LOW SERVO_6_PIN_LOW
#define SERVO_3_ARR_POS 5
#elif !defined(SERVO_4_HIGH)
#define SERVO_4_HIGH SERVO_6_PIN_HIGH
#define SERVO_4_LOW SERVO_6_PIN_LOW
#define SERVO_4_ARR_POS 5
#elif !defined(SERVO_5_HIGH)
#define SERVO_5_HIGH SERVO_6_PIN_HIGH
#define SERVO_5_LOW SERVO_6_PIN_LOW
#define SERVO_5_ARR_POS 5
#else
#define SERVO_6_HIGH SERVO_6_PIN_HIGH
#define SERVO_6_LOW SERVO_6_PIN_LOW
#define SERVO_6_ARR_POS 5
#endif
#endif
#if (PRI_SERVO_FROM <= 7 && PRI_SERVO_TO >= 7) || (SEC_SERVO_FROM <= 7 && SEC_SERVO_TO >= 7)
#undef LAST_LOW
#define LAST_LOW SERVO_7_PIN_LOW
#if !defined(SERVO_1_HIGH)
#define SERVO_1_HIGH SERVO_7_PIN_HIGH
#define SERVO_1_LOW SERVO_7_PIN_LOW
#define SERVO_1_ARR_POS 6
#elif !defined(SERVO_2_HIGH)
#define SERVO_2_HIGH SERVO_7_PIN_HIGH
#define SERVO_2_LOW SERVO_7_PIN_LOW
#define SERVO_2_ARR_POS 6
#elif !defined(SERVO_3_HIGH)
#define SERVO_3_HIGH SERVO_7_PIN_HIGH
#define SERVO_3_LOW SERVO_7_PIN_LOW
#define SERVO_3_ARR_POS 6
#elif !defined(SERVO_4_HIGH)
#define SERVO_4_HIGH SERVO_7_PIN_HIGH
#define SERVO_4_LOW SERVO_7_PIN_LOW
#define SERVO_4_ARR_POS 6
#elif !defined(SERVO_5_HIGH)
#define SERVO_5_HIGH SERVO_7_PIN_HIGH
#define SERVO_5_LOW SERVO_7_PIN_LOW
#define SERVO_5_ARR_POS 6
#elif !defined(SERVO_6_HIGH)
#define SERVO_6_HIGH SERVO_7_PIN_HIGH
#define SERVO_6_LOW SERVO_7_PIN_LOW
#define SERVO_6_ARR_POS 6
#else
#define SERVO_7_HIGH SERVO_7_PIN_HIGH
#define SERVO_7_LOW SERVO_7_PIN_LOW
#define SERVO_7_ARR_POS 6
#endif
#endif
#if (PRI_SERVO_FROM <= 8 && PRI_SERVO_TO >= 8) || (SEC_SERVO_FROM <= 8 && SEC_SERVO_TO >= 8)
#undef LAST_LOW
#define LAST_LOW SERVO_8_PIN_LOW
#if !defined(SERVO_1_HIGH)
#define SERVO_1_HIGH SERVO_8_PIN_HIGH
#define SERVO_1_LOW SERVO_8_PIN_LOW
#define SERVO_1_ARR_POS 7
#elif !defined(SERVO_2_HIGH)
#define SERVO_2_HIGH SERVO_8_PIN_HIGH
#define SERVO_2_LOW SERVO_8_PIN_LOW
#define SERVO_2_ARR_POS 7
#elif !defined(SERVO_3_HIGH)
#define SERVO_3_HIGH SERVO_8_PIN_HIGH
#define SERVO_3_LOW SERVO_8_PIN_LOW
#define SERVO_3_ARR_POS 7
#elif !defined(SERVO_4_HIGH)
#define SERVO_4_HIGH SERVO_8_PIN_HIGH
#define SERVO_4_LOW SERVO_8_PIN_LOW
#define SERVO_4_ARR_POS 7
#elif !defined(SERVO_5_HIGH)
#define SERVO_5_HIGH SERVO_8_PIN_HIGH
#define SERVO_5_LOW SERVO_8_PIN_LOW
#define SERVO_5_ARR_POS 7
#elif !defined(SERVO_6_HIGH)
#define SERVO_6_HIGH SERVO_8_PIN_HIGH
#define SERVO_6_LOW SERVO_8_PIN_LOW
#define SERVO_6_ARR_POS 7
#elif !defined(SERVO_7_HIGH)
#define SERVO_7_HIGH SERVO_8_PIN_HIGH
#define SERVO_7_LOW SERVO_8_PIN_LOW
#define SERVO_7_ARR_POS 7
#else
#define SERVO_8_HIGH SERVO_8_PIN_HIGH
#define SERVO_8_LOW SERVO_8_PIN_LOW
#define SERVO_8_ARR_POS 7
#endif
#endif
in Output.pde
Code: Select all
#define SERVO_FAST_PWM // to activate it
// for servo 2-8
// its almost the same as for servo 1
#define SERVO_PULSE(PIN_HIGH,ACT_STATE,SERVO_NUM,LAST_PIN_LOW) \
}else if(state == ACT_STATE){ \
LAST_PIN_LOW; \
PIN_HIGH; \
SERVO_CHANNEL+=SERVO_1K_US; \
state++; \
}else if(state == ACT_STATE+1){ \
SERVO_CHANNEL+=atomicServo[SERVO_NUM]; \
state++; \
ISR(SERVO_ISR) {
static uint8_t state = 0; // indicates the current state of the chain
if(state == 0){
SERVO_1_HIGH; // set servo 1's pin high
SERVO_CHANNEL+=SERVO_1K_US; // wait 1000us
state++; // count up the state
}else if(state==1){
SERVO_CHANNEL+=atomicServo[SERVO_1_ARR_POS]; // load the servo's value (0-1000us)
state++; // count up the state
#if defined(SERVO_2_HIGH)
SERVO_PULSE(SERVO_2_HIGH,2,SERVO_2_ARR_POS,SERVO_1_LOW); // the same here
#endif
#if defined(SERVO_3_HIGH)
SERVO_PULSE(SERVO_3_HIGH,4,SERVO_3_ARR_POS,SERVO_2_LOW);
#endif
#if defined(SERVO_4_HIGH)
SERVO_PULSE(SERVO_4_HIGH,6,SERVO_4_ARR_POS,SERVO_3_LOW);
#endif
#if defined(SERVO_5_HIGH)
SERVO_PULSE(SERVO_5_HIGH,8,SERVO_5_ARR_POS,SERVO_4_LOW);
#endif
#if defined(SERVO_6_HIGH)
SERVO_PULSE(SERVO_6_HIGH,10,SERVO_6_ARR_POS,SERVO_5_LOW);
#endif
#if defined(SERVO_7_HIGH)
SERVO_PULSE(SERVO_7_HIGH,12,SERVO_7_ARR_POS,SERVO_6_LOW);
#endif
#if defined(SERVO_8_HIGH)
SERVO_PULSE(SERVO_8_HIGH,14,SERVO_8_ARR_POS,SERVO_7_LOW);
#endif
}else{
LAST_LOW;
#if defined(SERVO_FAST_PWM)
#if defined(SERVO_3_HIGH) // if there are 3 or more servos we dont need to slow it down
SERVO_CHANNEL+=(SERVO_1K_US>>3); // 0 would be better but it cause bad jitter
state=0;
#else // if there are less then 3 servos we need to slow it to not go over 300Hz (the highest working refresh rate for the digital servos for what i know..)
SERVO_CHANNEL+=SERVO_1K_US;
if(state<4){
state+=2;
}else{
state=0;
}
#endif
#else // to have ~ 50Hz for all servos
SERVO_CHANNEL+=SERVO_1K_US;
if(state<30){
state+=2;
}else{
state=0;
}
#endif
}
}
regards
Felix