PID 테스트 시작

PID테스트하기 위한 개발환경 구축

PID테스트를 시작하기 시작하였다. 

우선 쿼드콥터를 제어하기위해 개발환경을 나름 최적으로 만들어야 했다.

우선 이때까지 아두이노 IDE를 쓰다가 그 불편함 때문에 visual studio에서 아두이노로 바로 업로드 할 수 있게 하였다.

그 프로그램은 visualmicro라고 하는 것인데 이것을 통해서 visual studio의 장점을 활용하면서 arduino의 코드를 디버깅 할 수 있게 되었다.

다음으로 PID제어를 할 수 있도록 드론을 지지대에 묶었고 긴 USB케이블로 노트북과 연결하여 시리얼 모니터 창으로 값들을 디버깅 할 수 있도록 하였다.

물론 조종은 무선 조종기로 throttle과 roll, pitch를 조종하도록 하였다.

yaw값은 일단은 조종에서 빼고 처음에 향하는 방향을 초깃값으로 설정하도록 하였다.

그리고 드론이 잘 못된 제어로 갑자기 확 쎄지거나 회전해서 선이 꼬여버리면 큰일 이기 때문에 그것을 방지 할 수 있게 안전 수단을 만들었다.

첫 번째로는 Throttle이 제일 아래로 되어 있으면 PID제어에 관계없이 모터의 모든 값을 0으로 되게 하였다.

두 번째로는 yaw키는 일단은 쓰지 않기 때문에 yaw 조종값이 제일 크게 올라가면 모든 모터 값이 0이 되도록 하였다.

이렇게 함으로써 소프트웨어적으로 긴급상황에 대처할 수 있게 하였다.

세 번째로는 배터리와 드론사이에 길게 선을 빼서 스위치를 달았다.

그래서 이 스위치를 켜면 드론에 전원이 공급되고 스위치를 끄면 배터리를 뗀것 같은 효과를 주어서 하드웨어적으로 긴급상황에 대처할 수 있도록 하였다.

PID테스트 

다음 코드를 이용하여 PID테스트를 해보았다.

나름 균형을 잡고 있는 것 같다.

#include "Servo.h"
 
/////////////////BLDC Motor/////////////////
#define MAX_THROTTLE 180
#define MIN_THROTTLE 0
#define MAX_PWM 150
#define MIN_PWM 0
#define MOTOR1 6  //motor pin number
#define MOTOR2 7  //motor pin number
#define MOTOR3 44  //motor pin number
#define MOTOR4 45  //motor pin number
 
int motor1Power, motor3Power; // Motors on the X axis
//=>; changing their speeds will affect the pitch
int motor2Power, motor4Power; // Motors on the Y axis
//=>; changing their speeds will affect the roll
 
Servo motor1;
Servo motor2;
Servo motor3;
Servo motor4;
////////////////////////////////////////////
 
 
/////////////////IMU Sensor/////////////////
 typedef union {
    signed short value;
    unsigned char byte[2];
} S2BYTE;
 
S2BYTE data_word;
 
unsigned char SBuf[100];
int SCnt = 0;
double  DegRoll, DegPitch, DegYaw;
////////////////////////////////////////////
 
 
/////////////////PID Conrtroll/////////////////
#define MINhover 0
#define INIThover  20
 
#define INITROLLKp 0.48//0.19//0.29
#define INITROLLKi 0.000850//0.000850//0.000287 
#define INITROLLKd 33
 
#define INITPITCHKp 0.29
#define INITPITCHKi 0.000287 
#define INITPITCHKd 33
 
#define INITYAWKp 0.29  
#define INITYAWKi 0.000287 
#define INITYAWKd 33
 
#define MAXSPEED  3
#define MINSPEED  100
 
#define INITROLLSETPOINT -0.60
#define INITPITCHSETPOINT -0.67
 
 
double roll_setpoint = INITROLLSETPOINT, pitch_setpoint = INITPITCHSETPOINT, yaw_setpoint = 0;
int hover = INIThover;
double roll_Kp = INITROLLKp, roll_Ki = INITROLLKi, roll_Kd = INITROLLKd;
double pitch_Kp = INITPITCHKp, pitch_Ki = INITPITCHKi, pitch_Kd = INITPITCHKd;
double yaw_Kp = INITYAWKp, yaw_Ki = INITYAWKi, yaw_Kd = INITYAWKd;
double roll_p, roll_i, roll_d, roll_val;
double pitch_p, pitch_i, pitch_d, pitch_val;
double yaw_p, yaw_i, yaw_d, yaw_val;
int rollControll, pitchControll, yawControll;
 
