Avr servo

From OmgaV Wiki

#include <avr/io.h>
#include <avr/interrupt.h>
#include "servoEspen.h"
 
#define NUM_SERVOS 3
#ifndef F_CLK
	#define F_CLK 8000000
#endif
 
//Calculate how many cycles the timer 
#define NUM_OVERF_CLOCKS (uint16_t)(F_CLK/8)/(NUM_SERVOS*50)
 
//Calculate the max-servopos
#define NUM_MAX_CLOCKS (uint16_t)(F_CLK/8)/(NUM_SERVOS*500)
//Calculate the min-servopos
#define NUM_MIN_CLOCKS (uint16_t)(F_CLK/8)/(NUM_SERVOS*1000)
#define NUM_MED_CLOCKS (uint16_t)(NUM_MIN_CLOCKS + ((NUM_MAX_CLOCKS - NUM_MIN_CLOCKS) / 2))
 
#define SERVO_PORT PORTE
#define SERVO_DDR DDRE
#define START_PIN PIN4
 
volatile uint16_t servoPos[NUM_SERVOS] = {NUM_MED_CLOCKS,NUM_MED_CLOCKS,NUM_MED_CLOCKS};
volatile uint8_t curServo = 0;
 
void setupServos(void){
	uint8_t i;
	for(i=0;i<NUM_SERVOS;i++)
		SERVO_DDR |= (1<<(START_PIN+i));
	TCCR1B = (1<<CS11) | (1<<WGM13) | (1<<WGM12);
	TIMSK1 = (1<<OCIE1A) | (1<<ICIE1);
	ICR1 = NUM_OVERF_CLOCKS;
}
 
ISR(SIG_OUTPUT_COMPARE1A){
	//Set nåverende port til 0, inkrement portnummer
	SERVO_PORT &= (0xff - (1<<(START_PIN+curServo)));
	curServo++;
	if(curServo==NUM_SERVOS)
		curServo = 0;
}
 
ISR(SIG_INPUT_CAPTURE1){
	//Set den nåverende(neste port) til 1
	SERVO_PORT |= (1<<(START_PIN+curServo));
	OCR1A = servoPos[curServo];
}
void setServo(uint8_t servoSelect, uint16_t value){
	//Set the position of the servos
	if (servoSelect < NUM_SERVOS){
		if (value != 0){
			if (value < NUM_MIN_CLOCKS){
				value = NUM_MIN_CLOCKS;
			}else if (value > NUM_MAX_CLOCKS){
				value = NUM_MAX_CLOCKS;
			}
		}
		servoPos[servoSelect] = value;
	}
}