目次
前
次
パルスモータチェック
MCRで用意されているコースを正確にトレースするには
位置決めを確実にできるパルスモータを利用するのが
最もよいはずです。
アマチュア無線の知人から、１２Ｖ電源で動くパルスモータと
ギアボックスを８個貰ったので、動作チェックしてみます。
マイコン、CPLF/FPGAの出力できるパルス波高値は３Ｖから５Ｖ
くらいなので、そのままではパルスモータの励磁は不可能です。
パワートランジスタを利用したテスト用ドライバがあったので
接続してみました。
回路は単純で、以下のようになっています。
パルスモータの励磁には、１相、２相、１−２相があるので
簡単な１相励磁を実現する回路をVHDLで定義して作ります。
タイミングチャートは、以下。
タイミングチャートから、発振器、カウンタ、デコーダの３
ブロックがあればよいとして、VHDLコードを記述します。
デコーダ
１相励磁では、A、B、nA、nBの４ワイヤーに順次
パルスを出力して行けばよいので、カウンタ値を
入力としたデコーダで、パルスを出力します。
２→８→１→４と出力します。
タイミングチャートでは、８→４→２→１となって
いますが、配線の関係で２→８→１→４とします。
iPOUT <= X"8" when ( iPCNT = 1 ) else
X"1" when ( iPCNT = 2 ) else
X"4" when ( iPCNT = 3 ) else
X"2" ;
カウンタ
１相励磁では、４パターンのパルス出力となるので
４進カウンタを使います。バイナリカウンタにて
定義しています。
process (nRESET,iCLKX)
begin
if ( nRESET = '0' ) then
iPCNT <= 0 ;
elsif rising_edge(iCLKX) then
if ( iPCNT = CNTPMAX ) then
iPCNT <= 0 ;
else
iPCNT <= iPCNT + 1 ;
end if ;
end if ;
end process ;
ハザードが心配になる高周波数では、シフトレジスタに
よるジョンソンカウンタにします。
デコーダ値とカウント値の対応を再定義するだけなので
モータが動いてから、修正します。
カウンタクロックは、３種類用意し、DIPスイッチで
選べるようにします。
-- generate clock
process (iCLK)
begin
if rising_edge(iCLK) then
if ( iCNTX = CNTXMAX ) then
iCNTX <= 0 ;
else
iCNTX <= iCNTX + 1 ;
end if ;
end if ;
end process ;
iCLKHML(3) <= '1' when ( iCNTX < 2 ) else '0' ;
iCLKHML(2) <= '1' when ( iCNTX < 4 ) else '0' ;
iCLKHML(1) <= '1' when ( iCNTX = 0 ) else '0' ;
iCLKHML(0) <= '1' when ( iCNTX < 2 ) else '0' ;
-- select clock
iCLKX <= iCLKHML( conv_integer( SEL ) ) ;
発振器
パルスを生成するために、クロックが必要です。
このクロックは、マスタークロック４ＭＨｚを
分周して生成します。
process (CLOCK)
begin
if rising_edge(CLOCK) then
if ( iCNT = CNTMAX ) then
iCNT <= 0 ;
else
iCNT <= iCNT + 1 ;
end if ;
end if ;
end process ;
iCLK <= '1' when ( iCNT < CNTHALF ) else '0' ;
分周比はパルスモータが脱調しない範囲で
次の値を選択しました。
CONSTANT CNTMAX : integer := 3999 ;
CONSTANT CNTHALF : integer := 2000 ;
VHDLコードにまとめると、以下。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity pm is
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ; -- 4MHz
-- enable
XENA :in std_logic ;
-- selector
SEL : in std_logic_vector(1 downto 0) ;
-- monitor
MCLK : out std_logic ;
-- pulse out
POUT : out std_logic_vector(7 downto 0) --;
);
end pm ;
architecture behavioral of pm is
--
CONSTANT CNTMAX : integer := 3999 ;
CONSTANT CNTHALF : integer := 2000 ;
CONSTANT CNTXMAX : integer := 7 ;
CONSTANT CNTPMAX : integer := 3 ;
-- divider
signal iCNT : integer range 0 to CNTMAX ;
signal iCLK : std_logic ;
signal iCNTX : integer range 0 to CNTXMAX ;
signal iCLKX : std_logic ;
signal iCLKHML : std_logic_vector(3 downto 0) ;
-- decode
signal iPOUT : std_logic_vector(3 downto 0) ;
signal iPCNT : integer range 0 to CNTPMAX ;
begin
-- output
POUT <= iPOUT & iPOUT when ( XENA = '1' ) else X"00" ;
MCLK <= iCLKX when ( nRESET = '1' ) else '0' ;
-- divider (generate 400Hz)
process (CLOCK)
begin
if rising_edge(CLOCK) then
if ( iCNT = CNTMAX ) then
iCNT <= 0 ;
else
iCNT <= iCNT + 1 ;
end if ;
end if ;
end process ;
iCLK <= '1' when ( iCNT < CNTHALF ) else '0' ;
-- generate clock
process (iCLK)
begin
if rising_edge(iCLK) then
if ( iCNTX = CNTXMAX ) then
iCNTX <= 0 ;
else
iCNTX <= iCNTX + 1 ;
end if ;
end if ;
end process ;
iCLKHML(3) <= '1' when ( iCNTX < 2 ) else '0' ;
iCLKHML(2) <= '1' when ( iCNTX < 4 ) else '0' ;
iCLKHML(1) <= '1' when ( iCNTX = 0 ) else '0' ;
iCLKHML(0) <= '1' when ( iCNTX < 2 ) else '0' ;
-- select clock
iCLKX <= iCLKHML( conv_integer( SEL ) ) ;
-- pulse counter
process (nRESET,iCLKX)
begin
if ( nRESET = '0' ) then
iPCNT <= 0 ;
elsif rising_edge(iCLKX) then
if ( iPCNT = CNTPMAX ) then
iPCNT <= 0 ;
else
iPCNT <= iPCNT + 1 ;
end if ;
end if ;
end process ;
-- decode
iPOUT <= X"8" when ( iPCNT = 1 ) else
X"1" when ( iPCNT = 2 ) else
X"4" when ( iPCNT = 3 ) else
X"2" ;
end behavioral;
パルス生成に利用しているクロックをモニタし、パルス
出力をするか否かの指定ができるようにしてあります。
XC9572の基板では、ピンアサインを以下としました。
# system
NET "CLOCK" LOC = "P5" ;
NET "nRESET" LOC = "P39" ;
# pulse out
NET "POUT<0>" LOC = "P1" ;
NET "POUT<1>" LOC = "P2" ;
NET "POUT<2>" LOC = "P3" ;
NET "POUT<3>" LOC = "P4" ;
# select , enable , monitor
NET "SEL<0>" LOC = "P11" ;
NET "SEL<1>" LOC = "P12" ;
NET "XENA" LOC = "P14" ;
NET "MCLK" LOC = "P22" ;
このVHDLコードでは、回転方向が固定なので
正転、逆転ができるようにします。
カウンタは、０から３を繰返しているので
デコードを２つ用意し、外部からの指示で
選択できるようにします。
デコーダは、次のように定義します。
iPOUT <= iPOUTY when ( XDIR = '1' ) else iPOUTX ;
iPOUTY <= X"8" when ( iPCNT = 1 ) else
X"1" when ( iPCNT = 2 ) else
X"4" when ( iPCNT = 3 ) else
X"2" ;
iPOUTX <= X"2" when ( iPCNT = 1 ) else
X"4" when ( iPCNT = 2 ) else
X"1" when ( iPCNT = 3 ) else
X"8" ;
VHDLコードにまとめると、以下。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity pm is
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ; -- 4MHz
-- enable
XENA : in std_logic ;
-- direction
XDIR : in std_logic ;
-- selector
SEL : in std_logic_vector(1 downto 0) ;
-- monitor
MCLK : out std_logic ;
-- pulse out
POUT : out std_logic_vector(7 downto 0) --;
);
end pm ;
architecture behavioral of pm is
--
CONSTANT CNTMAX : integer := 3999 ;
CONSTANT CNTHALF : integer := 2000 ;
CONSTANT CNTXMAX : integer := 7 ;
CONSTANT CNTPMAX : integer := 3 ;
-- divider
signal iCNT : integer range 0 to CNTMAX ;
signal iCLK : std_logic ;
signal iCNTX : integer range 0 to CNTXMAX ;
signal iCLKX : std_logic ;
signal iCLKHML : std_logic_vector(3 downto 0) ;
-- decode
signal iPOUT : std_logic_vector(3 downto 0) ;
signal iPOUTX : std_logic_vector(3 downto 0) ;
signal iPOUTY : std_logic_vector(3 downto 0) ;
signal iPCNT : integer range 0 to CNTPMAX ;
begin
-- output
POUT <= iPOUT & iPOUT when ( XENA = '1' ) else X"00" ;
MCLK <= iCLKX when ( nRESET = '1' ) else '0' ;
-- divider (generate 400Hz)
process (CLOCK)
begin
if rising_edge(CLOCK) then
if ( iCNT = CNTMAX ) then
iCNT <= 0 ;
else
iCNT <= iCNT + 1 ;
end if ;
end if ;
end process ;
iCLK <= '1' when ( iCNT < CNTHALF ) else '0' ;
-- generate clock
process (iCLK)
begin
if rising_edge(iCLK) then
if ( iCNTX = CNTXMAX ) then
iCNTX <= 0 ;
else
iCNTX <= iCNTX + 1 ;
end if ;
end if ;
end process ;
iCLKHML(3) <= '1' when ( iCNTX < 2 ) else '0' ;
iCLKHML(2) <= '1' when ( iCNTX < 4 ) else '0' ;
iCLKHML(1) <= '1' when ( iCNTX = 0 ) else '0' ;
iCLKHML(0) <= '1' when ( iCNTX < 2 ) else '0' ;
-- select clock
iCLKX <= iCLKHML( conv_integer( SEL ) ) ;
-- pulse counter
process (nRESET,iCLKX)
begin
if ( nRESET = '0' ) then
iPCNT <= 0 ;
elsif rising_edge(iCLKX) then
if ( iPCNT = CNTPMAX ) then
iPCNT <= 0 ;
else
iPCNT <= iPCNT + 1 ;
end if ;
end if ;
end process ;
-- decode
iPOUT <= iPOUTY when ( XDIR = '1' ) else iPOUTX ;
iPOUTY <= X"8" when ( iPCNT = 1 ) else
X"1" when ( iPCNT = 2 ) else
X"4" when ( iPCNT = 3 ) else
X"2" ;
iPOUTX <= X"2" when ( iPCNT = 1 ) else
X"4" when ( iPCNT = 2 ) else
X"1" when ( iPCNT = 3 ) else
X"8" ;
end behavioral;
内部では、４ビットのレジスタを２個用意し
制御ピンを１ピン増えします。UCFの内容は
次のように単純です。
# system
NET "CLOCK" LOC = "P5" ;
NET "nRESET" LOC = "P39" ;
# pulse out
NET "POUT<0>" LOC = "P1" ;
NET "POUT<1>" LOC = "P2" ;
NET "POUT<2>" LOC = "P3" ;
NET "POUT<3>" LOC = "P4" ;
# select , enable , monitor
NET "SEL<0>" LOC = "P11" ;
NET "SEL<1>" LOC = "P12" ;
NET "XDIR" LOC = "P13" ;
NET "XENA" LOC = "P14" ;
NET "MCLK" LOC = "P22" ;
４進カウンタをジョンソンカウンタを使い
デコーダ出力との対応を変更します。
-- pulse counter
process (nRESET,iCLKX)
begin
if ( nRESET = '0' ) then
iPCNT <= 0 ;
elsif rising_edge(iCLKX) then
case iPCNT is
when 0 => iPCNT <= 1 ;
when 1 => iPCNT <= 3 ;
when 3 => iPCNT <= 2 ;
when 2 => iPCNT <= 0 ;
when others =>
iPCNT <= 0 ;
end case ;
end if ;
end process ;
-- decode
iPOUT <= iPOUTY when ( XDIR = '1' ) else iPOUTX ;
iPOUTY <= X"8" when ( iPCNT = 1 ) else
X"1" when ( iPCNT = 3 ) else
X"4" when ( iPCNT = 2 ) else
X"2" ;
iPOUTX <= X"2" when ( iPCNT = 1 ) else
X"4" when ( iPCNT = 3 ) else
X"1" when ( iPCNT = 2 ) else
X"8" ;
４進カウンタをジョンソンカウンタで実現すると
クロックの変化で値が変化するのが１ビットだけ
になります。
バイナリカウンタでは、ハザードによるノイズを
生成し、マイコンの誤動作を起こす可能性があり
ますが、ジョンソンカウンタではハザード生成が
ないので、クロックを高速から低速に可変する時
のノイズ発生を考えなくてよくなります。
パルスモータは、２相励磁も可能なので
デコーダの内容を変更で対応できるかを
確認します。
２相の場合、４ビット中２ビットが１に
なるようにデコードします。
iPOUT <= iPOUTY when ( XDIR = '1' ) else iPOUTX ;
iPOUTY <= X"A" when ( iPCNT = 1 ) else
X"9" when ( iPCNT = 2 ) else
X"5" when ( iPCNT = 3 ) else
X"6" ;
iPOUTX <= X"6" when ( iPCNT = 1 ) else
X"5" when ( iPCNT = 2 ) else
X"9" when ( iPCNT = 3 ) else
X"A" ;
パルスモータは、励磁方法の他に周波数を可変にして
スピード制御できないと移動メカには使えません。
NCO(Nemerically Controlled Oscillators)を利用して
パルスモータに与えるパルス周波数を可変にします。
NCOは、固定周波数クロックで、与える数値で任意の
周波数を生成します。周波数可変に利用する加算器が
必要で、これを位相アキュムレータと呼びます。
位相アキュムレータは、歩くときの歩幅を変えて
目的値に到達するまでの時間を変えるカラクリです。
最小歩幅は固定で、この歩幅の何倍という係数を指定
します。この倍数を、マイコンで計算してCPLD/FPGAに
与えます。
24ビットの位相アキュムレータと係数に相当する
加算値レジスタを用意し、24MHzのクロックで加算
していきます。
24MHzを選択したのは、手持ちの水晶があったためで
16MHzでも20MHzでも構いません。
希望周波数frを出力するための加算値は
次の計算で算出します。（＾は、べき乗）
N = fr x (2^24) / (24x10^6)
VHDLコードは、以下。
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
entity nco is
port (
-- system
nRESET : in std_logic ;
CLOCK : in std_logic ; -- 24MHz
-- trigger
TRG : in std_logic ;
-- input data
ADR : in std_logic_vector(1 downto 0) ;
-- input data
Din : in std_logic_vector(7 downto 0) ;
--
COUT : out std_logic --;
);
end nco ;
architecture behavioral of nco is
-- phase accumlator
signal iPA : std_logic_vector(23 downto 0) ;
-- internal register
signal iDELTAX : std_logic_vector(23 downto 0) ;
signal iADR : std_logic_vector(1 downto 0) ;
-- synchronizer
signal iTRG_SFT : std_logic_vector(2 downto 0) ;
signal iTRG : std_logic ;
begin
-- output
COUT <= iPA(23) ;
-- input
iADR <= ADR ;
-- phase accumlator
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iPA <= (others => '0') ;
elsif rising_edge( CLOCK ) then
iPA <= iPA + iDELTAX ;
end if ;
end process ;
-- synchronizer
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iTRG_SFT <= "000" ;
elsif rising_edge(CLOCK) then
iTRG_SFT <= iTRG_SFT(1 downto 0) & TRG ;
end if ;
end process ;
iTRG <= '1' when ( iTRG_SFT = "011" ) else '0' ;
-- store internal register
process (nRESET,CLOCK)
begin
if ( nRESET = '0' ) then
iDELTAX <= (others => '0') ;
elsif rising_edge( CLOCK ) then
if ( iTRG = '1' ) then
-- lower
if ( iADR = "00" ) then
iDELTAX( 7 downto 0) <= Din ;
end if ;
-- middle
if ( iADR = "01" ) then
iDELTAX(15 downto 8) <= Din ;
end if ;
-- upper
if ( iADR = "10" ) then
iDELTAX(23 downto 16) <= Din ;
end if ;
end if ;
end if ;
end process ;
end behavioral;
XilinxのXC95108でどのくらいのマクロセルを使うのか
調べると、80マクロセルくらいでした。この程度ならば
FPGAに楽々入ります。
UCFファイルは、以下。
# system
NET "CLOCK" LOC = "P9" ;
NET "nRESET" LOC = "P74" ;
# interface
NET "TRG" LOC = "P1" ;
# address
NET "ADR<0>" LOC = "P2" ;
NET "ADR<1>" LOC = "P3" ;
# data
NET "Din<0>" LOC = "P4" ;
NET "Din<1>" LOC = "P5" ;
NET "Din<2>" LOC = "P6" ;
NET "Din<3>" LOC = "P7" ;
NET "Din<4>" LOC = "P10" ;
NET "Din<5>" LOC = "P11" ;
NET "Din<6>" LOC = "P12" ;
NET "Din<7>" LOC = "P13" ;
# clock out
NET "COUT" LOC = "P14" ;
24ビットの加算値を更新するには、マイクロプロセッサを
利用します。
ArduinoやZ80を利用した場合、３バイトのデータを更新する
ことになり、８ビットデータバスに加えて、アドレス、制御
のピンが必要となり、ピン数の多いプロセッサが必要です。
ピン数の多いプロセッサ基板を利用するのは、テスト段階に
限定します。実際の移動には、FPGAの中にプロセッサを入れ
上位プロセッサの負担を減らします。
ピン数の多いプロセッサは、Z84C011を利用し、これを実装した
Z80基板を使います。
Z84C011には、パラレルポートが５ポートあるので、24ビットを
一度に更新できます。シリアルインタフェースを持たないので
SPIを使ってZ84C011に値を渡します。
SPIインタフェースで渡した24ビットを、そのままパラレルポート
に出力し、他にトリガーを１ピン使います。
24ビットをパラレルポートに出力する処理を
Z80アセンブリ言語で記述すると、以下。
; FHB high byte (2^23 - 2^16)
; FMB middle byte (2^15 - 2^8)
; FLB low byte ( 2^7 - 2^0)
SET_FV:
; stack
push bc
push af
; high byte
ld a,(FHB)
ld c,PADAT
out (c),a
; middle byte
ld a,(FMB)
ld c,PBDAT
out (c),a
; low byte
ld a,(FLB)
ld c,PCDAT
out (c),a
; stack
pop af
pop bc
;
ret
アセンブリ言語で、大まかな動作チェックをしたなら
RTOSを使い、雛形のファームウエアを作成してみます。
/* define my system I/O area */
__sfr __at 0x10 CTC_C0 ;
__sfr __at 0x11 CTC_C1 ;
__sfr __at 0x12 CTC_C2 ;
__sfr __at 0x13 CTC_C3 ;
__sfr __at 0x54 PACON ;
__sfr __at 0x55 PBCON ;
__sfr __at 0x56 PCCON ;
__sfr __at 0x34 PDCON ;
__sfr __at 0x44 PECON ;
__sfr __at 0x50 PADAT ;
__sfr __at 0x51 PBDAT ;
__sfr __at 0x53 PCDAT ;
__sfr __at 0x30 PDDAT ;
__sfr __at 0x40 PEDAT ;
/* define data types */
typedef unsigned char UBYTE ;
typedef unsigned short UWORD ;
typedef unsigned long ULONG ;
typedef signed char SBYTE ;
typedef signed short SWORD ;
#define OFF 0
#define ON OFF+1
#define CLOSED 0
#define OPENED CLOSED+1
#define MASK0F 0x0f
#define MASKFF 0xff
#define SGATE_BIT 4
#define EDC_BIT 3
#define LM_BIT 1
#define RM_BIT 0
UBYTE sgate ;
UBYTE sensor ;
typedef union {
struct {
unsigned char B7:1;
unsigned char B6:1;
unsigned char B5:1;
unsigned char B4:1;
unsigned char B3:1;
unsigned char B2:1;
unsigned char B1:1;
unsigned char B0:1;
} BIT ;
unsigned char DR ;
} FLAGSP ;
FLAGSP x_flags ;
#define SFLAG x_flags.BIT.B0
#define UFLAG x_flags.BIT.B1
#define WFLAG x_flags.BIT.B2
#define TFLAG x_flags.BIT.B3
ULONG timcnt ;
UBYTE timeoutcnt ;
typedef struct {
void (*tsk)(void);
UWORD wcount ;
} TCBP ;
#define TSK_ID_MAX 8
#define TSK_ID0 0
#define TSK_ID1 1
#define TSK_ID2 2
#define TSK_ID3 3
#define TSK_ID4 4
#define TSK_ID5 5
#define TSK_ID6 6
#define TSK_ID7 7
/* define constant values */
#define LAST 10
#define TTS_SUSPEND 0
#define TTS_WAIT TTS_SUSPEND+1
#define TTS_READY TTS_SUSPEND+2
TCBP tcb[TSK_ID_MAX];
#define NORMAL TSK_ID1
#define CRANK TSK_ID2
#define ROTATE TSK_ID3
#define LANE TSK_ID4
#define CHANGE TSK_ID5
#define BLIND TSK_ID6
volatile UBYTE wflag ;
volatile UBYTE run_tsk ;
volatile UBYTE ready ;
volatile UBYTE suspend ;
volatile UBYTE waitq ;
UBYTE spat[13];
#define ALL_BLACK 0
#define ALL_WHITE 1
#define LEFT_WHITE 2
#define RIGHT_WHITE 3
#define CENTER 4
#define TINY_RIGHT 5
#define RIGHT 6
#define BIG_RIGHT 7
#define TINY_LEFT 8
#define LEFT 9
#define BIG_LEFT 10
#define BOTH_WHITE 11
#define ILLEAGAL 12
#define GEAR0 10
#define GEAR5 150
#define GEAR10 300
#define GEAR15 450
#define GEAR20 600
#define GEAR25 750
#define GEAR30 900
#define GEAR35 1050
#define GEAR40 1200
#define GEAR45 1350
#define GEAR50 1500
#define GEAR55 1650
#define GEAR60 1800
#define GEAR65 1950
#define GEAR70 2100
#define GEAR75 2250
#define GEAR80 2400
#define GEAR85 2550
#define GEAR90 2700
#define GEAR95 2850
#define GEAR100 2999
#define IDLE 0
#define RUN IDLE+1
#define DIR_CENTER 0
#define DIR_RIGHT 1
#define DIR_LEFT 2
UBYTE left ;
UBYTE right ;
UBYTE state ;
UBYTE mstate ;
UBYTE dir ;
/*--------------------------------*/
/* Insert user functions protoype */
/*--------------------------------*/
void user_initialize(void);
void show_road_state(UBYTE x);
void init_spat(void);
UBYTE get_sensor(void);
UBYTE get_start(void);
void show_state(UBYTE x);
void send_dc(UBYTE lx,UBYTE rx);
void delay_ms(UWORD x);
/*-------------*/
/* system call */
/*-------------*/
void cre_tsk(UBYTE tid,void (*tsk)(void));
void sta_tsk(UBYTE tid,UBYTE sta);
void rsm_tsk(UBYTE tid);
void sus_tsk(UBYTE tid);
void slp_tsk(void);
void wai_tsk(UWORD x);
void init_os(void);
UBYTE is_tsk_ready(UBYTE tid);
/*------*/
/* task */
/*------*/
void tsk0_proc(void);
void tsk1_proc(void);
void tsk2_proc(void);
void tsk3_proc(void);
void tsk4_proc(void);
void tsk5_proc(void);
void tsk6_proc(void);
void tsk7_proc(void);
void main(void)
{
UBYTE tmp ;
UBYTE i ;
/* disable interrupt */
/* cli(); */
/* initialize port and variables */
user_initialize();
/* initialize monitor */
init_os();
/* create task */
cre_tsk(TSK_ID0,tsk0_proc);
cre_tsk(NORMAL ,tsk1_proc);
cre_tsk(CRANK ,tsk2_proc);
cre_tsk(ROTATE ,tsk3_proc);
cre_tsk(LANE ,tsk4_proc);
cre_tsk(CHANGE ,tsk5_proc);
cre_tsk(BLIND ,tsk6_proc);
cre_tsk(TSK_ID7,tsk7_proc);
/* start task task */
sta_tsk(TSK_ID0,TTS_READY);
sta_tsk(NORMAL ,TTS_SUSPEND);
sta_tsk(CRANK ,TTS_SUSPEND);
sta_tsk(ROTATE ,TTS_SUSPEND);
sta_tsk(LANE ,TTS_SUSPEND);
sta_tsk(CHANGE ,TTS_SUSPEND);
sta_tsk(BLIND ,TTS_SUSPEND);
sta_tsk(TSK_ID7,TTS_READY);
/* enable interrupt */
/* sei(); */
/* endless loop */
run_tsk = TSK_ID0 ;
/* endless loop */
while ( ON ) {
/* RTOS */
if ( is_tsk_ready( run_tsk ) == ON ) {
switch ( run_tsk ) {
case 1 : tsk1_proc(); break;
case 2 : tsk2_proc(); break;
case 3 : tsk3_proc(); break;
case 4 : tsk4_proc(); break;
case 5 : tsk5_proc(); break;
case 6 : tsk6_proc(); break;
case 7 : tsk7_proc(); break;
default : tsk0_proc(); break;
}
}
/* round robin */
run_tsk++;
if ( run_tsk == TSK_ID_MAX ) { run_tsk = TSK_ID0 ; }
/* cyclic handling */
if ( wflag ) {
wflag = OFF ;
tmp = waitq ;
for ( i = 0 ; i < TSK_ID_MAX ; i++ ) {
if ( tmp & ON ) {
tcb[i].wcount-- ;
if ( tcb[i].wcount == 0 ) { rsm_tsk(i); }
}
tmp >>= 1 ;
}
}
}
}
void user_initialize(void)
{
/* I/O values */
PADAT = 0x00 ;
PBDAT = 0x00 ;
PCDAT = 0x00 ;
PDDAT = 0xff ;
PEDAT = 0xff ;
/* I/O directions */
PACON = 0x00 ;
PBCON = 0xff ;
PCCON = 0xff ;
PDCON = 0xff ;
PECON = 0xff ;
/* clear flags */
x_flags.DR = 0 ;
/* initialize */
timcnt = 0 ;
/* others */
left = 0 ;
right = 0 ;
state = IDLE ;
mstate = 0 ;
init_spat();
}
/* system call */
void cre_tsk(UBYTE tid,void (*tsk)(void))
{
tcb[tid].tsk = tsk;
tcb[tid].wcount = 0;
}
void sta_tsk(UBYTE tid,UBYTE sta)
{
UWORD tmp ;
tmp = (1 << tid);
if ( sta == TTS_READY ) { ready |= tmp; }
if ( sta == TTS_SUSPEND ) { suspend |= tmp; }
if ( sta == TTS_WAIT ) { waitq |= tmp; }
}
void rsm_tsk(UBYTE tid)
{
UWORD tmp ;
tmp = (1 << tid);
ready |= tmp;
suspend &= ~tmp;
waitq &= ~tmp;
}
void sus_tsk(UBYTE tid)
{
UWORD tmp ;
tmp = (1 << tid);
ready &= ~tmp;
suspend |= tmp;
waitq &= ~tmp;
}
void slp_tsk(void)
{
sus_tsk(run_tsk);
}
void wai_tsk(UWORD x)
{
UWORD tmp ;
tmp = (1 << run_tsk);
ready &= ~tmp;
suspend &= ~tmp;
waitq |= tmp;
tcb[run_tsk].wcount = x ;
}
void init_os(void)
{
ready = 0;
suspend = 0;
waitq = 0;
}
UBYTE is_tsk_ready(UBYTE tid)
{
return( (ready >> tid) & 1 ) ;
}
void show_road_state(UBYTE x)
{
/* impress */
PDDAT = x ^ MASKFF ;
}
UBYTE get_sensor(void)
{
return sensor ;
}
UBYTE get_start(void)
{
return sgate;
}
void init_spat(void)
{
*(spat+ 0) = 0x00 ; /* ALL_BLACK */
*(spat+ 1) = 0xff ; /* ALL_WHITE */
*(spat+ 2) = 0xf0 ; /* LEFT_WHITE */
*(spat+ 3) = 0x0f ; /* RIGHT_WHITE */
*(spat+ 4) = 0x18 ; /* CENTER */
*(spat+ 5) = 0x0c ; /* TINY_RIGHT */
*(spat+ 6) = 0x06 ; /* RIGHT */
*(spat+ 7) = 0x03 ; /* BIG_RIGHT */
*(spat+ 8) = 0x30 ; /* TINY_LEFT */
*(spat+ 9) = 0x60 ; /* LEFT */
*(spat+10) = 0xc0 ; /* BIG_LEFT */
*(spat+11) = 0xc3 ; /* BOTH_WHITE */
*(spat+12) = 0x5a ; /* ILLEAGAL */
}
void show_state(UBYTE x)
{
/* impress */
PEDAT = x ^ MASKFF ;
}
void send_dc(UBYTE lx,UBYTE rx)
{
/* left motor */
PBDAT = lx ;
PCDAT |= (1 << LM_BIT) ;
PCDAT &= ~(1 << LM_BIT) ;
/* right motor */
PBDAT = rx ;
PCDAT |= (1 << RM_BIT) ;
PCDAT &= ~(1 << RM_BIT) ;
}
void delay_ms(UWORD x)
{
ULONG target ;
/* calculate last value */
target = timcnt + x ;
/* wait */
while ( timcnt < target ) ;
}
/* system control */
void tsk0_proc(void)
{
/* start trigger */
if ( state == IDLE ) {
if ( get_start() == OPENED ) {
state = RUN ;
rsm_tsk(NORMAL);
/* enable motor power */
PCDAT |= (1 << EDC_BIT);
/* timer trigger clear */
timeoutcnt = 0 ;
TFLAG = OFF ;
}
}
/* timeout */
if ( state == RUN ) {
if ( TFLAG == ON ) {
state = IDLE ;
/* suspend move handling task */
state = IDLE ;
sus_tsk(NORMAL);
sus_tsk(CRANK);
sus_tsk(ROTATE);
sus_tsk(LANE);
sus_tsk(CHANGE);
sus_tsk(BLIND);
/* machine state */
mstate = 0 ;
/* disable motor power */
PCDAT &= ~(1 << EDC_BIT);
}
}
/* show state */
show_state( mstate );
}
/* NORMAL */
void tsk1_proc(void)
{
/* machine state */
mstate = 0;
/* show */
show_state((1 << mstate));
/* default */
dir = DIR_CENTER ;
/* get sensor */
if ( sensor == CENTER ) {
left = 50 ; right = 50 ;
}
if ( sensor == TINY_RIGHT ) {
left = 50 ; right = 55 ;
}
if ( sensor == RIGHT ) {
left = 50 ; right = 60 ;
}
if ( sensor == BIG_RIGHT ) {
left = 50 ; right = 70 ;
}
if ( sensor == TINY_LEFT ) {
left = 55 ; right = 50 ;
}
if ( sensor == LEFT ) {
left = 60 ; right = 50 ;
}
if ( sensor == BIG_LEFT ) {
left = 70 ; right = 50 ;
}
if ( sensor == ALL_BLACK ) {
left = 10 ; right = 10 ;
}
if ( sensor == BOTH_WHITE ) {
left = 20 ; right = 20 ;
}
if ( sensor == ILLEAGAL ) {
left = 10 ; right = 10 ;
}
/* update */
send_dc(left,right);
/* judge */
if ( sensor == ALL_WHITE ) {
rsm_tsk(CRANK);
slp_tsk();
}
if ( sensor == LEFT_WHITE ) {
dir = DIR_LEFT ;
rsm_tsk(LANE);
slp_tsk();
}
if ( sensor == RIGHT_WHITE ) {
dir = DIR_RIGHT ;
rsm_tsk(LANE);
slp_tsk();
}
}
/* ROTATE */
void tsk2_proc(void)
{
/* machine state */
mstate = 1;
/* show */
show_state((1 << mstate));
/* get sensor */
if ( sensor == CENTER ) {
left = 30 ; right = 30 ;
}
if ( sensor == TINY_LEFT ) {
left = 35 ; right = 30 ;
}
if ( sensor == LEFT ) {
left = 40 ; right = 30 ;
}
if ( sensor == BIG_LEFT ) {
left = 45 ; right = 30 ;
}
if ( sensor == TINY_RIGHT ) {
left = 30 ; right = 35 ;
}
if ( sensor == RIGHT ) {
left = 30 ; right = 40 ;
}
if ( sensor == BIG_RIGHT ) {
left = 30 ; right = 45 ;
}
if ( sensor == BOTH_WHITE ) {
left = 20 ; right = 20 ;
}
if ( sensor == ALL_BLACK ) {
left = 10 ; right = 10 ;
}
if ( sensor == ILLEAGAL ) {
left = 10 ; right = 10 ;
}
/* update */
send_dc(left,right);
/* judge */
if ( sensor == LEFT_WHITE ) {
dir = DIR_LEFT ;
rsm_tsk(ROTATE);
slp_tsk();
}
if ( sensor == RIGHT_WHITE ) {
dir = DIR_RIGHT ;
rsm_tsk(ROTATE);
slp_tsk();
}
}
/* ROTATE */
void tsk3_proc(void)
{
/* machine state */
mstate = 2;
/* show */
show_state((1 << mstate));
/* judge */
if ( dir == DIR_RIGHT ) {
left = 10 ; right = 50 ;
}
if ( dir == DIR_LEFT ) {
left = 50 ; right = 10 ;
}
/* update */
send_dc(left,right);
/* judge */
if ( sensor == LEFT_WHITE ) {
dir = DIR_LEFT ;
rsm_tsk(ROTATE);
slp_tsk();
}
/* judge */
if ( sensor == CENTER ) {
send_dc(50,50);
dir = DIR_CENTER ;
rsm_tsk(NORMAL);
slp_tsk();
}
}
/* LANE */
void tsk4_proc(void)
{
UBYTE tdir ;
/* machine state */
mstate = 3 ;
/* show */
show_state((1 << mstate));
/* direction */
tdir = dir ;
/* get sensor */
if ( sensor == CENTER ) {
left = 30 ; right = 30 ;
}
if ( sensor == TINY_LEFT ) {
left = 35 ; right = 30 ;
}
if ( sensor == LEFT ) {
left = 40 ; right = 30 ;
}
if ( sensor == BIG_LEFT ) {
left = 45 ; right = 30 ;
}
if ( sensor == TINY_RIGHT ) {
left = 30 ; right = 35 ;
}
if ( sensor == RIGHT ) {
left = 30 ; right = 40 ;
}
if ( sensor == BIG_RIGHT ) {
left = 30 ; right = 45 ;
}
if ( sensor == BOTH_WHITE ) {
left = 20 ; right = 20 ;
}
if ( sensor == ALL_BLACK ) {
left = 10 ; right = 10 ;
}
if ( sensor == ILLEAGAL ) {
left = 10 ; right = 10 ;
}
/* update */
send_dc(left,right);
/* judge */
if ( sensor == ALL_BLACK ) {
dir <<= 2 ;
if ( tdir == DIR_LEFT ) {
dir = DIR_RIGHT ;
}
if ( tdir == DIR_RIGHT ) {
dir = DIR_LEFT ;
}
dir <<= 2 ;
dir |= tdir ;
rsm_tsk(CHANGE);
slp_tsk();
}
}
/* CHANGE */
void tsk5_proc(void)
{
UBYTE tdir ;
/* machine state */
mstate = 4 ;
/* show */
show_state((1 << mstate));
/* default */
tdir = dir & 3 ;
dir >>= 2 ;
/* rotate */
if ( tdir == DIR_LEFT ) {
left = 30 ; right = 50 ;
}
if ( tdir == DIR_RIGHT ) {
left = 50 ; right = 30 ;
}
/* update */
send_dc(left,right);
/* delay */
delay_ms(100);
/* update */
left = 30 ; right = 30 ;
/* exit */
rsm_tsk(BLIND);
slp_tsk();
}
/* BLIND */
void tsk6_proc(void)
{
/* machine state */
mstate = 5 ;
/* show */
show_state((1 << mstate));
/* rotate */
if ( dir == DIR_LEFT ) {
left = 10 ; right = 30 ;
}
if ( dir == DIR_RIGHT ) {
left = 30 ; right = 10 ;
}
/* update */
send_dc(left,right);
/* delay */
delay_ms(100);
/* */
left = 50 ; right = 50 ;
/* update */
send_dc(left,right);
/* delay */
delay_ms(50);
/* sensor */
if ( sensor == CENTER ) {
rsm_tsk(NORMAL);
slp_tsk();
}
}
void tsk7_proc(void)
{
/* get sensor data */
sensor = PADAT ;
/* start gate */
sgate = CLOSED ;
if ( !(sensor & (1 << SGATE_BIT)) ) { sgate = OPENED ; }
/* sensor */
sensor &= MASK0F ;
/* time out */
if ( timeoutcnt > 119 ) { TFLAG = ON ; }
/* show road state */
show_road_state( sensor );
/* wait 50ms */
wai_tsk(50);
}
目次
前
次