エンドレスループ　２列車同時　自動運転

次はエンドレスループを８区間に分けて、２つの列車を同時に走らせ、ぶつからないように自動運転します。

 

Harmonyの設定

Harmonyの設定は前回PIC32で自動運転と同じです。

コードの変更

前回から変更があるのはapp.cとtimer_int.cです。

app.c

前回同様、APP_Tasks以下をすげ替えます。

void section_chg(unsigned char);
void reverse(unsigned char);
int speed_up_down(char, int);
int stoppage_time(char, int);
void ini_train(void);
void train_set(unsigned char, unsigned char, unsigned char, unsigned char, unsigned char);
unsigned char safe_check(unsigned char);

void APP_Tasks(void) {

    /* Check the application's current state. */
    switch (appData.state) {
            /* Application's initial state. */
        case APP_STATE_INIT:
        {
            bool appInitialized = true;

            DRV_TMR0_Start(); //TimerStart
            DRV_ADC_Open(); //Enable ADC

            //I2C Open
            hi2c = DRV_I2C_Open(DRV_I2C_INDEX_0, DRV_IO_INTENT_READWRITE | DRV_IO_INTENT_NONBLOCKING);
            if (hi2c != DRV_HANDLE_INVALID) {
                //LCD初期化
                LCD_int();
                LCD_str("LOOP_2TR");
            }

            //-----ATC-------
            cont_train[0] = 0;
            cont_train[1] = 1;
            //cont_train[1] = 0xff;

            train_sou[0] = NULL;
            train_sou[1] = NULL;

            ini_train();

            //列車A相
            train[0].mascon = 0x1c; //スピードを変える時はここを変更
            train_set(0, 0, 1, 0, 0); //n,rosen,section,dir,sou

            //列車B相
            train[1].mascon = 0x1c; //スピードを変える時はここを変更
            train_set(1, 0, 5, 0, 1); //n,rosen,section,dir,sou

            if (appInitialized) {

                appData.state = APP_STATE_SERVICE_TASKS;
            }
            break;
        }

        case APP_STATE_SERVICE_TASKS:
        {

            BSP_DelayMs(20);

            LCD_posyx(0, 0);

//            LCD_hex(train[0].speed_ret.BYTE.H);
//            LCD_hex(train[0].speed_ret.BYTE.L);

            LCD_posyx(1, 0);
            LCD_str("N");
            LCD_hex(train[0].NOW.BYTE);
            LCD_str(" X");
            LCD_hex(train[0].NEXT.BYTE);

            //Reverse SW
            if (_RB11 == 0)
                train[0].STATUS.BIT.REV = 1; //逆転

            char n;

            for (n = 0; n < TR_COUNT; n++) {
                if (train[n].NOW.BYTE != 0x38) {
                    //安全確認
                    switch (safe_check(n)) {
                        case 1:
                            //停止
                            train[n].STATUS.BIT.SAFE = 0;
                            train[n].STATUS.BIT.SLOW = 0;
                            break;
                        case 2:
                            //減速
                            train[n].STATUS.BIT.SAFE = 1;
                            train[n].STATUS.BIT.SLOW = 1;
                            break;
                        case 3:
                            //向き合い停止
                            train[n].STATUS.BIT.SAFE = 0;
                            train[n].STATUS.BIT.SLOW = 0;
                            train[n].STATUS.BIT.REV = 1; //逆転
                            break;

                        case 0:
                            //安全
                            train[n].STATUS.BIT.SAFE = 1;
                            train[n].STATUS.BIT.SLOW = 0;
                            break;
                    }


                    if (train[n].STATUS.BIT.REV == 1) {
                        //逆転命令
                        //減速
                        if (speed_up_down(n, 0) == 0) {
                            //完全停止
                            if (stoppage_time(n, 30) == 0) {
                                //一時停車後逆転
                                reverse(n);
                                
                            }
                        }

                    } else {
                        if (train[n].STATUS.BIT.SAFE == 1) {
                            //安全
                            if (train[n].STATUS.BIT.SLOW == 1) {
                                //SLOW
                                if (train[n].mascon > 0x10) {
                                    speed_up_down(n, 0x10);
                                } else {
                                    speed_up_down(n, train[n].mascon);
                                }
                            } else {
                                //加速
                                speed_up_down(n, train[n].mascon);
                            }

                        } else {
                            //停止
                            speed_up_down(n, 0);
                        }
                    }

                }
            }

            break;
        }

            /* TODO: implement your application state machine.*/


            /* The default state should never be executed. */
        default:
        {
            /* TODO: Handle error in application's state machine. */
            break;
        }
    }
}

