Discussions

The short of it:

Three students completely new to the hobby quadrotor scene would like:

a) either advice on how to properly implement feedback system for first quadrotor,

or

b) directions to documentation, source code or guides, etc. on how to do so, and what testing we need  in order to get it to work.

The long of it: 

Two friends and I are working on a research project in our robotics research laboratory during our senior year in high school. Our project is to build a quadrotor that is directed by wireless input from an electroencephalography (EEG) headset, specifically this one from Emotiv. However, while we did get some use of the headset before we undertook the project, none of us have actually worked with brushless motors, speed controllers or anything like this going into the project.

After several months of research and CAD-ing and building, starting in September 2011, we've managed to build a quadrotor that seems fairly flight-capable; we've managed to get the motors and propellers spinning at our will, we've got wireless communication working, and we have yet to lose more than a few drops of blood so far.

We're at the stage where we need to get stability in flight before we can proceed to connecting it to our EEG headset. However, we're running into major problems with implementing PID control in our code. We've looked at source code from projects such as Arducopter or Aeroquad, but either the code is difficult to read and understand, or the PID implementation is spread everywhere. Because of the posts DIY Drones, we believe we understand the theory of PID, but fumbling when it comes to actually implementing it with a quadrotor. 

We're controlling the quad by treating the ESC's as servos and using the servo library for our Arduino. We control the quad's speed and thrust by writing some value from 0 to 179. Typically, the ESC's turn on at an input ~66. However, they all start at different numbers and each motor outputs a slightly different thrust at some given number, complicating things. 

Heres our code so far (also attached file): 

//to run on copter:

//wiring: A = red, B = black, C = yellow
#include <Servo.h>
//servo stuff
Servo servoA; // create servo object to control a servo
Servo servoB;
Servo servoC;
Servo servoD;

//accel stuff
const int VxPin = 0;
const int VyPin = 1;
const int VzPin = 2;
const double sensitivity = 0.33; //300 mV/g
const int VrefPin = 3;
const double Vzero = 1.5; //V
const double Vref = 1.22;
const double VyTol = 3;
const double VxTol= 3;
const double K=.1;

double VXbase = 0; //V
double VYbase = 0; //V
double VZbase = 0; //V

double VXzero = 0; //V
double VYzero = 0; //V
double VZzero = 0; //V

double Vx = 0;
double Vy = 0;
double Vz = 0;
double VxBase;
double VyBase;
int reg[4] = {
0,0,0,0};

void setup(){
Serial.begin(9600);
servoC.attach(9); // attaches the servo on pin 9 to the servo object
servoB.attach(10);
servoA.attach(11);
servoD.attach(6);

pinMode(VxPin,INPUT);
pinMode(VyPin,INPUT);
pinMode(VzPin,INPUT);
pinMode(VrefPin,INPUT);

servoA.write(0);
servoB.write(0);
servoC.write(0);
servoD.write(0);
delay(3000);

VXzero = readAcc(VxPin);
VYzero = readAcc(VyPin);
//VZzero = 1.574;
VZzero = 1.464;

VXbase=getXAngle();
VYbase=getYAngle();
VZbase=getZAngle();

Serial.print("Vx: ");
Serial.println(VxBase);
Serial.print("Vy: ");
Serial.println(VyBase);
}

void loop() {
Serial.print("A: ");
Serial.print(servoA.read());
Serial.print(", B: ");
Serial.print(servoB.read());
Serial.print(", C: ");
Serial.print(servoC.read());
Serial.print(", D: ");
Serial.println(servoD.read());
//initial motor setting
char test;
if(Serial.available() > 0){
for(int i = 0; i < 4; i++){
reg[i] = Serial.read();
//reg = int(test);
if(reg[i]==200)
{
servoA.write(0);
servoB.write(0);
servoC.write(0);
servoD.write(0);
while(Serial.available()==0){
delay(5);
}
break;
}
Serial.println(reg[i]);
}
// waits for the servo to get there
//servoA.write(reg[0]+2); // sets the servo position according to the scaled value
//servoB.write(reg[1]+1);
servoC.write(reg[2]);
servoD.write(reg[3]+4);
Serial.print("hhurrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrrr");
}

//accel code to fix motors
/*
Serial.print("Vx: ");
Serial.println(Vx);
Serial.print("Vy: ");
Serial.println(Vy);
Serial.print("Vz: ");
Serial.println(Vz);
Serial.println();
*/
/*
A in +x, B in -x, D in +y, C in -y
Neutrals:
X: 2.94-2.955 but for humans: 2.8-3
Y: 2.860-80 but for humans: 2.8-3
Z: 1.814-1.826

Gy=260
Gx=260 (267?)
+gy means reduce c add to d
+gx means reduce b add to a
on copter neutrals:

*/

balance(); //uses the gyroscope and accelerometer to try and balance the quadcopter
delay(5);
}
double balance()
{
double Xang=getXAngle();
double Yang=getYAngle();
double Zang=getZAngle();
double aval;
double bval;
//+x means increace C decreace D
//+y means increace A decreace B
if(Yang>=VYbase+VyTol &&servoC.read()<80){
aval=servoC.read();
bval=servoD.read();
servoC.write(aval+(int)((Yang-(VYbase+VyTol))*K));
servoD.write(aval-(int)((Yang-(VYbase+VyTol))*K));
delay(20);
Yang=getYAngle();
}
if(Yang<=VYbase-VyTol&&servoD.read()<80){
aval=servoC.read();
bval=servoD.read();
servoC.write(aval-(int)(((VYbase-VyTol)-Yang)*K));
servoD.write(aval+(int)(((VYbase-VyTol)-Yang)*K));
delay(20);
}
/*
if(Xang>=VXbase+VxTol){
servoA.write(servoA.read()+1);
servoB.write(servoB.read()-1);
}
if(Xang<=VXbase-VxTol){
servoA.write(servoA.read()-1);
servoB.write(servoB.read()+1);
}

Serial.print("Xang: ");
Serial.println(Xang,3);
Serial.print("Yang: ");
Serial.println(Yang,3);
*/
}
/*
notes:
voltages accurate to +/- 0.02
formula to find force in terms of g: Fx or Fy or Fz = (Vx or Vy or Vz * Vref / 255 - VzeroG) / Sensitivity, F = sqrt(Fx^2 + Fy^2 + Fz^2)
formula to find angle: Xangle = arccos(Fx/F), Yangle = arccos(Fy/F), Zangle = arccos(Fz/F)
*/

