RC remote code and PID controller in one arduino!

#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*);*
value_=map(data*,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.0Xi)+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); //3derivative pitch_

pitch_cal =Yp+(0.0Yi)+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.0Zi);//0.006
if(yaw_cal>400)yaw_cal=400;*

if(yaw_cal<-400)yaw_cal=-400;

* }*

/ I love the use of italics.

Was there a question?

this was slow and jerky and disappointing , any opinion of improving this ?

adelphysi:
this was slow and jerky and disappointing , any opinion of improving this ?

Lower your expectations.

Use code tags.

Is this thread in any way related to this?

Xp=0.0X;//0.8 propotional roll
roll_cal =Xp+(0.0
Xi)+Xd;//0.007

Multiply by zero much?
Where and how is time in the D term?

avr_fred:
Xp=0.0X;//0.8 propotional roll
roll_cal =Xp+(0.0
Xi)+Xd;//0.007

Multiply by zero much?
Where and how is time in the D term?

i multiplied them with numbers in my main code but i multiplied them with 0 to test on term for each

there is no need for time , it is in degree/ sec

You're absolutely correct, degrees per second has no relationship to time. Right. Got it. Silly me.

:stuck_out_tongue_closed_eyes: unfortunately , for more information you can see the source code on Joop Brooking website .