mirror of
https://github.com/Kugelschieber/arduino-c.git
synced 2026-01-18 02:50:26 +00:00
54 lines
910 B
C
54 lines
910 B
C
#include "servo.h"
|
|
#include "../ard/pins.h"
|
|
#include <util/delay.h>
|
|
#include <avr/io.h>
|
|
#include <avr/interrupt.h>
|
|
|
|
#define MAX_SERVOS 8
|
|
#define SERVO_DELTA_MIN_US 1000
|
|
#define SERVO_DELTA_US 1000
|
|
|
|
Servo* servos[MAX_SERVOS];
|
|
unsigned char servo_i = 0;
|
|
|
|
void servo_init(Servo* servo){
|
|
if(servo_i >= MAX_SERVOS){
|
|
return;
|
|
}
|
|
|
|
servos[servo_i] = servo;
|
|
servo_i++;
|
|
|
|
sei();
|
|
pin_mode((*servo).pin, OUTPUT);
|
|
(*servo).val = 0;
|
|
|
|
TCCR1A = 0x00;
|
|
TCCR1B = (1<<WGM12);
|
|
TCCR1C = 0x00;
|
|
|
|
// 20ms, 1.5ms
|
|
OCR1A = 1250;
|
|
OCR1B = 3000;
|
|
|
|
// enable interrupt on timer match
|
|
TIMSK1 = (1<<OCIE1A)|(1<<OCIE1B);
|
|
|
|
// start interrupt, prescaler 8
|
|
TCCR1B |= 1<<CS12;
|
|
}
|
|
|
|
void servo_set(Servo* servo, unsigned char val){
|
|
(*servo).val = val;
|
|
}
|
|
|
|
ISR(TIMER1_COMPA_vect){
|
|
unsigned char i = 0;
|
|
|
|
for(i = 0; i < servo_i; i++){
|
|
digital_write(7, HIGH);
|
|
_delay_us(1000+(1000/255)*(*servos[i]).val);
|
|
digital_write(7, LOW);
|
|
}
|
|
}
|