ラジオコントロールの基礎技術修得(III) : (2軸加
速度センサーを使用した姿勢制御について)
著者
辻 正晴
雑誌名
技術報告集
巻
10 (2004年度)
ページ
51-56
発行年
2005-04-09
URL
http://hdl.handle.net/10098/7446
フジオコントロールの基礎技術修得 (ill)
(
2 軸加速度センサーを使用した姿勢制御について)
第三技術室システム制御技術班
辻
正晴
しはじめに
ラジオコントロールで対象機器を
一
定方向に制御している場合にも、風などの外乱によりその制
御は乱され、その都度コントローラによる修正が必要になる。これでは操作が非常に煩雑になり操
縦者の負担が大きくなるが、この負担を軽減するには、ジャイロスコープなどの姿勢検出センサを
使用し、外乱を打ち消すような動作を自動で行わせる必要がある。
今回の研修では、 2 軸加速度センサ ADXL202( アナロク守デパイセズ社製)を使用してその傾きを検出
し、 X-y軸に傾きを制御できる雲台モデルで、任意の姿勢を保持する姿勢制御を試みた
。
なお、 CPU
には H8/3052F( リニアテクノロジ社製)を使用し、プログラムはアセンブリ言語で記述した。
2
.
2 軸加速度センサー
加速度センサは物体の動きや振動の検出など動的加速度のほかに、地球重力などの静的加速度、
傾きなどの計測にも用いられる。 ADXL202 は 2 軸 2g計測用の加速度センサで、 19( 重力加速度)当たり
312mV のアナログ電圧を出力するほかに、 PWM( パルス幅変調)出力で直接マイコンに入力できるよう
になっている。 PWM の波長は外付け抵抗 RSET で決定され、 120 1\ Q では 960us となっている。水平に置
いた場合の Og でのデューティ比は 50% である(個体によるパラっき、温度によるドリフトあり〕。ま
た、動作周波数帯域はローパスフィルタの Cx ， Cy により決定され、 o. 1uF で 47Hz となっている。
3. 回路構成
- 5
1
-4. 回路の説明
2 軸加速度センサ ADXL202 からの X-Y 方向の傾きによる PWM パノレス幅の変化を ITU1C インテグレーテ
ッドタイマユニット〉、 ITU2 によるインプットキャプチャモードで読み込み、基準位置の値と比較し、
常に基準位置に復帰するよう X 軸 Y 軸に取付けたモータを駆動して雲台モデル(ここでは自動車の電動
サイドミラーを改造したもの〉を制御している。図中の DATUM スイッチは任意の傾きを基準点とする
場合の割込みスイッチである
。
加速度センサからの X 、 Yパ/レス幅とその時のだいたいの傾斜角度は
LCDCSC1602BS油秋月製〉に数値で表示されるが、視覚的に表現するため、 16*16 ドット LED マトリクス
モジュール CAD-501-B 秋月製)を使用して一目で現在の姿勢位置が確認できるようにした。
5. フローチャート
MAIN ルーチン1
R
Q
3 割込みルーチン(IRQ3)
加L8基LモEC田速D準D町マモ度点タ移ジトセのと動ュリの制ン平クサ御比均ルスデ較をににー
圭量
とタ
Z
不
る
E
読込み
6. プログラムの説明
シンボル定義の XDTM 、 YDTM は加速度計の X"Y基準データの登録番地、 XDAT ， YDAT は 8 回の移動平均値
の
登録番
地、 DATAVX， DATAVY は X 、 Y データの格納先頭番地、 DSINO ， DSIN8 は LED マトリクスモジュール
へのデータ格納番地である。 1
TU l,
1
TU2 でインプットキャプチャされた X 、 Y データはそのままではパ
タつきが多いため 8 回の移動平均値をデータとして使用している。 X 、 Y軸の駆動モータの速度制御は
1
TU3
,
1
TU4 の PWM 出力で行ないバッファ動作としている
。
スイッチによる外部割込みではそのときの
加速度センサの X"Y 値を新基準値としてメモリに登録している。なお、ここでは LCD に関するプログ
ラムは紙面の都合上省略している。7. おわりに
2 軸加速度センサ ADXL202 は個体のパラっき、温度によるドリフトが多いため、センサ毎に基準デ
ータの調整が必要である
。
また、出力データも安定性が悪くパタつくため平滑化が必要である。 AD
XL203 以降の製品を使用したいが、一般の入手は困難である
。
今回の研修では 2 軸加速度センサを使
用したが、今後ロボットなどの姿勢制御に役立てたいと考えている。
最後
に、この研修の機会を与
えていただいた関係各位に厚くお礼を申し上げます。
っ“F
b
参考文献
H8
/
3052F-ZTAT ハードウェアマニュアル
目
立
H8
/
300H シリーズプログラミンク
マ
マニュアル
目立
q < U F Oadx12026 r. src Page 1 adx12026 r. src Page 2 1 : H8-3052F . 25 暢 l z ADXL2026R . 5RC ROM 版 72 IIOV . B ROL .骨 TωRl : 1 NPUT CAPTURE GRB > UP EDGE 2 : 本本写本 * 毒事事本字本容字本本容本意本孝章孝章容本容本意事本本家傘字本***本意$写本字率本本本容牟傘字本*本字率本容本孝章容章容傘傘本字率傘 73 船 V . B R 弘前 IOR2 3 :キ 2 軸加プ加速チ 速度 度センサ (ADXL202l を使用した水平姿勢制御 1プログラム 本 74 船 V . B #H ' 23. ROL : GRA CD 岨 P ARE MA TCH TC 町 CLEAR 1/8 4 : 本 2 軸 センサの X 、 Y 軸の出力パルス幅の変化を、 ITU1 . 2 インプット 寧 75 船 V . B ROL .骨 TCR3 , 5
:本
キャ
ャ織に
IOC表能
A示1を
(P用しA
いて
B回の
X-移
Y軸動モ平ー均タで制求御めに
、平匂傾きをし
CD
とLED
本
76 IIOV. B ROL . @TCR4 6 :キマトリヲス 、雲台を より水平に姿勢制御するキ 77 MOV. W #500 . RO :500 刈 32=160us 7 :本 X-ÓUT -> TIO顴 f( PA4l. Tlocill (PA5l. Y-OUT-> TIOCA2(PA62. Tl OCB~(PAn 本 78 IIOV. W RO. @GRA3 : 160us(6250Hzl 8:*
LED MATRIX / P50-5IN 1. P51-5IN2. P52-5IN3 . P53-LATCH. P47-_CLOCK 本 79 踊 OV. W RO. 骨 GR A4 9 :本 ITU3.4 PWM 船 DE/ バッファ動作. Tl OCA3(PBO). C A4 ( PB2) P附 OUTPUT 4巳 80 MOV. W 非 400. RO :400 本0.32'128u s 10 :*傘孝章字率宰*事事事本字本本傘本本奪事本毒事事本家主本本本家事字本字本事本本意本字奪事*奪事本本奪事事*家本事事卒事事*卒事本本字本事事孝章 81 MOV . W RO.@G
RB3 :DUTY 80% 11 82 MOV . W RO , @8R83 :8UFFER 12 , CPU 300HA :CPU の指定 83 MOV , W RO , flGRB4 :NEUTRAL 13 . IN 乱 UDE " 3048equ 目 h" 84 MOV . W RO. @8R84 :8UFFER 114 5 , SECTI ON VECT ，∞慌 .L ∞ ATE.H ・000000 : セクションの宣言 85 MOV. B #H ・OA. ROL : GRB3 , GRB4 BUFFER 即日 E , DATA . l INIT :START VECT 叩 86 MOV . B ROL , @TFCR 16LLCCODTDE . EOU H' FFEF12 : LCD に転送するデータ 1^' イトを一時入れる臥剛の番地 87 MOV . 日露 H ' 18. R 乱 : ITU3 . 4 P附附促 17 LCD162 , EQU H' FFEF14 : LCD 表示 16 文字 2 行分のデータを入れておく臥 M番地 88 MOV , B R 乱， IITMDR :T I<削 3 ， Tl OC A4 PWMαR 附 T 18 XDTM , EQU H ' FFEF34 :X-D An瑚 89 B! 延 T #3 . @IER : ENABLE 1 R03 (P8 ・3 ) 19 YDTM , EOU H ' FFEF36 :Y-DATUM 90 A 附 C 前 H ' 7F . CCR : EI 20 XDT 附 1 , EOU H' FFEF38 :X-DATUM HIGH 91 MOV , B #H ' FE , ROL : TCNT 4 , 3 , 2 . 1 START 21 XDT 胤 EOU H ' FFEF3A : X -DA TUM LOW 92 MOV , B ROL. @ TSTR 22 YDT 蜘 1 , EOU H ' FFEF3C :Y-DA 刊 M HIGH 93 :---MA 1 N ルーチン一一 23 YDT 臥 EOU H' 昨 EF3E :Y-DATI 胤 LO 胃 94 DATAOU : 船 V. B #8 , R1L ;初回データ読み込み 8 回平均 24 DSINO , EOU H' FFEF40 :5IN lC Y) , SIN2(X) , SIN3( ー) 95 船 V. L 如 ATAVYtI6 ， ER3 25 DSIN8 • EOU H' FFEF48 :CENTER 96 MOV.l #DATAVXtI6 , ER4 26 RINGY , EOU H'FFEF50 :RI 陥 MEMORY POS 1 T 1 ON Y 97 1 NCP2A : BT5T 書 O. 骨 T5R2 : IMFA2 FLAG CHECK (Y-AX 15) 27 RINGX , EOU H' FFEF54 :RI 附 MEMORY P051 T 1 ON X 98 BEO 1 NCP2A : TI OCA2 1 NPUT CAPTURE TCNT2 ->GRA2 28 YDAT , EOU H' FFEF5C :Y AVERAGE DATA 99 INCP1A: BT5T 持 0， @T5Rl : IMFAl FLAG CHECK (X-AXIS) 29 XDAT , EOU H' FFEF5E : X AVERAGE DATA 100 BEO INCP1A : TI OCA 1 1 NPUT CAPTURE TCNTl•
GRAl 30 DATAVY , EOU H' FFEF60 : DATA AVERAGE Y 101 INCP28 : BT5T 書1.@ T5R2 : IMFB2 FLAG CHECK 31 DATAVX , EOU H' FFEF70 :DATA AVE 陥GE X 102 8EO INCP2B :Tl OCB2 INPUT C伊 TURE TCNT2->GR82 32 103 INCP1B: 8T5T 書l. @TSRl : IMFBl FLAG CHEα 33 .ORG H ・00003C : IR03(P8-3J 104 BEO INCP1B : T 1 OCBl 1 NPUT CAPT 四 E TCN Tl->GRBl 34 , DATA . L IR03 :DA 礼刷 5W 105 MOV . W flGR A2 , R2 :Y σz 35 106 MOV . W R2 , II-ER3 ，ら 36 , SEC Tl ON PR ∞ ， C∞ E ， L 民 ATE.H ' 000100 ; コードセクションの宣言 107 MOV . W @伺 Al ， R2 :X 37 INIT 船 V ， L 骨H ・ FFFFOO ， ER7 :スタックポインタの初期番地を Z量定する 108 齢 V . W R2 , II-ER4 38 IIOV , W 僻 4・ OO ，RO 109 MOV . B IITSR 1. R側 . 臥血刷 Y 39 MOV , B R 乱 .骨 PAOOR :PA4-Tl OCA 1( XJ , PA6-Tl OCA2(Y) 110 船 V ， B #0 , ROL 40 IIOV , B #H' FF , ROL 111 MOV. B ROL ，骨 TSRl : IMFA 1. Bl FLAG 乱 EAR 41 IIOV . B R 乱， @P41 踊 :P45 -P40 , LCD PORT , P47-CLOCK112
MOV . BIl
TSR2. ROH :DU 剛 Y 42 IIOV , B R 乱 ，@ P5 ∞ R :P50-SI N 1. P51-S IN2 , P52-SI N3. P53-LATCH 113 MOV . B ROL , IITSR2 : IMFA2 , B2 凡 AG 乱 EAR 43 園 OV ， B ROL , IIPBDDR :B7-YCW , B6-YCCW , B5 ・XCW ， B4 -XCCW , B2 四XPWM. BO-YP 州 114 DEC . B R1L 44 MOV . B #0 , ROL 115 BNE INCP2A 45 MOV , B ROl , @PBDR : MOTOR BRAKE 116 JSR 骨 OATAV 46 ORC 持 H' 80 . CCR :DI 117 RINGO ・ MOV. L #DATAVY+14 , ER3 47 MOV . B #16. R1L :DATA COUNT 118 MOV. L ER3 , @RI 附 Y 48 MOV , L 仰 SINOtI6 ， ER2 : INIT ADDRESS 119 MOV , L #DATAVXtI4 , ER4 49 MOV , B #H' 08 , ROL :LED MATRIX INIT DATA 120 MOV , L ER4 ，IIRI 附 X 50 INITD : 1I 0V , B ROL , @-ER2 121 RING: MOV , L @RINGY , ER3 51 DEC. B R1L 122 MOV , L @RINGX , ER4 52 BNE INITD 123 INCP4A : BT5T #0 , IJTSR2 :川 FA2 FLAG C雌 α(Y -AXIS) 53 MOV . B 韓 H' OB , ROL :L 回 MATRIX DATA 124 BEO INCP4A :TIOCA2 1M'肝 CAPTURE TCNT2-)GRA2 54 MOV , B R 乱.船岳山 8 : CENTER DATA 125 1 NCP3A : BTST #0 ，骨 TSRl : IMFAl FLAG CHECK ( X-AXISJ 55 MOV . W 韓 1320 ， RO : X DATUII 126 匹。 INCP3A : TI OCA 1 1 NPUT CAPTURE TCNTl•
GRAl 56 踊OV ， W RO ， 似 DTM 127 INCP4B : BTST 持1.@ T5R2 : 11FB 2 FLAG CHECK 57 MOV 胃 #1362 ， RO :Y DA 刊 M 128 BEQ INCP4B :TIα :8 2 1 NPUT CAPT lJl E TCNT2-縄問 2 58 MOV . W RO , @YDTII 129 INCP3B : BTST 時1.骨 TSRl : IIFB 1 FLAG CHECK 59 IIOV , W #1318 , RO :X DATUM LOW 130 BEO I NCI' 3B :Tl OCBI INPUT CAPT 四 E TωTl ー > GRBl 60 MOV. W RO. 似 DT 札 131 MOV , W flGR A2 , R2 : Y 61 MOV 明書 1322 ， RO :X DA 刊 11 HIGH 132 船 V ， W R2. 0 ER3 62 IIOV , W RO , @XDT 幽 133 IIOV . W@GR
A 1. R2 :X 63 MOV , W #1360 , RO :Y DATUII LOW 134 輔 OV . W R2 , @ER4 64 附 V ， W RO ，骨 YDT 慌 135 DEC , L 品 2 ， ER3 65 MOV , W #1364 , RO :Y DATUM HIGH 136 船 V ， L ER3 ，@RI 附 Y 66 MOV , W RO , @YDT 細 4 137 DEC , L 幅 2， ER4 67 JSR 骨 LCDINI42 138 摘。 V .l ER4 ，卵|附 X 68 MOV. B #H' C3 , ROL : GRB COMP ARE MATCH TCNT CLEAR 139 JSR 。 DATAV 69 MOV , B ROl , @TCRl : GRBl , UP EDGE ∞ UNT ， 1/8 CLOCK 140 船 V ， W 骨 XDAT. R2 770 1 船 V ， B ROL , @TCR2 :GRB2 , 0 ，04 柑 -0， 32u s 141 MOV , W 韓H' 0205 , R5 : 0 1 SP POS 1 TI ON , CHARACTER CαJNT IIOV , B 骨 H' 45 , ROL : 1 NPUT CAPTURE GRA > ∞ WN EDGE 142 JSR 骨凡 XDSPadxI 2026 r. src Page 3 adx12026r . src Page 4 143 MOV . W 骨 YDAT . R2 214 S ト4.R. 11 R3 :/2 144 鵬 V .I #H ・ 1205. R5 : D I SP POS I TI ON. CHARACTER CωNT 215 SHlR. It R3 :/ 4 145 J5R 自民 XDSP 216 制 P. 日韓 7. R3L 146 L5 : MOV. W !l YDAT. R2 : V ANGLE CONVERT
217
日 LS L8 147 船 V. W 自 VDT 蜘l. R1 218 船 V . B 品 7 . R3L 148 CMP . W R1. R2 219 L8: EXTU. 1I R3 149 BGT L51 :R2>VDTMH 220 EXTU. L ER3 150 船 V. '11 @VD 丁目 L.R1 221 MOV . L #DS I N8. ER4 151 CMP . W R 1. R2 222 5UB. L ER3. ER4 152 BLT L7 :R2<VDT 肌 223 B5ET #0.@ER4 153 BCLR 書 6.@PBOR ;BRAKE 224 LX01: MOV. 1I @XOAT. R2 ;X ANGLE ∞ NVERT 154 BCLR 書 7. Ii PBOR 225 MOV. 1I @XOTMH. R1 155 MOV . W #0. R2 226 CMP. W R 1. R2 156 MOV. W #0. R3 227 BGT LO :R2>XOTMH 157 BRA L56 228 削 V . W !l XOTML. R1 158 L51 ・ BCLR 持 6. @PBOR ;VCCW CLEAR 229 CIIP . W R 1. R2 159 85ET 時 7.Il PBOR ;VCW 5ET 230 乱 T L3 ; R2 <X OT 民 160 MOV . WI!
YDTM . R1 231 BCLR 時4. !lP BOR : BRAKE 161 5UB . W R 1. R2 :R2-YOTM 232 B 乱 R #5.@PBOR 162 MOV . 1I R2 . R3 233 MOV . W #0 . R2 163 5HA L. W R3 ;本 2 234 MOV . W 押 O. R3 164 明 P .1I #498. R3 235 BRA ι1 165 BLT L52 236 LO ・ BCLR 非 4 . @PBOR :XC 側 CLEAR 166 MOV 曹韓 498. R3 237 BSET 持 5 . !lP BOR :XCW 5ET 167 BRA L56 238 MOV . W @XDTM. R1 168 L52: CMP. W 帯 10. R3 239 5UB. 1I R 1. R2 ;R2-XOTM 169 BLT L56 240 MOV. W R2. R3 170 附 V . W #350. R3 241 SHA L. W R3 :*2 171 L56: MOV . 11 R3 . Il BRB3 242 CMP. W #498. R3 172 MOV . W R2. R3 243 BLT L01 173 5HAR. W R2 :/2 244 MOV. 11 #498. R3 11774 5 SHAR W RU2 H-:/4. 州Gl E 245 BRA L1 MOV. W #H' 1003. R5 246 L01: CMP . W 草 10. R3 176 J5R 解 LXOSP 247 BLT L1 。可 177 !!2~. ~ ~~Ç~16~!f8. ER4 : !表t" 示データ番地を ER4 にセットする 248 船 V . W 書 350. R3 178 MOV 日韓 H' 2B. ROL 249 L1 船 V. W R3. 網 RB4 。 1 179 MOV . B ROL . IlE R4 250 MOV . 1I R2. R3 180 SII.R. W R3 :/2 251 SHAR . 1I R2 181 Slt.R. W R3 :/4 LEO P0 51 T1 0N 252 SHAR . 1I R2 182 CMP. B 時 7 . R3L 253 MOV . W #H ・ 0003 . R5 183 BLS L6 254 JSR 肝 LXOSP 184 MOV. B 韓 7 .R3L 255 MOV. L #山 0162+12. ER4 ;表示データ番地を ER4 にセットする 185 L6: EXTU. W R3 256 MOV. B 韓 H ・ 2B. ROL :" t" 186 EX TU. L ER3 257 船 V. B R乱 .@ER4 187 AOO. L #DSIN8. ER3 ;5 I NHP5-0) 258 SHL R. 11 R3 :/2 188 MOV. L . ER3. ER4 259 SHL R. 11 R3 ;/4 189 B5ET 持 O . 骨 ER4 260 CMP . B 韓 7. R3L 190 BRA LX01 261 BLS L2 191 L7: BCLR #7 .@ PBDR ;VC 1I CLEAR 262 MOV . B 器 7. R3L 192 B5ET #6.@PBDR :VCC 1I 5ET 263 L2: EXTU. 1I R3 193 陣OV . W @VOTM. R1 264 EXTU. L ER3 194 5UB .胃 R2. R1 ;VDTM-R2 265 ADD . L #05 I N8. 四 3 195 IIOV. W R 1. R2 266 B5ET 書1.@ ER3 196 MOV. W R 1. R3 267 BRA L9 197 SHA L. W R3 ;柁 268 L3 BCLR 韓 5. !lP BDR ;XCW CLEAR 198 CMP . 11 #498. R3 269 BSET 骨 4. @P BDR ;XCCW SET 199 BLT L7 270 MOV 開。 XDTII. R1 200 IIOV.II #498. R3 271 SUB . W R2. R1 :XDTM-R2 201 BRA L7 6 272 船 V. W R 1. R2 202 L7 2 CMP . W れ O. R3 273 MSOHV AL W R1. R3 203 BLT L76 274 W R3 ;本 2 204 即 V. 明書 350. R3 275 CMP. 1I非 498. R3 205 L7 6 ・ MOV. 11 R3. Il BRB3 276 BLT L31 206 MOV. 1I R2 . R3 277 船 V. 1I韓 498. R3 207 SHA R. 11 R2 ;1/2 278 BRA L32 22008 9 5HAR. W R2 :1/4 279 L31: CMP. W 草 10. R3 MOV. 1I #H'lD03. R5 280 日 LT L32 210 JSR @FLXDSP 281 制 V . 1I #350. R3 211 MOV. L #LCD162t28. ER4 表示デ一歩番地を ER4 にセットする 282 L32: 船 V. W R3. jjBRB4 22 12 船 V . B 韓 H' 20. ROL ー 283 MOV. W R2 . R3 13 MOV . B ROL.@E
R4 284 SHAR. W R2adx 12026r . src Page 5 adx12026r. src Page 6 285 SHAR. W R2 356 MOV. L @RINGY. ER3 286 MOV. W #H' 0003. R5 357 CMP . L 時 DATAVY. ER3 287 JSR 。凡 XOSP 358 8LT RINGO 288 IIOV. L 札 C0162t 12 . ER4 表示データ番地を ER4 にセットする 359 8RA RING 289 IIOV 日韓 H' 20. R 札 -360 290 MOV. B ROL. II ER4 361 IR03 : P 凶 H . W RO 291 SHLR. W R3 :12 362 MOV. W 骨 GRA 1. RO 292 SHL R. W R3 :/4 363 MOV. W RO. 骨 XDTM 293 CMP. B #7. R3L 364 ADD. W #2. RO 294 BLS L4 365 MOV. W RO. 骨 XDT 剛 295 MOV.B れ R3L 366 MOV. W IIGRA 1. RO 296 L4 EXTU. W R3 367 SUB. W #2 . RO 297 EXTU. L ER3 368 MOV. W RO. @XDT 札 298 MOVι 韓関 IN8 ， ER4 369 MOV. W IIGRA2 , RO 299 SUB. L ER3. ER4 370 附 V . II RO , IIYDTM 300 BSET 韓1，e， RER14 L 371 AOD. W 非 2 ， RO 301 L9 MOV.B 韓 15 :DATA COUNT 372 冊OV. W RO , @YDT 附 302 MOV. L #DSIN0 t1 5. ER2 373 MOV. W 骨 GRA2 ， RO 303 MOV.B8DSlNO , 5 ROL 374 5U S. W 書 2 ， RO 304 MOV . B RO L. @P5DR 375 MOV. W RO , @YDT 札 305 B5ET 持 7， @P4DR ;CL 侃 K H 376 POP . W RO 306 船 P 377 RTE :EXCEP Tl ON 307 BCLR 豊韓田7. 82P , 4Rm OL :CLOCK L 378 :ーーー サブルーチン一一ー 308 L10 ・ MOV. B 379 :ー--8 回の平均値を求める 309 MOV. B R 乱， IIP5DR 380 DATAV: PUSH . W RO 310 BSET #7 , @P4DR ;CLOCK H 381 PUSH. W R1 311 NOP 382 MOV. W @DATAVY , RO 312 BCLR #7 , @P4DR :CLOCK L 383 MOV. W @DATAV Yt 2 , R1 313 DEC. L #1. ER2 384 ADD. W R 1. RO 314 DEC . B R1L 385 MOV. W IIDATAVYt4 , R1 315 BNE L10 386 ADD . W R 1. RO 316 帥 OV . 8 #0 , ROL 387 MOV. W