int speed_up_down(char n, int speed) {
    static char count[TR_COUNT];

    if (train[n].speed > speed)
        train[n].speed--;
    else {
        if (++count[n] > 4) {
            count[n] = 0;
            train[n].speed++;
        }
    }

    return train[n].speed_cont.INT;
}

void section_chg(unsigned char n) {
    train[n].BEFORE.BYTE = train[n].NOW.BYTE;
    train[n].NOW.BYTE = train[n].NEXT.BYTE;
    train[n].NEXT.BYTE = train[n].ANEXT.BYTE;

    if (train[n].NOW.BIT.DIR) {
        train[n].ANEXT.BIT.KUKAN--; //逆転
    } else {
        train[n].ANEXT.BIT.KUKAN++; //正転
    }


    //
    train[n].STATUS.BIT.CHG = 1;

}

void reverse(unsigned char n) {
    unsigned char before;

    unsigned char dir = train[n].NOW.BIT.DIR;
    if (dir) {
        dir = 0;
    } else {
        dir = 1;
    }
    train[n].NOW.BIT.DIR = dir;
    train[n].BEFORE.BIT.DIR = dir;

    before = train[n].BEFORE.BYTE;

    train[n].BEFORE.BYTE = train[n].NOW.BYTE;
    train[n].NOW.BYTE = before;
    train[n].NEXT.BYTE = before;
    train[n].ANEXT.BYTE = before;

    if (dir) {
        //逆転 
        train[n].NEXT.BIT.KUKAN--;
        train[n].ANEXT.BIT.KUKAN = train[n].NOW.BIT.KUKAN - 2;
    } else {
        //正転
        train[n].NEXT.BIT.KUKAN++;
        train[n].ANEXT.BIT.KUKAN += 2;
    }

    train[n].STATUS.BIT.REV = 0;
    train[n].STATUS.BIT.SAFE = 0;
}

int stoppage_time(char n, int t) {
    //停車時間
    static int timer[2];

    if (timer[n]-- == 0)
        timer[n] = t; //停車時間

    return timer[n];

}

void ini_train() {
    char n;

    for (n = 0; n < TR_COUNT; n++) {
        train[n].BEFORE.BYTE = 0x38;
        train[n].NOW.BYTE = 0x38;
        train[n].NEXT.BYTE = 0x38;
        train[n].ANEXT.BYTE = 0x38;
        train[n].STATUS.BYTE = 0;
        train[n].speed = 0;
        train[n].henka = 0;
    }

}

void train_set(unsigned char n, unsigned char rosen, unsigned char sec, unsigned char dir, unsigned char sou) {
    train[n].NOW.BIT.ROSEN = rosen;
    train[n].NOW.BIT.KUKAN = sec;
    train[n].NOW.BIT.DIR = dir;
    train[n].NOW.BIT.SOU = sou;

    train[n].BEFORE.BYTE = train[n].NOW.BYTE;
    train[n].NEXT.BYTE = train[n].NOW.BYTE;
    train[n].ANEXT.BYTE = train[n].NOW.BYTE;

    if (dir) {
        //逆転 
        train[n].BEFORE.BIT.KUKAN++;
        train[n].NEXT.BIT.KUKAN--;
        train[n].ANEXT.BIT.KUKAN = train[n].NOW.BIT.KUKAN - 2;
    } else {
        //正転
        train[n].BEFORE.BIT.KUKAN--;
        train[n].NEXT.BIT.KUKAN++;
        train[n].ANEXT.BIT.KUKAN += 2;
    }

    train[n].STATUS.BIT.CHG = 0;
    train[n].STATUS.BIT.SAFE = 1;

}