int initial = 0;
////////////////////////////////////////////
 
/////////////////Serial Monitor/////////////////
int Serial_print_sig = 1;
int roll_print = 1, pitch_print = 0, yaw_print = 0, hover_print = 0;
////////////////////////////////////////////
 
////////////////Wireless Controller///////////////////
#define CH1 2 // yaw
#define CH2 3 //pitch
#define CH3 0 //hover
#define CH4 1 //roll
 
#define CH1_MIN 1292//200 1264
#define CH1_MID 1464
#define CH1_MAX 1632//168
 
#define CH2_MIN 1224//304 1176
#define CH2_MID 1480
#define CH2_MAX 1736//256
 
#define CH3_MIN 1028
#define CH3_MID 1448
#define CH3_MAX 1808
 
#define CH4_MIN 1296//208 1264
#define CH4_MID 1472
#define CH4_MAX 1648//176
 
#define CUTFORZERO 3
#define DEGRANGE  45
#define TROTTLERANGE  150
#define WFLYOFFSET 18
#define CHENELNUM 4
#define CHECKINGNUM 4
 
volatile int  CH1_prev_time, CH1_pwm_value;
volatile int  CH2_prev_time, CH2_pwm_value;
volatile int  CH3_prev_time, CH3_pwm_value;
volatile int  CH4_prev_time, CH4_pwm_value;
int CH1_val = 0, CH2_val = 0, CH3_val = 0, CH4_val = 0;
int controll_val[CHENELNUM];
int* ptr;
/////////////////////////////////////////////////////////////////////
 
//---------------------------------------------------------------------//
//
//                              funtions
//
//--------------------------------------------------------------------//
 
