新システム設計
ハーフブリッジICの動作も確認できたので、2018年版の 新システムを設計します。 ブロック図でまとめると、以下。GBC(Game Boy Camera)とArduinoを接続し 路面センサーの情報処理は、Arduinoへと 丸投げ。移動は、２個のDCモータを利用。 Arduinoは、Processorに対して、３ビットで 路面情報を渡します。 ３ビットの内訳は、以下。 #define ALL_WHITE 0 #define LEFT 1 #define BOTH_BLACK 2 #define RIGHT_WHITE 3 #define RIGHT 4 #define CENTER 5 #define LEFT_WHITE 6 #define ALL_BLACK 7 他にスタートゲートセンサーを利用するので 論理値を決めておきます。 #define CLOSED 0 #define OPENED CLOSED+1 仕様を決めたならば、システムとしてどう動くのかを RTOS(Real Time Operating System)の利用を前提にし 考えていきます。 自作のRTOSであるUSOを利用して、５タスクで 仮システム動作を記述。 タスク０からタスク４の担当機能を、以下としました。
- タスク０ システム状態の初期化とシリアル通信
- タスク１ スタートゲート処理
- タスク２ 移動処理
- タスク３ タイムアップ監視
- タスク４ 移動中断と初期化
ポート１から４があるので、Arduino、スタートゲート
センサーおよびハーフブリッジICに接続するポートを
決めておきます。
- ポート２ Arduino
- ポート２ スタートゲートセンサー
- ポート３ ハーフブリッジIC
フォトカプラには、TLP521-1相当品を４個使います。
ポート２は、スタートゲートセンサーと接続しますが
このセンサーには、超音波センサーを利用。
ポート３は、ハーフブリッジICと接続しますが
ディスクリートで組んだ回路を挟んでおきます。
インタフェースを決めたので、センサーとアクチュエータ
のための関数を定義していきます。
路面センサー処理
路面センサーの情報は、４ビットでArudinoが出力して
くるので、ポート２の４ビットにフォトカプラを介して
接続していると、次の関数で対応。
UBYTE get_sensor(void)
{
UBYTE result;
ULONG tmp ;
/* set direction */
GP2CON = 0x00000000 ;
GP2DAT = 0xf0000000 ;
/* get */
tmp = GP2DAT ;
/* separate */
result = (UBYTE)(tmp & 0x0f);
return result ;
}
スタートゲートセンサー処理
スタートゲートセンサーでは、パルス幅で距離を示すので
PIC12F1501を使って、論理値を出力することにします。
回路は、以下。
ファームウエアは、以下。
/* redefine data type */
typedef unsigned char UBYTE ;
typedef unsigned int UWORD ;
typedef unsigned long ULONG ;
typedef union {
struct {
unsigned B0:1;
unsigned B1:1;
unsigned B2:1;
unsigned B3:1;
unsigned B4:1;
unsigned B5:1;
unsigned B6:1;
unsigned B7:1;
} BIT ;
unsigned char DR ;
} FLAGSP ;
volatile FLAGSP xflags ;
volatile UBYTE xcnt ;
volatile UBYTE dh ;
volatile UBYTE dl ;
volatile UWORD xtim ;
volatile UBYTE xlen ;
#define EFLAG xflags.BIT.B0
#define SFLAG xflags.BIT.B1
#define OFLAG xflags.BIT.B2
#define OFF 0
#define ON OFF+1
#define CNTBEGIN 6
#define DEFX 588
#define TOVER PORTA.F1
#define ECHO PORTA.F4
#define TRG PORTA.F5
/* function prototype */
void init_usr(void);
void dac_write(UBYTE x);
void dac_init(void);
/* interrupt handler */
void interrupt(void)
{
/* generate trigger 1ms interval */
if ( INTCON.T0IF == ON ) {
/* clear flag */
INTCON.T0IF = OFF ;
/* initialize */
TMR0 = CNTBEGIN ;
/* increment */
xcnt++ ;
/* judge */
if ( xcnt == 100 ) {
xcnt = 0 ;
EFLAG = ON ;
}
}
/* echo handling */
if ( PIR1.TMR1GIF == ON ) {
/* clear flag */
PIR1.TMR1GIF = OFF ;
/* set flag */
SFLAG = ON ;
}
/* echo handling (overflow) */
if ( PIR1.TMR1IF == ON ) {
/* clear flag */
PIR1.TMR1IF = OFF ;
/* set flag */
OFLAG = ON ;
}
}
void main(void)
{
UBYTE i ;
/* user initialize */
init_usr();
/* endless loop */
while ( ON ) {
/* sensor handling */
if ( SFLAG == ON ) {
/* clear flag */
SFLAG = OFF ;
/* stop timer1 */
T1CON.TMR1ON = OFF ;
/* get timer counter */
dl = TMR1L ;
dh = TMR1H ;
xtim = dh * 256 + dl ;
/* calculate distance 100mm : 588 us */
xlen = (UBYTE)((32.0 * xtim) / DEFX) ;
/* upper limiter */
if ( xlen > 21 ) { xlen = 21 ; }
/* calculate voltage */
dac_write(xlen);
}
/* sensor handling (overflow) */
if ( OFLAG == ON ) {
/* clear flag */
OFLAG = OFF ;
/* impress */
TOVER = ON ;
}
/* execute */
if ( EFLAG == ON ) {
/* clear flag */
EFLAG = OFF ;
/* clear */
TMR1L = 0 ;
TMR1H = 0 ;
/* start timer1 */
T1CON.TMR1ON = ON ;
/* indicator */
TOVER = OFF ;
/* send start trigger */
TRG = ON ;
for ( i = 0 ; i < 160 ; i++ ) ;
TRG = OFF ;
}
}
}
/* define function body */
void init_usr(void)
{
/* select 16MHz */
OSCCON = (15 << 3) | 0x03 ;
/* disable A/D converter */
ADCON0.ADON = OFF ;
ADCON2 = 0 ;
ANSELA = 0 ;
/* initialize D/A converter */
dac_init();
/* disable compare module */
CM1CON0.C1ON = OFF ;
CM1CON0.C1OE = OFF ;
/* I/O state */
PORTA = 0x00 ;
/* I/O directions */
TRISA = 0x1C ; /* bit0,1,5 as output , others as input */
/* pull up */
WPUA = 0x10 ;
/* initialize timer 0 */
{
/*
16MHz/4 = 4MHz (Fosc)
Fosc/4 = 1MHz
1MHz/4 = 250kHz prescaler = 1:4
*/
OPTION_REG = 0x01 ;
/* 256 - 250 = 6 */
TMR0 = CNTBEGIN ;
/* enable timer0 overflow interrupt */
INTCON.TMR0IE = ON ;
}
/* initialize timer 1 */
{
T1CON = 0x24 ;
T1GCON = 0xc0 ;
/* select RA4 */
APFCON.T1GSEL = OFF ;
/* enable peripheral TMR1GE */
PIE1.TMR1GE = ON ;
/* enable TMR1 overflow interrupt */
PIE1.TMR1IE = ON ;
/* enable peripheral interrupt */
INTCON.PEIE = ON ;
}
/* enable general interrupt */
INTCON.GIE = ON ;
/* clear flags */
xflags.DR = 0 ;
/* initialize variables */
xcnt = 0 ;
xtim = 0 ;
xlen = 0 ;
}
void dac_write(UBYTE x)
{
DACCON1 = (0x1f & x);
}
void dac_init(void)
{
/* reference Vdd */
DACCON0.DACPSS = OFF ;
/* enable DAC output #1 */
DACCON0.DACOE1 = ON ;
/* use DAC */
DACCON0.DACEN = ON ;
}
アクチュエータ処理
路面センサーが出力してくる情報は、以下なので
これらの情報が与えられたときの動作を考えます。
#define ALL_WHITE 0
#define LEFT 1
#define BOTH_BLACK 2
#define RIGHT_WHITE 3
#define RIGHT 4
#define CENTER 5
#define LEFT_WHITE 6
#define ALL_BLACK 7
AWKで、どう動かすのかを見ておきます。
# usage
# jgawk -f tty.awk ttyy.txt{enter}
function dir(x) {
# default
result = "straight"
# convert
xx = x % 8
# judge
if ( xx == 1 ) { result = "left" }
if ( xx == 4 ) { result = "right" }
return result
}
{
x = $1 % 8
printf("%3d : %d => %s\n",$1,x,dir($1))
}
文字列で、どうなるのかをテストすると以下。
状態を変化させ、ドライバICに与えるPWM波形
のDUTY比を変えます。状態遷移図を利用して
どうなればよいのかを考えます。
状態遷移図から、次のように考えました。
クランク、レーンチェンジに入ったときには
直進のスピードを変化させる。
スピードを変化させるには、係数を
乗算して、DUTY比を変更します。
係数をパーセンテージで表現すると、以下。
NONE 0
NORMAL 100
CRANK 80
ROTATE 50
LANE 90
CHANGE 60
BLIND 70
センサー情報から、DUTY比を決定し、モードを
判定して、係数を乗算して、ドライバICに出力
するPWM波形のDUTY比を決定します。
センサー情報から、DUTY比を決定する関数は以下。
UWORD get_sensor(UBYTE xsensor,UBYTE xmode)
{
UBYTE rrx ;
UBYTE llx ;
UBYTE xmul ;
UWORD result ;
/* generate duty ratio */
rrx = 80 ;
llx = 80 ;
if ( xsensor == 1 ) {
rrx = 90 ;
llx = 70 ;
}
if ( xsensor == 4 ) {
rrx = 70 ;
llx = 90 ;
}
/* set coefficient */
xmul = 100 ;
if ( xmode == CRANK ) { xmul = 80 ; }
if ( xmode == ROTATE ) { xmul = 50 ; }
if ( xmode == LANE ) { xmul = 90 ; }
if ( xmode == CHANGE ) { xmul = 60 ; }
if ( xmode == BLIND ) { xmul = 70 ; }
/* adjust duty ratio */
rrx = (xmul * rrx) / 100 ;
llx = (xmul * llx) / 100 ;
/* concatenate */
result = (llx << 8) | rrx ;
return result ;
}
目次 前 次