Hey guys,
I'm building a quadrotor (controlled by an Arduino Mega) and I have everything working except for sloppy %@#^@# throttle control. I wrote a Java program to send throttle commands derived from input from a joystick over a serial port using a 60mw XBee antenna. The arduino receives the command with another 60 mw XBee and sends the proper signals to the speed controllers. I'm using the servo library which allows me to attach the servos to digital pins instead of the analog ones. Is this maybe the reason for sloppy throttle control?
Anyway, I am sending throttle data every 100milliseconds, and the arduino successfully receives it every time, but the motors respond generally about 1 second afterward. This is a problem. With aircraft, realtime control is necessary. Please help!!!!!!
I'll post the Java program code in a separate response. It's too long for this one.
Example of throttle data sent from computer to arduino:
$1000.1000.1000.1000.J*
'$' = start
the four "1000"s represent the signal to send to the speedcontrollers (which are controlled exactly the same way as servos)
".J" = signature showing the message was the sent by the java program
'*' = end transmission
Arduino program:
#include <Servo.h>
Servo speedController[4];
char serial2[4][4];
char Sbuffer[21]; //so strange. need to bring this one to the forums.;
int throttle[4];
int bottom = 1000;
int zero = 1292;
int top = 2000;
int mainThrottle = zero;
long refTime = micros();
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 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];
}
//output global data;
Serial2.print(Sbuffer);
}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 setup(){
Serial2.begin(9600);
for(int i = 0; i < 4; i ++){
speedController[i].attach(30 + i);
}
}
void loop(){
if(Serial2.available()){
serial2In();
set0(decodeThrottle(1));
set1(decodeThrottle(6));
set2(decodeThrottle(11));
set3(decodeThrottle(16));
setThrottles(throttle);
}
}