//----------------------------------------------------------------//
//                      BLDC Motor
//---------------------------------------------------------------//
//esc占쏙옙占쏙옙 占싹깍옙
void callibration(void)
{
 
    Serial.println("Calibrate ESC? 'y' or 'n'.");
 
    while (!Serial.available());
    char cali = Serial.read();
    if (cali == 'y'){
        Serial.println("send max throttle");
 
        motor1.attach(MOTOR1);
        motor2.attach(MOTOR2);
        motor3.attach(MOTOR3);
        motor4.attach(MOTOR4);
        motor1.write(MAX_THROTTLE);
        motor2.write(MAX_THROTTLE);
        motor3.write(MAX_THROTTLE);
        motor4.write(MAX_THROTTLE);
 
        delay(5000);
 
        Serial.println("send min throttle");
        motor1.attach(MOTOR1);
        motor2.attach(MOTOR2);
        motor3.attach(MOTOR3);
        motor4.attach(MOTOR4);
        motor1.write(MIN_THROTTLE);
        motor2.write(MIN_THROTTLE);
        motor3.write(MIN_THROTTLE);
        motor4.write(MIN_THROTTLE);
 
        delay(2000);
 
        Serial.println("callibration complete");
 
    }
    else if (cali == 'n'){
        motor1.attach(MOTOR1);
        motor2.attach(MOTOR2);
        motor3.attach(MOTOR3);
        motor4.attach(MOTOR4);
        motor1.write(MIN_THROTTLE);
        motor2.write(MIN_THROTTLE);
        motor3.write(MIN_THROTTLE);
        motor4.write(MIN_THROTTLE);
 
    }
    Serial.println("If you want Program keep going. Press any key.");
    while (!Serial.available());
}
int motorBegin(void)
{
    int count = 0, throttle;
 
 
    motor1.attach(MOTOR1);
    motor2.attach(MOTOR2);
    motor3.attach(MOTOR3);
    motor4.attach(MOTOR4);
 
    delay(2000);
 
    while (count <5){
        vlaueable_wfly(&controll_val[0]);
        throttle = controll_val[2];
        count++;
    }
 
    do{
        vlaueable_wfly(&controll_val[0]);
        throttle = controll_val[2];
 
 
 
        if (throttle < 10){//no callibration
 
            Serial.println("no callibration.");
 
            motor1.write(MIN_THROTTLE);
            motor2.write(MIN_THROTTLE);
            motor3.write(MIN_THROTTLE);
            motor4.write(MIN_THROTTLE);
 
            delay(2000);
 
            Serial.println("program start!");
 
            delay(2000);
 
            return 0;
 
        }
        else if (throttle > 140){//yes callibration
            Serial.print("you send MAX_THROTTLE.   "); Serial.print("callibration begin.    "); Serial.println("wait 4 seconds");
 
            motor1.write(MAX_THROTTLE);
            motor2.write(MAX_THROTTLE);
            motor3.write(MAX_THROTTLE);
            motor4.write(MAX_THROTTLE);
 
            delay(4000);
 
            Serial.println("send min throttle for caliibraion.");
 
            while (throttle > 10){//wait until MIN_THROTTLE 
                vlaueable_wfly(&controll_val[0]);
                throttle = controll_val[2];
            }
 
            Serial.print("you send MIN_THROTTLE.    ");
 
            motor1.write(MIN_THROTTLE);
            motor2.write(MIN_THROTTLE);
            motor3.write(MIN_THROTTLE);
            motor4.write(MIN_THROTTLE);
 
            Serial.print("calibration complete.     ");
            Serial.println("program start!");
 
            delay(2000);
 
            return 1;
        }
    } while (1);
}
//占쏙옙占쏘값占쏙옙 占쌨어서 占쏙옙占쏙옙 占쏙옙占쏙옙
void run_motors(int throttle, int rollOffset, int pitchOffset, int yawOffset)
{
    //emergency
    if (controll_val[0] > 30 || throttle < 5){
        throttle = 0;
        rollOffset = 0;
        pitchOffset= 0;
        yawOffset = 0;
    }
 
    //Roll control
    motor1Power = throttle + rollOffset / 2;
    motor2Power = throttle + rollOffset / 2;
    motor4Power = throttle - rollOffset / 2;
    motor3Power = throttle - rollOffset / 2;
 
    //Pitch control
    motor4Power = motor4Power + pitchOffset / 2;
    motor1Power = motor1Power + pitchOffset / 2;
    motor3Power = motor3Power - pitchOffset / 2;
    motor2Power = motor2Power - pitchOffset / 2;
 
    /* //yaw control
    motor1Power = motor1Power - yawOffset;
    motor3Power = motor3Power - yawOffset;
    motor2Power = motor2Power + yawOffset;
    motor4Power = motor4Power + yawOffset;
    */
 
    limit_max_min(&motor1Power);
    limit_max_min(&motor2Power);
    limit_max_min(&motor3Power);
    limit_max_min(&motor4Power);
 
    motor1.write(motor1Power); motor3.write(motor3Power);
    motor2.write(motor2Power); motor4.write(motor4Power);
 
    /*
    Serial.print(motor1Power); Serial.print(", ");
    Serial.print(motor2Power); Serial.print(", ");
    Serial.print(motor3Power); Serial.print(", ");
    Serial.print(motor4Power); Serial.println(" ");
    */
}
 
void limit_max_min(int* pwm)//modify
{
    if (*pwm>MAX_PWM)
        *pwm = MAX_PWM;
    else if (*pwm<MIN_PWM)
        *pwm = MIN_PWM;
}
//---------------------------------------------------------//
//                     IMU Sensor
//--------------------------------------------------------//
//占쏙옙占쏙옙占쏙옙占쏙옙 占싻어서 roll,pitch,yaw 占쏙옙占쏙옙 占싯아놂옙占쏙옙.
int recieve_data(void)
{
    static int step_ = 0, check_all_recieve = 0;
    unsigned char data1byte;
    int n;
 
    data1byte = Serial1.read();
 
    switch (step_)
    {
    case 0:
        if (data1byte == 0x55) { step_ = 1, check_all_recieve = 0; }
        break;
 
    case 1:
        if (data1byte == 0x55) { step_ = 2;  SCnt = 0; }
        else step_ = 0;
        break;
 
    case 2:
        SBuf[SCnt++] = (unsigned char)data1byte;
        if (SCnt == 8)  // roll(2byte),pitch(2byte),yaw(2byte),checksum(2byte) 
        {
            step_ = 0;
            SCnt = 0;
            check_all_recieve = 1;
        }
        break;
 
    default:
        break;
    }
 
    return check_all_recieve;
}
 