unsigned char safe_check(unsigned char m) {
    //安全確認
    unsigned char n;
    unsigned char ret = 0;

    for (n = 0; n < TR_COUNT; n++) {
        if (n != m) {
            //次位置が他の列車の現、前、次位置と同じなら停止
            if ((train[m].NEXT.BYTE & 0x3f) == (train[n].NOW.BYTE & 0x3f)) {
                ret = 1;
                break;
            }
            if ((train[m].NEXT.BYTE & 0x3f) == (train[n].BEFORE.BYTE & 0x3f)) {
                ret = 1;
                break;
            }
            if ((train[m].NEXT.BYTE & 0x3f) == (train[n].NEXT.BYTE & 0x3f)) {
                ret = 1;
                break;
            }

            //次々位置が他の列車の現、前、次位置と同じなら減速
            if ((train[m].ANEXT.BYTE & 0x3f) == (train[n].NOW.BYTE & 0x3f)) ret = 2;
            if ((train[m].ANEXT.BYTE & 0x3f) == (train[n].BEFORE.BYTE & 0x3f)) ret = 2;
            if ((train[m].ANEXT.BYTE & 0x3f) == (train[n].NEXT.BYTE & 0x3f)) ret = 2;


        }

    }

    //停止の時、相手と方向が逆の時は優先順位が低いほうが逆転
    if (ret == 1) {
        if ((m > n) && (train[m].NOW.BIT.DIR != train[n].NOW.BIT.DIR)) {
            ret = 3;
        }
    }

    return ret;

}
/*******************************************************************************
 End of File
 */

timer_int.c

#include "system/common/sys_common.h"
#include "app.h"
#include "system_definitions.h"


void kukan_ON(unsigned char, unsigned char);
void kukan_OFF(unsigned char);
void speed_pulse_on(unsigned char);
void speed_pulse_off();
void section_change_monitor(unsigned char); //区間変化監視
void next_section_on(unsigned char); //次区間ＯＮ
void read_generated_voltage(unsigned char, unsigned char);
void speed_cont(unsigned char);
void section_change(unsigned char);

void timer_int_func() {

    _RB10 = 1;
    //インタラプトを0-0x1ffでカウント
    if (++int_counter > 0x1ff) {
        int_counter = 0;
    }

    switch (int_counter) {
        case 0:
            //インタラプトカウンタが０のときA相のspeedパルスON
            if (cont_train[0] != 0xff) {
                train_sou[0] = &train[cont_train[0]];
                section_change(0);
                speed_pulse_on(0);
            }
            break;

        case 2:
            //インタラプトカウンタが0x2のときA相の区間変化監視(B相は0x102のとき)
            section_change_monitor(0);
            break;

        case 3:
            //インタラプトカウンタが3のときNEXT区間パルスON(B相は0x103のとき)
            //区間変化確認後に次区間もON
            next_section_on(0);
            break;

        case 0xfa:
            //スピードコントロール
            speed_cont(0);
            break;

        case 0xf8:
            //スピード読み込み
            read_generated_voltage(0, 0);
            break;

        case 0xf9:
            //スピード読み込み
            read_generated_voltage(1, 0);
            break;

        case 0x100:
            //インタラプトカウンタが0x100のときB相のspeedパルスON
            if (cont_train[1] != 0xff) {
                train_sou[1] = &train[cont_train[1]];
                section_change(1);
                speed_pulse_on(1);
            }
            break;
        case 0x102:
            //インタラプトカウンタが0x2のときA相の区間変化監視(B相は0x102のとき)
            section_change_monitor(1);
            break;

        case 0x103:
            //インタラプトカウンタが3のときNEXT区間パルスON(B相は0x103のとき)
            //区間変化確認後に次区間もON
            next_section_on(0);
            break;


        case 0x1f8:
            //スピード読み込み
            read_generated_voltage(0, 1);
            break;
        case 0x1f9:
            //スピード読み込み
            read_generated_voltage(1, 1);
            break;


        case 0x1fa:
            speed_cont(1);

            break;

    }

    //スピードパルスOFF
    speed_pulse_off();


    _RB10 = 0;
}

void speed_pulse_on(unsigned char sou) {

    if (train_sou[sou]) {
        kukan[sou] = 0; //kukan clr

        //speed_pw[sou] = train_sou[sou]->speed;
        speed_pw[sou] = train_sou[sou]->speed_cont.INT;

        //区間メモリに現・前位置の区間を設定し区間ON
        if (train_sou[sou]->NOW.BIT.ROSEN == ROSEN_NUM) {
            kukan[sou] = EnableBit[train_sou[sou]->NOW.BIT.KUKAN];
            kanshi_now[sou] = train_sou[sou]->NOW.BIT.KUKAN;
        }
        if (train_sou[sou]->BEFORE.BIT.ROSEN == ROSEN_NUM) {
            kukan[sou] |= EnableBit[train_sou[sou]->BEFORE.BIT.KUKAN];
            kanshi_before[sou] = train_sou[sou]->BEFORE.BIT.KUKAN;
        }

        // 区間ＯＮ
        kukan_ON(kukan[sou], train_sou[sou]->NOW.BIT.DIR);

    }



}

