Avr servo

From OmegaV-wiki
Jump to: navigation, search

<source lang=c>

  1. include <avr/io.h>
  2. include <avr/interrupt.h>
  3. include "servoEspen.h"
  1. define NUM_SERVOS 3
  2. ifndef F_CLK

#define F_CLK 8000000

  1. endif

//Calculate how many cycles the timer

  1. define NUM_OVERF_CLOCKS (uint16_t)(F_CLK/8)/(NUM_SERVOS*50)

//Calculate the max-servopos

  1. define NUM_MAX_CLOCKS (uint16_t)(F_CLK/8)/(NUM_SERVOS*500)

//Calculate the min-servopos

  1. define NUM_MIN_CLOCKS (uint16_t)(F_CLK/8)/(NUM_SERVOS*1000)
  2. define NUM_MED_CLOCKS (uint16_t)(NUM_MIN_CLOCKS + ((NUM_MAX_CLOCKS - NUM_MIN_CLOCKS) / 2))
  1. define SERVO_PORT PORTE
  2. define SERVO_DDR DDRE
  3. 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; } } </source>

Personal tools