static int trans_data_to_degree(unsigned char *sdata)  // sdata : roll(2)+pitch(2)+yaw(2)+chk(2)
{
    int n;
    unsigned short chk = 0;
 
    ///////////// checksum /////////////////
    chk = 0x55 + 0x55;
    for (n = 0; n<6; n++) chk += sdata[n];
    if (chk != get_data_word(&sdata[6]))
    {
        Serial.print("checksum error!");
        return 0; //   
    }
    ////////////////////////////////////////
 
    DegRoll = get_data_word(&sdata[0]) / 100.;
    DegPitch = get_data_word(&sdata[2]) / 100. * -1;
    DegYaw = get_data_word(&sdata[4]) / 100.;
 
    return 1;
}
 
static signed short get_data_word(unsigned char *dat)
{
    // little endian..
    data_word.byte[0] = dat[1];
    data_word.byte[1] = dat[0];
 
    return data_word.value;
}
 
//-----------------------------------------------------------//
//                     PID Conrtroll
//----------------------------------------------------------//
//占쏙옙占쏙옙 占쏙옙占쏙옙 占쌨아쇽옙 PID占쏙옙占쏘값 占쏙옙占?double roll_pid(double setpoint, double degree)
{
    double val_out;
    static double roll_p_err_prv, roll_p_err, roll_i_err, roll_d_err;
 
    roll_p_err = setpoint - degree;
    roll_i_err += roll_p_err;
    roll_d_err = roll_p_err - roll_p_err_prv;
 
    roll_p_err_prv = roll_p_err;
    val_out = (roll_Kp*roll_p_err) + (roll_Ki*roll_i_err) + (roll_Kd*roll_d_err);
 
    roll_p = roll_Kp*roll_p_err;
    roll_i = roll_Ki*roll_i_err;
    roll_d = roll_Kd*roll_d_err;
    roll_val = val_out;
 
    return val_out;
}
 
double pitch_pid(double setpoint, double degree)
{
    double val_out;
    static double pitch_p_err_prv, pitch_p_err, pitch_i_err, pitch_d_err;
 
    pitch_p_err = setpoint - degree;
    pitch_i_err += pitch_p_err;
    pitch_d_err = pitch_p_err - pitch_p_err_prv;
 
    pitch_p_err_prv = pitch_p_err;
    val_out = (pitch_Kp*pitch_p_err) + (pitch_Ki*pitch_i_err) + (pitch_Kd*pitch_d_err);
 
    pitch_p = pitch_Kp*pitch_p_err;
    pitch_i = pitch_Ki*pitch_i_err;
    pitch_d = pitch_Kd*pitch_d_err;
    pitch_val = val_out;
 
    return val_out;
}
 
double yaw_pid(double setpoint, double degree)
{
    double val_out;
    static double yaw_p_err_prv, yaw_p_err, yaw_i_err, yaw_d_err;
 
    yaw_p_err = setpoint - degree;
    yaw_i_err += yaw_p_err;
    yaw_d_err = yaw_p_err - yaw_p_err_prv;
 
    yaw_p_err_prv = yaw_p_err;
    val_out = (yaw_Kp*yaw_p_err) + (yaw_Ki*yaw_i_err) + (yaw_Kd*yaw_d_err);
 
    yaw_p = yaw_Kp*yaw_p_err;
    yaw_i = yaw_Ki*yaw_i_err;
    yaw_d = yaw_Kd*yaw_d_err;
    yaw_val = val_out;
 
    return val_out;
}
 
