// ------------------------------------------------------------------ // Z-WING2025　PIC12F1572 (C)2025.12.31 inakakoubouKANAI // ABK // + :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 #include #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; // ※GYR2はコメントアウト（使用せず） int16_t GYR3SENTER, GYR3; int16_t MAINSENTER, MAIN0; int16_t ELESENTER, ELE; int16_t GS, G0, G1, G2, G3; int16_t h = 1; // ヒステリシス判定用 // --- GYR3 PDI制御用（GYR2はコメントアウト） --- int16_t gyro3_err_prev = 0; int32_t gyro3_integral = 0; //int16_t gyro2_err_prev = 0; // ※GYR2未使用 //int32_t gyro2_integral = 0; // --- ゲイン調整--- int16_t Kp3 = 0, Ki3 = 0, Kd3 = 1; // GYR3(PDI) //int16_t Kp2 = 4, Ki2 = 0, Kd2 = 5; // GYR2(PDI) ※未使用 // ------------------------------------------------------------------ // 初期化処理 // ------------------------------------------------------------------ void p_initialize() { OSCCON = 0b01111000; // 16MHz ANSELA = 0b00000000; TRISA = 0b1111101; 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); } // ------------------------------------------------------------------ // GYR3 専用PDI制御計算関数 // ------------------------------------------------------------------ int16_t pdi_control(int16_t gyro, int16_t center, int16_t *prev_err, int32_t *integral, int16_t Kp, int16_t Ki, int16_t Kd) { int16_t err = gyro - center; int16_t p = err * Kp; int16_t d = (err - *prev_err) * Kd; *prev_err = err; *integral += err; if (*integral > 2000) *integral = 2000; if (*integral < -2000) *integral = -2000; int16_t i = (int16_t)((*integral * 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出力処理本体（ABK用・GYR3 PDI制御） // ------------------------------------------------------------------ void out_steering() { // GYR3 PDI偏差演算のみ（GYR2はコメントアウト） //int16_t rud_offset = pdi_control(GYR3, GYR3SENTER, &gyro3_err_prev, &gyro3_integral, Kp3, Ki3, Kd3); int16_t rud_offset = d_control_only(GYR3, GYR3SENTER, &gyro3_err_prev, Kd3); // ---基本基準値 --- GS = MAIN0; //---GYR3によるラダーポイントの移動 RUD=RUD+GYR3-GYR3SENTER+rud_offset; // --- ヒステリシス条件 --- if (ELE > (ELESENTER + 360)) { h = 2; } else if (ELE < (ELESENTER + 350)) { h = 1; } // --- 通常／反転モード分岐（ABKオリジナル動作を保持）--- if(h > 1) { //----反転---- //---RUD操舵により尾を下げる　反転時は少ない---- //----ジャイロ制御------ if (GYR3 > GYR3SENTER) { int16_t GG20 = GYR3 - GYR3SENTER; int16_t GG21 = GG20 >> 3; int16_t GG22 = GG20 >> 2; GS = GS + GG21 + GG22; } else { int16_t GG20 = GYR3SENTER - GYR3; int16_t GG21 = GG20 >> 3; int16_t GG22 = GG20 >> 2; GS = GS + GG21 + GG22; } //----ラダー操舵による下げ-------------------------- if (RUDSENTER < RUD) { G0 = RUD - RUDSENTER; G1 = G0 >> 3; G2 = G0 >> 2; GS = GS + G1 + G2; } else { G0 = RUDSENTER - RUD; G1 = G0 >> 3; G2 = G0 >> 2; GS = GS + G1 + G2; } } else { //---RUD操舵により尾を下げる---- //----ジャイロ制御------ if (GYR3 > GYR3SENTER) { int16_t GG20 = GYR3 - GYR3SENTER; int16_t GG21 = GG20 >> 1; GS = GS + GG21; } else { int16_t GG20 = GYR3SENTER - GYR3; int16_t GG21 = GG20 >> 1; GS = GS + GG21; } //----ラダー操舵による下げ-------------------------- if (RUDSENTER < RUD) { G0 = RUD - RUDSENTER; G1 = G0 >> 1; GS = GS + G1; } else { G0 = RUDSENTER - RUD; G1 = G0 >> 1; GS = GS + G1; } } GS = clip_range(GS, 4000, 8000); // 140%60度範囲制限 out_PWM(GS); } // ------------------------------------------------------------------ // 各チャネル読み取り関数（GYR2はコメントアウト） // ------------------------------------------------------------------ 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);} // ※GYR2未使用 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);} // ------------------------------------------------------------------ // メインループ（GYR2読み取りをコメントアウト） // ------------------------------------------------------------------ void main(void) { p_initialize(); PWM_Initialize(); out_Initialize(); __delay_ms(10000); // ジャイロ安定待ち RUDSENTER = read_0(); //GYR2SENTER = read_2(); // ※GYR2未使用 GYR3SENTER = read_3(); MAINSENTER = read_4(); ELESENTER = read_5(); while(1) { RUD = read_0(); //GYR2 = read_2(); // ※GYR2未使用 GYR3 = read_3(); MAIN0 = read_4(); ELE = read_5(); out_steering(); } }