Mercurial > controller
changeset 0:2b5078b37907
Working servo PWM
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Tue, 12 Jan 2016 19:33:31 +0100 |
| parents | |
| children | 7b2d0a758e63 |
| files | .hgignore controller.ino pwm.S pwm.cpp pwm.h |
| diffstat | 5 files changed, 621 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/.hgignore Tue Jan 12 19:33:31 2016 +0100 @@ -0,0 +1,16 @@ +syntax: glob +*.pyc +*~ +.*.swp +docs/* +build/* +dist/* +pygments/* +werkzeug/* +bin/* +lib/* +include/* +local +*.egg-info +cache/* +*.DS_Store
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/controller.ino Tue Jan 12 19:33:31 2016 +0100 @@ -0,0 +1,26 @@ +#include "pwm.h" + +const uint8_t MAX_PINS = 18; +const uint32_t SERIAL_SPEED = 115200L; +const uint32_t PWM_PERIOD = 20000L; +volatile uint8_t *ports[18] = { + &PORTD, &PORTD, + &PORTD, &PORTD, &PORTD, &PORTD, &PORTD, &PORTD, &PORTB, &PORTB, + &PORTB, &PORTB, &PORTB, &PORTB, &PORTC, &PORTC, &PORTC, &PORTC, +}; +uint8_t pins[18] = { + 1, 0, + 2, 3, 4, 5, 6, 7, 0, 1, + 2, 3, 4, 5, 0, 1, 2, 3, +}; + +void setup() { + pwm_init(ports, pins, MAX_PINS, PWM_PERIOD); + for (uint8_t i = 0; i < MAX_PINS; ++i) { + pwm_set_phase_batch(i, 1500); + } + pwm_apply_batch(); +} + +void loop() { +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pwm.S Tue Jan 12 19:33:31 2016 +0100 @@ -0,0 +1,73 @@ +#include <avr/io.h> +#define PWM_PORTA_UNUSED + + .global TIMER1_COMPB_vect +TIMER1_COMPB_vect: + + ;Save calling state + PUSH r0 + IN r0, _SFR_IO_ADDR(SREG) + PUSH r24 + PUSH r25 + PUSH ZL + PUSH ZH + + ;Load the pointer address into register Z. As with all + ; 16 bit register accesses, you read the low byte first. + LDS ZL, _pwm_events_low_ptr + LDS ZH, _pwm_events_low_ptr+1 + + ; Move the pointer past the current OCR1B value + ADIW ZL, 0x02 + +#ifndef PWM_PORTA_UNUSED + IN r24, _SFR_IO_ADDR(PORTA) + LD r25, Z+ + AND r24, r25 + OUT _SFR_IO_ADDR(PORTA), r24 +#endif + + +#ifndef PWM_PORTB_UNUSED + IN r24, _SFR_IO_ADDR(PORTB) + LD r25, Z+ + AND r24, r25 + OUT _SFR_IO_ADDR(PORTB), r24 +#endif + + +#ifndef PWM_PORTC_UNUSED + IN r24, _SFR_IO_ADDR(PORTC) + LD r25, Z+ + AND r24, r25 + OUT _SFR_IO_ADDR(PORTC), r24 +#endif + + +#ifndef PWM_PORTD_UNUSED + IN r24, _SFR_IO_ADDR(PORTD) + LD r25, Z+ + AND r24, r25 + OUT _SFR_IO_ADDR(PORTD), r24 +#endif + + ; Update pointer with next value in array + STS _pwm_events_low_ptr+1, ZH + STS _pwm_events_low_ptr, ZL + + ;Load the new compare value into OCRnB. As with all + ; 16 bit register access, you write the high byte first. + LD r24, Z+ + LD r25, Z+ + STS OCR1BH, r25 + STS OCR1BL, r24 + + ;Restore calling state + POP ZH + POP ZL + POP r25 + POP r24 + OUT _SFR_IO_ADDR(SREG), r0 + POP r0 + + RETI
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pwm.cpp Tue Jan 12 19:33:31 2016 +0100 @@ -0,0 +1,396 @@ +#include "pwm.h" + +typedef struct pwm_pin_t { + volatile uint8_t *port; + uint8_t pin; + uint16_t compare_value; +} pwm_pin_t; + +typedef struct pwm_event_t { + //Only used in OCR1B; ignored for high event in OCR1A. This is the value which TCNT1 is firing + // on *now*; you need to reference the next element in the array to get the value which is + // to be reset for next time. + uint16_t compare_value; + + //Port masks +#ifndef PWM_PORTA_UNUSED + uint8_t porta_mask; +#endif +#ifndef PWM_PORTB_UNUSED + uint8_t portb_mask; +#endif +#ifndef PWM_PORTC_UNUSED + uint8_t portc_mask; +#endif +#ifndef PWM_PORTD_UNUSED + uint8_t portd_mask; +#endif +} pwm_event_t; + + +volatile void* _pwm_events_low_ptr; //Pointer to current value in _pwm_events_low. Reset in OCR1A, incremented in OCR1B. + + +static uint8_t _set_phase_batch = 0; //Set to 1 when set_phase_batch is called with a changed value. +static volatile uint8_t _set_phase = 0; //Set to 1 when phase is re-calculated; signals that the ISR COMPA needs to re-load phase values. +static volatile uint8_t _set_phase_lock = 0; //Set to 1 when we are in set_phase function. Prevents OCR1A from copying the double buffered pwm_events when this is 0. +static volatile uint8_t _set_stop = 0; //Set to 1 when stop is requested. ISR COMPA will turn off timer. + +static uint16_t _set_period = 0; //New period defined; set in set_period, and updated to OCR1A in changed in COMPA interrupt + +//Variables used to store config data +static pwm_pin_t _pwm_pins[PWM_MAX_PINS]; //Array of pins. Index is important here, as that is how we refer to the pins from the outside world. +static uint8_t _count; //How many pins should be used + +static pwm_event_t _pwm_event_high; //PWM event to set all pins high at start of period. Calculated on a non-zero set_phase. +static pwm_event_t _pwm_event_high_new; //Double buffer of pwm high event; copied to pwm_events in OCR1A when _set_phase is non-zero. + +static pwm_event_t _pwm_events_low[PWM_MAX_PINS + 1]; //Array of pwm events. Each event will set one or more pins low. +static pwm_event_t _pwm_events_low_new[PWM_MAX_PINS + 1]; //Double buffer of pwm events. Calculated in each set_phase call; copied to pwm_events in OCR1A when _set_phase is non-zero. + +static uint16_t _prescaler = 0x0; //Numeric prescaler (1, 8, etc). Required for _pwm_micros_to_clicks calls. +static uint8_t _prescaler_mask = 0x0; //Prescaler mask corresponding to _prescaler + + +static uint16_t _pwm_micros_to_clicks(uint32_t micros){ + //There is a potential for an overflow here if micros * (F_CPU in MHz) does not fit into + // a 32 bit variable. If this happens, we can divide micros by prescaler before multiplying + // with F_CPU, although this will give a loss of precision. Leave as-is for now. + return (((F_CPU / 1000000L) * micros) / _prescaler) & 0xFFF0; + + //TODO Rounding, based on 10MHz clock and various prescalers: + //Prescaler: 1, AND with 0xFF80 + //Prescaler: 8, AND with 0xFFF0 + //Prescaler: 64, AND with 0xFFFE + //Other prescalers should be fine as-is... + //More testing at different F_CPU values and prescalers are still required. +} + +/* + * Note: We extrapolate the DDR registers based off of the associated PORT + * register. This assumes that the DDR registers come directly after the PORT + * equivalents, with DDRX at the next address after PORTX. This is valid for + * all the chips I have looked at; however, it is highly recommended that you + * check any new chips which you want to use this library with. + */ +void pwm_init(volatile uint8_t *ports[], + uint8_t pins[], + uint8_t count, + uint32_t period) { + + _count = (count <= PWM_MAX_PINS ? count : PWM_MAX_PINS); + + //Store values + for (uint8_t i = 0; i < _count; i++){ + _pwm_pins[i].port = ports[i]; + _pwm_pins[i].pin = pins[i]; + *(_pwm_pins[i].port - 0x1) |= _BV(_pwm_pins[i].pin); + } + + //This is calculated by the focumula: + // CUTOFF_VALUE = PRESCALER * MAX_VALUE / (F_CPU / 1000000) + // where CUTOFF_VALUE is the period comparison for each if block, + // PRESCALER is the prescaler in the datasheet for a given CSx selection, + // and MAX_VALUE is the largest integer of the selected bit depth (256 / 65536). + +#ifdef PWM_8_BIT + uint32_t max_value = 255; +#else + uint32_t max_value = 65535L; +#endif + + if (period < (1 * max_value / (F_CPU / 1000000L))){ + _prescaler = 1; + _prescaler_mask = _BV(CS10); + } + else if (period < (8 * max_value / (F_CPU / 1000000L))){ + _prescaler = 8; + _prescaler_mask = _BV(CS11); + } + else if (period < (64 * max_value / (F_CPU / 1000000L))){ + _prescaler = 64; + _prescaler_mask = _BV(CS11) | _BV(CS10); + } + else if (period < (256 * max_value / (F_CPU / 1000000L))){ + _prescaler = 256; + _prescaler_mask = _BV(CS12); + } + else if (period < (1024 * max_value / (F_CPU / 1000000L))){ + _prescaler = 1024; + _prescaler_mask = _BV(CS12) | _BV(CS10); + } + else { + period = (1024 * max_value / (F_CPU / 1000000L)); //The largest possible period + _prescaler = 1024; + _prescaler_mask = _BV(CS12) | _BV(CS10); + } + + TCCR1A = 0x00; + TCCR1B &= ~(_BV(CS12) | _BV(CS11) | _BV(CS10)); + TCCR1B |= _prescaler_mask; + + uint16_t clicks = _pwm_micros_to_clicks(period); + //OCR1A controls the PWM period + OCR1A = clicks; + //OCR1B controls the PWM phase. It is initialized later. + + //Enable compare interrupt on both channels + TIMSK1 = _BV(OCIE1A) | _BV(OCIE1B); + + //Enable interrupts if the NO_INTERRUPT_ENABLE define is not set. If it is, you need to call sei() elsewhere. +#ifndef NO_INTERRUPT_ENABLE + sei(); +#endif + + //Force interrupt on compare A initially + TCCR1C |= _BV(FOC1A); +} + +void pwm_start(){ + TCNT1 = 0x00; //Restart timer counter + TIMSK1 = _BV(OCIE1A) | _BV(OCIE1B); //Enable output compare match interrupt enable + TCCR1B |= _prescaler_mask; //Enable +} + +void pwm_stop(){ + _set_stop = 1; +} + +//The comparison method used to sort pwm_pin variables +static int16_t _compare_values(const void *pin1, const void *pin2){ + const pwm_pin_t *pwm1 = (const pwm_pin_t*) pin1; + const pwm_pin_t *pwm2 = (const pwm_pin_t*) pin2; + if (pwm1->compare_value < pwm2->compare_value) return -1; + else if (pwm1->compare_value > pwm2->compare_value) return 1; + else return 0; +} + +void pwm_set_phase(uint8_t index, uint32_t phase){ + pwm_set_phase_batch(index, phase); + pwm_apply_batch(); +} + +void pwm_set_phase_batch(uint8_t index, uint32_t phase){ + //Bounds checking + if (index >= _count) { + return; + } + + //The new compare value, in clock ticks + uint16_t new_clicks = _pwm_micros_to_clicks(phase); + pwm_pin_t *p = &(_pwm_pins[index]); + + if (p->compare_value != new_clicks){ + _set_phase_batch = 1; + } + //Set the new compare value + p->compare_value = new_clicks; +} + +void pwm_apply_batch(){ + if (_set_phase_batch == 0){ + return; //No need to re-calculate if there was no changed phases. + } + + _set_phase_lock = 1; + + //Copy _pwm_pins to _pwm_pins_sorted, and then sort it by value + pwm_pin_t _pwm_pins_sorted[PWM_MAX_PINS]; + for (uint8_t i = 0; i < _count; i++){ + _pwm_pins_sorted[i] = _pwm_pins[i]; + } + qsort(_pwm_pins_sorted, _count, sizeof _pwm_pins_sorted[0], _compare_values); + + //Populate the _pwm_events_high variable and _pwm_events_low_new array, used in + // OCR1A and OCR1B respectively to turn pins on / off. + //First we reset everything in this array... +#ifndef PWM_PORTA_UNUSED + _pwm_event_high_new.porta_mask = 0x00; +#endif +#ifndef PWM_PORTB_UNUSED + _pwm_event_high_new.portb_mask = 0x00; +#endif +#ifndef PWM_PORTC_UNUSED + _pwm_event_high_new.portc_mask = 0x00; +#endif +#ifndef PWM_PORTD_UNUSED + _pwm_event_high_new.portd_mask = 0x00; +#endif + for (uint8_t i = 0; i < _count + 1; i++){ + _pwm_events_low_new[i].compare_value = 0xFFFF; +#ifndef PWM_PORTA_UNUSED + _pwm_events_low_new[i].porta_mask = 0xFF; +#endif +#ifndef PWM_PORTB_UNUSED + _pwm_events_low_new[i].portb_mask = 0xFF; +#endif +#ifndef PWM_PORTC_UNUSED + _pwm_events_low_new[i].portc_mask = 0xFF; +#endif +#ifndef PWM_PORTD_UNUSED + _pwm_events_low_new[i].portd_mask = 0xFF; +#endif + } + //... then we look through the sorted _pwm_pins list, and collect all of the ports / pins which are + // set at the same compare values into a single event. + uint16_t last_compare_value = _pwm_pins_sorted[0].compare_value; + uint8_t last_index = 0; + + for (uint8_t i = 0; i < _count; i++){ + pwm_pin_t *p = &(_pwm_pins_sorted[i]); + if (p->compare_value > last_compare_value){ + //We don't want an empty pwm_event at the beginning of the array, so verify whether last_compare_value was zero before incrementing + if (last_compare_value > 0) { + last_index++; + } + last_compare_value = p->compare_value; + } + + if (p->compare_value > 0){ + //Set pins to high +#ifndef PWM_PORTA_UNUSED + if (p->port == &PORTA) _pwm_event_high_new.porta_mask |= _BV(p->pin); + else +#endif +#ifndef PWM_PORTB_UNUSED + if (p->port == &PORTB) _pwm_event_high_new.portb_mask |= _BV(p->pin); + else +#endif +#ifndef PWM_PORTC_UNUSED + if (p->port == &PORTC) _pwm_event_high_new.portc_mask |= _BV(p->pin); + else +#endif +#ifndef PWM_PORTD_UNUSED + if (p->port == &PORTD) _pwm_event_high_new.portd_mask |= _BV(p->pin); + else +#endif + if (0); //Nop so that ignored ports dont break things + + //Set pins to low + pwm_event_t *e = &(_pwm_events_low_new[last_index]); + e->compare_value = last_compare_value; +#ifndef PWM_PORTA_UNUSED + if (p->port == &PORTA) e->porta_mask &= ~_BV(p->pin); + else +#endif +#ifndef PWM_PORTB_UNUSED + if (p->port == &PORTB) e->portb_mask &= ~_BV(p->pin); + else +#endif +#ifndef PWM_PORTC_UNUSED + if (p->port == &PORTC) e->portc_mask &= ~_BV(p->pin); + else +#endif +#ifndef PWM_PORTD_UNUSED + if (p->port == &PORTD) e->portd_mask &= ~_BV(p->pin); + else +#endif + if (0); //Nop so that ignored ports dont break things + } + } + + _set_phase_batch = 0; + + //Signal OCR1A that we are ready to load new values + _set_phase = 1; + _set_phase_lock = 0; +} + +void pwm_set_period(uint32_t period){ + _set_period = _pwm_micros_to_clicks(period); +} + + + +/* + * The frequency comparison. When it overflows, we reset the timer to 0. + */ +#ifdef PWM_8_BIT +EMPTY_INTERRUPT(TIM0_OVF_vect) +ISR(TIM0_COMPA_vect){ +#else +EMPTY_INTERRUPT(TIMER1_OVF_vect) +ISR(TIMER1_COMPA_vect){ +#endif + //Update values if needed + if (_set_phase && !_set_phase_lock){ + for (uint8_t i = 0; i < _count; i++){ + _pwm_events_low[i] = _pwm_events_low_new[i]; + } + _pwm_event_high = _pwm_event_high_new; + _set_phase = 0; + } + if (_set_period){ + OCR1A = _set_period; + _set_period = 0; + } + if (_set_stop){ + TCCR1B = 0x00; + +#ifndef PWM_PORTA_UNUSED + PORTA &= ~_pwm_event_high.porta_mask; +#endif +#ifndef PWM_PORTB_UNUSED + PORTB &= ~_pwm_event_high.portb_mask; +#endif +#ifndef PWM_PORTC_UNUSED + PORTC &= ~_pwm_event_high.portc_mask; +#endif +#ifndef PWM_PORTD_UNUSED + PORTD &= ~_pwm_event_high.portd_mask; +#endif + + _set_stop = 0; + return; + } + //Reset counter after the new compare values are updated (otherwise it affects the phase + // of the next execution, causing jitter). + TCNT1 = 0; + + //Set to the first (sorted) compare value in the pwm_events_low array. This is calculated + // in pwm_apply_batch(), and updated above in this ISR. + _pwm_events_low_ptr = (void*) _pwm_events_low; + OCR1B = _pwm_events_low[0].compare_value; + + //Set pins high. We do this after re-enabling the clock so that we do not artificially increase + // the phase. We turn off the ports (in COMPB) in the same order that we turn them on here, + // so that the delta of any delay between PORTA and PORTD should be reduced or eliminated. +#ifndef PWM_PORTA_UNUSED + PORTA |= _pwm_event_high.porta_mask; +#endif +#ifndef PWM_PORTB_UNUSED + PORTB |= _pwm_event_high.portb_mask; +#endif +#ifndef PWM_PORTC_UNUSED + PORTC |= _pwm_event_high.portc_mask; +#endif +#ifndef PWM_PORTD_UNUSED + PORTD |= _pwm_event_high.portd_mask; +#endif +} + +/* + * The phase comparison, implemented in C. When it overflows, we find the next highest value. + * For a faster alternative, comment this out and use the ASM version in pwm.S + * +ISR(TIMER1_COMPB_vect){ + pwm_event_t* e = (pwm_event_t*) _pwm_events_low_ptr; + _pwm_events_low_ptr = (uint8_t*) _pwm_events_low_ptr + sizeof(pwm_event_t); + +#ifndef PWM_PORTA_UNUSED + PORTA &= e->porta_mask; +#endif +#ifndef PWM_PORTB_UNUSED + PORTB &= e->portb_mask; +#endif +#ifndef PWM_PORTC_UNUSED + PORTC &= e->portc_mask; +#endif +#ifndef PWM_PORTD_UNUSED + PORTD &= e->portd_mask; +#endif + + //Set the timer for the next lowest value. + OCR1B = ((pwm_event_t*) _pwm_events_low_ptr)->compare_value; +} +*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/pwm.h Tue Jan 12 19:33:31 2016 +0100 @@ -0,0 +1,110 @@ +/* + * PWM library which allows you to control an arbitrary number of outputs. All + * outputs must have the same period, but each one can have a different phase. + * This library relies on a 16 bit TIMER1, so cannot be run on any chip which + * does not have access to this hardware. + * + * Please set CDEF PWM_PORTx_UNUSED for all ports which either are not on your + * chip, or which are not being used for PWM. (If your chip does not have a + * given port, e.g. the ATMega168 does not have a PORTA, then there will be + * compile errors. If you have the hardware, but are not using it for PWM, + * then it will just use (slightly) more memory and will be (slightly) slower. + * + * Written by Wyatt Olson and Warren Janssens http://digitalcave.ca Released + * under the Creative Commons Attribution-NonCommercial-ShareAlike license. + */ + +#ifndef PWM_H +#define PWM_H + +#define PWM_PORTA_UNUSED + +#include <avr/io.h> +#include <avr/interrupt.h> +#include <math.h> +#include <stdlib.h> + +// The absolute maximum number of pins defined. The actual 'used' count is +// passed into pwm_init, and there is no problem with having a larger number +// here than is actually used (other than a bit of wasted SRAM space for +// pre-defined arrays). By default we set this to 4 as that is a decently +// large number, but not so huge that tons of excess SRAM is wasted. If you +// are low on memory, or need more outputs, feel free to redefine this in your +// makefile (in the CDEFS variable, beside where F_CPU is defined). +#ifndef PWM_MAX_PINS +#define PWM_MAX_PINS 18 +#endif + + +/* + * Initializes the PWM library at a given period, using the specified ports. + * You can have up to PWM_MAX_PINS pins defined here. + * + * To initialize this, you must pass in references to each of the PORTs, DDRs, + * and the values of each pin. You can do it like this: + * + * volatile uint8_t *ports[8]; ports[0] = &PORTB; ports[1] = &PORTD; .... + * ports[5] = &PORTD; + * + * uint8_t pins[8]; pins[0] = 1; .... pins[5] = 3; + * + * pwm_init(ports, pins, 6, 20000); + * + * While the code to do this is uglier than if it were to just be a function + * call, by allowing an arbitrary number of PWM pins, it is best to do it like + * this. + * + * The period must be less than 3354624us (about 3.3 seconds); any more than + * this and we just set it to the maximum. There is no theoretical minimum, + * but in practice you should probably keep it at 1000us or so (1ms). + */ +void pwm_init(volatile uint8_t *ports[], uint8_t pins[], uint8_t count, + uint32_t period); + + +/* + * Sets the phase of the pin at the given index to the specified value. The + * index matches the index used for the arrays in pwm_init(). Internally, this + * calls pwm_set_phase_batch() and then pwm_apply_batch(). + */ +void pwm_set_phase(uint8_t index, uint32_t phase); + +/* + * Sets the phase of the pin at the given index to the specified value, but + * does not apply the change (recalculate all pin values). This can be called + * multiple times for different pins with minimal overhead; when completed all + * changes, call pwm_apply_batch(). + */ +void pwm_set_phase_batch(uint8_t index, uint32_t phase); + +/* + * Recalculates all pin / timer values. Required after calling + * pwm_set_phase_batch(). + */ +void pwm_apply_batch(); + +/* + * Sets the period of the PWM generator, changing the period specified in + * init(). Applies to all active PWM pins. + */ +void pwm_set_period(uint32_t period); + +/* + * Turns off the PWM generator and stops the timer clock. Does not clear any + * config variables. You can turn PWM back on again using the pwm_start() + * function. + */ +void pwm_stop(); + +/* + * Turns on the PWM generator, starting back at 0. This should be called after + * pwm_off() has been called, but pwm_init() *must* already have been called. + * Calling this when the generator is already on will reset the timer to 0, but + * otherwise will have no effect. + * + * If TIMER1 control registers have been modified by other libraries, you must + * call pwm_init() instead of pwm_start(). + */ +void pwm_start(); + +#endif
