// ------------------------------------------------------------------
// Z-WING2025　PIC12F1572                 (C)2025.12.31 inakakoubouKANAI
// FLP3
//          +  :Vcd   Vss : -
//      ELE IN :RA5   RA0: IN RUD
//     MAIN IN :RA4   RA1: OUT PWM1 → Servo
//　GYR3RUD IN :RA3   RA2: IN GYR2 ELE or AIL
// ------------------------------------------------------------------
#include <xc.h>
#include <stdint.h>

#define _XTAL_FREQ 16000000 //delayマクロ用 16MHz

 //CONFIG
#pragma config FOSC = INTOSC   // 内蔵クロック使用
#pragma config WDTE = OFF      // Watchdog Timerを無効
#pragma config PWRTE = ON      // Power-up Timerを有効
#pragma config MCLRE = OFF     // MCLRは、デジタル入力　内部でVDDに接続
#pragma config CP = OFF        // コードプロテクトは無効
#pragma config BOREN = ON      // ブラウンアウトは、有効
#pragma config LVP = OFF       // 低電圧プログラミング無効　有効だとRA3ピンの入力が使えない

// --- 変数宣言 (int16_tに統一) ---
int16_t RUDSENTER; //RA0 RUDのニュートラル値
int16_t RUD;       //RA0 RUD入力信号数
int16_t GYR2SENTER;  //RA2 ジャイロのニュートラル値
int16_t GYR2;         //RA2のジャイロ入力信号
int16_t GYR3SENTER;  //RA3 ジャイロのニュートラル値
int16_t GYR3;       //RA3 ジャイロの入力信号
int16_t MAINSENTER;  //RA4 受信機からの信号
int16_t MAIN0;       //RA4 受信機からの出力信号
int16_t ELESENTER; //RA5 ELEのニュートラル値
int16_t ELE;       //RA5 ELE入力信号

int16_t GS;
int16_t G0, G1, G2, G3;
int16_t h = 1;  //ヒステリシス

// --- PDI制御用変数 ---
int16_t gyro2_err_prev = 0, gyro3_err_prev = 0;// 前回の偏差（D項用）
int32_t gyro2_integral = 0, gyro3_integral = 0;// 偏差の累積（I項用：32bitで溢れ防止）

// --- PDIゲイン調整 ---
int16_t Kp3 = 0, Ki3 = 0, Kd3 = 1; // GYR3: ラダー（ヨー軸）

void p_initialize()
{
    //レジスタの設定
    OSCCON = 0b01111000;   //クロック周波数を16MHzに設定
    ANSELA = 0b00000000 ;    //全てのピンをデジタルモードに設定
    TRISA = 0b1111101;    // RA0,2,3,4,5 = input(1), RA1 = output(0)
    WPUA = 0b1111101;     // RA0,2,3,4,5 のプルアップ有効、RA1はプルアップなし
    OPTION_REGbits.nWPUEN = 0;  // ポート全体の内部プルアップ有効化

    INTCON = 0x00;   //割り込み不可
    LATA = 0b00000000; //出力ピンの初期化(全てLOW)
}

void PWM_Initialize(void)
{   
    // 内部クロック16MHzに設定
    OSCCON = 0x7A;
    while(!OSCSTATbits.HFIOFS); // HFIOSCが安定するまで待つ 
}

void out_Initialize(void)
{
    PWM1CLKCON = 0b00000000;    // クロックソースをFoscに設定    
    // PWMタイマー設定
    PWM1TMR = 0;                // タイマーカウンタをリセット
    PWM1CLKCON |= (0b100 << 3); // プリスケーラ設定 周期16ms  
    // PWM出力を有効化
    PWM1CON = 0b11000000;       // PWMモジュールと出力ピンを有効化
    //------デューティサイクルを指定--------
    PWM1DCH = (6000 >> 8) & 0xFF;//4000=1ms 6000=1.5ms 8000=2ms
    PWM1DCL = 6000 & 0xFF;  
    // 位相の設定（0）
    PWM1PHH = 0;
    PWM1PHL = 0;
    // 更新のためにロードビットを設定
    PWM1LDCONbits.LDA = 1;
    while(PWM1LDCONbits.LDA == 1);
}

