// ------------------------------------------------------------------
// Z-WING2025　PIC12F1572                 (C)2025.12.31 inakakoubouKANAI
//  FLP1
//           +  :Vcd   Vss: -
//       ELE IN :RA5   RA0: IN RUD
//      MAIN IN :RA4   RA1: OUT PWM1 → Servo
//　GYR3 RUD 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, WDTE = OFF, PWRTE = ON, MCLRE = OFF, CP = OFF, BOREN = ON, LVP = OFF

// --- 変数宣言 (int16_t統一) ---
int16_t RUDSENTER, RUD;
int16_t GYR2SENTER, GYR2;
int16_t GYR3SENTER, GYR3;
int16_t MAINSENTER, MAIN0;
int16_t ELESENTER, ELE;
int16_t GS, G0, G1, G2, G3,GELE0,GELE1;
int16_t h = 1; //ヒステリシス

// --- PDI制御用変数 ---
int16_t gyro2_err_prev = 0, gyro3_err_prev = 0;
int32_t gyro2_integral = 0, gyro3_integral = 0;

// --- PDIゲイン（個別軸調整） ---
int16_t Kp2 = 0, Ki2 = 0, Kd2 = 1; // ピッチ制御 (GYR2)
int16_t Kp3 = 0, Ki3 = 0, Kd3 = 1; // ヨー制御 (GYR3)

//--------------------------------- 初期化 ---------------------------------
void p_initialize() {
    OSCCON = 0b01111000;   // 16MHz
    ANSELA = 0b00000000;   // すべてデジタル
    TRISA  = 0b1111101;    // RA1出力
    WPUA   = 0b1111101;
    OPTION_REGbits.nWPUEN = 0;
    INTCON = 0x00;
    LATA   = 0b00000000;
}

void PWM_Initialize(void) {
    OSCCON = 0x7A;
    while(!OSCSTATbits.HFIOFS);
}

void out_Initialize(void) {
    PWM1CLKCON = 0b00000000;
    PWM1TMR = 0;
    PWM1CLKCON |= (0b100 << 3); // プリスケール設定
    PWM1CON = 0b11000000;
    PWM1DCH = (6000 >> 8) & 0xFF;
    PWM1DCL = 6000 & 0xFF;
    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;  
    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制御用合成＋PDI演算 ■■■
//---------------------------------------------------------
void out_steering()
{

   //---GYR3によるラダーポイントの移動
    RUD=RUD+GYR3-GYR3SENTER;
    
 
    if (ELE>(ELESENTER+360)){
     h=2;       
    } else
    {
       if (ELE<(ELESENTER+350)){
       h=1;         
     }     
    } 
   
 if(h>1)
 {
  //----反転-----
     if (RUDSENTER<RUD)
     { 
         G0=RUD-RUDSENTER;
         G2=G0 >> 2;
     GS=MAIN0+G2;  
     }
     else
     {
         G0=RUDSENTER-RUD;
         G2=G0 >> 2;
     GS=MAIN0-G2; 
     } 
 }
 else
 {
 //---通常----
     if (RUDSENTER<RUD)
     {
         G0=RUD-RUDSENTER;
         //G2=G0 >> 2; 
         G1=G0 >> 1;
     GS=MAIN0+G1;   
     }
     else
     {
         G0=RUDSENTER-RUD;
         G2=G0 >> 2;        
     GS=MAIN0+G2;
     } 
     
 }
  
//---送信機からのピッチコントロール
 GELE0=ELESENTER >> 2;
 GELE1=ELE >> 2 ;  
 GS=GS-GELE0+GELE1;
 
 
 //--- GYR2/GYR3 両軸PDI制御 (ピッチ + ヨー)
   // int16_t pitch_offset = pdi_control(GYR2, GYR2SENTER, &gyro2_err_prev, &gyro2_integral, Kp2, Ki2, Kd2);
   // int16_t yaw_offset   = pdi_control(GYR3, GYR3SENTER, &gyro3_err_prev, &gyro3_integral, Kp3, Ki3, Kd3);   
   int16_t pitch_offset = d_control_only(GYR3, GYR3SENTER, &gyro3_err_prev, Kd3);
   int16_t yaw_offset   = d_control_only(GYR3, GYR3SENTER, &gyro3_err_prev, Kd3);   
    
    
 //---GYR21でピッチコントロール
    GS-=(GYR2-pitch_offset)-GYR2SENTER;
    
//---GYR3でヨーコントロール GYRから左舵が来たら上げる
 if((GYR3+yaw_offset)>GYR3SENTER)
 {
    GS+=(GYR3+yaw_offset)-GYR3SENTER;
 }


 GS = clip_range(GS, 4000, 8000); // 140%60度範囲制限 
 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();
    GYR2SENTER = read_2();
    GYR3SENTER = read_3();
    MAINSENTER = read_4();
    ELESENTER  = read_5();

    while(1) {
        RUD  = read_0();
        GYR2 = read_2();
        GYR3 = read_3();
        MAIN0= read_4();
        ELE  = read_5();
        out_steering();
    }
}