OK guys, I have an arduino trying to control 4 speed controllers on a quadrotor. The speed controllers can be controlled as standard servos, so I am using the arduino servo library. I am using the writeMicroseconds() command to send the signals to the speed controllers. The signals are read from an R/C receiver, stored in an array and written to the speed controllers.
Everything in the loop seems to be working well, just not fast enough. The loop takes between 107ms and 110ms to run. By this measurement, the values sent to the speed controllers should be updated 10 times per second. The arduino can read the values from the receiver at this rate, and even write them to the serial port at this rate, but the values sent to the speed controllers changes only about 5 times per second (This, I tested by using a standard servo in place of the speed controller for simplicity.)
Any solutions? I tried changing the frame rate in the servo library and this just caused the servo to freak out, for lack of a better technical term. Any advice is appreciated. Code will be posted below.
#include <Servo.h>
Servo speedController[4];
int timers[4];
long time = millis();
int NORTH = 0;
int SOUTH = 1;
int EAST = 2;
int WEST = 3;
int throttle[4];
int channel_1 = 42;
int xCoefficient = 100;
int yCoefficient = 100;
int zCoefficient = 100;
int throttleCoefficient = 700;
int coefficient[] = {yCoefficient, xCoefficient, throttleCoefficient, zCoefficient};
int ppm_bottom = 20140;
int ppm_top = 21320;
int servo_ppm[4];
long raw_ppm[4];
char Sbuffer[21]; //so strange. need to bring this one to the forums.;
int bottom = 1000;
int zero = 1292;
int top = 2000;
int mainThrottle = zero;
long refTime = millis();
int decodeThrottle(int start){
int throttle = 0;
int power = 1000;
for(int i = start; i < start + 4; i ++){
throttle += (int(Sbuffer[i]) - 48) * power;
power /= 10;
}
return throttle;
}
void PANIC(){
setAll(1000);
}
void ResetRefTime(){
refTime = micros();
}
void set0(int SIGNAL){
speedController[0].writeMicroseconds(SIGNAL);
}
void set1(int SIGNAL){
speedController[1].writeMicroseconds(SIGNAL);
}
void set2(int SIGNAL){
speedController[2].writeMicroseconds(SIGNAL);
}
void set3(int SIGNAL){
speedController[3].writeMicroseconds(SIGNAL);
}
void setThrottles(int throttle[]){
for(int i = 0; i < 4; i ++){
speedController[i].writeMicroseconds(throttle[i]);
}
}
void setAll(int SIGNAL){
for(int i = 0; i < 4; i ++){
speedController[i].writeMicroseconds(SIGNAL);
}
}
void serial2In(){
long local_time = millis();
boolean flag = true;
int i = 0;
char in;
char buffer[22]; //very very strange. if problems arise with buffer, come here first.;
in = Serial2.read();
//reset the flag time;
time = millis();
while(in != '*' &&
flag){
//wait for Serial2 data, but don't wait too long;
while((!Serial2.available()) &&
millis() - time < 300){}
//if more than 1/3 of a second passes without receiving
//Serial2 data, break the loop;
if(millis() - time >=300){
flag = false;
}
//add the character, move in and i forward in sequence;
buffer[i] = in;
in = Serial2.read();
i ++;
time = millis();
}
if(flag){
//add signature and end character to message;
buffer[21] = 'A';
buffer[22] = '*';
//set global variable Sbuffer equal to local variable buffer;
for(int i = 0; i < 23; i ++){
Sbuffer[i] = buffer[i];
}
}if(!flag){
Serial2.println("Serial2 data corrupt.");
Serial2.flush();
}
time = millis();
}
void arm(){
char prompt1[] = "Waiting...";
char prompt2[] = "Ready.";
Serial2.println(top);
setAll(top);
Serial2.println(prompt1);
delay(3000);
Serial2.println();
Serial2.println(bottom);
setAll(bottom);
Serial2.println(prompt1);
delay(3500);
Serial2.println();
Serial2.println(mainThrottle);
setAll(mainThrottle);
Serial2.println(prompt2);
}
void inData(){
servo_ppm[0] = map(pulseIn(channel_1 + 0, LOW), ppm_bottom, ppm_top, -1 * coefficient[0], coefficient[0]);
servo_ppm[1] = map(pulseIn(channel_1 + 1, LOW), ppm_bottom, ppm_top, -1 * coefficient[1], coefficient[1]);
servo_ppm[2] = map(pulseIn(channel_1 + 2, LOW), ppm_bottom, ppm_top, 0, coefficient[2]);
servo_ppm[3] = map(pulseIn(channel_1 + 3, LOW), ppm_bottom, ppm_top, -1 * coefficient[3], coefficient[3]);
}
void outData(){
Serial2.print("$");
for(int i = 0; i < 4; i ++){
Serial2.print(throttle[i]);
Serial2.print(".");
}Serial2.println("A*");
}
void setThrottles(){
throttle[0] = 1000 + servo_ppm[2] + servo_ppm[1];
throttle[1] = 1000 + servo_ppm[2] - servo_ppm[1];
throttle[2] = 1000 + servo_ppm[2] + servo_ppm[0];
throttle[3] = 1000 + servo_ppm[2] - servo_ppm[0];
for(int i = 0; i < 4; i ++){
speedController[i].writeMicroseconds(throttle[i]);
}
}
void digitalWriteThrottles(){
long ref = millis();
time = micros();
for(int i = 0; i < 4; i ++){
digitalWrite(i + 30, HIGH);
}
while(millis() - ref > 200){
for(int i = 0; i < 4; i ++){
if(throttle[i] >= micros() - time){
digitalWrite(i + 30, LOW);
}
}
}
}
void setup(){
Serial.begin(9600);
for(int i = 0; i < 4; i ++){
speedController[i].attach(30 + i);
}
}
void loop(){
inData();
setThrottles();
}