// 上限・下限両方の範囲制限
int16_t clip_range(int16_t a, int16_t min, int16_t max) {
    if (a < min) return min;
    if (a > max) return max;
    return a;
}

//-----------------サーボへPWM出力-----------------------------------------------
void out_PWM(int16_t pulse)
{
    //------デューティサイクルを指定--------
    PWM1DCH = (pulse >> 8) & 0xFF;
    PWM1DCL = pulse & 0xFF;  
    // 位相の設定（0）
    PWM1PHH = 0;
    PWM1PHL = 0;
    // 更新のためにロードビットを設定
    PWM1LDCONbits.LDA = 1;
    while(PWM1LDCONbits.LDA == 1);
}
// --- 汎用PDI演算 ---
int16_t pdi_control(int16_t gyro, int16_t center, 
                    int16_t *prev, int32_t *integ,
                    int16_t Kp, int16_t Ki, int16_t Kd) {
    int16_t err = gyro - center;
    int16_t p = err * Kp;
    int16_t d = (err - *prev) * Kd;
    *prev = err;
    *integ += err;
    if (*integ > 2000) *integ = 2000;
    if (*integ < -2000) *integ = -2000;
    int16_t i = (int16_t)((*integ * Ki) >> 6);
    return (p + i + d) >> 2;
}
// D制御のみに特化したシンプルな関数例
int16_t d_control_only(int16_t gyro, int16_t center, int16_t *prev, int16_t Kd) {
    int16_t err = gyro - center;
    int16_t d = (err - *prev) * Kd;
    *prev = err;
    //return d >> 2; // スケーリング（必要に応じて調整）
    return d ;
}

//■■■■■■■■■■■■■■■  Z-WING制御用に合成  ■■■■■■■■■■■■■■
void out_steering()
{  
    //int16_t yaw_offset   = pdi_control(GYR3, GYR3SENTER, &gyro3_err_prev, &gyro3_integral, Kp3, Ki3, Kd3);   
     int16_t yaw_offset = d_control_only(GYR3, GYR3SENTER, &gyro3_err_prev, Kd3);
    //---GYR3によるラダーポイントの移動
    RUD=RUD+GYR3-GYR3SENTER+yaw_offset;
    
 
    //--- ヒステリシス状態 ---
    if (ELE > (ELESENTER + 360)) {
        h = 2;       
    } else if (ELE < (ELESENTER + 350)) {
        h = 1;         
    }
    
    if(h > 1) {
        // ---- 反転モード ----- FLP3
        if (RUDSENTER > RUD) {
            // 左舵角で上がっているときは大きめ
            G0 = RUDSENTER - RUD;
            G1 = G0 >> 1; G2 = G0 >> 2; G3 = G0 >> 3;
            GS = MAIN0 - G0;  
            if (ELE > (ELESENTER+450)) GS = MAIN0 - G1 - G2 - G3; //0.875
            if (ELE > (ELESENTER+540)) GS = MAIN0 - G1 - G2;      //0.75
            if (ELE > (ELESENTER+630)) GS = MAIN0 - G1 - G3;      //0.625
            if (ELE > (ELESENTER+720)) GS = MAIN0 - G1;           //0.5
        } else {
            G0 = RUD - RUDSENTER;
            G1 = G0 >> 1; G2 = G0 >> 2; G3 = G0 >> 3;
            GS = MAIN0 + G0;     
            if (ELE > (ELESENTER+450)) GS = MAIN0 + G1 + G2 + G3; //0.875
            if (ELE > (ELESENTER+540)) GS = MAIN0 + G1 + G2;      //0.75
            if (ELE > (ELESENTER+630)) GS = MAIN0 + G1 + G3;      //0.625
            if (ELE > (ELESENTER+720)) GS = MAIN0 + G1;           //0.5
        }
    } else {
        // --- 通常モード ---- FLP3
        if (RUDSENTER >= RUD) { 
            // 右舵角でさがっているときは大きめ
            G0 = RUDSENTER - RUD;
            G1 = G0 >> 1; G2 = G0 >> 2; G3 = G0 >> 3;
            GS = MAIN0 + G0 + G1 + G2 + G3; // 1.875倍
            if (ELE < (ELESENTER+270)) GS = MAIN0 + G0 + G1 + G2; //1.75
            if (ELE < (ELESENTER+180)) GS = MAIN0 + G0 + G1 + G3; //1.675
            if (ELE < (ELESENTER+90))  GS = MAIN0 + G0 + G1;      //1.5
            if (ELE < (ELESENTER))     GS = MAIN0 + G0 + G2 + G3; //1.375
            if (ELE < (ELESENTER-90))  GS = MAIN0 + G0 + G2;      //1.25
            if (ELE < (ELESENTER-180)) GS = MAIN0 + G1 + G3;      //1.125
        } else {         
            G0 = RUD - RUDSENTER; 
            G1 = G0 >> 1; G2 = G0 >> 2; G3 = G0 >> 3;
            GS = MAIN0 - G0 - G1 - G2 - G3; // 1.875倍
            if (ELE < (ELESENTER+270)) GS = MAIN0 - G0 - G1 - G2; //1.75
            if (ELE < (ELESENTER+180)) GS = MAIN0 - G0 - G1 - G3; //1.625
            if (ELE < (ELESENTER+90))  GS = MAIN0 - G0 - G1;      //1.5
            if (ELE < (ELESENTER))     GS = MAIN0 - G0 - G2 - G3; //1.375
            if (ELE < (ELESENTER-90))  GS = MAIN0 - G1 - G2;      //1.25
            if (ELE < (ELESENTER-180)) GS = MAIN0 - G1 - G3;      //1.125
        }
    }

    GS = clip_range(GS, 4833, 7182); // 35度範囲制限 
    out_PWM(GS);   
}

