PIC32で自動運転
次は自動運転です。まずはブレッドボードでやってみます。
【3区間での自動往復運転】
・レールにギャップを入れて３つの区間に分け、それぞれの区間を別のドライブ回路で駆動します。両端の区間に差し掛かったことを検知して列車を減速、停止後に逆転し、この動作を繰り返します。
電源部に使用している4556はオーディオ用のOPアンプですが、出力電流が73ｍAと大きく、これでPIC32用電源3.3Vの仮想GNDを作ります。PIC32のGNDを１２V電源のGNDから4.35V上げることにより、PICのPortの電圧の中心を６Vにしています。PortがInPortのときハイインピーダンスでOFF、OutPortで出力1のとき正転、０の時逆転します。共通側のRB12はその反対に正転で０、逆転で１にします。
PIC32のコード
前回と同様にMPLABでプロジェクトを作ります。名前は”3sections”にしました。
Harmonyの設定
・コードの編集前に文字コードをShift-Jisに変更します。
・３つのファイルをプロジェクトに追加します。
- atc.h
- timer_int.c
- lcd.c
コードのダウンロード 3sections
・まずヘッダーファイル’atc.h’を追加します。
作ったプロジェクトのあるフォルダー 3sections(プロジェクト名)-firmware-src のフォルダーに追加するファイルを入れておいて、Header Filesのappを右クリック、Add Existing Itemでプロジェクトに追加します。
・’timer_int.c’ と’lcd.c’ はSource Filesのappを右クリック、Add Existing Itemでプロジェクトに追加します。
または、New->xc32_header.h…で新規に作成し、名前を’atc.h’にします。出来たコードをすべて削除し、以下のコードをコピペします。’timer_int.c’と’lcd.c’も同様にNew->xc32_newfile.cで作成します。
atc.h
//I2C用変数
DRV_HANDLE hi2c;
DRV_I2C_BUFFER_HANDLE bhi2c;
//16bitのINT型の上位H byte、下位L byte読み込み用共用体
union byte_access {
int INT; // Int Access
struct { // byte Access
unsigned char L;
unsigned char H;
} BYTE;
};
//位置情報の構造体
struct PSI_BIT {
unsigned char KUKAN : 3; /* 区間 Bit 0-2 */
unsigned char ROSEN : 3; /* 路線 Bit 3-5 */
unsigned char SOU : 1; /* 相 Bit 6 */
unsigned char DIR : 1; /* 方向 Bit 7 */
};
struct STATUS_BIT {
unsigned char CHG : 1; //区間変化
unsigned char REV : 1; //逆転
unsigned char DUMY : 4; //DUMY
unsigned char SLOW : 1; //減速
unsigned char SAFE : 1; //安全
};
//列車の位置情報の構造体
struct st_position {
//現在位置
union { /* Position */
unsigned char BYTE; /* Byte Access */
struct PSI_BIT BIT; /* Bit Access */
} NOW;
//前の位置
union { /* Position */
unsigned char BYTE; /* Byte Access */
struct PSI_BIT BIT; /* Bit Access */
} BEFORE;
//次の位置
union { /* Position */
unsigned char BYTE; /* Byte Access */
struct PSI_BIT BIT; /* Bit Access */
} NEXT;
//次々の位置
union { /* Position */
unsigned char BYTE; /* Byte Access */
struct PSI_BIT BIT; /* Bit Access */
} ANEXT;
union {
unsigned char BYTE; /* Byte Access */
struct STATUS_BIT BIT; /* Bit Access */
} STATUS;
int speed;
//int speed_cont;
union byte_access speed_cont;
int speed_err;
int speed_peak;
int mascon;
unsigned char henka;
//unsigned char safe;
union byte_access speed_ret; //読み取りスピード
union byte_access speed_ret_rx; //読み取りスピード
union byte_access speed_ret_tx; //読み取りスピード
unsigned char point;
unsigned char yard;
union byte_access speed_rx;
};
//監視区間設定用の共用体
union scan_port {
unsigned char BYTE; /* Byte Access */
struct {
unsigned char DUMMY : 2; /* Bit 0-1 */
unsigned char KUKAN : 3; /* Bit 2-4 監視区間*/
unsigned char DISABLE : 1; /* Bit 5 */
unsigned char B6 : 1; /* Bit 6 */
} BIT;
};
#define TR_COUNT 1 //列車数
#define ROSEN_NUM 0 //路線（ボード）番号
unsigned char EnableBit[] = {1, 2, 4, 8, 16, 32, 64, 128};
unsigned int int_counter; //インタラプトカウンタ
unsigned int speed_pw[2]; //スピードパルス幅
struct st_position train[TR_COUNT]; //列車位置情報
struct st_position* train_sou[2];
unsigned char cont_train[2]; //制御する列車番号
unsigned char kukan[2]; //区間
unsigned char kanshi_now[2]; //スピード読み込み用
unsigned char kanshi_before[2]; //スピード読み込み用
union scan_port kanshi; //区間監視用
union byte_access adc_ret; //ADC値
union byte_access debug1; //ADC値
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 timer_int_func() {
//インタラプトを0-0x1ffでカウント
if (++int_counter > 0x1ff) {
int_counter = 0;
}
switch (int_counter) {
case 0:
//インタラプトカウンタが０のときA相のspeedパルスON
train_sou[0] = &train[cont_train[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;
}
//スピードパルスOFF
speed_pulse_off();
}
void speed_pulse_on(unsigned char sou) {
if (train_sou[sou]) {
kukan[sou] = 0; //kukan clr
speed_pw[sou] = train_sou[sou]->speed;
//区間メモリに現・前位置の区間を設定し区間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) {
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
kukan_OFF(kukan[n]);
} else {
speed_pw[n]--; //speedをマイナス1
}
}
}
}
void section_change_monitor(unsigned char sou) {
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 (sou == 0)debug1.INT = adc_ret.INT;
if (train_sou[sou]->NOW.BIT.DIR == 0) {
//正方向のとき
if (adc_ret.BYTE.H >= 0x3) {
train_sou[sou]->henka = 0x80; //区間変化
}
} else {
//逆方向のとき
if (adc_ret.BYTE.H < 0x1) {
train_sou[sou]->henka = 0x80; //区間変化
}
}
}
}
}
}
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);
}
}
}
/* *****************************************************************************
End of File
*/
lcd.c
#include "system/common/sys_common.h"
#include "app.h"
#include "system_definitions.h"
#define ADR 0x7C
void LCD_cmd(char);
void LCD_int(void);
void LCD_str(char *);
void LCD_dat(char);
void LCD_hex(char);
void LCD_posyx(char, char);
void BSP_DelayMs(unsigned short);
void BSP_DelayUs(unsigned short);
uint8_t data[5];
//遅延調整用関数（ｍｓ）
void BSP_DelayMs(unsigned short milliseconds) {
unsigned long time;
time = _CP0_GET_COUNT(); //Read Core Timer
time += (SYS_CLK_FREQ / 2 / 1000) * milliseconds; //calc the Stop Time
while ((long) (time - _CP0_GET_COUNT()) > 0) {
};
}
//遅延調整用関数（μｓ）
void BSP_DelayUs(unsigned short microseconds) {
unsigned long time;
time = _CP0_GET_COUNT(); //Read Core Timer
time += (SYS_CLK_FREQ / 2 / 1000000) * microseconds; //calc the Stop Time
while ((long) (time - _CP0_GET_COUNT()) > 0) {
};
}
void LCD_cmd(char cmd) {
data[0] = 0x80;
data[1] = cmd;
bhi2c = DRV_I2C_Transmit(hi2c, ADR, data, 2, NULL);//I2C送信
while (DRV_I2C_TransferStatusGet(hi2c, bhi2c) != DRV_I2C_BUFFER_EVENT_COMPLETE); /*I2C完了待ち*/
if (cmd & 0xFC) // LCDのコマンドにより待ち時間が違う
BSP_DelayUs(60); // 60usec
else
BSP_DelayMs(3); // 3msec
}
void LCD_int(void) {
BSP_DelayMs(100);
LCD_cmd(0x38);
LCD_cmd(0x39);
LCD_cmd(0x14);
LCD_cmd(0x73); //7A(forAQM1602)
LCD_cmd(0x56); //54(forAQM1602)
LCD_cmd(0x6C);
BSP_DelayMs(200);
LCD_cmd(0x0C); //Disp ON/OFF
LCD_cmd(0x01);// Clear Display
BSP_DelayUs(1100);
}
//文字列表示
void LCD_str(char *str) {
while (*str) //0x00まで繰り返し
LCD_dat(*str++); //１文字表示
}
//１文字表示
void LCD_dat(char chr) {
data[0] = 0x40;
data[1] = chr;
//I2C送信
bhi2c = DRV_I2C_Transmit(hi2c, ADR, data, 2, NULL);
while (DRV_I2C_TransferStatusGet(hi2c, bhi2c) != DRV_I2C_BUFFER_EVENT_COMPLETE);
BSP_DelayUs(60); // 60usec
}
//-------- １６進文字変換表示 ----------------
void LCD_hex(char c) {
const char hexch[] = "0123456789ABCDEF";
LCD_dat(hexch[c >> 4]); //上位4bit表示
LCD_dat(hexch[c & 0xF]); //下位4bit表示
}
//--------３桁 １０進文字表示 ----------------
void LCD_dec(int i) {
const char decch[] = "0123456789 ";
char n[3];
n[2] = i / 100;
n[1] = (i % 100) / 10;
n[0] = i % 10;
if (n[2] == 0){
n[2] = 10;//100の桁は０ならスペース
if (n[1] == 0){
n[1] = 10;//100の桁０で10の桁０ならスペース
}
}
LCD_dat(decch[n[2]]);
LCD_dat(decch[n[1]]);
LCD_dat(decch[n[0]]);
}
//-------- カーソル位置指定 --------------------------------------
void LCD_posyx(char ypos, char xpos) {
unsigned char pcode;
switch (ypos & 0x03) { // 縦位置を取得
case 0: pcode = 0x80;
break; // 1行目
case 1: pcode = 0xC0;
break; // 2行目
case 2: pcode = 0x94;
break; // 3行目
case 3: pcode = 0xD4;
break; // 4行目
}
LCD_cmd(pcode += xpos); // 横位置を加える
}
/*******************************************************************************
End of File
*/
・次に
‘app.h’
‘app.c’
‘system_interrupt.c’
を編集します。
app.hの編集
#include “atc.h”を追加します。
// ***************************************************************************** // ***************************************************************************** // Section: Included Files // ***************************************************************************** // ***************************************************************************** #include <stdint.h> #include <stdbool.h> #include <stddef.h> #include <stdlib.h> #include "system_config.h" #include "system_definitions.h" #include "atc.h"//この行を追加 // DOM-IGNORE-BEGIN
app.cの編集
void APP_Tasks ( void )以下の部分をすべて以下のコードにすげ替えれば大丈夫だと思います。
/******************************************************************************
Function:
void APP_Tasks ( void )
Remarks:
See prototype in app.h.
*/
void section_chg(unsigned char);
void reverse(unsigned char);
void accel(char, int);
int slow_down(char);
int stoppage_time(int);
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("3section");
}
//-----ATC-------
cont_train[0] = 0;
cont_train[1] = 1;
train_sou[0] = NULL;
train_sou[1] = NULL;
//列車A相
train[0].speed = 0x0;
train[0].mascon = 0x80; //スピードを変える時はここを変更
train[0].NOW.BYTE = 3; //始めの列車位置は区間３
train[0].BEFORE.BYTE = 2;
train[0].NEXT.BYTE = 4;
train[0].ANEXT.BYTE = 5;
train[0].henka = 0;
train[0].STATUS.BIT.SAFE = 1;
if (appInitialized) {
appData.state = APP_STATE_SERVICE_TASKS;
}
break;
}
case APP_STATE_SERVICE_TASKS:
{
BSP_DelayMs(20);
LCD_posyx(1, 0);
//LCD_hex(debug1.BYTE.H);
//LCD_hex(debug1.BYTE.L);
LCD_str("N");
LCD_hex(train[0].NOW.BYTE);
LCD_str(" X");
LCD_hex(train[0].NEXT.BYTE);
//区間変化
char n;
for (n = 0; n < TR_COUNT; n++) {
if (train[n].henka == 0x80) {
train[n].henka = 0;
//区間が変わったら区間変更し、逆転
section_chg(n);
train[n].STATUS.BIT.REV = 1; //逆転
}
}
for (n = 0; n < TR_COUNT; n++) {
if (train[n].NOW.BYTE != 0x38) {
if (train[n].STATUS.BIT.REV == 1) {
//逆転命令
//減速
if (slow_down(n) == 0) {
//完全停止
if (stoppage_time(100) == 0) {
//一時停車後逆転
reverse(n);
}
}
} else {
if (train[n].STATUS.BIT.SAFE == 1) {
//安全
if (train[n].STATUS.BIT.SLOW == 1) {
//SLOW
if (train[n].mascon > 2) {
train[n].speed = 2;
} else {
train[n].speed = train[n].mascon;
}
} else {
//加速
accel(n, train[n].mascon);
}
} else {
//停止
train[n].speed = 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 slow_down(char n) {
if (train[n].speed > 0)
train[n].speed--;
return train[n].speed;
}
void accel(char n, int speed) {
if (train[n].speed < speed)
train[n].speed++;
}
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;
}
int stoppage_time(int t) {
//停車時間
static int timer;
if (timer-- == 0)
timer = t; //停車時間
return timer;
}
/*******************************************************************************
End of File
*/
system_interrupt.cの編集
以下の部分に
timer_int_func();
の一行を追加します。
void __ISR(_TIMER_1_VECTOR, ipl7AUTO) IntHandlerDrvTmrInstance0(void)
{
PLIB_INT_SourceFlagClear(INT_ID_0,INT_SOURCE_TIMER_1);
timer_int_func();
}
以上でコードは完成です。PicKitで書き込みましょう。
