Mercurial > d1-mini-20ch-servo

changeset 14:326ed6d18e6b draft default tip

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Fix address jumpers, add dead band, keep low when duty 0
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Fri, 20 Jan 2017 20:55:28 +0100
parents 846c874c742c
children
files firmware/Makefile firmware/main.c
diffstat 2 files changed, 76 insertions(+), 33 deletions(-) [+]
line wrap: on
line diff
--- a/firmware/Makefile	Fri Jan 20 17:47:37 2017 +0100
+++ b/firmware/Makefile	Fri Jan 20 20:55:28 2017 +0100
@@ -4,7 +4,7 @@
 OBJECTS = main.o
 
 FUSES = -U lfuse:w:0xE2:m -U hfuse:w:0xDF:m
-AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) -B 100
+AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) # -B 100
 COMPILE = avr-gcc -std=gnu99 -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE)
 
 all:	main.hex
--- a/firmware/main.c	Fri Jan 20 17:47:37 2017 +0100
+++ b/firmware/main.c	Fri Jan 20 20:55:28 2017 +0100
@@ -16,13 +16,19 @@
 
 
 #define MAX_SERVO 20
+#define DEAD_BAND 9
+
+
+struct masks {
+    uint8_t mask_b;
+    uint8_t mask_c;
+    uint8_t mask_d;
+};
 
 static uint16_t servo_pulse[MAX_SERVO];
 
 volatile static uint16_t servo_delay[MAX_SERVO + 1];
-volatile static uint8_t servo_mask_b[MAX_SERVO];
-volatile static uint8_t servo_mask_c[MAX_SERVO];
-volatile static uint8_t servo_mask_d[MAX_SERVO];
+volatile static struct masks servo_masks[MAX_SERVO];
 volatile static uint8_t servo_enable_b;
 volatile static uint8_t servo_enable_c;
 volatile static uint8_t servo_enable_d;