//---------------------------------------------------------//
//                      Serial Monitor
//----------------------------------------------------------//
void from_computer(void)
{
    char c = Serial.read();
 
    //roll debuging
    if (c == 'q'){
        roll_Kp += 0.01;
        Serial.print("roll_Kp = "); Serial.println(roll_Kp);
    }
    else if (c == 'w'){
        roll_Kp -= 0.01;
        Serial.print("roll_Kp = "); Serial.println(roll_Kp);
    }
    else if (c == 'e'){
        roll_Kp = INITROLLKp;
        Serial.print("roll_Kp = "); Serial.println(roll_Kp);
    }
    else if (c == 'a'){
        roll_Ki += 0.000001;
        Serial.print("roll_Ki = "); Serial.println(roll_Ki * 10000);
    }
    else if (c == 's'){
        roll_Ki -= 0.000001;
        Serial.print("roll_Ki = "); Serial.println(roll_Ki * 10000);
    }
    else if (c == 'd'){
        roll_Ki = INITROLLKi;
        Serial.print("roll_Ki = "); Serial.println(roll_Ki * 10000);
    }
    else if (c == 'z'){
        roll_Kd += 1;
        Serial.print("roll_Kd = "); Serial.println(roll_Kd);
    }
    else if (c == 'x'){
        roll_Kd -= 1;
        Serial.print("roll_Kd = "); Serial.println(roll_Kd);
    }
    else if (c == 'c'){
        roll_Kd = INITROLLKd;
        Serial.print("roll_Kd = "); Serial.println(roll_Kd);
    }
 
 
 
    //pitch debuging
    else if (c == 'r'){
        pitch_Kp += 0.01;
        Serial.print("pitch_Kp = "); Serial.println(pitch_Kp);
    }
    else if (c == 't'){
        pitch_Kp -= 0.01;
        Serial.print("pitch_Kp = "); Serial.println(pitch_Kp);
    }
    else if (c == 'y'){
        pitch_Kp = INITPITCHKp;
        Serial.print("pitch_Kp = "); Serial.println(pitch_Kp);
    }
    else if (c == 'f'){
        pitch_Ki += 0.000001;
        Serial.print("pitch_Ki = "); Serial.println(pitch_Ki * 10000);
    }
    else if (c == 'g'){
        pitch_Ki -= 0.000001;
        Serial.print("pitch_Ki = "); Serial.println(pitch_Ki * 10000);
    }
    else if (c == 'h'){
        pitch_Ki = INITPITCHKi;
        Serial.print("pitch_Ki = "); Serial.println(pitch_Ki * 10000);
    }
    else if (c == 'v'){
        pitch_Kd += 1;
        Serial.print("pitch_Kd = "); Serial.println(pitch_Kd);
    }
    else if (c == 'b'){
        pitch_Kd -= 1;
        Serial.print("pitch_Kd = "); Serial.println(pitch_Kd);
    }
    else if (c == 'n'){
        pitch_Kd = INITPITCHKd;
        Serial.print("pitch_Kd = "); Serial.println(pitch_Kd);
    }
 
    //yaw debuging
    else if (c == 'u'){
        yaw_Kp += 0.01;
        Serial.print("yaw_Kp = "); Serial.println(yaw_Kp);
    }
    else if (c == 'i'){
        yaw_Kp -= 0.01;
        Serial.print("yaw_Kp = "); Serial.println(yaw_Kp);
    }
    else if (c == 'o'){
        yaw_Kp = INITYAWKp;
        Serial.print("yaw_Kp = "); Serial.println(yaw_Kp);
    }
    else if (c == 'j'){
        yaw_Ki += 0.000001;
        Serial.print("yaw_Ki = "); Serial.println(yaw_Ki * 10000);
    }
    else if (c == 'k'){
        yaw_Ki -= 0.000001;
        Serial.print("yaw_Ki = "); Serial.println(yaw_Ki * 10000);
    }
    else if (c == 'l'){
        yaw_Ki = INITYAWKi;
        Serial.print("yaw_Ki = "); Serial.println(yaw_Ki * 10000);
    }
    else if (c == 'm'){
        yaw_Kd += 1;
        Serial.print("yaw_Kd = "); Serial.println(yaw_Kd);
    }
    else if (c == ','){
        yaw_Kd -= 1;
        Serial.print("yaw_Kd = "); Serial.println(yaw_Kd);
    }
    else if (c == '.'){
        yaw_Kd = INITYAWKd;
        Serial.print("yaw_Kd = "); Serial.println(yaw_Kd);
    }
 
    else if (c == 'p'){
        Serial_print_sig = !(Serial_print_sig);
    }
    else if (c == '/'){
        roll_print = !(roll_print);
    }
    else if (c == '*'){
        pitch_print = !(pitch_print);
    }
    else if (c == '-'){
        yaw_print = !(yaw_print);
    }
    else if (c == '+'){
        hover_print = !(hover_print);
    }
 
}
 
 
void Serial_Monitor(void)
{
    if (Serial_print_sig == 1){
        if (roll_print == 1){
            Serial.print("Roll=> "); Serial.print(DegRoll); Serial.print(", "); Serial.print(roll_p); Serial.print(", "); Serial.print(roll_i); Serial.print(", "); Serial.print(roll_d); Serial.print(", "); Serial.print(roll_val); Serial.print(", "); Serial.print(motor2Power); Serial.print(", "); Serial.println(motor4Power);
        }
        else if (pitch_print == 1){
            Serial.print("Pitch=> "); Serial.print(DegPitch); Serial.print(", "); Serial.print(pitch_p); Serial.print(", "); Serial.print(pitch_i); Serial.print(", "); Serial.print(pitch_d); Serial.print(", "); Serial.print(pitch_val); Serial.print(", "); Serial.print(motor1Power); Serial.print(", "); Serial.println(motor3Power);
        }
        else if (yaw_print == 1){
            Serial.print("Yaw=> "); Serial.print(DegYaw); Serial.print(", "); Serial.print(yaw_p); Serial.print(", "); Serial.print(yaw_i); Serial.print(", "); Serial.print(yaw_d); Serial.print(", "); Serial.println(yaw_val);
        }
    }
 
    if (Serial_print_sig == 0){
        if (hover_print == 1){
            Serial.print("hover = "); Serial.println(hover);
            hover_print = 0;
        }
        else if (roll_print == 1){
            Serial.print("roll_Kp = "); Serial.print(roll_Kp); Serial.print(", roll_Ki = "); Serial.print(roll_Ki * 10000); Serial.print(", roll_Kd = "); Serial.println(roll_Kd);
            roll_print = 0;
        }
        else if (pitch_print == 1){
            Serial.print("pitch_Kp = "); Serial.print(pitch_Kp); Serial.print(", pitch_Ki = "); Serial.print(pitch_Ki * 10000); Serial.print(", pitch_Kd = "); Serial.println(pitch_Kd);
            pitch_print = 0;
        }
        else if (yaw_print == 1){
            Serial.print("yaw_Kp = "); Serial.print(yaw_Kp); Serial.print(", yaw_Ki = "); Serial.print(yaw_Ki * 10000); Serial.print(", yaw_Kd = "); Serial.println(yaw_Kd);
            yaw_print = 0;
        }
    }
}
 
