servo geht

firmware/Makefile

@@ -21,7 +21,7 @@ HEX        = b15f.hex
 MCU        = atmega1284p
 CFLAGS     = -Wall -Wextra -std=c++14 -O3 -mmcu=$(MCU) -DF_CPU=$(F_CPU) $(DEBUG)
 LDFLAGS    = 
-OBJECTS    = main.o spi.o mcp23s17.o tlc5615.o adu.o selftest.o global_vars.o usart.o request_handlers.o interrupts.o pwm.o
+OBJECTS    = main.o spi.o mcp23s17.o tlc5615.o adu.o selftest.o global_vars.o usart.o request_handlers.o interrupts.o pwm.o servo.o
 
  
 

firmware/global_vars.cpp

@@ -11,3 +11,4 @@ volatile USART usart;
 volatile PWM pwm;
 volatile bool nextRequest = false;
 volatile uint16_t interruptCounters[35] = { 0 }; // alle Einträge mit 0 initialisieren
+volatile Servo servo;

firmware/global_vars.h

@@ -6,6 +6,7 @@
 #include "adu.h"
 #include "usart.h"
 #include "pwm.h"
+#include "servo.h"
 
 
 #define WDT_TIMEOUT WDTO_15MS
@@ -21,5 +22,6 @@ extern volatile USART usart;
 extern volatile PWM pwm;
 extern volatile bool nextRequest;
 extern volatile uint16_t interruptCounters[];
+extern volatile Servo servo;
 
 #endif // GLOBAL_VARS_H

firmware/interrupts.cpp

@@ -49,6 +49,7 @@ ISR(WDT_vect)
 ISR(TIMER2_COMPA_vect)
 {
     interruptCounters[9]++;
+    servo.handleTimer2();
 }
 
 ISR(TIMER2_COMPB_vect)
@@ -69,6 +70,7 @@ ISR(TIMER1_CAPT_vect)
 ISR(TIMER1_COMPA_vect)
 {
     interruptCounters[13]++;
+    servo.handleTimer1();
 }
 
 ISR(TIMER1_COMPB_vect)

firmware/request_handlers.cpp

@@ -91,6 +91,18 @@ void handleRequest()
         case RQ_COUNTER_OFFSET:
             rqGetInterruptCounterOffset();
             break;
+            
+        case RQ_SERVO_ENABLE:
+            rqServoEnable();
+            break;
+            
+        case RQ_SERVO_DISABLE:
+            rqServoDisable();
+            break;
+            
+        case RQ_SERVO_SET_POS:
+            rqServoSetPosition();
+            break;
 
 		default:
 			break;
@@ -312,3 +324,31 @@ void rqGetInterruptCounterOffset()
 	usart.writeInt((volatile uint16_t) &interruptCounters[0]);
 	usart.flush();
 }
+
+void rqServoEnable()
+{
+	usart.initTX();
+    servo.enable();
+
+	usart.writeByte(USART::MSG_OK);
+	usart.flush();
+}
+
+void rqServoDisable()
+{
+	usart.initTX();
+    servo.disable();
+
+	usart.writeByte(USART::MSG_OK);
+	usart.flush();
+}
+
+void rqServoSetPosition()
+{
+	usart.initTX();
+	uint16_t pos = usart.readInt();
+    servo.setPosition(pos);
+
+	usart.writeByte(USART::MSG_OK);
+	usart.flush();
+}

firmware/request_handlers.h

@@ -35,5 +35,8 @@ void rqGetMem8(void);
 void rqSetMem16(void);
 void rqGetMem16(void);
 void rqGetInterruptCounterOffset(void);
+void rqServoEnable(void);
+void rqServoDisable(void);
+void rqServoSetPosition(void);
 
 #endif // REQUEST_HANDLERS_H

firmware/servo.cpp

@@ -1 +1,51 @@
 #include "servo.h"
+
+void Servo::enable() volatile
+{
+    DDRB |= _BV(PB2);
+    
+    // configure impulse timer 1
+    TCCR1A = 0;
+    TCCR1B = _BV(WGM12);
+    TCCR1C = 0;
+    TIMSK1 = _BV(OCIE1A);
+    
+    // configure repeatition timer 2
+    TCCR2A = _BV(WGM21);
+    TCCR2B = _BV(CS22) | _BV(CS21); // prescaler 256
+    TIMSK2 = _BV(OCIE2A);
+    OCR2A = 124;
+    repetition_counter = 0;
+    
+    OCR1A = 1500; // 1,5 ms init value
+}
+    
+void Servo::disable() const volatile
+{
+    PORTB &= _BV(PB2);
+    DDRB &= _BV(PB2);
+    TCCR1B &= ~PRESCALER_IMPULSE; // stop timer 1
+    TCCR2B = 0; // stop timer 2
+}
+    
+void Servo::setPosition(uint16_t pos) const volatile
+{
+    OCR1A = pos;
+}
+
+void Servo::handleTimer1(void) const volatile
+{
+    PORTB &= ~_BV(PB2); // end pulse (set pin low)
+    TCCR1B &= ~PRESCALER_IMPULSE; // stop timer 1
+}
+
+void Servo::handleTimer2(void) volatile
+{
+    repetition_counter++;
+    if(repetition_counter >= 5)
+    {
+        repetition_counter = 0;
+        TCCR1B |= PRESCALER_IMPULSE; // start timer 1
+        PORTB |= _BV(PB2); // start impuls (set pin high)
+    }
+}

firmware/servo.h

@@ -1,14 +1,40 @@
 #ifndef SERVO_H
 #define SERVO_H
 
-class Server
+#include <avr/io.h>
+
+class Servo
 {
 public:
-	
-	/**
-	 * Initialisiert die Servo Funktion (Timer 1 & 2).
-	 */
-	void init(void) const volatile;
+    
+    /**
+     * Aktiviert das Servo Signal
+     */
+    void enable(void) volatile;
+    
+    /**
+     * Deaktiviert das Servo Signal
+     */
+    void disable(void) const volatile;
+    
+    /**
+     * Setzt die Pulselänge und damit die Servo Position
+     */
+    void setPosition(uint16_t pos) const volatile;
+    
+    /**
+     * Beendet den aktuellen Impuls. Wird von der zugehörigen Interrupt-Routine aufgerufen.
+     */
+    void handleTimer1(void) const volatile;
+    
+    /**
+     * Inkrementiert die Frequenzsteuerung und löst gegebenenfalls einen neuen Impuls aus.
+     */
+    void handleTimer2(void) volatile;
+    
+private:
+    uint8_t repetition_counter;
+    static constexpr uint8_t PRESCALER_IMPULSE = _BV(CS11); // prescaler 8
 };
 
 #endif // SERVO_H