double readAcc(int pin){
int repeat = 20;
double V = 0;
for(int x=0; x<repeat; x++)
V += analogRead(pin);
V /=repeat;
V = V* Vref / 255;
//V -= Vzero;
return V;
}
double getXAngle(){
double Vx = 0;
double Vy = 0;
double Vz = 0;
double Fx = 0;
double Fy = 0;
double Fz = 0;
double F = 0;

Vx = readAcc(VxPin);
Vy = readAcc(VyPin);
Vz = readAcc(VzPin);
Vx -= VXzero;
Vy -= VYzero;
Vz -= VZzero;

Fx = Vx/sensitivity;
Fy = Vy/sensitivity;
Fz = Vz/sensitivity;

F = sqrt(sq(Fx)+sq(Fy)+sq(Fz));
return asin(Fx/F)*(180/PI);
}
double getYAngle(){
double Vx = 0;
double Vy = 0;
double Vz = 0;
double Fx = 0;
double Fy = 0;
double Fz = 0;
double F = 0;

Vx = readAcc(VxPin);
Vy = readAcc(VyPin);
Vz = readAcc(VzPin);
Vx -= VXzero;
Vy -= VYzero;
Vz -= VZzero;

Fx = Vx/sensitivity;
Fy = Vy/sensitivity;
Fz = Vz/sensitivity;

F = sqrt(sq(Fx)+sq(Fy)+sq(Fz));
return asin(Fy/F)*(180/PI);
}
double getZAngle(){
double Vx = 0;
double Vy = 0;
double Vz = 0;
double Fx = 0;
double Fy = 0;
double Fz = 0;
double F = 0;

Vx = readAcc(VxPin);
Vy = readAcc(VyPin);
Vz = readAcc(VzPin);
Vx -= VXzero;
Vy -= VYzero;
Vz -= VZzero;

Fx = Vx/sensitivity;
Fy = Vy/sensitivity;
Fz = Vz/sensitivity;

F = sqrt(sq(Fx)+sq(Fy)+sq(Fz));
return asin(Fz/F)*(180/PI);
}

Most of our problems lie with this thing not being able to properly adjust itself if it overshoots a turn or causes a far too much of an increase in thrust at once. We've built a metal stand for testing rotation about on axis (see picture at the end of the post) and see if it can right itself. Our first goal for now is to reach a stable hover; I think if we can manage that, we can work out the rest from there. I've seen people building a mount for calculating thrust using a scale; would this help us at all? Any help, thoughts or criticism that we can get would be greatly appreciated. 

Build details:

(4x) HobbyKing Donkey ST3508 730kv Brushless Motor

(4x) TURNIGY Sentry 40amp Speed Controller (apparently no longer sold at Hobbyking)

Seeeduino V2.2 Arduino Compatible board

(2x) APC 12 x 3.8 Slow Flyer Counter Rotating Pair

(3x) ZIPPY Flightmax 5000mAh 3S1P 15C (we're probably not using more than 1 at a time, 2 at most. The 3rd is a reserve in case something happens)

Mini Wireless Color Camera with Microphone

XBee Pro 60mW Chip Antenna - Series 1 (we're using the Arduino shield version)

and gratuitous amounts of aluminum square tubes (1/2" x 1/2"), polycarbonate sheets, hardboard, heat shrink tubing, zip ties and VEX screws, nylon washers and Keps nuts.

Total Weight with 1 battery: 4.75 lbs (76 oz)

The following is a photo of our quadrotor on it's test mount. It's supposed to rotating around a single fixed axis and try to adjust itself. We send specific motor commands and shut if off wirelessly using a script on a nearby computer. Forgive the loose cabling, that's been repaired :P

Quadcopter_Code_as_of_Jan_29.ino