Skip to content

Instantly share code, notes, and snippets.

@pepijndevos pepijndevos/pilot.ino
Last active Aug 29, 2015

Embed
What would you like to do?
Arduino servo controller
#include <Wire.h>
#include <Servo.h>
#define MAX_POS 2000
#define MIN_POS 1000
Servo servos[8];
volatile uint16_t servo_inputs[] = {1500,1500,1500,1500,1500,1500,1500,1500};
void setup() {
Serial.begin(9600);
Serial.println("hello");
servos[0].attach(6);
servos[1].attach(7);
servos[2].attach(8);
servos[3].attach(9);
// enable pin change interrupt
PCICR = _BV(PCIE2);
PCMSK2 = 0b00111100;
// I2C slave
Wire.begin(23);
Wire.onReceive(receiveEvent);
Wire.onRequest(requestEvent);
}
void receiveEvent(int howMany) {
if(Wire.available()) {
int idx = Wire.read();
while(1 < Wire.available()) {
uint16_t value = Wire.read() | (Wire.read() << 8);
servos[idx].writeMicroseconds(value);
idx++;
}
}
}
void requestEvent() {
Wire.write((char*)servo_inputs, 16);
}
void loop() {
for(int i = 0; i < 8; i++) {
Serial.println(servo_inputs[i]);
}
for(int i = 0; i < 8; i++) {
Serial.println(servos[i].read());
}
delay(1000);
}
ISR(PCINT2_vect) {
static uint8_t pin_state = 0;
static unsigned long servo_timers[] = {0,0,0,0,0,0,0,0};
uint8_t old_pins = pin_state;
pin_state = PIND;
uint8_t i = 1;
for(int j = 0; j < 8 ; j++) {
if((old_pins & i) && ~(pin_state & i)) { // falling edge
unsigned long val = micros() - servo_timers[j];
if(val < MAX_POS && val > MIN_POS) {
servo_inputs[j] = val;
}
} else if(~(old_pins & i) && (pin_state & i)) { // rising edge
servo_timers[j] = micros();
}
i = i << 1;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.