marvin/arduino-c
1
0
Fork 0
mirror of https://github.com/Kugelschieber/arduino-c.git synced 2026-01-18 11:00:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

Finished servo lib.

Browse Source
This commit is contained in:
Marvin Blum
2017-04-24 23:50:20 +02:00
parent 55261de66c
commit 9a62dff10f
4 changed files with 107 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
servo/servo.c
View File

@@ -1,5 +1,6 @@
#include "servo.h"
#include "../ard/pins.h"
#include "../ard/util.h"
#include <util/delay.h>
#include <avr/io.h>
#include <avr/interrupt.h>
@@ -9,19 +10,21 @@
#define SERVO_DELTA_US 1000
Servo* servos[MAX_SERVOS];
unsigned char servo_i = 0;
unsigned char servo_n = 0, servo_i = 0;
void servo_init(Servo* servo){
if(servo_i >= MAX_SERVOS){
void servo_init(unsigned char pin, Servo* servo){
if(servo_n >= MAX_SERVOS){
return;
}
servos[servo_i] = servo;
servo_i++;
(*servo).no = servo_n;
(*servo).pin = pin;
servos[servo_n] = servo;
servo_n++;
pin_mode(pin, OUTPUT);
servo_set(servo, 128);
sei();
pin_mode((*servo).pin, OUTPUT);
(*servo).val = 0;
TCCR1A = 0x00;
TCCR1B = (1<<WGM12);
@@ -29,7 +32,7 @@ void servo_init(Servo* servo){
// 20ms, 1.5ms
OCR1A = 1250;
OCR1B = 3000;
OCR1B = 94;
// enable interrupt on timer match
TIMSK1 = (1<<OCIE1A)|(1<<OCIE1B);
@@ -39,15 +42,44 @@ void servo_init(Servo* servo){
}
void servo_set(Servo* servo, unsigned char val){
// calculate impuls time
(*servo).val = val;
(*servo).impuls = (unsigned char)(63+(63.0/255.0)*((float)val)); // 63-125
// set timer for first or other servos
if((*servo).no == 0){
(*servo).timer = (*servo).impuls;
}
else{
(*servo).timer = (*servo).impuls+(*servos[(*servo).no-1]).timer;
}
// update following servos
unsigned char i = 0;
for(i = (*servo).no+1; i < servo_n; i++){
(*servos[i]).timer = (*servos[i]).impuls+(*servos[i-1]).timer;
}
}
unsigned char servo_get(Servo* servo){
return (*servo).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);
if(servo_n > 0){
servo_i = 0;
OCR1B = (*servos[0]).timer;
digital_write((*servos[0]).pin, HIGH);
}
}
ISR(TIMER1_COMPB_vect){
digital_write((*servos[servo_i]).pin, LOW);
if(servo_i < servo_n-1){
servo_i++;
OCR1B = (*servos[servo_i]).timer;
digital_write((*servos[servo_i]).pin, HIGH);
}
}