HI
i have been working one my cuad and i used virtual library and timerservo2 library and wire library in one arduino uno but i have got a result that D controller get some constant values in specific times and this reduces the efficiency of the controller unfortunately and made it jerky ,i think if i have a faster arduino microcontroller i will achieve balancing control system for the quad in one microcontroller than using standard RC remote control with a controller .
any opinions?
here is my code :
#include <VirtualWire.h>
#include <ServoTimer2.h>
#include<Wire.h>
#define pinmyservo1 4
#define pinmyservo2 5
#define pinmyservo3 6
#define pinmyservo4 7
ServoTimer2 myservo1; // declare variables for up to eight servos
ServoTimer2 myservo2;
ServoTimer2 myservo3;
ServoTimer2 myservo4;
int gyro_roll_cal ,gyro_pitch_cal,gyro_yaw_cal;
int esc_1, esc_2 ,esc_3, esc_4;
int battery_voltage;
int start;
///////////////////////////////////////////////////////////
const int numberOfAnalogPins = 4; // how many analog integer values to receive
int data[numberOfAnalogPins]; // the data buffer
int value[numberOfAnalogPins];// the number of bytes in the data buffer
const int dataBytes = numberOfAnalogPins * sizeof(int);
byte msgLength = dataBytes;
/////////////////////////////////////////////////////////
float gyro_roll_input,gyro_pitch_input,gyro_yaw_input;
float gyro_roll,gyro_pitch,gyro_yaw;
const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int throttle,pitch ,roll, yaw , pitch_control ,roll_control , yaw_control;
float X,Y,Z,Xp ,Xi ,Xd ,Yp ,Yi ,Yd ,Zp,Zi,Xl,Yl,roll_cal,pitch_cal,yaw_cal; //pids
int cal_int ;
void setup(){
Serial.begin(9600);
vw_set_ptt_inverted(true); // Required for DR3100
vw_setup(2500); // Bits per sec
vw_set_rx_pin(11);
vw_rx_start(); // Start the receiver
////////////////////////////////////////////////////
Xi=0;
X=0;
Xl=0;
Yl =0;
Zi=0;
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
/////////////////////////////////////////////
myservo1.attach(pinmyservo1); // attach a pin to the servos and they will start pulsing
myservo2.attach(pinmyservo2);
myservo3.attach(pinmyservo3);
myservo4.attach(pinmyservo4);
myservo1.write(1000);
myservo2.write(1000);
myservo3.write(1000);
myservo4.write(1000);
delay(6000);
for (cal_int = 0; cal_int < 4000 ; cal_int ++){ //Take 2000 readings for calibration.
//Change the led status to indicate calibration. gyro_signalen(); //Read the gyro output.
gyro_roll_cal += gyro_roll; //Ad roll value to gyro_roll_cal.
gyro_pitch_cal +=gyro_pitch; //Ad pitch value to gyro_pitch_cal.
gyro_yaw_cal += gyro_yaw;
}
gyro_roll_cal /= 4000; //Divide the roll total by 2000.
gyro_pitch_cal/= 4000; //Divide the pitch total by 2000.
gyro_yaw_cal /= 4000;
start =0 ;
}
int incPulse(int val, int inc){
if( val + inc > 2000 )
return 1000 ;
else
return val + inc;
}
void loop(){
Serial.println(3*Xd);
//int timer1 = micros();
if( throttle< 1050 && yaw < 1050){start = 1;
Xi=0;
Xl=0;
Xi=0;
Yi=0;
Yl=0;
Zi=0;}
if(start == 1 && throttle < 1050 && yaw > 1450){
start = 2;
Xi=0;
Xl=0;
Xi=0;
Yi=0;
Yl=0;
Zi=0;
}
if(vw_get_message((byte*)data, &msgLength)) {
if(msgLength == dataBytes){
for (int i = 0; i < numberOfAnalogPins; i++) {
//Serial.print("pin ");
//Serial.print(i);
//Serial.print("=");
//Serial.println(value[i]);
value[i]=map(data[i],0,1023,1000,2000); // Write into the servo
throttle= value[0];
roll= value[1];
pitch = value[2];
yaw = value[3];
}}
if(start == 2 && throttle < 1050 && yaw > 1900)
{
start = 0;
esc_1=0;
esc_2=0;
esc_3=0;
esc_4=0;
}
pid_cal ();
battery_voltage = battery_voltage * 0.92 + (analogRead(0) + 65) * 0.09853;
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
gyro_roll=Wire.read()<<8|Wire.read();
if(cal_int == 4000) gyro_roll -= gyro_roll_cal;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
gyro_pitch=Wire.read()<<8|Wire.read();
if(cal_int == 4000) gyro_pitch -= gyro_pitch_cal;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
gyro_yaw=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
if( cal_int == 4000) gyro_yaw -= gyro_yaw_cal;// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
gyro_roll_input = ((gyro_roll_input * 0.7) + ((gyro_roll / 65.5) * 0.3)); //Gyro pid input is deg/sec.
gyro_pitch_input = (gyro_pitch_input * 0.7) + ((gyro_pitch /65.5) * 0.3); //Gyro pid input is deg/sec.
gyro_yaw_input = (gyro_yaw_input * 0.7) + ((gyro_yaw / 65.5) * 0.3); //Gyro pid input is deg/sec.
/////////////////////////////////////
if(throttle > 1800)throttle=1800;
esc_1 =throttle-pitch_control-roll_control +roll_cal+pitch_cal -yaw_cal; // throttle+ roll_cal- pith_cal
esc_2 =throttle+pitch_control-roll_control +roll_cal-pitch_cal+yaw_cal; //throttle+ roll_cal+ pith_cal
esc_3 =throttle+pitch_control+roll_control -roll_cal-pitch_cal -yaw_cal; //throttle- roll_cal+ pith_cal
esc_4 =throttle-pitch_control+roll_control -roll_cal+pitch_cal+yaw_cal; //throttle- roll_cal- pith_cal
/////////////////////////////////////////////////
if( start != 1 ){
if (esc_1<1050)esc_1=1050;
if (esc_2<1050)esc_2=1050;
if (esc_3<1050)esc_3=1050;
if (esc_4<1050)esc_4=1050;
}
else {
esc_1=0;
esc_2=0;
esc_3=0;
esc_4=0;
}
roll_control =0;
if (roll > 1510)roll_control=(roll-1510)/5;
else if (roll < 1490)roll_control=(roll-1490)/5;
pitch_control =0;
if ( pitch> 1510) pitch_control=( pitch-1510)/5;
else if ( pitch < 1490) pitch_control=( pitch-1490)/5;
yaw_control=0;
if ( yaw> 1512)yaw_control=( yaw-1512)/8;
else if ( yaw < 1488) yaw_control=(yaw-1488)/8;
if (battery_voltage < 1240 && battery_voltage > 800){ //Is the battery connected?
esc_1 += esc_1 * ((1240 - battery_voltage)/(float)3500); //Compensate the esc-1 pulse for voltage drop.
esc_2 += esc_2 * ((1240 - battery_voltage)/(float)3500); //Compensate the esc-2 pulse for voltage drop.
esc_3 += esc_3 * ((1240 - battery_voltage)/(float)3500); //Compensate the esc-3 pulse for voltage drop.
esc_4 += esc_4 * ((1240 - battery_voltage)/(float)3500); //Compensate the esc-4 pulse for voltage drop.
}
//int timer1 = micros();
myservo1.write(esc_1);
myservo2.write(0);
myservo3.write(0);
myservo4.write(esc_4);
//int timer2 = micros();
//Serial.println("");
//Serial.println(esc_1);
//Serial.println(esc_2);
//Serial.println(esc_3);
//Serial.println(esc_4);
//Serial.println(timer2-timer1);
//Serial.println(pitch_cal);
//delay(100);
}
}
void pid_cal(){
//roll cal
X=(gyro_roll_input)+6.37;//+6.37
Xp=0.0*X;//0.8 propotional roll
Xi+=X;
if(Xi>400)Xi=200;
if(Xi<-400)Xi=-200;
Xd=0.8*(X-Xl); //3derivative roll
roll_cal =Xp+(0.0*Xi)+Xd;//0.007
if(roll_cal>200)roll_cal=200;
if(roll_cal<-200)roll_cal=-200;
Xl=X ;
//pitch cal
X=(((gyro_pitch_input)*-1)+0.23)*-1;
Yp=0.0*X; //0.01 propotional pitch
Yi+=X ;
if(Yi>200)Yi=200;
if(Yi<-200)Yi=-200;
Yd=0.0*(X-Yl); //3*derivative pitch
pitch_cal =Yp+(0.0*Yi)+Yd;//0.007
if(pitch_cal>200)pitch_cal=200;
if(pitch_cal<-200)pitch_cal=-200;
Yl=X ;
// yaw cal
Z=(gyro_yaw_input*-1)-7.7;//-7.7
Zi+=Z;
Zp=0*Z;//1.5 propotional pitch
yaw_cal =Zp+(0.0*Zi);//0.006
if(yaw_cal>400)yaw_cal=400;
if(yaw_cal<-400)yaw_cal=-400;
}
