Help! Autonomous Quadrotor

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("");
}

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • Unlike brushed motors for which this code agrees; brushless motors have to start with a intialization of 0 speed.

    i.e the loop providing the signal for motor control has to start with an intialization of i=0.

  • It's always best to start by checking that the kit actually works in the first place.

    Step one:
    Check your ESCs and motors with a normal radio Rx.

    - You will also need to read the manual for your ESCs as to how to arm them. Most ESCs need some kind of 'arming' sequence and won't drive the motor until they get that.

    Brushless ESCs usually give a beeping jingle as they arm - the beeps come from the motors themselves.

    - You didn't say which ESCs you are using.

    Step two:
    Oscilloscope time! Check that you really are outputting the signal you think you are on each ESC (servo) output.

    Finally, never try doing everything at once.
    If you put the whole application into the board and it doesn't work, how do you know which bit is buggy?

    For your first test on real hardware, put in something very minimal.
    Write a simple program that ramps one motor from zero to full and back to zero.

    Then do the same for four motors.

    Then add the rest of the firmware in. If it doesn't work at this stage, you'll know it's not the motor arming and driving.

    (This also has the benefit of reducing frustration, because at each stage you have tangible results as the program gets better.)

    Finally, you probably didn't mean to connect two of your Servo objects to the same physical pin. Check which Arduino pins your ESCs are actually connected to.

This reply was deleted.

Activity