//区間ON用

void kukan_ON(unsigned char kukan, unsigned char d) {
    // d=0:正回転, 1:負回転
    if (d == 0) {
        LATB |= kukan;
        _LATB12 = 0; //Port RB12は共通側
    } else {
        LATB &= ~kukan;
        _LATB12 = 1;
    }

    TRISB &= ~kukan;
    _TRISB12 = 0;

}

//区間OFF用

void kukan_OFF(unsigned char kukan) {
    if (kukan) {
        TRISB |= kukan;
        _TRISB12 = 1;
    }
}

void speed_pulse_off() {
    //インタラプトごとにspeedをマイナス1して０になったらパルスOFF
    unsigned char n;

    for (n = 0; n < 2; n++) {

        if (train_sou[n]) {
            if (speed_pw[n] == 0) {
                //speedが０で区間パルスOFF
                if (kukan[n]) {
                    TRISB |= kukan[n];
                    if ((TRISB & 0xff) == 0xff)
                        _TRISB12 = 1;

                    kukan[n] = 0;
                }


            } else {
                speed_pw[n]--; //speedをマイナス1
            }
        }
    }
}

void section_change_monitor(unsigned char sou) {
    static char sum_count;
    static int gnd_sum;


    if (train_sou[sou]) {
        if (train_sou[sou]->STATUS.BIT.SAFE)
            if (!train_sou[sou]->henka) {
                if (train_sou[sou]->NEXT.BIT.ROSEN == ROSEN_NUM) {
                    //次位置区間の監視
                    kanshi.BIT.KUKAN = train_sou[sou]->NEXT.BIT.KUKAN;
                    LATA &= 0xC3;
                    LATA |= kanshi.BYTE;

                    //ADC読み込み

                    PLIB_ADC_SamplingStart(ADC_ID_1);
                    while (!PLIB_ADC_ConversionHasCompleted(ADC_ID_1));
                    adc_ret.INT = PLIB_ADC_ResultGetByIndex(ADC_ID_1, 0);



                    if (train_sou[sou]->NOW.BIT.DIR == 0) {
                        //正方向のとき
                        if (adc_ret.INT >= 0x2c0) {
                            train_sou[sou]->henka = 0x80; //区間変化
                            if (sou == 0)debug1.INT = adc_ret.INT;
                        }

                    } else {
                        //逆方向のとき
                        if (adc_ret.INT < 0x140) {
                            train_sou[sou]->henka = 0x80; //区間変化
                            if (sou == 0)debug1.INT = adc_ret.INT;
                        }
                    }

                    //区間変化でないときGND値とする。
                    if (!train_sou[sou]->henka) {
                        gnd_adc = adc_ret.INT;

                        //区間変更なしのときのADC値をGND Levelにします。
                        //16回たして4bit右シフトで平均をとります。
                        gnd_sum += adc_ret.INT;
                        if (sum_count++ == 15) {
                            gnd_level.INT = gnd_sum >> 4;
                            sum_count = 0;
                            gnd_sum = 0;
                        }

                    }

                }
            }
    }

}

void next_section_on(unsigned char sou) {
    //次区間をON
    if (train_sou[sou]) {
        if (train_sou[sou]->STATUS.BIT.SAFE) {
            //安全でないときは次区間に他の列車がかかっているのでONしない。
            if (train_sou[sou]->NEXT.BIT.ROSEN == ROSEN_NUM)
                kukan[sou] |= EnableBit[train_sou[sou]->NEXT.BIT.KUKAN];

            kukan_ON(kukan[sou], train_sou[sou]->NOW.BIT.DIR);
        }

    }
}

