Discussions

Hi all,

I am final year student working on flight control system for autonomous quadrotor. My aim is to take off, hover and land quadrotor autonomously. I already simulated my control algorithm in Matlab and got good and stable results.

I am done the coding in arduino environment. When I transfer my code via usb cable to the microprocessor, i don't get any response from motors. They don't even run (start). I am using arduino Duemilanove board. Motors that I am using are brushless dc motors. Also, I am using 7.5V rechargeable battery. I am working with GAUI 330X quadrotor model.

Here is my code, can u please help in knowing if my code is wrong or there is any problem with my hardware?

#include <Servo.h>
Servo myspeedcont1;
Servo myspeedcont2;
Servo myspeedcont3;
Servo myspeedcont4;

int pos=0;

#define INPUT_COUNT 6
#define VDD 3300.0f
#define PI 3.14159265f
//gyro at x
#define Kp_gx 0.7
#define Kd_gx 500
#define ki_gx 0
//imu at x
#define Kp_x 5
#define Kd_x 0
#define Ki_x 0.0004
//gyro at y
#define Kp_gy 0.7
#define Kd_gy 500
#define Ki_gy 0
//lmu at y
#define Kp_y 5
#define Kd_y 0
#define Ki_y 0.0004
// gyro at z:
#define Kp_gz 0.7
#define Kd_gz 500
#define Ki_gz 0
//imu at z
#define Kp_z 5
#define Kd_z 0
#define Ki_z 0.0004

int an[INPUT_COUNT];
char firstSample;
struct
{
char inpInvert[INPUT_COUNT];
int zeroLevel[INPUT_COUNT];
int inpSens[INPUT_COUNT];
float wGyro;
} config;

float RwEst[3];
unsigned long lastMicros;
unsigned long intertime=0;
unsigned long pretime=0;
unsigned long interval_millis=0;
unsigned long time_millis=0;
unsigned long interval;
float RwAcc[3];
float RwGyro[3];
float Awz[2];
//gyro at y- axiz
float g_last_error = 0;
float g_pid =0;
//imu
float last_error = 0;
float pid = 0;

float last_error2 = 0;
float pid2 = 0;
float speed1;

float speed_gy=0;
float speed_y=0;
float speed_gx=0;
float speed_x=0;
float g_error_y=0;
float g_error_x=0;
float error_x=0;
float error_y=0;
float trim_x=0;
float trim_y=0;


void setup()
{
Serial.begin(57600);
myspeedcont1.attach(2);
myspeedcont2.attach(5);
myspeedcont3.attach(4);
myspeedcont4.attach(2);
delay(10);
myspeedcont1.write(180);
myspeedcont2.write(180);
myspeedcont3.write(180);
myspeedcont4.write(180);
delay(500);
myspeedcont1.write(40);
myspeedcont2.write(40);
myspeedcont3.write(40);
myspeedcont4.write(40);
delay(2000);
myspeedcont1.write(41);
myspeedcont2.write(41);
myspeedcont3.write(41);
myspeedcont4.write(41);
delay(2000);
myspeedcont1.write(43);
myspeedcont2.write(43);
myspeedcont3.write(43);
myspeedcont4.write(43);
delay(2000);

static int i;

config.zeroLevel[0] = 1698.0f; // '0' is zero
config.inpSens[0] = 330.0f;
config.zeroLevel[1]= 1672.0f;
config.inpSens[1] = 330.0f;
config.zeroLevel[2] = 2031.0f;
config.inpSens[2] = 330.0f;
config.zeroLevel[3] = 1210.0f;
config.inpSens[3] = 9.1f;
config.zeroLevel[4] = 1230.0f;
config.inpSens[41] = 9.1f;
config.zeroLevel[5] = 1204.0f;
config.inpSens[41] = 3.3f;


config.inpInvert[0]=1;
config.inpInvert[1]=1;
config.inpInvert[2]=1;
config.inpInvert[3]=-1;
config.inpInvert[4]=-1;
config.inpInvert[5]=-1;

config.wGyro = 10;
firstSample = 1;
pretime=millis();
}



