Hello, today I uploaded my quadrotor code to the custom controller board to start testing its stabilization. I've been trying to tune it all day but I cant seem to make any progress... I put the copter on a test stand to hold two opposite sides and the other two swivle freely. Once the code was uploaded and the controller turned on it gos all the way to the left waits a little then gos to the right and repeats. Does anyone have any tips I'm posting the code below.
#include <Servo.h>
#define X_ZERO 332
#define Y_ZERO 324
#define Z_ZERO 396
#define PITCH_ZERO 249
#define ROLL_ZERO 249
#define YAW_ZERO 248
#define GYRO_CON 1.47
#define ACCEL_CON 0.93
#define TIME_CON 0.02
#define SEN_CON 0.95
Servo FrontESC;
Servo BackESC;
Servo LeftESC;
Servo RightESC;
float p=2.5;
float d= .3;
float pitch, roll, yaw;
float xin, yin, zin;
float pitchin, rollin, yawin, zhuman;
float xaverage=0, yaverage=0;
int pitchzero, rollzero;
int y=0;
int blah;
void setup()
{
Serial.begin(9600);
zhuman=0;
rollin=0;
pinMode(2, OUTPUT);
pinMode(3, OUTPUT);
digitalWrite(2, 1);
digitalWrite(3, 1);
FrontESC.attach(8);
BackESC.attach(9);
LeftESC.attach(10);
RightESC.attach(11);
}
void loop()
{
xin=(analogRead(5)-X_ZERO)*ACCEL_CON;
yin=(analogRead(4)-Y_ZERO)*ACCEL_CON;
zin=(analogRead(3)-Z_ZERO)*ACCEL_CON;
pitch=(pitchzero-analogRead(0))*GYRO_CON;
roll=(rollzero-analogRead(1))*GYRO_CON;
yaw=(analogRead(2)-YAW_ZERO)*GYRO_CON;
if(blah==0) {
yawin=0.06*((signed int) pulseIn(6,HIGH)-1500);
pitchin=0.06*((signed int) pulseIn(5,HIGH)-1500);
blah=1;
}
else {
zhuman=(signed int) pulseIn(7,HIGH);
rollin=0.06*((signed int) pulseIn(4,HIGH)-1400);
blah=0;
}
//averaging, etc.
xaverage= SEN_CON *( xaverage + TIME_CON * pitch) + ( 1 - SEN_CON ) * xin;
yaverage= SEN_CON *( yaverage + TIME_CON * roll) + ( 1 - SEN_CON ) * yin;
if(zhuman<1150) {
for(int x=0; x<4; x++) {
FrontESC.write(zhuman);
BackESC.write(zhuman);
LeftESC.write(zhuman);
RightESC.write(zhuman);
}
}
else {
if(zhuman > 1450) {
zhuman = 1450;
}
Serial.println(zhuman - p*(xaverage - pitchin) - p*(yawin) - d*pitch);
FrontESC.write(zhuman - p*(xaverage - pitchin) - p*(yawin) - d*pitch);
BackESC.write(zhuman - p*(pitchin - xaverage) - p*(yawin) + d*pitch);
LeftESC.write(zhuman - p*(yaverage - rollin) + p*(yawin) - d*roll);
RightESC.write(zhuman - p*(rollin - yaverage) + p*(yawin) + d*roll);
}
}
Replies