@@ -37,28 +43,51 @@
 };
 
 ISR(TIMER1_COMPA_vect) {
+    OCR1B = servo_delay[0];
+    TCNT1 = 0;
     servo_event = 0;
-    PORTB = 0xff;
-    // leave out RESET, A, SCL, PC7
-    // pull up SDA and SCL
-    PORTC = 0xff & 0x3f;
-    PORTD = 0xff;
-    TCNT1 = 0;
-    OCR1B = servo_delay[0];
+    PORTB = servo_enable_b;
+    PORTC = servo_enable_c;
+    PORTD = servo_enable_d;
 }
 
 
 ISR(TIMER1_COMPB_vect) {
-    PORTB &= servo_mask_b[servo_event];
-    PORTC &= servo_mask_c[servo_event] | 0x30;
-    PORTD &= servo_mask_d[servo_event];
-    servo_event += 1;
-    OCR1B = servo_delay[servo_event];
+    struct masks masks = servo_masks[servo_event];
+    OCR1B = servo_delay[++servo_event];
+    PORTB &= masks.mask_b;
+    PORTC &= masks.mask_c;
+    PORTD &= masks.mask_d;
 }
 
 
 void servo_update() {
+    uint8_t mask;
     uint16_t last_pulse = 0;
+
+    mask = 0x00;
+    for (uint8_t servo = 0; servo < 8; ++servo) {
+        if (servo_pulse[servo]) {
+            mask |= 1 << servo;
+        }
+    }
+    servo_enable_b = mask;
+    mask = 0x00;
+    for (uint8_t servo = 0; servo < 4; ++servo) {
+        if (servo_pulse[8 + servo]) {
+            mask |= 1 << servo;
+        }
+    }
+    // leave out RESET (PC6) and PC7
+    // pull up SDA (PC4) and SCL (PC5)
+    servo_enable_c = mask & 0x0f | 0x30;
+    mask = 0x00;
+    for (uint8_t servo = 0; servo < 8; ++servo) {
+        if (servo_pulse[12 + servo]) {
+            mask |= 1 << servo;
+        }
+    }
+    servo_enable_d = mask;
     for (uint8_t event = 0; event < 8; ++event) {
         uint16_t smallest_pulse = 0xffff;
         for (uint8_t servo = 0; servo < MAX_SERVO; ++servo) {
@@ -68,34 +97,45 @@
             }
         }
         servo_delay[event] = smallest_pulse;
-        servo_mask_b[event] = 0xff;
+        if (smallest_pulse != 0xffff) {
+            smallest_pulse += DEAD_BAND;
+        }
+
+        mask = 0x00;
         for (uint8_t servo = 0; servo < 8; ++servo) {
             if (servo_pulse[servo] &&
                 servo_pulse[servo] <= smallest_pulse) {
-                servo_mask_b[event] &= ~(1 << servo);
+                mask |= 1 << servo;
             }
         }
-        servo_mask_c[event] = 0xff;
+        servo_masks[event].mask_b = ~mask;
+
+        mask = 0x00;
         for (uint8_t servo = 0; servo < 4; ++servo) {
             if (servo_pulse[8 + servo] &&
                 servo_pulse[8 + servo] <= smallest_pulse) {
-                servo_mask_c[event] &= ~(1 << servo);
+                mask |= 1 << servo;
             }
         }
-        servo_mask_d[event] = 0xff;
+        // leave out rst, sda, scl and pc7
+        servo_masks[event].mask_c = (~mask) | 0xf0;
+
+        mask = 0x00;
         for (uint8_t servo = 0; servo < 8; ++servo) {
             if (servo_pulse[12 + servo] &&
                 servo_pulse[12 + servo] <= smallest_pulse) {
-                servo_mask_d[event] &= ~(1 << servo);
+                mask |= 1 << servo;
             }
         }
+        servo_masks[event].mask_d = ~mask;
+
         last_pulse = smallest_pulse;
     }
 }
 
 
 uint16_t adc(uint8_t pin) {
-    ADMUX = 0<<ADLAR | 0<<REFS1 | 0<<REFS0 | 0x03 & pin;
+    ADMUX = 0<<ADLAR | 0<<REFS1 | 0<<REFS0 | 0x07 & pin;
     ADCSRA |= 1<<ADSC;
     while (ADCSRA & (1<<ADSC)) {
         // Pass.
@@ -106,18 +146,19 @@
 
 int main() {
     uint8_t i2c_address = I2C_ADDRESS;
+
     // Init ADC.
-//    ADCSRA = 1<<ADEN | 1<<ADPS2 | 1<<ADPS1 | 0<<ADPS0; // Pre-scaler 64.
-//    if (adc(6) > 511) {
-//        i2c_address += 1;
-//    }
-//    if (adc(7) > 511) {
-//        i2c_address += 2;
-//    }
-//    ADCSRA = 0<<ADEN;
+    ADCSRA = 1<<ADEN | 1<<ADPS2 | 1<<ADPS1 | 0<<ADPS0; // Pre-scaler 64.
+    if (adc(6) > 511) {
+        i2c_address += 1;
+    }
+    if (adc(7) > 511) {
+        i2c_address += 2;
+    }
+    ADCSRA = 0;
 
     // setup i2c slave
-    //PORTC |= 1<<PC4 | 1<<PC5; // enable pullups
+    PORTC |= 1<<PC4 | 1<<PC5; // enable pullups
     TWAR = i2c_address<<1;
 
     // setup servos
@@ -127,7 +168,7 @@
     OCR1A = 20000; // period
     TIMSK1 = 1<<OCIE1A | 1<<OCIE1B;
     DDRB = 0xff;
-    DDRC = 0x0f;
+    DDRC = 0x0f; // leave out RST, SDA, SCL and PC7
     DDRD = 0xff;
     for (uint8_t i = 0; i < MAX_SERVO; ++i) {
         servo_pulse[i] = 0;
@@ -175,6 +216,8 @@
                 }
                 dirty = 1;
                 break;
+            default: // unexpected status
+                TWCR = 1<<TWINT | 1<<TWSTO | 1<<TWEN;
         }
     }
 }