//-----------------------------------------------------------------//
//                    Wireless Controller
//-----------------------------------------------------------------//                    
void wirelessControllerBegin()
{
    attachInterrupt(CH1, rising1, RISING);
    attachInterrupt(CH2, rising2, RISING);
    attachInterrupt(CH3, rising3, RISING);
    attachInterrupt(CH4, rising4, RISING);
}
void rising1()
{
    attachInterrupt(CH1, falling1, FALLING);
    CH1_prev_time = micros();
}
 
void falling1()
{
    attachInterrupt(CH1, rising1, RISING);
    CH1_pwm_value = micros() - CH1_prev_time;
}
 
void rising2()
{
    attachInterrupt(CH2, falling2, FALLING);
    CH2_prev_time = micros();
}
 
void falling2()
{
    attachInterrupt(CH2, rising2, RISING);
    CH2_pwm_value = micros() - CH2_prev_time;
}
 
void rising3()
{
    attachInterrupt(CH3, falling3, FALLING);
    CH3_prev_time = micros();
}
 
void falling3()
{
    attachInterrupt(CH3, rising3, RISING);
    CH3_pwm_value = micros() - CH3_prev_time;
}
 
void rising4()
{
    attachInterrupt(CH4, falling4, FALLING);
    CH4_prev_time = micros();
}
 
