diff options
Diffstat (limited to 'ports/teensy/servo.c')
| -rw-r--r-- | ports/teensy/servo.c | 108 |
1 files changed, 54 insertions, 54 deletions
diff --git a/ports/teensy/servo.c b/ports/teensy/servo.c index 09155f942..33fb0c034 100644 --- a/ports/teensy/servo.c +++ b/ports/teensy/servo.c @@ -17,7 +17,7 @@ #define REFRESH_INTERVAL 20000 // minumim time to refresh servos in microseconds #define PDB_CONFIG (PDB_SC_TRGSEL(15) | PDB_SC_PDBEN | PDB_SC_PDBIE \ - | PDB_SC_CONT | PDB_SC_PRESCALER(2) | PDB_SC_MULT(0)) + | PDB_SC_CONT | PDB_SC_PRESCALER(2) | PDB_SC_MULT(0)) #define PDB_PRESCALE 4 #define usToTicks(us) ((us) * (F_BUS / 1000) / PDB_PRESCALE / 1000) #define ticksToUs(ticks) ((ticks) * PDB_PRESCALE * 1000 / (F_BUS / 1000)) @@ -36,14 +36,12 @@ typedef struct _pyb_servo_obj_t { #define clamp(v, min_val, max_val) ((v) < (min_val) ? (min_val) : (v) > (max_val) ? (max_val) : (v)) -static float map_uint_to_float(uint x, uint in_min, uint in_max, float out_min, float out_max) -{ - return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + (float)out_min; +static float map_uint_to_float(uint x, uint in_min, uint in_max, float out_min, float out_max) { + return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + (float)out_min; } -static uint map_float_to_uint(float x, float in_min, float in_max, uint out_min, uint out_max) -{ - return (int)((x - in_min) * (float)(out_max - out_min) / (in_max - in_min) + (float)out_min); +static uint map_float_to_uint(float x, float in_min, float in_max, uint out_min, uint out_max) { + return (int)((x - in_min) * (float)(out_max - out_min) / (in_max - in_min) + (float)out_min); } static mp_obj_t servo_obj_attach(mp_obj_t self_in, mp_obj_t pin_obj) { @@ -110,9 +108,9 @@ static mp_obj_t servo_obj_angle(int n_args, const mp_obj_t *args) { if (n_args == 1) { // get float angle = map_uint_to_float(servo_ticks[self->servo_id], - usToTicks(self->min_usecs), - usToTicks(self->max_usecs), - 0.0, 180.0); + usToTicks(self->min_usecs), + usToTicks(self->max_usecs), + 0.0, 180.0); return mp_obj_new_float(angle); } // Set @@ -124,9 +122,9 @@ static mp_obj_t servo_obj_angle(int n_args, const mp_obj_t *args) { angle = 180.0F; } servo_ticks[self->servo_id] = map_float_to_uint(angle, - 0.0F, 180.0F, - usToTicks(self->min_usecs), - usToTicks(self->max_usecs)); + 0.0F, 180.0F, + usToTicks(self->min_usecs), + usToTicks(self->max_usecs)); return mp_const_none; } @@ -207,7 +205,7 @@ mp_obj_t pyb_Servo(void) { /* Find an unallocated servo id */ self->servo_id = 0; - for (mask=1; mask < (1<<MAX_SERVOS); mask <<= 1) { + for (mask = 1; mask < (1 << MAX_SERVOS); mask <<= 1) { if (!(servo_allocated_mask & mask)) { servo_allocated_mask |= mask; servo_active_mask &= ~mask; @@ -221,45 +219,47 @@ mp_obj_t pyb_Servo(void) { return mp_const_none; } -void pdb_isr(void) -{ - static int8_t channel = 0, channel_high = MAX_SERVOS; - static uint32_t tick_accum = 0; - uint32_t ticks; - int32_t wait_ticks; +void pdb_isr(void) { + static int8_t channel = 0, channel_high = MAX_SERVOS; + static uint32_t tick_accum = 0; + uint32_t ticks; + int32_t wait_ticks; - // first, if any channel was left high from the previous - // run, now is the time to shut it off - if (servo_active_mask & (1 << channel_high)) { - digitalWrite(servo_pin[channel_high], LOW); - channel_high = MAX_SERVOS; - } - // search for the next channel to turn on - while (channel < MAX_SERVOS) { - if (servo_active_mask & (1 << channel)) { - digitalWrite(servo_pin[channel], HIGH); - channel_high = channel; - ticks = servo_ticks[channel]; - tick_accum += ticks; - PDB0_IDLY += ticks; - PDB0_SC = PDB_CONFIG | PDB_SC_LDOK; - channel++; - return; - } - channel++; - } - // when all channels have output, wait for the - // minimum refresh interval - wait_ticks = usToTicks(REFRESH_INTERVAL) - tick_accum; - if (wait_ticks < usToTicks(100)) wait_ticks = usToTicks(100); - else if (wait_ticks > 60000) wait_ticks = 60000; - tick_accum += wait_ticks; - PDB0_IDLY += wait_ticks; - PDB0_SC = PDB_CONFIG | PDB_SC_LDOK; - // if this wait is enough to satisfy the refresh - // interval, next time begin again at channel zero - if (tick_accum >= usToTicks(REFRESH_INTERVAL)) { - tick_accum = 0; - channel = 0; - } + // first, if any channel was left high from the previous + // run, now is the time to shut it off + if (servo_active_mask & (1 << channel_high)) { + digitalWrite(servo_pin[channel_high], LOW); + channel_high = MAX_SERVOS; + } + // search for the next channel to turn on + while (channel < MAX_SERVOS) { + if (servo_active_mask & (1 << channel)) { + digitalWrite(servo_pin[channel], HIGH); + channel_high = channel; + ticks = servo_ticks[channel]; + tick_accum += ticks; + PDB0_IDLY += ticks; + PDB0_SC = PDB_CONFIG | PDB_SC_LDOK; + channel++; + return; + } + channel++; + } + // when all channels have output, wait for the + // minimum refresh interval + wait_ticks = usToTicks(REFRESH_INTERVAL) - tick_accum; + if (wait_ticks < usToTicks(100)) { + wait_ticks = usToTicks(100); + } else if (wait_ticks > 60000) { + wait_ticks = 60000; + } + tick_accum += wait_ticks; + PDB0_IDLY += wait_ticks; + PDB0_SC = PDB_CONFIG | PDB_SC_LDOK; + // if this wait is enough to satisfy the refresh + // interval, next time begin again at channel zero + if (tick_accum >= usToTicks(REFRESH_INTERVAL)) { + tick_accum = 0; + channel = 0; + } } |