From 9a62dff10f87c81b31e236d9215b84853a45385a Mon Sep 17 00:00:00 2001
From: Marvin Blum <marvin@marvinblum.de>
Date: Mon, 24 Apr 2017 23:50:20 +0200
Subject: [PATCH] Finished servo lib.

---
 main.c                        | 17 ++++++++--
 servo/example/servo_example.c | 40 +++++++++++++++++++++++
 servo/servo.c                 | 60 +++++++++++++++++++++++++++--------
 servo/servo.h                 |  8 ++++-
 4 files changed, 107 insertions(+), 18 deletions(-)
 create mode 100644 servo/example/servo_example.c

diff --git a/main.c b/main.c
index 2be8f5d..96eb2ce 100644
--- a/main.c
+++ b/main.c
@@ -5,7 +5,9 @@
 #include "ard/util.h"
 #include "servo/servo.h"
 
-Servo servo = {7};
+Servo servo1;
+Servo servo2;
+Servo servo3;
 
 void prepare();
 void loop();
@@ -21,9 +23,18 @@ int main(){
 void prepare(){
 	pins_init();
 	serial_init(9600);
-	servo_init(&servo);
+	servo_init(7, &servo1);
+	servo_init(6, &servo2);
+	servo_init(8, &servo3);
 }
 
 void loop(){
-
+	servo_set(&servo1, 0);
+	servo_set(&servo2, 255);
+	servo_set(&servo3, 0);
+	_delay_ms(500);
+	servo_set(&servo1, 128);
+	servo_set(&servo2, 128);
+	servo_set(&servo3, 255);
+	_delay_ms(500);
 }
diff --git a/servo/example/servo_example.c b/servo/example/servo_example.c
new file mode 100644
index 0000000..96eb2ce
--- /dev/null
+++ b/servo/example/servo_example.c
@@ -0,0 +1,40 @@
+#include <util/delay.h>
+#include <stdio.h>
+#include "ard/serial.h"
+#include "ard/pins.h"
+#include "ard/util.h"
+#include "servo/servo.h"
+
+Servo servo1;
+Servo servo2;
+Servo servo3;
+
+void prepare();
+void loop();
+
+int main(){
+	prepare();
+
+	while(1){
+		loop();
+	}
+}
+
+void prepare(){
+	pins_init();
+	serial_init(9600);
+	servo_init(7, &servo1);
+	servo_init(6, &servo2);
+	servo_init(8, &servo3);
+}
+
+void loop(){
+	servo_set(&servo1, 0);
+	servo_set(&servo2, 255);
+	servo_set(&servo3, 0);
+	_delay_ms(500);
+	servo_set(&servo1, 128);
+	servo_set(&servo2, 128);
+	servo_set(&servo3, 255);
+	_delay_ms(500);
+}
diff --git a/servo/servo.c b/servo/servo.c
index a45f993..160ddc1 100644
--- a/servo/servo.c
+++ b/servo/servo.c
@@ -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);
 	}
 }
diff --git a/servo/servo.h b/servo/servo.h
index 247e1da..4d3b936 100644
--- a/servo/servo.h
+++ b/servo/servo.h
@@ -1,12 +1,18 @@
 #ifndef SERVO_H_
 #define SERVO_H_
 
+// should not directly be modified!
+// use functions instead
 typedef struct{
+	unsigned char no;
 	unsigned char pin;
 	unsigned char val; // 0-255
+	unsigned char impuls; // 1-2ms
+	unsigned int timer;
 } Servo;
 
-void servo_init(Servo*);
+void servo_init(unsigned char, Servo*);
 void servo_set(Servo*, unsigned char);
+unsigned char servo_get(Servo*);
 
 #endif