void falling4()
{
    attachInterrupt(CH4, rising4, RISING);
    CH4_pwm_value = micros() - CH4_prev_time;
}
 
 
void vlaueable_wfly(int* storage)
{
    int temp_storage[4];
 
    //---------------CH1------------------------
    if (CH1_pwm_value>CH1_val + WFLYOFFSET | CH1_pwm_value<CH1_val - WFLYOFFSET)
        CH1_val = CH1_pwm_value;
 
    temp_storage[0] = map(CH1_val, CH1_MIN, CH1_MAX, (DEGRANGE*-1), DEGRANGE);
 
    catch_jumping_error(&temp_storage[0], &storage[0], CH1);
 
    if (storage[0] >= (CUTFORZERO*-1) & storage[0] <= CUTFORZERO)
        storage[0] = 0;
 
    //----------------CH2----------------------
    if (CH2_pwm_value>CH2_val + WFLYOFFSET | CH2_pwm_value<CH2_val - WFLYOFFSET)
        CH2_val = CH2_pwm_value;
 
    temp_storage[1] = -1 * map(CH2_val, CH2_MIN, CH2_MAX, (DEGRANGE*-1), DEGRANGE);
 
    catch_jumping_error(&temp_storage[1], &storage[1], CH2);
 
    if (storage[1] >= (CUTFORZERO*-1) & storage[1] <= CUTFORZERO)
        storage[1] = 0;
 
    //----------------CH3----------------------
    if (CH3_pwm_value>CH3_val + WFLYOFFSET | CH3_pwm_value<CH3_val - WFLYOFFSET)
        CH3_val = CH3_pwm_value;
 
    temp_storage[2] = map(CH3_val, CH3_MIN, CH3_MAX, 0, TROTTLERANGE);
 
    catch_jumping_error(&temp_storage[2], &storage[2], CH3);
    //-----------------CH4----------------------
    if (CH4_pwm_value>CH4_val + WFLYOFFSET | CH4_pwm_value<CH4_val - WFLYOFFSET)
        CH4_val = CH4_pwm_value;
 
    temp_storage[3] = -1 * map(CH4_val, CH4_MIN, CH4_MAX, (DEGRANGE*-1), DEGRANGE);
 
    catch_jumping_error(&temp_storage[3], &storage[3], CH4);
 
    if (storage[3] >= (CUTFORZERO*-1) & storage[3] <= CUTFORZERO)
        storage[3] = 0;
 
    /*
    Serial.print(controll_val[0]);Serial.print(", ");
    Serial.print(controll_val[1]);Serial.print(", ");
    Serial.print(controll_val[2]);Serial.print(", ");
    Serial.print(controll_val[3]);Serial.print(", ");
    Serial.println("");
    */
}
 
void catch_jumping_error(int * temp_val, int * val, int ch_num)
{
    static int checkArray[CHENELNUM][CHECKINGNUM];
    int sum = 0;
 
    checkArray[ch_num][0] = *temp_val;
 
 
    //equal??
    for (int i = 0; i<CHECKINGNUM; i++)
        sum += checkArray[ch_num][i];
 
    if (*temp_val == sum / CHECKINGNUM)
        *val = *temp_val;
 
    //shift
    for (int i = CHECKINGNUM - 1; i >= 0; i--)
        checkArray[ch_num][i + 1] = checkArray[ch_num][i];
 
 
 
 
}
 
//---------------------------------------------------------------------//
//
//                                main
//
//--------------------------------------------------------------------//
void setup() {
    Serial.begin(115200);
    Serial1.begin(115200);
    //callibration();
    wirelessControllerBegin();
    motorBegin();
}
 
void loop() {
 
    if (Serial1.available()) //占쏙옙占쏙옙占쏙옙 占쌨깍옙.
    {
        if (recieve_data() == 1){  //占쏙옙占쏙옙占쏙옙 占쏙옙 占쏙옙占쏙옙?
 
            vlaueable_wfly(&controll_val[0]); //占쏙옙占쏙옙占쏙옙
 
            
 
            if (trans_data_to_degree(SBuf) == 1) //占쏙옙占쏙옙占쏙옙. 
            {
                if (initial == 0){
                    yaw_setpoint = DegYaw;
                    initial = 1;
                    //Serial.println(yaw_setpoint);
                }
 
                //PID
                rollControll = roll_pid(controll_val[3], DegRoll);
                pitchControll = pitch_pid(controll_val[1], DegPitch);
                yawControll = yaw_pid(yaw_setpoint, DegYaw);
 
                
 
                //Run motor
                run_motors(controll_val[2], rollControll, pitchControll, yawControll);  
 
                if (Serial.available()){ //占쏙옙占쏙옙韜占?
                    from_computer(); //占쌉력받깍옙.
                }
 
                Serial_Monitor(); //Debugging
            }
        }
 
    }
 
 
 
 
 
}
 
 

    

참고

-visualmicro 개발 환경 구축 방법

http://talkingaboutme.tistory.com/206