void read_generated_voltage(unsigned char n, unsigned char sou) {
    static char count[2], p[2];
    static int speed_buf[2][16];
    static int read_speed[2];


    if (train_sou[sou]) {

        //現位置と前位置を交互に読み込み
        if (n == 0) {
            kanshi.BIT.KUKAN = kanshi_now[sou];
        } else {
            kanshi.BIT.KUKAN = kanshi_before[sou];
        }

        LATA &= 0xC3;
        LATA |= kanshi.BYTE;

        //ADC読み込み
        PLIB_ADC_SamplingStart(ADC_ID_1);
        while (!PLIB_ADC_ConversionHasCompleted(ADC_ID_1));
        adc_ret.INT = PLIB_ADC_ResultGetByIndex(ADC_ID_1, 0);

        //ADC値からGND値を引いて絶対値をとります。
        adc_ret.INT = adc_ret.INT - gnd_level.INT;

        if (adc_ret.INT < 0)
            adc_ret.INT = -adc_ret.INT;


        //8回のピーク値をスピードデータにします。
        if (count[sou]++ == 7) {
            read_speed[sou] = adc_ret.INT;
            count[sou] = 0;
        } else if (adc_ret.INT > read_speed[sou])
            read_speed[sou] = adc_ret.INT;

        //train_sou[sou]->speed_peak = read_speed;

        speed_buf[sou][++p[sou] & 0xf] = read_speed[sou];

        //移動平均
        char m;
        unsigned int avr = 0;

        for (m = 0; m < 16; m++) {
            avr += speed_buf[sou][m];
        }

        train_sou[sou]->speed_ret.INT = avr >> 4;
    }
}

void speed_cont(unsigned char sou) {
    int err; //スピードのエラー
    int err_abs; //err絶対値
    int err_def; //errの差
    int k1 = 0x55; //感度１ 0x10
    int d; //エラー補正値

    static char count[2];

    if (!train_sou[sou])return;

    // スピード設定値と実際スピードの比較
    err = train_sou[sou]->speed - train_sou[sou]->speed_ret.INT;

    //エラーの絶対値をとります
    if (err < 0)
        err_abs = -err;
    else
        err_abs = err;

    //前回のエラーと比較
    err_def = err_abs - train_sou[sou]->speed_err;
    train_sou[sou]->speed_err = err_abs;

    k1 += err_def;

    //エラー補正値の計算
    d = (err * k1) / 0x100;

    //スピード設定が０のとき補正値が負でない場合
    //完全に停止させるため補正値を-1にします
    if (train_sou[sou]->speed == 0) {
        if (d>-0x5)
            d = -0x5;
    }


    //スピードコントロール値をエラー補正値で補正
    if (++count[sou] > 10) {
        count[sou] = 0;
        train_sou[sou]->speed_cont.INT += d;
    } else {
        return;
    }



    //結果が負にならないようにする
    if (train_sou[sou]->speed_cont.INT < 0)
        train_sou[sou]->speed_cont.INT = 0;

    //接触不良時の制限(0x90)
    if (train_sou[sou]->speed_ret.INT < 0x8)
        if (train_sou[sou]->speed_cont.INT > 0x90)
            train_sou[sou]->speed_cont.INT = 0x90;

    //Speedの最大値を制限(0x1F0)
    if (train_sou[sou]->speed_cont.INT > 0xc0) //1f0
        train_sou[sou]->speed_cont.INT = 0xc0;


}

void section_change(unsigned char sou) {
    if (!train_sou[sou])return;

    if (train_sou[sou]->henka == 0x80) {
        train_sou[sou]->BEFORE.BYTE = train_sou[sou]->NOW.BYTE;
        train_sou[sou]->NOW.BYTE = train_sou[sou]->NEXT.BYTE;
        train_sou[sou]->NEXT.BYTE = train_sou[sou]->ANEXT.BYTE;

        if (train_sou[sou]->NOW.BIT.DIR) {
            train_sou[sou]->ANEXT.BIT.KUKAN--; //逆転
        } else {
            train_sou[sou]->ANEXT.BIT.KUKAN++; //正転
        }

        train_sou[sou]->STATUS.BIT.CHG = 1;
        train_sou[sou]->henka = 0;
    }


}


/* *****************************************************************************
 End of File
 */

(注意)PICKit３で書き込む時は、レールから列車をどけるか、共通側の線をはずしてください。

【動作】

・始めの列車の位置は区間１(列車１)と区間５(列車２)です。その区間に列車を置いて電源を入れると両方の列車が徐々に加速します。そして列車同士が近づくと徐行、停止します。

・逆転SWを押すと列車１が減速、停止後逆転し、列車２は列車１の方向に強制され、続いて逆転し、両方の列車の方向が変わります。