void loop()
{

time_millis=millis();
interval_millis = time_millis-pretime;
intertime = intertime+interval_millis;
updatePid_xaxis();
updatePid_yaxis();
getEstimatedInclination();


Serial.print(intertime);
Serial.print("\t");
Serial.print(an[0]);
Serial.print("\t");
Serial.print(an[1]);
Serial.print("\t");
Serial.print(an[2]);
Serial.print("\t");
Serial.print(an[3]);
Serial.print("\t");
Serial.print(an[4]);
Serial.print("\t");
Serial. print(an[5]);
Serial.print("\t");
Serial.print(RwAcc[0]);
Serial.print("\t");
Serial.print(RwAcc[1]);
Serial.print("\t");
Serial.print(RwAcc[2]);
Serial.print("\t");
Serial.print(RwGyro[3]);
Serial.print("\t");
Serial.print(RwGyro[4]);
Serial.print("lt");
Serial.print(RwGyro[5]);
Serial.print("\t");
Serial.print(RwEst[0]);
Serial.print("\t");
Serial.print(RwEst[1]);
Serial.print("\t");
Serial.print(RwEst[2]);
Serial.println("");
delay (10);
if(intertime<1000)
{
myspeedcont1.write(90);
myspeedcont2.write(80);
myspeedcont3.write(80);
myspeedcont4.write(90);
}

if(intertime<1000 && intertime<10000)
{
myspeedcont1.write(120);
myspeedcont2.write(110);
myspeedcont3.write(110);
myspeedcont4.write(110);
}

if(intertime>10000 && intertime<20000)
{
myspeedcont1.write(130);
myspeedcont2.write(120);
myspeedcont3.write(120);
myspeedcont4.write(120);
}

if(intertime>20000 && intertime<30000)
{
myspeedcont1.write(130);
myspeedcont2.write(120);
myspeedcont3.write(120);
myspeedcont4.write(120);
}

if(intertime>30000 && intertime<40000)
{
myspeedcont1.write(120);
myspeedcont2.write(110);
myspeedcont3.write(110);
myspeedcont4.write(110);
}

if(intertime>40000 && intertime<60000)
{
myspeedcont1.write(90);
myspeedcont2.write(80);
myspeedcont3.write(80);
myspeedcont4.write(80);
}
pretime = time_millis;
}


void getEstimatedInclination(){
static int i,w;
static float tmpf,tmpf2;
static unsigned long newMicros;
static char signRzGyro;

newMicros = millis();
for(i=0;i<INPUT_COUNT;i++) an[i]=analogRead(i);


interval= newMicros-lastMicros;
lastMicros = newMicros;

for(w=0;w<=2;w++) RwAcc[w] = getInput(w);

for(w=3;w<=5;w++) RwGyro[w] = getInput(w);

normalize3DVector(RwAcc);

if(firstSample)

{
for(w=0;w<=2;w++) RwEst[w] = RwAcc[w];
}
else
{
if(abs(RwEst[2]) < 0.1){
for(w=0;w<=2;w++) RwGyro[w] = RwEst[w];}

else
{
for(w=0;w<=1;w++){
tmpf = getInput(3+w);
tmpf*= interval/1000.0f;
Awz[w] = atan2(RwEst[w],RwEst[2]) *180/PI;
Awz[w] += tmpf;}


signRzGyro=(cos(Awz[0]*PI/180)>=0)? 1:-1;

for(w=0;w<=1;w++){
RwGyro[w] = sin(Awz[w]*PI/180);
RwGyro[w] /= sqrt((1+ (cos(Awz[w]*(PI)/180)))*(squared(tan(Awz[1-w]*(PI)/180))));
}

RwGyro[2] = signRzGyro*sqrt(1-squared(RwGyro[0])-squared(RwGyro[1]));
}

for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + config.wGyro*RwGyro[w])/(1+ config.wGyro);
normalize3DVector(RwEst);
}
firstSample = 0;

}





void normalize3DVector(float*vector)
{
static float R;
R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
vector[0] /= R;
vector[1] /= R;
vector[2] /= R;
}

float squared(float x){
return x*x;
}


float getInput(char i){
static float tmpf;
tmpf =an[i]*VDD/1023.0f;
tmpf -=config.zeroLevel[i];
tmpf /= config.inpSens[i];
tmpf *= config.inpInvert[i];
return tmpf;}




void updatePid_xaxis()
{
float g_pTerm, g_dTerm, g_iTerm; //gyro
float pTerm, dTerm, iTerm; //IMU
float targetPosition =0;



g_error_x = 273-an[4]; //gyro
error_x = targetPosition - RwEst[1]+trim_x;

g_pTerm = g_error_x;
g_dTerm = (g_error_x-g_last_error)/interval;
g_iTerm = g_iTerm + (g_error_x*interval);
g_last_error = g_error_x;
g_pid =(Kp_gx * g_pTerm) + (Kd_gx * g_dTerm);

pTerm = error_x;
dTerm = (error_x - last_error)/interval;
iTerm = iTerm +(error_x*interval);
last_error = error_x;
pid = (Kp_x*pTerm)+(Kd_x*dTerm)+(Ki_x * iTerm);

speed_gx = g_pid;
speed_x = pid;

//torqestimate_xaxis(speed_gx,speed_x);
Serial.print(g_pid);
Serial.println("");

}

void updatePid_yaxis()
{
float g_pTerm, g_dTerm, g_iTerm; //gyro
float pTerm, dTerm, iTerm; //IMU
float targetPosition =0;


g_error_y = 273-an[3]; //gyro
error_y = targetPosition - RwEst[0]+trim_y;

g_pTerm = g_error_y;
g_dTerm = (g_error_y-g_last_error)/interval;
g_iTerm = g_iTerm + (g_error_y*interval);
g_last_error =g_error_y;
g_pid =(Kp_gy * g_pTerm) + (Kd_gy * g_dTerm);

pTerm = error_y;
dTerm = (error_y - last_error)/interval;
iTerm = iTerm +(error_y*interval);
last_error = error_y;
pid = (Kp_y*pTerm)+(Kd_y*dTerm)+(Ki_y * iTerm);

speed_gy = g_pid;
speed_y = pid;

//torqestimate_yaxis(speed_gy,speed_y);


Serial.print(error_y);
Serial.println("");
}