jikken.dvi

Size: px

Start display at page:

Download "jikken.dvi"

せとかみうら
6 years ago
Views:

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 表示

MPU H8/3672F OFF ON Resetボタン Power SW (b) 後部明るさセンサ x2 CdS セルマイクエンコーダ AD DIO マイコン 4 連 LED 表示スピーカー AD: AD 変換 ( アナログデジタル信号 )

1 1 1 PI ( 1) H8-Tiny H8/3672 HD64F3672FP 16 CPU ( 16MHz) ROM 16KB, RAM KB 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 LED モータディスク LED 受光素子 ( フォトダイオード ) パルスを数えるデジタル回路を構成すると回転角度が検出可拡大図モータ回転軸スリットがあるロータリーエンコーダ回転方向は 2 つの信号の位相差から特定できる 2: 3 LED Wheel LED& 受光素子車輪の回転に伴ってパルス波形が生じるギア Motor ギアボックス内部白黒の帯が描かれた回転面車輪 3:

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 トルク定数 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.

4 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()

5 #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, P P 1 1 θ right = wheel cnt right 2π (1) 120 PC π= π

6 #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

7 #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 ; // #pragma section rad/s

8 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) : 4.3 PI P

9 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]

10 2 K p = 2.0 K i K i (rad/s) 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 rad/s rad/s 2 3rad/s 2 [rad/s] ω cmd 6.0 加速区間等速区間減速区間停止 Time [sec] 9: 1 9 2

11 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

12 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)

13 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 積分電流 K T A Kt p P 制御ゲイン座標変換モータ駆動回路外乱 ( 摩擦など ) T dis トルク定数 - + モータ 1 Js + D ω ロボットの速度 v ω T 座標変換車輪の回転速度 ω ω right left 11: PI 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]

14 #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 ; #pragma section //

15 3 v cmd ω cmd 0.25m 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

16 P PI PI K p K i 2

17 (BANDAI) P.42

18 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] ( 250 ) p1: 1 31 p2: [cm] ( 250 ) p1: 1 31 p2: [deg] ( 400 ) p1: 1 31 p2: [deg] ( 400 ) p1: 1 31 p2: [deg] ( 400 ) p1: 1 31 p2: [deg] ( 400 ) p1: 1 31 p2: [deg] ( 400 ) p1: 1 31 p2: [deg] ( 400 ) ( ) p: ( ) p: 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.

19 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

20 C while while( 1) 1 1 ( ) 1 i=0; while( i< 10 ){ ( 1) i++; i=0 1 i i++ 2 i 10 for while(1){ ( 2) for(;;) break (break ). while(1)

21 for for( 1; 2; 3) for(i=0; i< 10; i++){ ( 1) i=0 1 i++ i++ i=i+1 i i for(;;){ ( 2) break (break ).

22 do while do { 1 while( 1) ( ) 1 i=0; do{ ( 1) i++; while( i < 100 ); 1 i 100 do{ 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

23 if if( 1){ ( 1) else{ ( 2) 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

24 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 switch

25 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

26 == = = = 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 , --, i=0; j=0; i++; j--; val=++i; val=i++; ++(- -) val=++i; val i i=i+1 val=i++; i val i=i+1

27 && AND OR 1&& 2, 1 2 &&: 1 2 : if( a==1 && b==1 && c==1) i=10; if( x==1 y==1 z==1 ) i=0; & AND OR ˆ OR 1& 2, 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

28 rand() srand() rand(), srand( seed value ) seed value 1 value=rand(); value rand() rand() srand() 2 srand( time(null) ); //srand value=rand();

29 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 =

jikken.dvi

jikken.dvi I 1 ROM( ) RAM 1 2 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