controller: pwm.cpp annotate

annotate pwm.cpp @ 2:9f41bcaa3cee

Handle servo updates in the main loop

author	Radomir Dopieralski <sheep-devel@sheep.art.pl>
date	Wed, 27 Jan 2016 11:59:41 +0100
parents	7b2d0a758e63
children

rev	line source
0 2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	1 #include "pwm.h"
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	2
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	3 typedef struct pwm_pin_t {
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	4 volatile uint8_t *port;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	5 uint8_t pin;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	6 uint16_t compare_value;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	7 } pwm_pin_t;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	8
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	9 typedef struct pwm_event_t {
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	10 //Only used in OCR1B; ignored for high event in OCR1A. This is the value which TCNT1 is firing
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	11 // on now; you need to reference the next element in the array to get the value which is
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	12 // to be reset for next time.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	13 uint16_t compare_value;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	14
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	15 //Port masks
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	16 #ifndef PWM_PORTA_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	17 uint8_t porta_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	18 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	19 #ifndef PWM_PORTB_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	20 uint8_t portb_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	21 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	22 #ifndef PWM_PORTC_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	23 uint8_t portc_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	24 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	25 #ifndef PWM_PORTD_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	26 uint8_t portd_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	27 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	28 } pwm_event_t;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	29
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	30
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	31 volatile void* _pwm_events_low_ptr; //Pointer to current value in _pwm_events_low. Reset in OCR1A, incremented in OCR1B.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	32
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	33
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	34 static uint8_t _set_phase_batch = 0; //Set to 1 when set_phase_batch is called with a changed value.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	35 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.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	36 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.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	37 static volatile uint8_t _set_stop = 0; //Set to 1 when stop is requested. ISR COMPA will turn off timer.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	38
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	39 static uint16_t _set_period = 0; //New period defined; set in set_period, and updated to OCR1A in changed in COMPA interrupt
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	40
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	41 //Variables used to store config data
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	42 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.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	43 static uint8_t _count; //How many pins should be used
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	44
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	45 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.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	46 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.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	47
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	48 static pwm_event_t _pwm_events_low[PWM_MAX_PINS + 1]; //Array of pwm events. Each event will set one or more pins low.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	49 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.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	50
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	51 static uint16_t _prescaler = 0x0; //Numeric prescaler (1, 8, etc). Required for _pwm_micros_to_clicks calls.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	52 static uint8_t _prescaler_mask = 0x0; //Prescaler mask corresponding to _prescaler
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	53
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	54
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	55 static uint16_t _pwm_micros_to_clicks(uint32_t micros){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	56 //There is a potential for an overflow here if micros * (F_CPU in MHz) does not fit into
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	57 // a 32 bit variable. If this happens, we can divide micros by prescaler before multiplying
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	58 // with F_CPU, although this will give a loss of precision. Leave as-is for now.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	59 return (((F_CPU / 1000000L) * micros) / _prescaler) & 0xFFF0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	60
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	61 //TODO Rounding, based on 10MHz clock and various prescalers:
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	62 //Prescaler: 1, AND with 0xFF80
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	63 //Prescaler: 8, AND with 0xFFF0
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	64 //Prescaler: 64, AND with 0xFFFE
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	65 //Other prescalers should be fine as-is...
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	66 //More testing at different F_CPU values and prescalers are still required.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	67 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	68
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	69 /*
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	70 * Note: We extrapolate the DDR registers based off of the associated PORT
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	71 * register. This assumes that the DDR registers come directly after the PORT
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	72 * equivalents, with DDRX at the next address after PORTX. This is valid for
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	73 * all the chips I have looked at; however, it is highly recommended that you
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	74 * check any new chips which you want to use this library with.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	75 */
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	76 void pwm_init(volatile uint8_t *ports[],
1 7b2d0a758e63 Add pcb and finish i2c slave Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: 0 diff changeset	77 uint8_t pins[],
7b2d0a758e63 Add pcb and finish i2c slave Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: 0 diff changeset	78 uint8_t count,
7b2d0a758e63 Add pcb and finish i2c slave Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: 0 diff changeset	79 uint32_t period) {
0 2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	80
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	81 _count = (count <= PWM_MAX_PINS ? count : PWM_MAX_PINS);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	82
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	83 //Store values
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	84 for (uint8_t i = 0; i < _count; i++){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	85 _pwm_pins[i].port = ports[i];
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	86 _pwm_pins[i].pin = pins[i];
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	87 *(_pwm_pins[i].port - 0x1) \|= _BV(_pwm_pins[i].pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	88 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	89
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	90 //This is calculated by the focumula:
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	91 // CUTOFF_VALUE = PRESCALER * MAX_VALUE / (F_CPU / 1000000)
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	92 // where CUTOFF_VALUE is the period comparison for each if block,
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	93 // PRESCALER is the prescaler in the datasheet for a given CSx selection,
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	94 // and MAX_VALUE is the largest integer of the selected bit depth (256 / 65536).
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	95
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	96 #ifdef PWM_8_BIT
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	97 uint32_t max_value = 255;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	98 #else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	99 uint32_t max_value = 65535L;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	100 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	101
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	102 if (period < (1 * max_value / (F_CPU / 1000000L))){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	103 _prescaler = 1;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	104 _prescaler_mask = _BV(CS10);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	105 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	106 else if (period < (8 * max_value / (F_CPU / 1000000L))){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	107 _prescaler = 8;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	108 _prescaler_mask = _BV(CS11);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	109 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	110 else if (period < (64 * max_value / (F_CPU / 1000000L))){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	111 _prescaler = 64;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	112 _prescaler_mask = _BV(CS11) \| _BV(CS10);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	113 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	114 else if (period < (256 * max_value / (F_CPU / 1000000L))){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	115 _prescaler = 256;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	116 _prescaler_mask = _BV(CS12);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	117 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	118 else if (period < (1024 * max_value / (F_CPU / 1000000L))){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	119 _prescaler = 1024;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	120 _prescaler_mask = _BV(CS12) \| _BV(CS10);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	121 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	122 else {
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	123 period = (1024 * max_value / (F_CPU / 1000000L)); //The largest possible period
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	124 _prescaler = 1024;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	125 _prescaler_mask = _BV(CS12) \| _BV(CS10);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	126 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	127
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	128 TCCR1A = 0x00;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	129 TCCR1B &= ~(_BV(CS12) \| _BV(CS11) \| _BV(CS10));
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	130 TCCR1B \|= _prescaler_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	131
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	132 uint16_t clicks = _pwm_micros_to_clicks(period);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	133 //OCR1A controls the PWM period
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	134 OCR1A = clicks;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	135 //OCR1B controls the PWM phase. It is initialized later.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	136
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	137 //Enable compare interrupt on both channels
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	138 TIMSK1 = _BV(OCIE1A) \| _BV(OCIE1B);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	139
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	140 //Enable interrupts if the NO_INTERRUPT_ENABLE define is not set. If it is, you need to call sei() elsewhere.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	141 #ifndef NO_INTERRUPT_ENABLE
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	142 sei();
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	143 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	144
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	145 //Force interrupt on compare A initially
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	146 TCCR1C \|= _BV(FOC1A);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	147 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	148
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	149 void pwm_start(){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	150 TCNT1 = 0x00; //Restart timer counter
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	151 TIMSK1 = _BV(OCIE1A) \| _BV(OCIE1B); //Enable output compare match interrupt enable
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	152 TCCR1B \|= _prescaler_mask; //Enable
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	153 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	154
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	155 void pwm_stop(){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	156 _set_stop = 1;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	157 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	158
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	159 //The comparison method used to sort pwm_pin variables
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	160 static int16_t _compare_values(const void pin1, const void pin2){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	161 const pwm_pin_t pwm1 = (const pwm_pin_t) pin1;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	162 const pwm_pin_t pwm2 = (const pwm_pin_t) pin2;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	163 if (pwm1->compare_value < pwm2->compare_value) return -1;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	164 else if (pwm1->compare_value > pwm2->compare_value) return 1;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	165 else return 0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	166 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	167
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	168 void pwm_set_phase(uint8_t index, uint32_t phase){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	169 pwm_set_phase_batch(index, phase);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	170 pwm_apply_batch();
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	171 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	172
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	173 void pwm_set_phase_batch(uint8_t index, uint32_t phase){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	174 //Bounds checking
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	175 if (index >= _count) {
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	176 return;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	177 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	178
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	179 //The new compare value, in clock ticks
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	180 uint16_t new_clicks = _pwm_micros_to_clicks(phase);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	181 pwm_pin_t *p = &(_pwm_pins[index]);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	182
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	183 if (p->compare_value != new_clicks){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	184 _set_phase_batch = 1;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	185 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	186 //Set the new compare value
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	187 p->compare_value = new_clicks;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	188 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	189
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	190 void pwm_apply_batch(){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	191 if (_set_phase_batch == 0){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	192 return; //No need to re-calculate if there was no changed phases.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	193 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	194
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	195 _set_phase_lock = 1;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	196
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	197 //Copy _pwm_pins to _pwm_pins_sorted, and then sort it by value
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	198 pwm_pin_t _pwm_pins_sorted[PWM_MAX_PINS];
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	199 for (uint8_t i = 0; i < _count; i++){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	200 _pwm_pins_sorted[i] = _pwm_pins[i];
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	201 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	202 qsort(_pwm_pins_sorted, _count, sizeof _pwm_pins_sorted[0], _compare_values);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	203
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	204 //Populate the _pwm_events_high variable and _pwm_events_low_new array, used in
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	205 // OCR1A and OCR1B respectively to turn pins on / off.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	206 //First we reset everything in this array...
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	207 #ifndef PWM_PORTA_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	208 _pwm_event_high_new.porta_mask = 0x00;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	209 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	210 #ifndef PWM_PORTB_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	211 _pwm_event_high_new.portb_mask = 0x00;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	212 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	213 #ifndef PWM_PORTC_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	214 _pwm_event_high_new.portc_mask = 0x00;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	215 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	216 #ifndef PWM_PORTD_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	217 _pwm_event_high_new.portd_mask = 0x00;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	218 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	219 for (uint8_t i = 0; i < _count + 1; i++){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	220 _pwm_events_low_new[i].compare_value = 0xFFFF;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	221 #ifndef PWM_PORTA_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	222 _pwm_events_low_new[i].porta_mask = 0xFF;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	223 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	224 #ifndef PWM_PORTB_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	225 _pwm_events_low_new[i].portb_mask = 0xFF;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	226 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	227 #ifndef PWM_PORTC_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	228 _pwm_events_low_new[i].portc_mask = 0xFF;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	229 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	230 #ifndef PWM_PORTD_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	231 _pwm_events_low_new[i].portd_mask = 0xFF;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	232 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	233 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	234 //... then we look through the sorted _pwm_pins list, and collect all of the ports / pins which are
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	235 // set at the same compare values into a single event.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	236 uint16_t last_compare_value = _pwm_pins_sorted[0].compare_value;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	237 uint8_t last_index = 0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	238
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	239 for (uint8_t i = 0; i < _count; i++){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	240 pwm_pin_t *p = &(_pwm_pins_sorted[i]);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	241 if (p->compare_value > last_compare_value){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	242 //We don't want an empty pwm_event at the beginning of the array, so verify whether last_compare_value was zero before incrementing
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	243 if (last_compare_value > 0) {
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	244 last_index++;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	245 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	246 last_compare_value = p->compare_value;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	247 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	248
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	249 if (p->compare_value > 0){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	250 //Set pins to high
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	251 #ifndef PWM_PORTA_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	252 if (p->port == &PORTA) _pwm_event_high_new.porta_mask \|= _BV(p->pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	253 else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	254 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	255 #ifndef PWM_PORTB_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	256 if (p->port == &PORTB) _pwm_event_high_new.portb_mask \|= _BV(p->pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	257 else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	258 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	259 #ifndef PWM_PORTC_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	260 if (p->port == &PORTC) _pwm_event_high_new.portc_mask \|= _BV(p->pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	261 else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	262 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	263 #ifndef PWM_PORTD_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	264 if (p->port == &PORTD) _pwm_event_high_new.portd_mask \|= _BV(p->pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	265 else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	266 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	267 if (0); //Nop so that ignored ports dont break things
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	268
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	269 //Set pins to low
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	270 pwm_event_t *e = &(_pwm_events_low_new[last_index]);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	271 e->compare_value = last_compare_value;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	272 #ifndef PWM_PORTA_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	273 if (p->port == &PORTA) e->porta_mask &= ~_BV(p->pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	274 else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	275 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	276 #ifndef PWM_PORTB_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	277 if (p->port == &PORTB) e->portb_mask &= ~_BV(p->pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	278 else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	279 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	280 #ifndef PWM_PORTC_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	281 if (p->port == &PORTC) e->portc_mask &= ~_BV(p->pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	282 else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	283 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	284 #ifndef PWM_PORTD_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	285 if (p->port == &PORTD) e->portd_mask &= ~_BV(p->pin);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	286 else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	287 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	288 if (0); //Nop so that ignored ports dont break things
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	289 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	290 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	291
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	292 _set_phase_batch = 0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	293
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	294 //Signal OCR1A that we are ready to load new values
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	295 _set_phase = 1;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	296 _set_phase_lock = 0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	297 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	298
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	299 void pwm_set_period(uint32_t period){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	300 _set_period = _pwm_micros_to_clicks(period);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	301 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	302
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	303
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	304
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	305 /*
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	306 * The frequency comparison. When it overflows, we reset the timer to 0.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	307 */
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	308 #ifdef PWM_8_BIT
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	309 EMPTY_INTERRUPT(TIM0_OVF_vect)
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	310 ISR(TIM0_COMPA_vect){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	311 #else
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	312 EMPTY_INTERRUPT(TIMER1_OVF_vect)
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	313 ISR(TIMER1_COMPA_vect){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	314 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	315 //Update values if needed
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	316 if (_set_phase && !_set_phase_lock){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	317 for (uint8_t i = 0; i < _count; i++){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	318 _pwm_events_low[i] = _pwm_events_low_new[i];
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	319 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	320 _pwm_event_high = _pwm_event_high_new;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	321 _set_phase = 0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	322 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	323 if (_set_period){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	324 OCR1A = _set_period;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	325 _set_period = 0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	326 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	327 if (_set_stop){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	328 TCCR1B = 0x00;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	329
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	330 #ifndef PWM_PORTA_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	331 PORTA &= ~_pwm_event_high.porta_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	332 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	333 #ifndef PWM_PORTB_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	334 PORTB &= ~_pwm_event_high.portb_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	335 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	336 #ifndef PWM_PORTC_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	337 PORTC &= ~_pwm_event_high.portc_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	338 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	339 #ifndef PWM_PORTD_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	340 PORTD &= ~_pwm_event_high.portd_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	341 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	342
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	343 _set_stop = 0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	344 return;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	345 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	346 //Reset counter after the new compare values are updated (otherwise it affects the phase
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	347 // of the next execution, causing jitter).
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	348 TCNT1 = 0;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	349
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	350 //Set to the first (sorted) compare value in the pwm_events_low array. This is calculated
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	351 // in pwm_apply_batch(), and updated above in this ISR.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	352 _pwm_events_low_ptr = (void*) _pwm_events_low;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	353 OCR1B = _pwm_events_low[0].compare_value;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	354
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	355 //Set pins high. We do this after re-enabling the clock so that we do not artificially increase
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	356 // the phase. We turn off the ports (in COMPB) in the same order that we turn them on here,
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	357 // so that the delta of any delay between PORTA and PORTD should be reduced or eliminated.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	358 #ifndef PWM_PORTA_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	359 PORTA \|= _pwm_event_high.porta_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	360 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	361 #ifndef PWM_PORTB_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	362 PORTB \|= _pwm_event_high.portb_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	363 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	364 #ifndef PWM_PORTC_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	365 PORTC \|= _pwm_event_high.portc_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	366 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	367 #ifndef PWM_PORTD_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	368 PORTD \|= _pwm_event_high.portd_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	369 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	370 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	371
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	372 /*
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	373 * The phase comparison, implemented in C. When it overflows, we find the next highest value.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	374 * For a faster alternative, comment this out and use the ASM version in pwm.S
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	375 *
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	376 ISR(TIMER1_COMPB_vect){
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	377 pwm_event_t* e = (pwm_event_t*) _pwm_events_low_ptr;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	378 _pwm_events_low_ptr = (uint8_t*) _pwm_events_low_ptr + sizeof(pwm_event_t);
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	379
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	380 #ifndef PWM_PORTA_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	381 PORTA &= e->porta_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	382 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	383 #ifndef PWM_PORTB_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	384 PORTB &= e->portb_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	385 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	386 #ifndef PWM_PORTC_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	387 PORTC &= e->portc_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	388 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	389 #ifndef PWM_PORTD_UNUSED
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	390 PORTD &= e->portd_mask;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	391 #endif
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	392
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	393 //Set the timer for the next lowest value.
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	394 OCR1B = ((pwm_event_t*) _pwm_events_low_ptr)->compare_value;
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	395 }
2b5078b37907 Working servo PWM Radomir Dopieralski <sheep-devel@sheep.art.pl> parents: diff changeset	396 */

Mercurial > controller

annotate pwm.cpp @ 2:9f41bcaa3cee