緩やかに発進停止できる 全方向移動型歩行訓練機
Contents
1. I J PC20 UPP O PCAOA 000000000 E EEB EB EH B 0D0DD000000000000000000 IROOAODOAODADOOOAO x 4 ISAOOO 64000 40000000000 00000000 0 20 0000000000 21 2 AJD D A D OO 2 9 4 29 A D4D A 22000 12000 000000 AJD Ll D LI D D 12bit 00000000 8ch OO 00 4ch 8u sec 000000000 000000 00 5V OO 10VQ 2 5VO 2 5V0 D A LH B LU LU HU 12bit 00000000 4ch 8u sec max 000000 05v O10VO 5v 5vo ch 000 U uuu 90 1mmx 95 8mm 0 20 0000000000 22 VD 320mA OD 90 1mmx 958mm 00000000000000 0 000 112W100000 U DOOD 0 PC 1042. king Ads OOODOOOODODDODOODODOODODOODODDODOOOODY OOODOOOODOUDODODODDODOODOLD O10 SRC DB BH BL D D B lU SKOODAAODAAODAAODAOODAAODAAODOAODAADAADAAODAAD OOODODOOD OOODODOODOODDODO AEAOAEREAEAAAABAAAHABAAREEDE OOODODOODODODODOODDO OOODOUDOODODDODOODODOO OOODOUDOODODDODOODOD BB 1 9 0606 La wm so es mo 75 mm B jote kosi 190 240 270 270 c 7 7 450 600 500 700 650 900 760 1100 mm ua ES 3 870 I AME nan go y E ES we s wo so 1 10 3 00 1 1 O10 10 DERE D 10 4 OO SKO0D00000000000000000D MOODODOOODODODODODOODODOODODOODOODDO O20 0000000000 11 020 OOODOODODOOODODODDODOODODO 1 OOOAOAd 3600 OOAAODAODAODAODOAOO 2 0000 36 2 5 Lo EP AE LEI A tH ipi ELE ta ENE
3. unt 0000 VT1 VT2 VT3 KPDD O VT4 0000000 kt D O VT5 SW1 2 3 2 4 o gt OODOODO o gt 110000000 O gt OPEN gt OODOOOAOOAODOAODODOAOAODAOODAOODAAODOAAD OOODOOODUDOODOODOODODODODOODDODOOD 2 Radii 000 2D0D00 WIQO0O0000000 VI res 100000 500 Ra 3 Nturns KO 020 VILE OM TinadO 1000000000 000000070 WriposD0D0 22000000 VTlres 0 20 00000000000 29 VT Ires VT1 max D 2 2 Nuns 39 00000 Vilnax 20K0 Dan Vip O00 VID0000000000D00000000000000000D OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO VTlpos OOODOODODODOD aep 4O0ADIGBAODAODAODAOOO pseudo pseudo rpri Ten OO H I I I Voseudo VA n 2 300000 000000 Vrated ops
4. 7000000 000000 00000000 8000000 000000 00000000 O 20 0000000000 15 000 000 10 0000000 0 2 2 3 1 00 2 5 O2500000000000000000000 40000000 O 2 60 0 MM 260000000000 MM 26 00000000000 30 W D 7 8 amp kgcm OO 00000000000000000000000000 2600000 000000000000000 O20 0000000000 16 ERB6J CT0845CT 18 GATE crit ME A 4 a p 4 Mi m Bu 109 x 118 EE METER 2 2 4 0000000 ODAODOAOAODAOAOOAELDAIODAODAOAODAAABOAADADAODOO TTL ONO Du 00000000 ONOOAAODAAODOAODAAODAADAAODAADAADAODO 0000000000000000000 22 000000 0 20 0000000000 2 7 000000000 000000000 23 1 D Uu o Key n DD 000000000000 2 20 O000000000000 OU O DODDO DODDO DODDO DUDU DUDU DUDU DODDO L J aI Oo 0 0000 00 00 0100 00 OOO o00000 000 00000D 000000 0000000 000000 0000000 000000 0000000 000000 0000000 17 0 20 00000000000 0 0000 100000000 0000 000 OODUOODODIDOODDOOD DOO ODOD00 1000M0000 000 gt x ODD00 1000M0000 000 gt x 0 0000 0 0 0000000000 0 0 Key 0000 000 O
5. e e 00000000000000 O10 D 1 1 MAODODAODDAWOAADAOAILLODALIADAAODAADAAODAADOA 0000000 O10 0000 00 MOOO 07 ms DDD 80000000000000 O 30000 000000000 150 30 kg 300 50 kg MOOD 500 70 kg BEER OD O10 Du 0000000000 O Q 9606 606 6 OU e OOODOODODOODODODODOODODOADODODODOODODO OOODOODODOODODDODOO OOODOODODOODODODODOODODOOODODOADODDODO 00000 OOODOODODOODODDODOODODOODODOADOODDODO 0000000000 OOODOODODOODODDODOODODOODODOODOODDODO OOODOODODOODODODODOODOOOODODOODOODDODO OOODOODODOO O 0000000 OOMODODOODODDOLIDDOODODOADOD
6. 2 00000000000 3 0000000000000000 a 4 OOODOOOODODODODODDODOODODO 0000000000 000000 O10 Du 2 2 000000000 30 D000000000000000000000000000000000000D OOODOODODOODODODODODOODODIDODODADOODODOO OOODOODUDOODODODODOODOODOULODODODDODOOOODY 4 OOODOODODOODODODODOODODOODODOODOODOOLD 13100000000 000000000000000000000 D UOULU LU x 00000 0011 1 O10 Du D Uu 141 o SRCH O0 0 O0 O Spontaneous Reaction Control WI ker grand 000000000 o
7. st p O lt i lt 4 Yes calculation isetda No Y locate 1 10 Y display Message 52 0 40 00000 53 EI AM EE OODDDDDDDDODDDOOOOOOOOuUg OODDDDUDDDDOOODOOOOOOODODOLDDLDULDDDODOOOOOI OODDDDUDDDDOOOOOOAOOOODODOLDLDLUDDDODOOOOOY ee A E E I I PE E E E E E E E E E EE EEE OODDDDDDDDODDOOOOOOOOuUg Add Em dd E HERE Er HEH UB as Ar or d d Be Ua AB a EE me d d ee AE GE HE SD Speed 300 ad data 5 5 0 1 4 1 D 4 0 0 4 20 D00000000000000000000 Speed 100 ad data8 5 5 0 p 4 2 DU DU DU EEE AE EA Bop AOL EEB A HE EJ Gay EI EI EE ul t EST c Es EI LT EA EI LIEGE d Zl ELE Ed Ed Ep de Hs BR d HELD GAAP He Stop n Stop 0 uu uut Stop 1 uut Stop 2 uu uut Stop 3 OOOO DUDDDDDUU uu Stop n 00 A0 AOOAODOADAAODAAODAAODAODOAODAODA 0 40 00000 54 0000004100 Stop 2 TEAR Stop 1 DRI ABE Stop 3 ARE Stopz0 FHAR Stop feiETARE 4 1 5 0 4 i 0
8. 2 3 0000000 0D000000 00000000000 A D O 900 1 11V 227 2 50 512 U 900 3 89 796 2 3 157 253VO0O0OO ADOOOOOO 10bit 00000 OVOOAODODAODDADOA 51800 144 0 6 0000000 2 12 0000000000 T TECH R ERI VER 2 12000000 OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO 0 20 00000000000 27 OOOODOOODDOUDDOODOUDDOODOUDDOODOOODOODOOO OOOODOOODOUDDOODOUDDOODOUDDOODOUADOODOOO OOOODOOODDOUDDOODOUDDOODOUDDOODOUADOODOOO OD LU LU 000000 30V 000000 3 8A 000000 45V 000000 OO 16 7A 0000 750W 0000000000 OU DC12VO 48V 000000 10V 0000 100x 90x 35 1309 00000 00000 0000 L 0000 0000 0N 16 74 00000000 0000000 0000000000 DU uuu 0000000 0000000 OOODOODODOO OD 0 OOOOOOOOOGSOWO 0 0 MOAAODOOD 0 DOODODOOODODODODUDOODOLD U DOODODOOODODODDUDOODODOOD 0 20 00000000000 28
9. E EA rs r N EA EI E FA EA 2 2 00000000000 2 1 OOODOOODUDOODOODOODOULODADOODDODOODODIDOODDODOO OOOODOOODDOUDDOODOUDDOODOUDDOODOOADOODOOO OOODOOOODODDODOODODOODODDODOODDOD 00000000000000 0 20 0000000000 12 2 2 1 000000 2 000000000 OUOADOODOODDODOO OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO 0 20 0000000000 13 2 2 0000070 230 0 0000000000000000000000000 ODOOODOOODDOUDDOODOUDDOODOUDDOODOOODOODOOO OOODOOODODODODODOODODOODODOODODDODOOOODO TYPE2Z5 70D AB 48mnp P315 8 2nn amp mnd 7 HB 58 gto HW 7U 25 503 27 OLN BA LTS Ila HEXOTYPE2571 T DEE AD 2571 Ott ET m w PRN mm i 2 3 O 20 0000000000 14 2 2 2 00000000 2 4 2 jusy p JSK 104000000000 Dugnuunuunuunuununnugnpnudg 2000000000000000 DU 1100000 000 00 2000000000 00 3000000 0007 00000 4000000 000700000 S000000 000000 00000000 6
10. 5 BS E 3R void isetda int ch int data gt BEZRIEE UTR L void Set Motor void B9XITIKHL void wait int time B9XITIKHIL unsigned char Getch int flag flag 0 255 DAA void JoyTest void B9ITUHL double JoyDetect1 void double JoyDetect2 void void DATest void gt BUFU HL void Out Speed double vx double vy double ee int Stop B9XFWK HL xyAngle n 22 Joysutick DBE err xyAngl ef 0 xyAngl 1 gt xyAng ef 01 11000000 dt enp xyAngl el 0 xyAngl el 1 2 0 0000 10 10 1 1 Ln HO OOOO 10 00000005 0000000 1 0000000000 0 30 0000000 44 Left 18117919 8 Right 31 dt enp xyAngl el xyAndl 1 2 0 00000 10 s err s 10 at enp xyAngl el 0 xyAngl el 1 1360 2 0 00000 360 err 350 dt enp xyAngl el 0 xyAngl el 1 360 2 0 00000 350 s err 360 31 abs abs err 180 errRange 2 2 err Joystick DODO errRange2 Joystick OOOOOO 30 OOODOOOODOOOOOOOOOAOOOd ad dat a85 gt 0 0 0000000000000 A 0 30 0000000 330000000 0000000
11. 00 3 CPU OO 2 10 CPU 0000 JPC10 V25CPU OOOO 2100000000000000000000 000 0000000000 000 J PC10 v25 000D CPU u PD70352GJ 10 000000 19 6608 MHz 000000 9 8304 MHz EEPROM HN58C1001P 15 RAM 512K 70000 SRAM Size 96mm x 90mm x 15mm 000000 00000 4Ch PC 104 0 0 00000000 0000000 Cho RS 485 1 0 20 0000000000 23 Ch1 RS 232C 0 D O000 1109 1159 2Kbps 96mmx 90mmx 15mm 75g uui 3 596 200Ma OO O PC14000000D 4 00000 2 11 000D00000000000000000000 PCPOWER OOOOOOOOO OO 144V1000000 4V 0 HAV 0000 000000 1700s5CRODD 7 2V 1700mA D0 000000 x 2n bv 0000 ZUS151205 5V 2 4A au 254mm 00 O 250 x100 862014200000000 0000 125V 10A 1 1 109mmx 90mmx 20mm 100g 0 20 00000000000 24 000000 OO 60 C OU 5 PWM Li H PWM 0 O00000 J PC20 UPP PWMDRV OD 000000000 V201 V202 CN2500000000000000000000 0 0 DODOODODOODOODOOLD D JPC20UPPDODDD LEDIUUS O UUSO OOOAOOAOAOO CN
12. if Stop i switch i 1 case 1 Speed 100 ad data8 5 5 0 case 2 Speed 150 ad data8 5 5 0 case 3 Speed 50 ad data8 5 5 0 UUDDUDDUDDBDDUDBDDUDDBDSHUDDBGuHGBDBBDUHUDBUpDUDUDBuDpDUun Add dB DAE 0 40 00000 55 L N EMEN EEE AE 3 Period OOODODOOODOOOOOAAOAAII OD M 0 5 1 cos PI nStart Period M 0 5 1 cos PI nStop Period 4 3 x MOOO M01000 M 1 0 If Stop 1 Period 100 if nStart lt Period M 0 5 1 cos PI nStop Period nStart Jjel sei Stop 2 if nStop gt Period Stop 0 return M 0 5 1 cos PI nStop Period nStop DUDDDDDUUGUudu OODDDDLDDDDDDOOO O50 000 55 0 50 DDD OOODOODUDOODODOODOODOODODODODODDODOODODY 1000000 00000000000000000000000000000000 52 000000 00000000000000000000000000000000000000 0D00000000000000000000000000000000000000D 00000000000000 0D00000000000000000000000000000000000000D OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO OOODOODODOADODODODOOOODODODOD
13. O OOOODDODOOODUDOOO 18 0 D OU OU UPPOOO J PC20 UPP 1 A D DAD OO T104 ADA TAC INC 1 CPU OO J PC10 25V 1 00000 J PC POWER 1 PWM OO J PC20 UPP PWMDRV 1 0000000 TITECH Robot Driver PCO121 1 4 BATTERY DC12V 7Ah 5 2 1 000000000 0000000000000D O 20 0000000000 19 LUPP I OO l O AIKANANI ALAA TAKA O r i mia t EETTTTI 22000001 e l i l LH 4 f J TT LT ir JUO rio 2 8 AD OO 10 CH OAOOAODAAODAAODAAODAAOD PWMOOAAODA 50000000 7 pu mn n nn 0000 D 00000000 DODO 0000 000000 0000 0000 0000 0O00 000000000000 0000 000000 0000 00000 0 0 0000 0000000000 D 0000000 0000000 Bu 0000 00000 0000000000 00000 0000000 OO 000000000 NM 000000 0 DD 000000 0 20 00000000000 20
14. chan data 0 30 0000000 39 locate 1 4 printf DA test Y n while ch 0x1b f dsxy 1 8 printf Please input channel 1 4 data 2048 2048 scanf d d amp chan amp data isetda chan int data 2048 printf YnYnPress lt ESC gt break and other key continue ch getch 560 Motor void MotionTest void i double xyAngle 2 5peed vx 100 vy 100 ee 0 err 0 errRange 10 double dtemp int Stop 0 fdef DEBUG ON locate 1 4 printf M otion test Y n printf Press lt ESC gt exit n endif while Getch 1 20x1b 1 xyAngle 0 J oyDetect1 xyAngle 1 J oyDetect2 if xyAngle 0 gt 800 xyAngle 1 gt 800 if Stop 1 Stop 2 Stop 3 else if Stop 0 Vx vy ee 0 Jelsef 0 30 0000000 40 if Stop 0 Stop 1 Speed 100 ad_data8 5 5 0 err xyAngle 0 xyAngle 1 if abs err lt errRange dtemp xyAnglel0 xyAngle 1 2 vx Speed cos dtemp 180 3 1415 vy Speed sin dtemp 180 3 1415 Jelse if abs err 360 lt errRange dtemp xyAngle 0 4 xyAngle 1 360 2 vx Speed cos dtemp 180 3 1415 vy Speed sin dtemp 180 3 1415 Jelse if abs err 360 errRange dtemp xyAngle 0 xyAngle 1 360 2 Vx Speed cos dtemp 180 3 1415 vy Speed sin dtemp 180 3 1415 Jelse if abs abs err 180 lt errRange if err gt 0 ee 100 else ee 100 OutSpeed vx vy ee amp Stop void OutSpeed double vx double vy double ee int Stop int i double M 1 0 Period static int n
15. 000 0mm 00 pokeb V25BASE PM2 0x00 00000000 void isetda int ch int data t ch 1 4 data 0 0x0fff 00000 if ch lt 0 ch24 return if data lt 0 data 0 else if data gt Oxfff data Oxfff outp DA1L Hch 2 2 data amp Oxff outp DA1L Hch 2 1 data gt gt 8 inp LDAC float ad datal int ch 0 30 0000000 35 float x outp ADL ch 1 select ch amp convert start while inp ADSTS amp 0x80 0 xx inp ADU 8 Hnp ADL 5 4096 0 return x float ad data8 int ch 1 8 t float sum 0 int i ch for i 0 i lt 6 i 4 outp ADL ch select ch amp convert start while inp ADSTS amp 0x80 92 sum Sinp ADL Hinp ADU lt lt 8 J sum sum 6 0 5 0 4096 0 return sum void SetO Motor void t isetda 1 2048 isetda 2 2048 isetda 3 2048 isetda 4 2048 flag 0 wait for enter flag 1 detect if enter unsigned char Getch int flag t unsigned char ch 0 30 0000000 switch flag case 0 case 1 return ch double J oyDetect1 void 36 while kbhit ch getch if ch 0 break ch getch if kbhit ch getch if ch 0 ch getchf else ch 0 break float xDelta 0 5 yDelta 0 5 dxDelta 0 05 dyDelta 0 05 static float xOld 0 yOId 0 xN ew 0 yN ew 0 dx dy static double xyAngle 888 xOld xNew yOld yNew xNew ad_data8 1 2 25 yNew ad data8 2 2 15 if fabs xN ew lt xDel
16. 0000000000 0000000000 1 Mi n0 O 20000000000 3 Test DAN O 4 Motion Test 5 Out Speed OOODODOODODODODOODODO 45 0 30 0000000 Main Function D gt define ch define port define Motor cls Display Main Menue Input ch keyboard ria ch 1 JoyTest break DA Test break Motion Test break Set Motor lt No Esc 46 0 30 0000000 47 Joytest Flowchart JoyTest Function Cm D define xMew 2 yhlew 2 Speed meter display Message locate 1 8 display Parameter DUDUDUDAest 000 Hovwhart 1 D DE D U 0 30 0000000 Test D start Y format amp define ch 0 define chan data Y locate 1 4 display Message ch Is xIbrEsc cixy 1 8 Main Function Y display Message Y input channel amp data Y output Voltage Y display Message ch getch Y stop Motor 48 0 3 0000000 MotionTest Function start Y define xyAngle 2 S
17. 0AIODOAODAADO CN1010D00D000000000000D 1UU0O 100 D 0000 00000 2 UU1O 504100000 VUOOODOAODOAO O LOW ONG HIO OFF OO 200000 40000000000000000000 D0DD000000000000000070 0 2mm puise 0 00000 PWMQO000d00 J PC20 UPP PWMDRV QUPP Q O J UPC20 UPPOOOOOOO 22 00000000 PWMOO000000000 100000 ANO AN1 AN2 0000000 AN3 cCN104000000000000000000000 0000000 AN4 cN20100000000000000000000000 000000000000000 AN5 CN2210000000000000000000000 AN6 7 cN240 0000000000000000000000 AN8 000000000 000000 AN9 Du 2 2 O 20 0000000000 26 OAAODAOODAAODAAODAADAOAODAAODAADOAODAOOSVAOAODAOAD 10bit 00000 0 5V 470000 1400 OD 340000000 D 10
18. 20600000000000000000000 2 LEDIUU10 O UU1OOOAOAAOAOOOO PWMOOOOOOLOUYPCZO UPP PWMDRV O UPPOOD PWM 0 0 J PC20 UPP PWMDRV UPP O OO PC20 UPP 000000000 OOOOUVPPOOODODAAADDOAODOOAODDDAELAODDAOO 00000000 PWD 0000000000 10000 UUO CN10100000000000000 PWM ENABLE OO 000000000 UU1 CN10100000000000000 DIRPWM 100000000 UU2 CN1200000000000000 PWM ENABLE OO 000 UU3 CN1200000000000000 DIRPWM 100 UU4 CN1030 0000000000000 PWM ENABLE OO 000000 UUS CN10300000000000000 DIRPWM 100000 UU6 CN1040 0000000000000 PWM ENABLE OO 000000 UUT cCN1040 0000000000000 DIRPWM 1000000 UU8 CN2510000000000000 A000 1000000000 UU9 CN2510000000000000 8000 1000000000 UU10 cN2060 0000000000000 A000 1000000000 0 20 00000000000 25 UU11 CN2060 0000000000000 UU12 CN2070 0000000000000 On UU13 cN2070 0000000000000 BOOD UU14 cN2080 0000000000000 UU15 CN2080 0000000000000 BOOD 2 1 0000000000 000000000 S
19. OODODOO 000 UU 0D00000000000000000000000000000000000000D 000000000 0000000 0D00000000000000000000000000000000000000 DU Uu LU 1 RPS O 0 0 http Awww joho kochi or jp johosi 0311 rsp html 2 http www1 odn ne jp aae76220 flora pdf Vol 13 No 1 2003 63 67 00000000 SRCD B n B U 0000 0000000D00D00000000 lt http shuttle kawasaki m ac jp mw J ournal HP jjournal 2003 13 1 07 shigenari pdf IA E AAA NA E BASE JA E A E EA E E E J E E E J E J E E JJ E E E J J E E E EA E EEE 2001 2003
20. OODOODOOLD 7 7 D 10 LU 0600 i m ien E ra E EP sd UU OOODOOOODODODODOODODOMOODADODOADOODDOD OOODOODOODODOODOOODODOODODODODOODOODDULD OOODOOOODODODODOODDOD Ey EET E EN ESI TES EST a L1 L1 OOODOODODOODOODODODODOODODODODOODOODDULD eG OOODOODODOODOODODODODOODODODODOODOODDULD OOODOODODOODOODODODODOODODODODOODOODDULD OOODOODODOODOODODODODOODODODODOODOODDULD OOODOODODOODOODODODODOODODODODOODOODDULD OOOODOAOODAODOOAAODAAODAAODAODODAODO OOODOODODOODOODODODODUOODODODODOODOODDOLD 0000000 O10 M DO MODOODODOODODODODOOOODY MOODODODOODOODOODOUDOODOADOOODDODDODODOOD D uu SRC 0 O Spontaneous Reaction Control Val ker D D 3 3 OOODOOOODODODODODDODOODODO OOODOOODODOODOODOODOULDADOODDODOODODODODDODOOD
21. Start nStop static int v 4 0 0 0 0 if Stop 0 0 30 0000000 nStart nStop 0 v 0 2 vx vy4ee v 1 vx vy4ee v 2 vx vy 166 v 3 vx4vy4ee M 1 0 if Stop 1 f Period 100 if nStart lt Period M 0 5 1 cos PI nStart Period nStart Jelse Stop 2 if Stop 3 Period 10 if nStop gt Period Stop 0 return M 0 5 1 cos PI nStop Period nStop 4 for i 0 i 4 i isetda i L int v i M 2048 fdef DEBUG ON locate 1 10 printf Stop 3d 41 0 30 0000000 M 2 LIf n 4d 4d 4d 4dY n Stop M v 0 v 1 v 2 v 3 endif 3200000000 00000000 define DEBUG ndude v25 h gt v25 h 000000000 define BIO 0x300 D A 1 0 address define DAIL BI0 0x00 define LDAC BI0 0x00 A D 1 0 address define ADL BI0 0x08 define ADU BI0 0x09 define ADSTS BI0 0x0a T104 ADA base 1 0 adress OE define clsQ printf Yxib 2J cls FARO Es define locate x y printf Yxlb d dH y x Iocate x y HADER define clsxy x y printf x1b d dH x1b K y x clsxy x y BOE define Rad2Deg 57 2958 Rad2Deg 57 2985 LEH define Deg2Rad 0 01745 Deg2Rad 0 01745 CE 0 30 0000000 43 define PI 3 1415926 P 3 1415926 ER void iniz port2 void BE TOMEUY HL float ad datal int ch 5 BS o E S amp float ad data8 int ch
22. dOOn ER END BEEN AN EE H HEB L IE LE ul umm OOOOODDODOOOO OOOOODDODOOOLO 1040125 uu O10 Du 1 1 OOOO 120 0000 1 3 1 4 0 20 0000000000 2 1 2 20 2211000000 2 2 20 00000000 2 2 30 000 2 240 0000000 2 30 247 O30 0000000 3 10 00000 3 20 00000000 3 30 0000000 D4D O50 00 510 000000 5 20 0000000 OD DU Uu 11 12 14 15 16 17 18 31 42 44 52 55 O10 1 0 10 DD 11 OOOO 0D00000000000000000000000000000000000000D OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO dB RE A dB Oo AD BA Ad BA H BA Hd de BA EI dB Hd Ed E OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO OOODOODUDODODDDOODOODODOODOODODOODODODODDO OOODOODUDOODODODDODOODODOODDODOOODODOODDODOO OOOODOOODDOUDDOODOUDDOODOUDDOODOOODOODOOO OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO OOOODOOODDOUDDOODOUDDOODOUDDOODOUODOODOOO OOODOOODUDOODOMDOODOULODODOODDODOAODODOODDODOO OOODOODUODOODODODO 1 2 0000 OOODOODODOODODODODOODOOLD 1
23. eudo A LEGE DO AWated MOAOOOAOOODWredrpdn OAODAODAODAOAOOADA OOODOOOODODODODOODODOO OAOOAODAODAAODAODODAWFSOOOAODOAODODADAA WS VAD ODO D VT4pos 07 10040 0 D D 220 000000 4 E uM 100VT 4 pos 47 wpseudoMXD 22 20 000 Veseudo dVO 0000000000 opseudoMAX SEER BES 5 25 50 Nturns DA 0125 Nurns 3 Is max O 20 0000000000 30 000000 Nur OOOOADAUADAOOODAOOOAADAD OOODADIA D Uu 7 BATTERY O 2 13 zvzyhg 5 00000000 000000000 000000000000000000000T 0 30 0000000 03 31 00000 0D000 00000000000000000 0D000000 define DEBUG ON fndef WALKER H define WALKER H ndude lt stdio h gt nclude lt conio h gt nclude lt math h gt nclude lt dos h gt nclude lt stdlib h gt ndude v25 h T104 ADA base I O address begin define BIO 0x300 D A I O address define DAIL BIO 0x00 define LDAC BIO 0x00 A D MO address BIO 0x08 Hdefine ADU BIO40x09 define ADSTS BIO 0x0a T104 ADA base I O address end 31 0 30 0000000 32 define ds printf Yx1b 2 define locate x y pri
24. ntf Yx1b d 9 dH y x define dsxylx y printf x1b d dH x1b K y x define Rad2Deg 57 2958 define Deg2Rad 0 01745 define PI 3 1415926 void iniz port2 void float ad datal int ch float ad data8 int ch void isetda int ch int data void 560 Motor void void wait int time flag 0 wait for enter flag 1 detect if enter unsigned char Getch int flag void J oyTest void double J oyDetect1 void double J oyDetect2 void void DATest void void MotionTest void void OutSpeed double vx double vy double ee int Stop endif OOODOODUODOODODODODOO OOODODDODOODODOOLD 0 30 0000000 Main 000000 Walker O ndude walker h void main void 1 int ch iniz port2 Seto Motor fdef DEBUG ON dot ds printf Xn Main M enu X nY n printf 1 J oystick test n printf 2 Test D AX n printf 3 Motion Test n printf E sc gt Terminate program n printf X N printf Please choose ch getch ds switch ch case 1 J oyTest break case 2 DATest break case 3 33 0 30 0000000 endif fdef DEBUG_ON Set0_Motor while ch 0x1b endif 00000000 ndude walker h void iniz port2 void t 34 MotionTest break CN202 0000 pokeb V25BASE PMC2 0x00 00
25. peed define amp format vx vy ee err errRange define dtemp Y define amp format Stop 0 lacate 1 45 Y display Message A3 gt Getch 1 Oxlb Esc Yes zyAngle zyAngle 1 xy ngle 0 gt 800 xyAngle 1 gt 800 Main Function 49 0 30 0000000 50 lt Y abs err gt errRange abs ent360 lt enRange dtemp abs err 360 Y lt errFange dtemp abs 180 lt Y Y errRange vx dtemp v Yes Y Y Y vy dtemp Y Y xy Vx Y vy B Y Y l de Y OutSpeed Function A3 Mtionret 000000 000000 OtSped 100000000 3 0000000 OutSpeed Function Y define M Period Y define nStart define amp format v 4 format M Al 0 30 0000000 4 Al format Period nstart lt Period Yes calculation M nstart No Stop2 Stop Yes n top gt Period Yes Btop
26. ta amp amp fabs yN ew lt yDelta f xyAngle 888 return xyAngle dx xN ew xOld 0 30 0000000 37 dy yNew yOld if fabs dx lt dxDelta amp amp fabs dy lt dyDelta xNew xOld yNew yOld return xyAngle xyAngle Rad2Deg atan2 yN ew xN ew if fabs xyAngle 180 0 01 xyAngle 180 return xyAngle double oyDetect2 void t float xDelta 0 5 yDelta 0 5 dxDelta 0 05 dyDelta 0 05 static float xOld 0 yOId 0 xN ew 0 yN ew 0 dx dy static double xyAngle 888 xOld xN ew yOld yN ew xNew ad data8 3 2 49 yNew ad data8 4 2 50 if fabs xN ew lt xDelta amp amp fabs yN ew lt yDelta f xyAngl e 888 return xyAngle dx xN ew xOld dy yN ew yOld if fabs dx lt dxDelta amp amp fabs dy lt dyDelta f xN ew xOld 0 30 0000000 38 yNew yOld return xyAngle xyAngle Rad2Deg atan2 yN ew xN ew if fabs xyAngle 180 0 01 xyAngle 180 return xyAngle void J oyTest void t float xN ew 2 yN ew 2 k printf n J oystick test n printf Press any key and break X n while kbhit 1 getch xN ew 0 ad data8 1 yN ew 0 ad data8 xN ew 1 ad data8 3 yN ew 1 ad data8 4 k ad data8 5 locate 1 8 printf Left xNew 5 2f yN ew 5 2f angle 6 1If n n xNew 0 yNew 0 J oyDetect1 printf Right xNew 5 2f yNew 5 2F angle 6 1I1f n n xNew 1 yNew 1 J oyDetect2 printf Potentiometer 5 2f n k void DATest void int ch 0
Download Pdf Manuals
Related Search