Hi all!
So i'm building my own quadcopter at this moment and I have designed a flight controller board for it using two atmega328p's. I've programmed it so that the first atmega receives al the data from sensors and the radio transmitter and then calculates the values for the motors, LEDs and servos. To have it run smooth, the motor controller (ESC) gets updated 250 times per second (250Hz). So the first atmega sends all the data via i2c to the second atmega 250 times per second. When the second atmega receives the data it causes an interrupt via Wire.onReceive(event);. This then sets a flag so the main loop starts when there is new data. Here is a really simplified version of my code:
atmega 1:
#include <Wire.h>
#include <CapacitiveSensor.h>
int servo1;
int val, i;
int escLR;
int escRR;
int escLF;
int escRF;
byte send_array[11];
byte error;
unsigned long int loop_timer;
void setup() {
Wire.begin(); //joins the i2c bus
TWBR = 12; //set i2c clock to 400kHz
GyroAccelInit();
delay(3000);
loop_timer = micros(); //set the timer for the loop
}
/*************************************************************************************************************************/
void loop() {
//ReadGyro(); //commented out because not relevant
//ReadAccel();
//PPM_get();
if (error == 1) {
send_array[10] &= B11111000;
send_array[10] |= B00000001; //set led bits to red
}
SendData(); //send the data
if (micros() - loop_timer > 4050) { //set error if the loop takes too long
error = 1;
}
while (micros() - loop_timer < 4000) {} //wait until 4 milliseconds have passed
loop_timer = micros();
}
/*************************************************************************************************************************/
void SendData() { //sends the data to second atmega
send_array[1] = escLR & 0xFF; //isolates the low bits
send_array[0] = escLR >> 8; //shifts high bits to the right
send_array[3] = escRR & 0xFF; //isolates the low bits
send_array[2] = escRR >> 8; //shifts high bits to the right
send_array[5] = escLF & 0xFF; //isolates the low bits
send_array[4] = escLF >> 8; //shifts high bits to the right
send_array[7] = escRF & 0xFF; //isolates the low bits
send_array[6] = escRF >> 8; //shifts high bits to the right
send_array[9] = servo1 & 0xFF; //isolates the low bits
send_array[8] = servo1 >> 8; //shifts high bits to the right
Wire.beginTransmission(42); //begins the transmission to device 42
for (int n = 0; n < 11; n++) {
Wire.write(send_array[n]);
}
Wire.endTransmission(); //sends the buffer to the slave
}
atmega 2:
#include <ServoTimer2.h>
#include <Wire.h>
int count;
int servo1_val;
int escLR_val;
int escRR_val;
int escLF_val;
int escRF_val;
byte state;
byte new_data;
byte buff[11];
unsigned long int loop_count;
ServoTimer2 escLR, escRR, escLF, escRF, servo1, servo2; //initialize ESCs
WS2812 LED(1); //one LED
cRGB value;
/*************************************************************************************************************************/
void setup() {
Wire.begin(42); //joins the i2c bus as a slave with an adress of 42
Wire.onReceive(event); //sets up the handler that gets activated when there is data coming in
Serial.begin(57600);
escLR.attach(8); //set ESC output to pin 8
escRR.attach(7); //set ESC output to pin 7
escLF.attach(6); //set ESC output to pin 6
escRF.attach(5); //set ESC output to pin 5
}
/*************************************************************************************************************************/
void loop() {
if (new_data == 1) { //start loop if there is new data
escLR_val = buff[0] << 8 | buff[1]; //get the two bytes to make a 16 bit integer
escRR_val = buff[2] << 8 | buff[3]; //get the two bytes to make a 16 bit integer
escLF_val = buff[4] << 8 | buff[5]; //get the two bytes to make a 16 bit integer
escRF_val = buff[6] << 8 | buff[7]; //get the two bytes to make a 16 bit integer
servo1_val = buff[8] << 8 | buff[9]; //get the two bytes to make a 16 bit integer
state = buff[10]; //get the state byte
escLR.write(escLR_val); //write to all the ESCs
escRR.write(escRR_val);
escLF.write(escLF_val);
escRF.write(escRF_val);
new_data = 0;
loop_count++;
}
}
/*************************************************************************************************************************/
void event(int howMany) { //handler that gets activated when there is data coming in
byte count = 0;
while (Wire.available()) { //wait until all bytes are in
buff[count] = Wire.read();
count++;
}
new_data = 1;
}
Now here's the problem: because of the interrupt that is activated when there is data coming in, the signal for the servos and motor controller is really unstable and the servos jitter all the time. I know this because when I disable the interrupt, the servos and motors behave perfectly normal. Other forums have suggested to use the servoTimer2 library (which I do) but that didn't change a thing. Also the servos only turn 90 degrees instead of 180 but that could be because I'm running them on 250Hz instead of 50Hz. So if anyone knows how to write to the servos at 50Hz and the motor controllers at 250Hz please let me know.
So does anyone have suggestions? I'm happy with all suggestions so if I have to I will rearrange my entire code to solve this problem. I was thinking about using the using the mills() function, using requestFrom() or disabling interrupt for a while and even using hardware PWM. So maybe that brings you an idea. I'm really stuck here and I have to have it finished in less than a week so any help would be more than welcome!
by the way, english isn't my first language and am really bad at explaining things so if things are not clear I'll be happy to explain!