Mercurial > d1-mini-20ch-servo

changeset 2:eebf26b86400 draft

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Working 18ch code
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Mon, 19 Dec 2016 00:40:57 +0100
parents 4c4f0373ef4c
children 0afff2c18af7
files firmware/Makefile firmware/main.c firmware/servo.c firmware/servo.h
diffstat 4 files changed, 126 insertions(+), 114 deletions(-) [+]
line wrap: on
line diff
--- a/firmware/Makefile	Mon Dec 19 00:11:30 2016 +0100
+++ b/firmware/Makefile	Mon Dec 19 00:40:57 2016 +0100
@@ -1,9 +1,9 @@
 DEVICE      = atmega328p
 CLOCK      = 16000000
 PROGRAMMER = -c usbasp
-OBJECTS    = main.o servo.o
+OBJECTS    = main.o
 
-#FUSES       = -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -U efuse:w:0xff:m
+FUSES       = -U lfuse:w:0xE2:m -U hfuse:w:0xDF:m
 
 # Tune the lines below only if you know what you are doing:
 AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE)
--- a/firmware/main.c	Mon Dec 19 00:11:30 2016 +0100
+++ b/firmware/main.c	Mon Dec 19 00:40:57 2016 +0100
@@ -1,24 +1,129 @@
-#include "servo.h"
 #include <avr/io.h>
+#include <avr/interrupt.h>
+#include <inttypes.h>
 
 
 #define I2C_ADDRESS 0x10
 #define I2C_BUFFER_SIZE 32
 
 