//-----------------------信号読み取り-------------------------------------------
int16_t read_0() {
    while (RA0 == 1); while (RA0 == 0);
    T1CON = 0b00000001; TMR1H = 0; TMR1L = 0;
    while (RA0 == 1); T1CONbits.TMR1ON = 0;
    return (int16_t)((TMR1H << 8) | TMR1L);
}
int16_t read_2() {
    while (RA2 == 1); while (RA2 == 0);
    T1CON = 0b00000001; TMR1H = 0; TMR1L = 0;
    while (RA2 == 1); T1CONbits.TMR1ON = 0;
    return (int16_t)((TMR1H << 8) | TMR1L);
}
int16_t read_3() {
    while (RA3 == 1); while (RA3 == 0);
    T1CON = 0b00000001; TMR1H = 0; TMR1L = 0;
    while (RA3 == 1); T1CONbits.TMR1ON = 0;
    return (int16_t)((TMR1H << 8) | TMR1L);
}
int16_t read_4() {
    while (RA4 == 1); while (RA4 == 0);
    T1CON = 0b00000001; TMR1H = 0; TMR1L = 0;
    while (RA4 == 1); T1CONbits.TMR1ON = 0;
    return (int16_t)((TMR1H << 8) | TMR1L);
}
int16_t read_5() {
    while (RA5 == 1); while (RA5 == 0);
    T1CON = 0b00000001; TMR1H = 0; TMR1L = 0;
    while (RA5 == 1); T1CONbits.TMR1ON = 0;
    return (int16_t)((TMR1H << 8) | TMR1L);
}

//----------------------メイン--------------------------------------------------
void main(void)
{
    p_initialize();
    PWM_Initialize();
    out_Initialize(); //1.5msで出力
    __delay_ms(10000); //ジャイロ安定待ち 

    RUDSENTER = read_0();
    GYR3SENTER = read_3();
    MAINSENTER = read_4();
    ELESENTER = read_5();

    while(1)
    {
        RUD = read_0();
        GYR3 = read_3();
        MAIN0 = read_4();
        ELE = read_5();

        out_steering();
    }
}