Svenska ElektronikForumet

Kod: Markera allt

//2ch Servo Controller for Attiny84
#define F_CPU 8000000L

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#define THROW_MOD 4
#define THROW_DIVISOR 215
#define MID_INPUT_CORR 128
#define INPUT_MIN 0
#define INPUT_MAX 255
#define PULSE_INVERT_OFFSET 1000
#define PULSE_MIN 1000
#define PULSE_MAX 2000

struct ChannelConfig
{
	uint8_t InputIndex;
	uint8_t ThrowIndex;
	uint8_t CenterIndex;
	uint8_t InvertedPin;
};

static struct ChannelConfig _channelConfig[2] = {
	{0,1,2,PB0},
	{3,4,7,PB1},
};
static uint8_t _adcNext[8] = {1,2,3,4,7,0,0,0}; //Indexed by ADMUX to select next adc channel. Index 5 and 6 are not used.
volatile int32_t _adcValue[8] = {0,0,0,0,0,0,0,0}; //Indexed by ADMUX to store adc values.     --||--

int32_t Map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max)
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void SetupADC()
{
	//setup adc, prescaler @ 128, interrupt enable, adc enable
	ADCSRA |= 1<<ADPS2 | 1<<ADPS1 | 1<<ADPS0 | 1<<ADIE | 1<<ADEN;
	ADCSRB |= 1<<ADLAR; //left adjust result => ADCH 0-255
}

void StartADC()
{
	ADCSRA |= 1<<ADSC;
}

void SetupPWM()
{
	TCCR1B |= 1<<WGM13 | 1<<WGM12;	// WGM mode 14, Fast pwm, top=ICR1
	TCCR1A |= 1<<WGM11;	
	TCCR1B |= 1<<CS11; // prescaler: 8
	ICR1 = 19999; //8MHz w ps8, 20k = 20ms

	TCCR1A |= 1<<COM1A1 // set OC0A/OC0B on Compare Match, clear OC0A/OC0B at BOTTOM
		    | 1<<COM1A0
		    | 1<<COM1B1
		    | 1<<COM1B0; 

	DDRA |= 1<<PA6 | 1<<PA5; //a6 OC1A, a5 OC1B -> output
}

void SetupInvertionSelection()
{
	PORTB |= 1<<PB1 | 1<<PB0; // enable pull-up	
}

int main(void)
{	
	SetupADC();	
	SetupPWM();	
	SetupInvertionSelection();
		
	sei();
	
	StartADC();
	
	uint8_t i=0;
		
    while (1) 
    {
		uint16_t offset = Map(_adcValue[_channelConfig[i].CenterIndex], INPUT_MIN,INPUT_MAX, PULSE_MIN,PULSE_MAX);
		uint16_t pulseLength = offset+(THROW_MOD*_adcValue[_channelConfig[i].ThrowIndex]*(_adcValue[_channelConfig[i].InputIndex]-MID_INPUT_CORR))/THROW_DIVISOR;
		
		if(PINB & (1<<_channelConfig[i].InvertedPin))
			pulseLength = PULSE_MAX - pulseLength + PULSE_INVERT_OFFSET;

		if(i==0)
			OCR1A = ICR1 - pulseLength;
		else
			OCR1B = ICR1 - pulseLength;
				
		if(++i>1)
			i=0;
    }
}

ISR(ADC_vect)
{
	_adcValue[ADMUX] = ADCH;
	ADMUX = _adcNext[ADMUX];

	StartADC();
}