+unsigned char i2c_buffer[I2C_BUFFER_SIZE];
+unsigned char i2c_cursor = 0;
+unsigned char servo;
+union {
+    unsigned char bytes[2];
+    unsigned int value;
+} bytes2int;
+
+
+#define MAX_SERVO 20
+
+static uint16_t servo_pulse[MAX_SERVO];
+
+volatile static uint16_t servo_delay[MAX_SERVO];
+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 uint8_t event;
+
+// Remap servo numbers to pins.
+// 0 = PB0
+// 1 = PB1
+// 2 = PB2
+// 3 = PB3
+// 4 = PB4
+// 5 = PB5
+// 6 = PC0
+// 7 = PC1
+// 8 = PC2
+// 9 = PC3
+// 10 = PD0
+// 11 = PD1
+// 12 = PD2
+// 13 = PD3
+// 14 = PD4
+// 15 = PD5
+// 16 = PD6
+// 17 = PD7
+// 18 unused
+// 19 unused
+const static uint8_t servo_map[MAX_SERVO] = {
+    0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19
+};
+
+ISR(TIMER1_COMPA_vect) {
+    event = 0;
+    PORTB = 0b00111111; // leave out XTAL pins
+    PORTC = 0b00001111; // leave out RESET, SDA and SCL
+    PORTD = 0b11111111;
+    TCNT1 = 0;
+    OCR1B = servo_delay[event];
+}
+
+
+ISR(TIMER1_COMPB_vect) {
+    event += 1;
+    PORTB &= servo_mask_b[event];
+    PORTC &= servo_mask_c[event];
+    PORTD &= servo_mask_d[event];
+    OCR1B = servo_delay[event];
+}
+
+
+void servo_update() {
+    uint16_t last_pulse = 0;
+    for (uint8_t event = 0; event < 8; ++event) {
+        uint16_t smallest_pulse = 0xffff;
+        for (uint8_t servo = 0; servo < MAX_SERVO; ++servo) {
+            if (servo_pulse[servo] > last_pulse &&
+                servo_pulse[servo] < smallest_pulse) {
+                    smallest_pulse = servo_pulse[servo];
+            }
+        }
+        servo_delay[event] = smallest_pulse;
+        servo_mask_b[event] = 0xff;
+        for (uint8_t servo = 0; servo < 6; ++servo) {
+            if (servo_pulse[servo] <= smallest_pulse) {
+                servo_mask_b[event] &= ~(1 << servo);
+            }
+        }
+        servo_mask_c[event] = 0xff;
+        for (uint8_t servo = 0; servo < 4; ++servo) {
+            if (servo_pulse[6 + servo] <= smallest_pulse) {
+                servo_mask_c[event] &= ~(1 << servo);
+            }
+        }
+        servo_mask_d[event] = 0xff;
+        for (uint8_t servo = 0; servo < 8; ++servo) {
+            if (servo_pulse[10 + servo] <= smallest_pulse) {
+                servo_mask_d[event] &= ~(1 << servo);
+            }
+        }
+        last_pulse = smallest_pulse;
+    }
+}
+
+
 int main() {
-    unsigned char i2c_buffer[I2C_BUFFER_SIZE];
-    unsigned char i2c_cursor = 0;
-    unsigned char servo;
-    union {
-        unsigned char bytes[2];
-        unsigned int value;
-    } bytes2int;
-
+    // setup i2c slave
     PORTC |= 1<<PC4 | 1<<PC5; // enable pullups
     TWAR = I2C_ADDRESS<<1;
-    servo_setup();
 
+    // setup servos
+    TCCR1A = 0x00;
+    TCCR1B &= ~(1<<CS12 | 1<<CS11 | 1<<CS10);
+    TCCR1B |= 0x02; // prescaler 8
+    OCR1A = 20000; // period
+    TIMSK1 = 1<<OCIE1A | 1<<OCIE1B;
+    DDRD = 0xff;
+    for (int i = 0; i < MAX_SERVO; ++i) {
+        servo_pulse[i] = 0;
+    }
+    servo_update();
+    sei();
+    TCCR1C |= 1<<FOC1A; // trigger COMPA
+
+    // run i2c slave
     while (1) {
         TWCR = 1<<TWEN | 1<<TWINT | 1<<TWEA;
         while (!(TWCR & (1<<TWINT))) {}
@@ -31,21 +136,20 @@
                 if (i2c_cursor < I2C_BUFFER_SIZE) {
                     i2c_buffer[i2c_cursor++] = TWDR;
                 }
+                break;
             case 0xa0: // stop or repeated start received
                 if (i2c_cursor > 0) {
                     servo = i2c_buffer[0];
-                    if (servo < 20) {
-                        for (unsigned char i = 1; i < i2c_cursor - 1; i += 2) {
-                            bytes2int.bytes[0] = i2c_buffer[i];
-                            bytes2int.bytes[1] = i2c_buffer[i + 1];
-                            servo_pulse[servo] = bytes2int.value;
-                            ++servo;
-                            if (servo >= 20) {
-                                servo = 0;
-                            }
+                    for (unsigned char i = 1; i < i2c_cursor - 1; i += 2) {
+                        if (servo >= 20) {
+                            servo = 0;
                         }
-                        servo_update();
+                        bytes2int.bytes[0] = i2c_buffer[i];
+                        bytes2int.bytes[1] = i2c_buffer[i + 1];
+                        servo_pulse[servo_map[servo]] = bytes2int.value;
+                        servo += 1;
                     }
+                    servo_update();
                 }
                 break;
         }
--- a/firmware/servo.c	Mon Dec 19 00:11:30 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,84 +0,0 @@
-#include <avr/interrupt.h>
-#include <avr/io.h>
-#include <inttypes.h>
-
-
-uint16_t servo_pulse[20];
-
-static uint16_t prescaler;
-volatile static uint16_t servo_delay[8];
-volatile static uint8_t servo_mask_b[8];
-volatile static uint8_t servo_mask_c[8];
-volatile static uint8_t servo_mask_d[8];
-volatile static uint8_t event;
-
-
-ISR(TIMER1_COMPA_vect) {
-    event = 0;
-    PORTB = 0b00111111; // leave out XTAL pins
-    PORTC = 0b00001111; // leave out RESET, SDA and SCL
-    PORTD = 0b11111111;
-    TCNT1 = 0;
-    OCR1B = servo_delay[event];
-}
-
-
-ISR(TIMER1_COMPB_vect) {
-    event += 1;
-    PORTB &= servo_mask_b[event];
-    PORTC &= servo_mask_c[event];
-    PORTD &= servo_mask_d[event];
-    OCR1B = servo_delay[event];
-}
-
-
-void servo_update() {
-    uint16_t last_pulse = 0;
-    for (uint8_t event = 0; event < 8; ++event) {
-        uint16_t smallest_pulse = 0xffff;
-        for (uint8_t servo = 0; servo < 20; ++servo) {
-            if (servo_pulse[servo] > last_pulse &&
-                servo_pulse[servo] < smallest_pulse) {
-                    smallest_pulse = servo_pulse[servo];
-            }
-        }
-        servo_delay[event] = smallest_pulse;
-        servo_mask_b[event] = 0xff;
-        for (uint8_t servo = 0; servo < 6; ++servo) {
-            if (servo_pulse[servo] <= smallest_pulse) {
-                servo_mask_b[event] &= ~(1 << servo);
-            }
-        }
-        servo_mask_c[event] = 0xff;
-        for (uint8_t servo = 0; servo < 4; ++servo) {
-            if (servo_pulse[6 + servo] <= smallest_pulse) {
-                servo_mask_c[event] &= ~(1 << servo);
-            }
-        }
-        servo_mask_d[event] = 0xff;
-        for (uint8_t servo = 0; servo < 8; ++servo) {
-            if (servo_pulse[10 + servo] <= smallest_pulse) {
-                servo_mask_d[event] &= ~(1 << servo);
-            }
-        }
-        last_pulse = smallest_pulse;
-    }
-}
-
-
-void servo_setup() {
-    // period 20000
-    // prescaler 8
-    TCCR1A = 0x00;
-    TCCR1B &= ~(_BV(CS12) | _BV(CS11) | _BV(CS10));
-    TCCR1B |= 0x02;
-    OCR1A = 20000;
-    TIMSK1 = _BV(OCIE1A) | _BV(OCIE1B);
-    DDRD = 0xff;
-    for (int i = 0; i < 8; ++i) {
-        servo_pulse[i] = 1500;
-    }
-    servo_update();
-    sei();
-    TCCR1C |= 1<<FOC1A; // trigger COMPA
-}
--- a/firmware/servo.h	Mon Dec 19 00:11:30 2016 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,8 +0,0 @@
-#include <inttypes.h>
-
-
-extern uint16_t servo_pulse[8];
-
-
-void servo_update();
-void servo_setup();