#define RC_YAW 2 // pin 21 //arduino pins attached to the reciever
#define RC_THRO 3 // pin 20
#define RC_PITCH 4 // pin 19
#define RC_ROLL 5 // pin 18
#define RC_SWITCH 0 // pin 2
#define LED1 51
#define LED2 53
#define int0 (PIND & 0b00000001) // Faster than digitalRead
#define int1 (PIND & 0b00000010)
#define int2 (PIND & 0b00000100)
#define int3 (PIND & 0b00001000)
#define int4 (PINE & 0b00010000)
long count0, count1, count2, count3, count4; // Provisional values for micros() at the positive edge (read_RC)
int yaw_pulse=0, thro_pulse=0, roll_pulse=0, pitch_pulse=0; // Provisional values for pulse length in microsecond, pros filter_RC
float prog_period=0.1; //The time period of a full cycle, goes to PID
int thro_setpoint=1010;
float roll_setpoint=0, pitch_setpoint=0, yaw_setpoint=0;
float error_x=0, error_y=0, error_z=0; //the errors for the PID
float previous_error_x=0, previous_error_y=0, previous_error_z=0;
float integral_x=0.01, integral_y=0.01, integral_z=0;
float derivative_x, derivative_y, derivative_z;
float Kp_x=6.5, Kp_y=6.5, Kp_z=0.2;
float Ki_x=0.4, Ki_y=0.5, Ki_z=0.1;
float Kd_x=-11, Kd_y=-11, Kd_z=0;
float output_x, output_y, output_z;
float I_limit_x=270, I_limit_y=270, I_limit_z=2000; // limit for the integral
int continue_program=0;
int PID_on=0;
int PID_angle_on=0;
int motor_off_val=1000;
int servo_off_val=1500;
int valservo=1500;
int valESCyaw=0;
int valESCright=0;
int valESCleft=0;
int valESCyaw_final=0;
int valESCright_final=0;
int valESCleft_final=0;
long temp_time=0;
long temp_time_holder=0;
float nums[42];
int battery_mes=0;
int batt_time=0;
char recieved_char; // values for serial with IMU
byte peekchr=0;
char pointer;
float x, y, gyroZ, gyroX, gyroY; //the angles from the imu
float gyroXtable[]={0,0,0,0,0,0,0,0,0,0};
float last_x, last_y; // values for PID
String xtemp, ytemp, gyroZtemp;
unsigned int RC_switch_value = 0; //the value of the switch
unsigned int tmp_time_y[]={1500,1500}; // table for LP filter at the input of RC YAW
unsigned int tmp_time_t[]={1030,1030}; // table for LP filter at the input of RC THR
unsigned int tmp_time_p[]={1500,1500}; // table for LP filter at the input of RC PITCH
unsigned int tmp_time_r[]={1500,1500}; // table for LP filter at the input of RC ROLL
int valESCyaw_table[]={1030,1030,1030,1030,1030};
int valESCright_table[]={1030,1030,1030,1030,1030};
int valESCleft_table[]={1030,1030,1030,1030,1030};
int jy=0, ky=0; // counter LP filter RC YAW
int jt=0, kt=0; // counter LP filter RC THR
int jp=0, kp=0; // counter LP filter RC PITCH
int jr=0, kr=0; // counter LP filter RC ROLL
long LEDtime;
byte LEDi;
byte serial_send_counter=0;
void setup() {
pinMode (5, OUTPUT);
pinMode (6, OUTPUT);
pinMode (7, OUTPUT);
pinMode (8, OUTPUT);
TCCR3A&= 0b11111100; TCCR3B&= 0b11110111; TCCR3B|= 0b00010000; //WGMn4 to WGMn0 at 1 0 0 0 for mode PWM, Phase and Frequency
Correct, me xrisi tou ICRn
TCCR3B&= 0b11111010; TCCR3B|= 0b00000010; //CSn2 to CSn0 at 0 1 0 for prescaler 8
TCCR3A&= 0b10101011; TCCR3A|= 0b10101000; //COMnA1 to COMnC0 at 0 1 0 1 0 1 for PWM
ICR3 = 20000; //50Hz gia to prescaler 8
TCCR4A&= 0b11111100; TCCR4B&= 0b11110111; TCCR4B|= 0b00010000; //WGMn4 to WGMn0 at 1 0 0 0 for mode PWM, Phase and Frequency
Correct, me xrisi tou ICRn
TCCR4B&= 0b11111010; TCCR4B|= 0b00000010; //CSn2 to CSn0 at 0 1 0 for prescaler 8
TCCR4A&= 0b10101011; TCCR4A|= 0b10101000; //COMnA1 to COMnC0 at 0 1 0 1 0 1 for PWM
ICR4 = 20000; //50Hz for prescaler 8
pinMode(LED1, OUTPUT);
pinMode(LED2, OUTPUT);
Serial3.begin(115200);
Serial2.begin(38400);
attachInterrupt(RC_YAW, rc_yaw_interupt, CHANGE); // attach a PinChange Interrupt
attachInterrupt(RC_THRO, rc_thro_interupt, CHANGE); // attach a PinChange Interrupt
attachInterrupt(RC_PITCH, rc_pitch_interupt, CHANGE); // attach a PinChange Interrupt
attachInterrupt(RC_ROLL, rc_roll_interupt, CHANGE); // attach a PinChange Interrupt
attachInterrupt(RC_SWITCH, rc_sw_interupt, CHANGE); // attach a PinChange Interrupt
}
void loop() {
if (RC_switch_value==0) {
OCR3A=motor_off_val;
OCR4A=motor_off_val;
OCR4B=motor_off_val;
OCR4C=servo_off_val;
}
else if (RC_switch_value<1500) { //if controller on and rc switch is off
OCR3A=motor_off_val;
OCR4A=motor_off_val;
OCR4B=motor_off_val;
OCR4C=servo_off_val;
continue_program=0;
PID_on=0;
unsigned int tmp_time_y[]={1500,1500}; // table for the LP filter at the input of RC
YAW
unsigned int tmp_time_t[]={1030,1030}; // table for the LP filter at the input of RC
THR
unsigned int tmp_time_p[]={1500,1500}; // table for the LP filter at the input of RC
PITCH
unsigned int tmp_time_r[]={1500,1500}; // table for the LP filter at the input of RC
ROLL
}
else if (RC_switch_value < 2300) continue_program=1;
if (millis() - LEDtime > 100 ){
LEDi++;
if (battery_mes > 15) {
if (LEDi == 5) { digitalWrite(LED1, HIGH); }
if (LEDi == 7) { digitalWrite(LED1, LOW); }
if (LEDi == 8) { digitalWrite(LED2, HIGH); }
if (LEDi == 10) { digitalWrite(LED2, LOW); LEDi = 0; }
}
else {
if (LEDi % 2 == 0) {digitalWrite(LED1, HIGH); digitalWrite(LED2, HIGH);}
if (LEDi % 2 == 1) {digitalWrite(LED1, LOW); digitalWrite(LED2, LOW);}
if (LEDi == 10) {LEDi = 0;}
}
LEDtime = millis();
 batt_time++;
}
 if (batt_time == 10) {
 battery_mes = analogRead(1);
 battery_mes = map (battery_mes, 600, 850, 0, 100);
 batt_time = 0;
 }
read_IMU();
serial_COMM ();
filter_RC();
}
void PID() {
temp_time_holder=micros()-temp_time;
temp_time=micros();
if ((thro_setpoint > 1400)&&(RC_switch_value>1500))
PID_on=1;
if ((thro_setpoint <1400)||(RC_switch_value<1500))
PID_on=0;
if (PID_on==1) {
// ------ X PID ------
error_x= - x - roll_setpoint;
integral_x=integral_x+(error_x*prog_period);
integral_x = constrain(integral_x, - I_limit_x, I_limit_x);
derivative_x=Kd_x*(x-last_x)/prog_period;
last_x=x;
output_x=(Kp_x*error_x)+(Ki_x*integral_x)+derivative_x;
// ------ Y PID ------
error_y = - y + pitch_setpoint;
integral_y = integral_y + (error_y * prog_period);
integral_y = constrain(integral_y, - I_limit_y, I_limit_y);
derivative_y = Kd_y * (y - last_y) / prog_period;
last_y = y;
output_y = (Kp_y * error_y) + (Ki_y * integral_y) + derivative_y;
valESCright = output_x - output_y * 0.9 / 2;
valESCleft = - output_x - output_y * 0.9 / 2;
valESCyaw = output_y;
}
else
{
integral_x = 0;
integral_y = 0;
valESCright = 0;
valESCleft = 0;
valESCyaw = 0;
}
// ------ Z PI ------
error_z = yaw_setpoint + gyroZ - 1500;
integral_z = integral_z + (error_z * prog_period);
integral_z = constrain(integral_z, - I_limit_z, I_limit_z);
output_z = constrain(1500 + error_z * Kp_z + (Ki_z * integral_z), 1300, 1700);
valservo = output_z;
if (continue_program==1) send_PWM();
}
void read_IMU() {
while (Serial3.available() > 0){
if (Serial3.peek()=='z'){
peekchr=1;
} else if (peekchr==0) {
Serial3.read();
}
if ((Serial3.available() > 0)&&(peekchr==1))
{
recieved_char = Serial3.read();
if ((recieved_char == 'x')||(recieved_char == 'y')||(recieved_char == 'z')||(recieved_char ==
'e'))
{
if ((pointer != ' ')&&(recieved_char == 'z'))
{ xtemp = "";
ytemp = "";
gyroZtemp = "";
}
pointer
= recieved_char;
if (xtemp!="") {
x=(str2flt(xtemp))/100;
xtemp
= "";
}
if (ytemp!="") {
y=(str2flt(ytemp))/100;
ytemp
= ""
;
}
if (gyroZtemp!="") {
gyroZ=(str2flt(gyroZtemp));
gyroZtemp
= ""
;
}
if (recieved_char == 'e'){
pointer=' '
;
peekchr=0;
PID();
}
}
else {
switch (pointer){
case 'x'
:
xtemp+=recieved_char;
break
;
case 'y'
:
ytemp+=recieved_char;
break;
case 'z'
:
gyroZtemp+=recieved_char;
break; }
}}}}
float str2flt (String readstring)
{
char carray[readstring.length() + 1]; //determine size of the array
readstring.toCharArray(carray, sizeof(carray)); //put readStringinto an array
float n = atof(carray); //convert the array into a Float
return n;
}
void rc_yaw_interupt() { //YAW Interrupt
if(int0)
count0=micros(); // we got a positive edge
else
yaw_pulse=micros()-count0; // Negative edge: get pulsewidth
}
void rc_thro_interupt() { //THROTTLE Interrupt
if(int1)
count1=micros(); // we got a positive edge
else
{
thro_pulse=micros()-count1; // Negative edge: get pulsewidth
if (thro_pulse<1150) thro_pulse=1050;
else thro_pulse=map (thro_pulse, 1150, 2000, 1400, 1650);
}
}
void rc_pitch_interupt() { //PITCH Interrupt
if(int2)
count2=micros(); // we got a positive edge
else
pitch_pulse=micros()-count2; // Negative edge: get pulsewidth
}
void rc_roll_interupt() { //ROLL Interrupt
if(int3)
count3=micros(); // we got a positive edge
else
roll_pulse=micros()-count3; // Negative edge: get pulsewidth
}
void rc_sw_interupt() { //SWITCH Interrupt
if(int4)
count4=micros(); // we got a positive edge
else
RC_switch_value=micros()-count4; // Negative edge: get pulsewidth
}
void filter_RC () { //FILTER RC
if (yaw_pulse>0) {
tmp_time_y[jy]=yaw_pulse; 
jy++; 
if (jy>1) jy=0;
yaw_setpoint=0;
for (ky=0; ky<2; ky++) yaw_setpoint+=tmp_time_y[ky];
yaw_setpoint=yaw_setpoint/2;
if (( yaw_setpoint < 1550) && ( yaw_setpoint > 1450)) yaw_setpoint= 1500;
yaw_pulse=0;
}
if (thro_pulse>0) { //throttle (pin 20)
tmp_time_t[jt]=thro_pulse;
jt++;
if (jt>1) jt=0;
thro_setpoint=0;
for (kt=0; kt<2; kt++) thro_setpoint+=tmp_time_t[kt];
thro_setpoint= thro_setpoint/2;
thro_pulse=0;
}
if (roll_pulse>0) { //roll (pin 19)
tmp_time_r[jr]=roll_pulse;
jr++;
if (jr>1) jr=0;
roll_setpoint=0;
for (kr=0; kr<2; kr++) roll_setpoint+=tmp_time_r[kr];
roll_setpoint = (roll_setpoint / 2) + 50;
if (( roll_setpoint < 1550) && ( roll_setpoint > 1450)) roll_setpoint= 1500;
 roll_setpoint=map (roll_setpoint, 1000, 2000, -20, 20);
roll_pulse=0;
}
if (pitch_pulse>0) { //pitch (pin 18)
tmp_time_p[jp]=pitch_pulse;
jp++;
if (jp>1) jp=0;
pitch_setpoint=0;
for (kp=0; kp<2; kp++) pitch_setpoint+=tmp_time_p[kp];
pitch_setpoint = pitch_setpoint / 2;
if (( pitch_setpoint < 1550) && ( pitch_setpoint > 1450)) pitch_setpoint= 1500;
 pitch_setpoint= map (pitch_setpoint, 1000, 2000, -20, 20);
pitch_pulse=0;
}
}
void send_PWM() {
valESCyaw_final = valESCyaw+thro_setpoint;
valESCright_final = valESCright+thro_setpoint;
valESCleft_final = valESCleft+thro_setpoint;
valESCyaw_final = constrain (valESCyaw_final, 1050, 1900);
valESCright_final = constrain (valESCright_final, 1050, 1900);
valESCleft_final = constrain (valESCleft_final, 1050, 1900);
OCR3A = valESCright_final;
OCR4A = valESCyaw_final;
OCR4B = valESCleft_final;
OCR4C = valservo;
}
void serial_COMM() {
if (Serial2.available() > 0){
char command = Serial2.read();
switch (command) {
case '1': Kp_x+=0.01; break;
case 'q': Kp_x=Kp_x-0.01; break;
case '2': Ki_x+=0.01; break;
case 'w': Ki_x=Ki_x-0.01; break;
case '3': Kd_x+=0.1; break;
case 'e': Kd_x+=-0.1; break;
case '4': I_limit_x+=1; break;
case 'r': I_limit_x=I_limit_x-1; break;
103
case '5': Kp_y+=0.01; break
;
case 't': Kp_y=Kp_y
-0.01; break
;
case '6': Ki_y+=0.01; break
;
case 'y': Ki_y=Ki_y
-0.01; break
;
case '7': Kd_y+=0.1; break
;
case 'u': Kd_y=Kd_y
-0.1; break
;
case '8': I_limit_y+=1; break
;
case 'i': I_limit_y=I_limit_y
-1; break
;
case 's': Kp_z+=0.01; break
;
case 'x': Kp_z=Kp_z
-0.01; break
;
case 'd': Ki_z+=0.01; break
;
case 'c': Ki_z=Ki_z
-0.01; break
;
case 'f': I_limit_z+=1; break
;
case 'v': I_limit_z=I_limit_z
-1; break
;
}}
// Angles
nums[0]=(x);
nums[1]=(y);
// Gyros
nums[2]=(gyroX);
nums[3]=(gyroY);
nums[4]=(gyroZ);
//Errors
nums[5]=(error_x);
nums[6]=(error_y);
nums[7]=(error_z);
//Setpoints
nums[8]=(roll_setpoint);
nums[9]=(pitch_setpoint);
nums[10]=(yaw_setpoint);
nums[11]=(RC_switch_value);
//Outputs
nums[12]=(valESCright_final);
nums[13]=(valESCleft_final);
nums[14]=(valESCyaw_final);
nums[15]=(valservo);
// PID of X
nums[16]=(Kp_x);
nums[17]=(Ki_x);
nums[18]=(Kd_x*10);
nums[19]=(I_limit_x);
nums[20]=(integral_x);
// PID of Y
nums[21]=(Kp_y);
nums[22]=(Ki_y);
nums[23]=(Kd_y*10);
nums[24]=(I_limit_y);
nums[25]=(integral_y);
// PID of Z
nums[26]=(Kp_z);
nums[27]=(Ki_z);
nums[28]=(Kd_z*10);
nums[29]=(I_limit_z);
nums[30]=(integral_z);
nums[31]=(thro_setpoint);
nums[32]=(temp_time_holder);
nums[33]=(battery_mes);
nums[34]=(batt_time);
Serial2.print(nums[serial_send_counter]);Serial2.print(",");
serial_send_counter++;
if (serial_send_counter>34) {
serial_send_counter=0;
Serial2.println ();
}
}