1 1 PI 2 1 2 ( 1) H8-Tiny H8/3672 HD64F3672FP 16 CPU ( 16MHz) ROM 16KB, RAM 2048 2KB AD LED 1(c) ( A ) 赤外 LED 前 CdS セル LED 表示 RS232C 端子 (PC との接続端子 ) センサ類 ラインセンサ x3 (LED+CdS セル ) モータドライバ ターゲット Start ボタン ª ª Boot Mode 切替スイッチ 後 (a) Top View ª ªªªª«tˆ«ªªªª MPU H8/3672F OFF ON Resetボタン Power SW (b) 後部 明るさセンサ x2 CdS セル マイク エンコーダ AD DIO マイコン 4 連 LED 表示 スピーカー AD: AD 変換 ( アナログ デジタル信号 ) 赤外線受信 IC DIO: デジタル信号の入出力エンコーダ : 車輪の回転に伴いパルス発生 *) 本実験に利用しないデバイスも含まれています *) 詳細な配線図は章末付録を参照 (c) 簡単な内部接続図 開発ホスト RS232C 端子へ接続 (d) PC との接続 1:
3 3.1 2 LED 2 2 2 2 モータ ディスク LED 受光素子 ( フォトダイオード ) パルスを数えるデジタル回路を構成すると回転角度が検出可 拡大図 モータ回転軸 スリットがある ロータリーエンコーダ 回転方向は 2 つの信号の位相差から特定できる 2: 3 LED Wheel LED& 受光素子 車輪の回転に伴ってパルス波形が生じる ギア Motor ギアボックス内部 白黒の帯が描かれた回転面 車輪 3:
3.2 PI P 4 外乱 ( 摩擦など ) T dis 目標の速度 ω cmd + - 電流 K A Kt p P 制御ゲイン トルク定数モータ駆動回路 - + モータ 1 Js + D ω 実際の速度 4: P P 1 I PI 5 目標の速度 ω cmd + I 制御ゲイン - Ki 1 s 積分 + + 電流 K A Kt p P 制御ゲイン 外乱 ( 摩擦など ) T dis トルク定数 - + 1 Js + D ω モータ駆動回路 モータ 実際の速度 5: PI
4 1 4.1 1 C PC AM PM X Y 2 HEW4 RobotJikken.hws MPU HEW4 6 OK 6: HEW4 Assembly source file C source file main.c main.c C 3 main while(1) while(1) Start 1 user main() main Start user main() #pragma RAM user main() wheel cnt right wheel cnt left PC int user main()
#pragma section USR void user_main(void) { float t=0.0; wheel_cnt_right=0; wheel_cnt_left=0; while(1){ Wait( 100 );//1s t=t+1.0; #pragma section // RS-232C PC print_float( t ); // t print_string( "\t" ); // print_int(wheel_cnt_right); // print_string( "\t" ); // print_int( wheel_cnt_left); // print_string( "\r\n" ); // 4 PC SysJikken.exe RAM ROM RAM C SysJikken.exe SysJikken.exe, 1 4.2 P P 1 1 θ right = wheel cnt right 2π (1) 120 PC π=3.14 1 2π
#pragma section USR void user_main(void) { float t=0.0; -> float theta_r, theta_l; // wheel_cnt_right=0; wheel_cnt_left=0; while(1){ Wait( 100 );//1s // -> theta_r = (float)wheel_cnt_right/120.0 * 2.0* 3.14 ; -> theta_l = (float)wheel_cnt_left /120.0 * 2.0* 3.14 ; // RS-232C PC print_float( t ); print_string( "\t" ); -> print_float(theta_r); // _float() print_string( "\t" ); -> print_float( theta_l); // _float() print_string( "\r\n" ); t=t+1.0; #pragma section 2 (rad/s) 1 (1 ) ω right ω right (k)= θ right(k) θ right (k 1) Δt (2) θ right (k) θ right (k 1) 1 (2) (2) ω right (k)=αω right(k)+(1 α) ω right (k 1) (3) (3) ω right (k) ω right (k 1) α α = 0.1
#pragma section USR void user_main(void) { float t=0.0; float theta_r, theta_l; -> float theta_old_r=0.0, theta_old_l=0.0; -> float omega_r, omega_l; -> float omega_old_r=0.0, omega_old_l=0.0; -> float alpha=0.1; -> int loop_counter=0; wheel_cnt_right=0; wheel_cnt_left=0; while(1){ -> wait_for_10ms_timer(); // 10ms // theta_r = (float)wheel_cnt_right/120.0 * 2.0* 3.14 ; theta_l = (float)wheel_cnt_left /120.0 * 2.0* 3.14 ; // -> omega_r= (theta_r-theta_old_r)/0.01; //(2) -> omega_l= (theta_l-theta_old_l)/0.01; -> omega_r= alpha * omega_r +(1-alpha)*omega_old_r;//(3) -> omega_l= alpha * omega_l +(1-alpha)*omega_old_l; -> theta_old_r=theta_r;// (k-1) -> theta_old_l=theta_l; -> omega_old_r=omega_r;// (k-1) -> omega_old_l=omega_l; // // P PI // // RS-232C PC -> if( (loop_counter%10)==0 ){ // 10 1 print_float( t ); print_string( "\t" ); -> print_float( omega_r); // print_string( "\t" ); -> print_float( omega_l); // print_string( "\r\n" ); -> -> loop_counter++; -> t=t + 0.01; // 1 0.01 #pragma section rad/s
3 P P u right = K p (ω cmd ω right ) (4) K p ω cmd (rad/s) ω right u right ( ) Motor R( (int)u right ); Motor L( (int)u left ); Motor R()/Motor L() B P K p = 2.0, ω cmd = 2π [rad/s] 4 SysJikken.exe 7 K p K p (rad/s) 1.0 2.0 4.0 6.0 7: 4.3 PI P
I P I PI PID 1 PI e right e right = ω cmd ω right (5) s 1/s e right u right = K p e right +K i {{{{ s P I (6) 1 P 2 2 e right K i 8 長方形で近似 現在時刻 Δt e right time 長方形の面積を足し合わせていく 8: 8 u integ right = u integ right+ K i e right Δt (7) u integ right (6) 2 Δt 0.01 P PI K p = 2.0, K i = 5.0 ω cmd = 2π [rad/s]
2 K p = 2.0 K i K i (rad/s) 0.0 2.0 5.0 8.0 3 P PI P PI P K i K p =2.0 PI K p =2.0, K i =5.0 SysJikken.exe 4.4 PI 9 2 3 rad/s 2 2 6 rad/s 2 3rad/s 2 [rad/s] ω cmd 6.0 加速区間等速区間減速区間 停止 0 2 4 6 Time [sec] 9: 1 9 2
3 main.c user main() user main() OK #pragma section USR void user_main(void) { // // P PI // if( t < 2 ){ // omega_cmd = ; else if( t < 4 ){ // omega_cmd = ; else if( t < 6 ){ // omega_cmd = ; else{ // omega_cmd = 0 ; //PI #pragma section
5 2 5.1 1 10 v m/s ω rad/s 1 ω left 車輪の間隔 W 並進速度 v ω right ω R 車輪の半径 並進速度 v ω 回転速度 10: 5.2 [ω right,ω left ] (8) [v, ω] [ ] [ ][ ] v R R ω = 2 2 ω right (8) ω left R W R W R W 10 v, ω PI e v e ω e v = v cmd v e ω = ω cmd ω (9) v cmd ω cmd PI u v = K p e v + K i e v s u ω = K p e ω + K i e ω s (10)
u v u ω (8) [ω right,ω left ] [ ] [ ] 1 [ ] ω R R right = 2 2 v R ω left W R W ω [ ][ ] 1 W = R 2R v (11) ω 1 R u v u ω (11) (11) u v u ω [ ] [ ][ ] u 1 W right = R 2R u v (12) u left u ω 1 R u right u left Motor R( (int)u right ); Motor L( (int)u left ); 11 T (8) W 2R W 2R ロボットの目標速度 v cmd ω cmd + I 制御ゲイン - Ki 1 s 積分 + + 1 電流 K T A Kt p P 制御ゲイン 座標変換 モータ駆動回路 外乱 ( 摩擦など ) T dis トルク定数 - + モータ 1 Js + D ω ロボットの速度 v ω T 座標変換 車輪の回転速度 ω ω right left 11: PI 1 1 1 RobotJikken.hws HEW4 2 1 R W ( m ) K p = 6.0, K i = 12.0, v cmd = 0.2 [m/s], ω cmd = 0.0 [rad/s]
#pragma section USR void user_main(void) { float t=0.0; float theta_r, theta_l; float theta_old_r=0.0, theta_old_l=0.0; float omega_r, omega_l; float omega_old_r=0.0, omega_old_l=0.0; float alpha=0.1; int loop_counter=0; -> // -> wheel_cnt_right=0; wheel_cnt_left=0; while(1){ wait_for_10ms_timer(); // 10ms // theta_r = (float)wheel_cnt_right/120.0 * 2.0* 3.14 ; theta_l = (float)wheel_cnt_left /120.0 * 2.0* 3.14 ; // omega_r= (theta_r-theta_old_r)/0.01; //(2) omega_l= (theta_l-theta_old_l)/0.01; omega_r= alpha * omega_r +(1-alpha)*omega_old_r;//(3) omega_l= alpha * omega_l +(1-alpha)*omega_old_l; theta_old_r=theta_r;// (k-1) theta_old_l=theta_l; omega_old_r=omega_r;// (k-1) omega_old_l=omega_l; -> // (8) v, -> // (9) e_v, e_ -> // (10) PI 1 -> // (12) Motor_R( (int)u_right ); Motor_L( (int)u_left ); // RS-232C PC if( (loop_counter%10)==0 ){ print_float( t ); print_string( "\t" ); print_float( omega_r); print_string( "\t" ); print_float( omega_l); print_string( "\r\n" ); loop_counter++; t=t + 0.01; #pragma section // 1 0.01
3 v cmd ω cmd 0.25m 4 4 1 360 4 rad/s 4 12 (A) (B) (B) (A) (8) 1 1 if 4 辺の直線はすべて 0.1m 0.1m 8 秒で 1 周 ( 等速 ) 曲率半径 0.15m 0.1m 8 秒で 1 周 ( 等速 ) 曲率半径 0.15m 4 隅のカーブはすべて同じ円弧 スタート地点 3 つのカーブはすべて同じ曲率の円弧 0.1m スタート地点 45 度 (A) (B) 12: 5 main.c user main() user main() OK
6 1 2 1 1 2 1 P PI PI K p K i 2
(BANDAI) P.42
Act Fwd(int p1,int p2) Act Bwd(int p1,int p2) Act Turn FR(int p1,int p2) Act Turn FL(int p1,int p2) Act Turn BR(int p1,int p2) Act Turn BL(int p1,int p2) Act Rot R(int p1,int p2) Act Rot L(int p1,int p2) Act Stop(void) Motor R(int p) Motor L(int p) Wait(int t) wait for 10ms timer(void) wait for 1ms timer(void) AD get ad(int ch) get line sensor(int ch) PC print int( int val ) print float(float val ) print string( char str[] ) Wait(0) Wait p1: 1 31 p2: [cm] 1 200 ( 250 ) p1: 1 31 p2: [cm] 1 200 ( 250 ) p1: 1 31 p2: [deg] 1 360 ( 400 ) p1: 1 31 p2: [deg] 1 360 ( 400 ) p1: 1 31 p2: [deg] 1 360 ( 400 ) p1: 1 31 p2: [deg] 1 360 ( 400 ) p1: 1 31 p2: [deg] 1 360 ( 400 ) p1: 1 31 p2: [deg] 1 360 ( 400 ) ( ) p: -31 31 ( ) p: -31 31 t: 10ms 1=10ms, 100=1s t=0 10ms 1ms AD int ch: 0 3 (CdS) (AD ) int ch: 0 2 0= 1= 2= int PC val: int float PC val: float 2 PC str: "YrYn"
RAM ROM RAM 13 MPU RAM 2KB user main RAM RAM RAM 1 user main() RAM ROM RAM int sci3() Down Data() RAM RAM X Y YRobotJikkenYRelease RobotJikken.mtr RobotJikken.mot user main() Step1 RAM SysJikken.exe Step2 RobotJikken.mtr!! Step3 RAM 4 LED 2 Start RAM RAM #pragma section 2 ROM ROM 内蔵メモリのアドレス空間 アドレス ( 番地 ) 0x0000 割り込みベクタ 0x0034 0x3FFF ROM ここにプログラムコードが格納される «ªªªªªªªª«電源を切っても保持される!! 16KBのROM( フラッシュメモリ ) 先頭の 割り込みベクタ は ハードウェア割り込みの設定用 ROM は実行中 書き換えることができないため プログラムや定数データの格納に使う ブートモードに設定することで ROM の内容は書き換えることが可能 未使用アドレス空間 16KB の ROM( フラッシュメモリ ) と RAM を内蔵 0xF780 RAM スタック領域に利用 0xFF80 内部 I/Oレジスタ «ªªª ªªª «0xFFFF 電源を切るとデータは消滅する! 2KB の RAM RAM は実行中 自由に書き換えることができるため 通常では値の変更を伴う変数データの格納に使う またプログラムコードを置くなど任意に使うこともできる 13: HD64F3672FP
C while while( 1) 1 1 ( ) 1 i=0; while( i< 10 ){ ( 1) i++; i=0 1 i 10 1 1 1 1 1 i++ 2 i 10 for 1 10 2 while(1){ ( 2) 1 1 0 for(;;) break (break ). while(1)
for for( 1; 2; 3) 1 2 3 1 for(i=0; i< 10; i++){ ( 1) i=0 1 i++ i++ i=i+1 i 10 2 1 i 10 1 1 10 2 for(;;){ ( 2) 1 2 3 break (break ).
do while do { 1 while( 1) 1 1 1 ( ) 1 i=0; do{ ( 1) i++; while( i < 100 ); 1 i 100 do{ 1 1 99 break break; do, for, while, switch 1 while(1){ ( 1) if( value == 0) break; value 0 (if ), continue continue; do, for, while 1 while(1){ 1 if( value == 0) continue; 2; value 0 2
if if( 1){ ( 1) else{ ( 2) 1 1 2 1 if(i==1){ ( 1) else{ ( 2) i 1 1 (== ) 2 2 if(i==1){ ( 1) else 3 if( i>=0 && i<=5 ){ ( 1) && i =0 i =5 AND 4 if( i!=0 i!=5 ){ ( 1) i!=0 i!=5 OR!= 5 if( i<5 ){ ( 1) else if( i<10 ){ ( 2) else{ ( 3) else if i 5 1 else if i 10 2 3
switch, case switch( 1 ){ case 1: 1 (break;) case 2: 2 (break;) default: 3 1 default case case break switch 1 if switch 1 switch( value ){ case 0: 1; break; case 1: 2; break; case 2: 3 break; default: 4; value 0 1 1 2 2 3 4 switch
goto goto ; goto 1 my_func(){ if(i == 1) goto label1; i=i+3; label1: i=j+2; if label1: switch continue, return { goto goto
== = = = 1== 2 1 = 2 2 True, False if while 1 if( i==1 ) i=i+1; if( j<=1 ) j=j+1; if( k>=1 ) k=k+1; if( m!=1 ) m=m+1; if( n <1 ) n=n+2; if( i=1 ) break; == = i=1 i=1 break ++ -- ++, --,++ -- 1 1 - - 1 i=0; j=0; i++; j--; val=++i; val=i++; ++(- -) val=++i; val i i=i+1 val=i++; i val i=i+1
&& AND OR 1&& 2, 1 2 &&: 1 2 : 1 2 1 if( a==1 && b==1 && c==1) i=10; if( x==1 y==1 z==1 ) i=0; & AND OR ˆ OR 1& 2, 1 2 1 a1=0x00ff; a2=0xff00; a3= a1 & a2; // a3=0x0000 a4= a1 a2; // a4=0xffff 2 a1 a2 AND OR 2 a1=0x01; a2=0x10; a3= a1<<1; // a3=0x02 a4= a2>>1; // a4=0x08 2 a1 a2
rand() srand() 0 32767 rand(), srand( seed value ) seed value 1 value=rand(); 0 32767 value rand() rand() srand() 2 srand( time(null) ); //srand value=rand();
printf() printf(...,...) printf: 1 printf("my Name is CIST.\n");... \ n 2 i=100; printf("answer= %d \n", i); %d i %d 3 value=0.1234; printf("answer= %f \n", value); %f value %f 4 i=10; value=0.1234; printf("answer= %d, Value = %f \n", i, value); Answer = 10, Value = 0.123400