..

KPEMfIEB,

.. , .. fl

.r:~I

2013


. 5
1. LEGO MINDSTORMS . . 7
1.1 LEGO MINDSTORMS NXT...... . 9
1.2 EMBEDDED CODER ROBOT NXT

nxtOSEK. 11
1.2.1 ECRobot. 12
1.2.2 ECRobot.. 15
1.2.3 ECRobot NXT. 17
1.3
LEGO MINDSTORMS.

26

1.3.1 .

30

1.3.1.1 30
1.3.1.2
32
1.3.1.3 .

33

1.3.1.4 . 34
1.3.1.5 Motobot..

35

1.3.1.6 39
1.3.2 Segway.. 40
1.3.2.1 .. 40
1.3.2.2 44
1.3.2.3 . 45
1.3.2.4 . 46
1.3.2.5 Segway 47
1.3.2.6 55
1.3.3 .. 56
1.3.3.1 .. 56
1.3.3.2 . 59
3

1.3.3.3 . 61
1.3.3.4 Ballbot..

61

1.3.3.5 67
2 BIOLOID PREMIUM KIT.. 69
2.1 Dynamixel 71
2.1.1 Dynamixel.. 74
2.1.2 Dynamixel. 75
2.2 .. 76
2.3 BIOLOID
.. 83
2.3.1 CM-510..

84

2.3.2 Dynamixel.

85

2.3.3 . 88
2.3.4 88
2.3.5 . 89
2.3.6 .. 91
2.3.7 . 92
2.3.8 92
2.3.9 Dynamixel 94
2.3.10 95
2.3.11 Zigbee . 98
2.3.12 Bioloid
.. 101
. 113
. 115


.
, ;
.
:
, . ,
,
, , , ,
, .
,
.
, , ,
, ,
[2-3, 5].
,
.

. , ,
.

[1, 3, 4, 7].
,
. , ,

.
,
, , . ,
,

,
.

,
: VexRobotis, Mechatronics Control Kit,
FestoDidactic, LEGO Mindstorms, OLLO, fischertechnik, Bioloid .

, : LEGO
Mindstorms Bioloid.
5

 ,
,
,
.. , ,
,
Bioloid, ,
Lego [19-21].


(Lego Mindstorms 2.0 Bioloid Premium Kit),
. ,

.
,
221000.62
.
,
, ,
,
,
,
.
,
,
, .

1 LEGO MINDSTORMS
LEGO Mindstorms (
) .
LEGO 1998 . 8 (2006)
LEGO Mindstorms NXT, 2009 LEGO Mindstorms NXT 2.0.
LEGO Mindstorms NXT 2.0:
NXT
( 1.1).

1.1 NXT LEGO

4 ( 1.2 1.4):

;
;
: 6
(, , , , ),
.

1.2

 1.3

1.4
3
( 1.5).

1.5
USB .
7 .
.
Windows Mac OS.
.
613 Lego ( 1.6).

1.6 - Lego LEGO Mindstorms NXT 2.0

, ,
LEGO,

(HiTechnic, Mindsensors, Dexter Industries).

1.6:

1.6 ) - HiTechnic NXT; )

Lego 9749; ) - HiTechnic NXT; )
NXTCam-v4 Mindsensors
Lego Mindstorms

, , ,
, ..

.
1.1

LEGO

MINDSTORMS NXT
LEGO MINDSTORMS NXT
Software [9] NXT-G
,
.

NXT,
( 1.1).
(, ,
,
.) .

 1.1 NXT,

NXC
www.bricxcc.sourceforge.net
NXT-G
www.mindstorms.lego.com
RobotC
www.robotc.net
RoboLab
www.mindstorms.lego.com
Microsoft Robotics Developer Studio
www.mrds.com
LabVIEW Education Edition
www.ni.com
NBC
www.bricxcc.sourceforge.net
pbLua
www.hempeldesigngroup.com
ICON
www.teamhassenplug.org
leJOS NXJ
www.lejos.sourceforge.net
LEJOS OSEK
www.lejos-osek.sourceforge.net
, LabView MATLAB/Simulink
NXT - , ,
. ,
, ,
, MATLAB/Simulink
LabView
, - .
1.2
LEGO Mindstorms NXT
.
1.2 LEGO Mindstorms
NXT.

Robot C

NXC

leJOS NXJ

nxtOSEK

leJOS

LabView
Toolkit

Java

/++

, Simulink

leJOS

MATLAB/
Simulink

leJOS

10

LabView

MATLAB/
Simulink

LEGO MINDSTORM
Toolkit LabView Embedded Coder Robot NXT MATLAB/Simulink.
, LabView,
LEGO. LabView ,

, .
MATLAB
leJOS.
,
, NXC leJOS.
NXC (Not Exactly C) C-
NXT ,

LEGO.

IDE, ,
,
.
LeJOS NXT

leJOS

(Java).

[9].

leJOS

, , ..
1.2 EMBEDDED CODER ROBOT
NXT
nxtOSEK
NxtOSEK ( 2008 LEJOS OSEK)
: leJOS NXJ TOPPERS OSEK [11, 12]. LeJOS NXJ
ANSI C/C++ NXT
API-. ,
11

 NXT ,

TOPPERS

OSEK

.
nxtOSEK
Embedded Coder Robot NXT ( ECRobot)
MATLAB/Simulink,
.
Simulink-
. nxtOSEK
.
1.2.1 ECRobot

ECRobot

NXT. [13]:
1. , MATLAB
, . :
. C:\MATLAB
. C:\MATLAB R2010a
2. , MATLAB ( R2008b),
Simulink, Real-Time Workshop and Real-Time Workshop Embedded
Coder. MATLAB
ver, MATLAB
.
3. Cygwin / GNU Make
. Cygwin 1.5.x - Cygwin:
http://www.cygwin.com/
. Cygwin C:\cygwin ( 1.6)
. make Devel .

12

 1.6 Cygwin
4. GNU ARM
. GCC-4.0.2 FILES GNU ARM: http://www.gnuarm.com/
. GNU ARM
C:/cygwin/GNUARM
. 1.7
. Install Cygwin DLLs, Cygwin
( 1.8)
.
.

1.7 GNU
1.8 GNU
ARM(1)
ARM(2)
5. NXT USB:
. ( LEGO
)
MINSTORMS NXT Driver v1.02 -
LEGO:
http://mindstorms.lego.com/Support/Updates/ - . . 1.9
13

 1.9 Mindstorms
6. NeXTTool:
. NeXTTool http://bricxcc.sourceforge.net/utilities.html
c:\cygwin/nexttool
7. NXT:
. NXT
, : http://bricxcc.sourceforge.net/lms_arm_jch.zip
( 1.06 )
. C:\cygwin\nexttool
lms_arm_nbcnxc_X, X=106
1.06
. Windows =>=>
cmd ok
. c:
. cd cygwin\NeXTTool
. NXT USB .
, ( )
, , lms_arm_nbcnxc_106 ,
b.
nexttool /COM=usb firmware=lms_arm_nbcnxc_106.rfw
8. Embedded Coder Robot NXT:
. ECROBOT NXT
http://www.mathworks.fr/matlabcentral/fileexchange/13399
MATLAB.
9. nxtOSEK ecrobotnxtsetup.m:

14

. nxtOSEK - nxtOSEK( http://lejososek.sourceforge.net/download.htm?group_id=196690 )

ecrobotNXT/environment ( 1.10).

1.10 nxtOSEK ecrobotNXT/environment

. m- ecrobotmxtsetup.m
1.11. .

1.11 ecrobotnxtsetup
1.2.2 ECRobot

MATLAB/Simulink ECRobot NXT Blockset,
Simulink Library Browser ECRobot.
NXT
(, , Bluetooth- ..),
.
1.2.3 ECRobot NXT
, ,
-.
MATLAB Real-Time Workshop Embedded Coder Real-Time
Workshop.
15


.rxe. GNU ARM toolchain 4.0.2.
Cygwin ( 1.5) Linux-
Windows, Linux-
Windows. Cygwin GCC (GNU Compiler Collection),
NXT.
NXT.
NeXTTool ,
NXT ( .rxe)
( .rfw).
NeXTTool LEGO Mindstorms NXT Driver
Windows NXT USB-.
64 .
NXT BIOS ( 224 ).
-
NXT
( 1.12). Generate code and built the generated code
. nxtprj
.
.rxe,
NXT, ***_app_ert_rtw ( *** ) ***_app.c ,
. Download
(NXT enhanced firmware) -
NXT (
USB-). NXT.
MATLAB
.
16

 1.12 Simulink- ,
- NXT
, NXT
nxtOSEK, NXT
LEGO MINDSTORMS NXT Sofware
, (.rxe-).
, nxtOSEK
NXT,
( nxtOSEK 10
).
nxtOSEK
ECRobot .
,

.
1.2.3 ECRobot NXT
ECRobot NXT , ECRobot
NXT Simulink ( 1.13).

17

 1.13 - ECRobot Simulink

, ECRobot NXT.
Revolution Sensor :

1.14 Revolution Sensor

Revolution Sensor
. Revolution Sensor Interface - ,
NXT
. Revolution Sensor Read
. ;
API
(
In\Out).
1.3 Revolution Sensor

int32

[1 1]

int32 []
ID
A/B/C
Servo Motor :

18

 1.15 Servo Motor

Servo Motor . Servo
Motor . Servo Motor Interface
NXT . Servo Motor
Write .
In\Out.
1.4 Servo Motor

int8

[1 1]

-100 100
ID
A/B/C

/
Battery Voltage :

1.16 Battery Voltage

Battery Voltage
NXT. Battery Voltage Interface ,
NXT. Battery Voltage Read
.
1.5 Battery Voltage

Int16

[1 1]

0
[] (, 9000=9.000)
ID

System Clock :

19

 1.17 System Clock

System Clock
NXT. System Clock Interface /,
. ,
System Clock Read.
System Clock Read
. NXT 0
.
1.5 System Clock

uint32

[1 1]

0 uint32

ID

Sound Volume Tone :

1.18 Sound Volume Tone

Sound Volume Tone ,
, .
1.6 Sound Volume Tone

: uint32, : uint32

[1 1]

: 31 [] 14080 []
: 10 2560 []
: 0 100 (0 - )
ID

Ultrasonic Sensor :

1.19 - Ultrasonic Sensor

Ultrasonic Sensor
. Ultrasonic
Sensor Interface NXT. Ultrasonic
20

Sensor Read
.
1.7 Ultrasonic Sensor

Int32

[1 1]

0 255 [], -1 ( )
ID
S1/S2/S3/S4
Light Sensor :

1.20 - Light Sensor

Light Sensor ,
. ,
( ). .
Light Sensor Interface NXT. Light
Sensor Read .
1.8 Light Sensor

Unt16

[1 1]

0 1023
ID
S1/S2/S3/S4
Sound Sensor :

1.21 - Sound Sensor

Sound Sensor .
, . Sound Sensor Interface
NXT. Sound Sensor Read
.
1.9 Sound Sensor

Unt16

[1 1]

0 1023
ID
S1/S2/S3/S4
21

Touch Sensor :

1.22 - Touch Sensor

Touch Sensor
. ,
1. Touch Sensor Interface NXT.
Touch Sensor Read .
1.10 Touch Sensor

Unt8

[1 1]

0 ( ), 1 ( )
ID
S1/S2/S3/S4
Bluetooth Rx :

1.23 - Bluetooth Rx
Bluetooth Rx Bluetooth
Bluetooth-.
. Bluetooth Rx Interface
NXT. Bluetooth Rx Read
. Bluetooth Rx
( ),
-NXT ( NXT-NXT).
1.11 Bluetooth Rx

Uint8

32

0 255
ID

Bluetooth Tx :

22

 1.24 - Bluetooth Tx
Bluetooth Tx Bluetooth
Bluetooth-.
. Bluetooth Tx Interface ,
NXT. Bluetooth Tx Write .
Bluetooth Tx
( ) -NXT NXTNXT.
1.12 Bluetooth Tx

Uint8

32

0 255
ID

NXT GamePad Data Logger :

1.25 - NXT GamePad Data Logger

NxtOSEK , NXT GamePad.
NXT,
nxtOSEK Bluetooth,
(). 1.01

Analog Stick Control NXT Data
Acquisition, .
, NXT GamePad

[15].

NXTGamePad.exe COM-
. Data1 Data2
Analog GamePad.
23

 CSV
NXT MATLAB. NXT GamePad Data Logger
Bluetooth Tx Write .
1.13 NXT GamePad Data Logger

int8

[1 1]

-128 127
ID

1.26 - NXT GamePad

NXT GamePad ADC Data Logger :

1.27 - NXT GamePad ADC Data Logger

NXT GamePad ADC Data Logger NXT
GamePad Data Logger, 4
. NXT GamePad ADC Data Logger
Bluetooth Tx Write .
1.11 NXT GamePad ADC Data Logger

Data1/Data2
Int8
-128 127
1
ADC1/ADC2/ADC3/ADC4 Int16
-32768 32767 1
24

Gyro Sensor :

1.28 - Gyro Sensor

Gyro Sensor ()
, . Gyro
Sensor Interface - NXT. Gyro
Sensor Read .
1.12 Gyro Sensor

Uint16

[1 1]

0 1023
ID
S1/S2/S3/S4
Exported Function-Calls Scheduler :

1.29 - Exported Function-Calls Scheduler

(,
,
). Exported Function-Calls Scheduler
Function-Call
, Block Parameters.
OSEK Resource :

1.30 - OSEK Resource

OSEK Resource
OSEK GetResource/ReleaseResource API /
, ,
. Simulink,
Atomic
Subsystem.
25

 Exported Function-Call Scheduler. OSEK Resource

.

1.31 - OSEK Resource

1.3

LEGO MINDSTORMS

LEGO Mindstorms,
, ECRobot NXT.
.

( 1.32). [14] 1,
4.
, .

1.32
26

 Simulink- Test.mdl
( 1.33), .
,
. .

1.33 Simulink- test.mdl

ExpFcnCalls Scheduler ()
.
Fcn_100ms 0,1 .
Explorer_app .
.
.
(0,01 ),
Function-Call Subsystem.
27

 Function-Call Subsystem
( 1.34). 20
( ), 0,
1. 1
( Touch Sensor Read) 0 .

1.34 - Function-Call Subsystem

, , 0,
.
If Action Subsystem TURN (
), If Action Subsystem FORWARD (
). ,
.
, 70,
( 1.35).

28

 1.35 -
,
.
nxtconfig (gsc, 'float') -
.
nxtbuild ('Explorer_app', 'cgen') -
(
).
nxtprj
ecrobot_main.c Fcn_10ms,
Explorer_app_ert_rtw Explorer_app.c ,
Explorer_app
.
nxtbuild ('Explorer_app', 'build') -
. nxtprj
Explorer_app.rxe
NXT.
nxtbuild ('Explorer_app', 'rxeflash') -
(Explorer_app.rxe) NXT (
, USB-).
NXT. ,
. [14]
1.3.1-1.3.3
, , ,
29

 nxtOSEK.

www.nxtprograms.com/NXT2/explorer/index.html [14].
1.3.1

, ( 1.36).

1.36 - ,

,
.
.
,
.

.
1.3.1.1

, ,
, ,
.
: , .
7 , 3
30

.
: ,
,
. [5]

1.37 -

O
x , y z ( 1.37). R,
B. B
: x B 0 , y B 0 , z B 0 .
(3 3) ,
x z .
H x H 0 , y H 0 ,
z H 0 . :
I Bxx
I B 0
I Bxz

0
I Byy
0

I Bxz
I Hxx

0 , I H 0
I Hxz
I Bzz

0
I Hyy
0

I Hxz
0 .
I Hzz

:
P Fi i T T T R ,
R

T , T , T

(1)

F .

, v R rR .
, ,

. ,
( ) ,
x z , :
[rR2 mT I Ryy (rR / rF ) 2 I Fyy ] R TR ,

(2)

,

31

 .
, ,
C q K q f .
Mq

v C1 q [ g K 0 v 2 K 2 ]q f ,
Mq
T
q [, ]T f [T , T ] , M ,

. C v C1
v ,
- .
C1 ,
. K :
g K 0
,
, v 2 K 2 ,
.
, :
det(M 2 v C1 g K 0 v 2 K 2 ) 0 .
(3)
(3) .
,
, .
,
.
:
q S(q, t )u , u [M(q, t )]1 Q(q, u, t ) ,
(4)
q , u , S
,
, M ,
Q .
1.3.1.2

: R , B
, H
, F , T , A
.
(
P):
mT mR mB mH mF , xT ( xB mB xH mH wmF ) / mT ,
32

zT (rR mR zB mB zH mH rF mF ) / mT .

z

P ,
:
I Tzz I Rzz I Bzz I Hzz I Fzz mB x B2 mH x H2 mF w2

A.
sin , 0, cos T , ,
x z . ,
u A ( xA w c) cos z A sin .
:
, ,
, .
, - .
(
)
(c / w) cos .
y,
,
:
SR I Ryy / rR , S F I Fyy / rF , ST SR SF .

.
M
K 0
, K0

M
K 0

M
M
M

K 0
K 2
, K2

K 0
K 2

K 2
C1
, C1

K 2
C1

C1
,
C1

(5)

M ITxx , M I Ax ITxz , M M , M I A 2I Az 2 ITzz

M;

K 0 mT zT ,

K 0 S A ,

K 0 K 0 ,

K0 S A sin (
g );

K 2 0 ,

K 2 0 ,

K 2 ((ST mT zT ) / w) cos , K 2 ((S A S F sin ) / w) cos

C1 ST SF cos ( ITxz / w) cos mT zT ,

v 2 );

C1 0 ,

C1 ( I Az / w) cos (S A ( ITxz / w) cos ) ,

C1 (ST S F cos ) C .

1.3.1.3
.
:
I Ax
vST vSF cos TB gmT zT gS A .
mT yP zT ITxx I Txz
33

 P:
I Az vST vSF sin wFFy .
mT yP xT I Txz I Tzz
:
I A vSF ( cos
sin ) TH cFFy cos g ( sin )S A .
mA yPu A I Az

,

:
yP ((v 2 vc ) / w) cos

:
x P v cos , y P v sin .

--, x , , , ,
u T .
T

.[27]
-

J ( x T Q u x u T R u u )dt , Q u R u
0

. Qu diag{0, 0,1, 0} ,
,
, .
R u 1 .

-
(-), k p 8,23 , ki 1,48
k d 2,76 .

1.3.1.4

1.38
34

 1.
1.13
Physical

g=9.81
m=0.03
R=0.04
M=0.678
M_front=0.2
M_rear=0.478
L_base =0.225

/2

L_trail = 0.011

Head_angle =67

V=0.6

(w)
,


(c)

1.3.1.5 Motobot
( 1.39). ,
, .
, - . ,
k-, .
, :
, ,
Bluetooth, , ,
, ,
,
.
nxtway_app.

35

 1.39 -
1.40
/. , ,
Bluetooth. A B
,

.

1.40 -
36

 1.41 .
:
nxtway_app/Balance & Drive control/Balance & Drive Control/Controller
k_thetadot thetadot_ref, .
(x1_ref-x1), x1=[phi
phidot phidotdot] , motobota
.

1.41 -
.
phi_cmd motobota,
0. , (
)
.
thetadot_cmd.

.
Blutooth. [15]

37

 1.42 -
.
( 1.43), theta_in ( )
thetadot (/).
0.004 .

x1, phi_dotdot
phi. 0.004 .

38

 1.43
, .

( 1.44).
.

1.44 ,

1.3.1.6
NXT GamePad

. 1.45 ,
.
39

1.45 :
) [./c.]; )
[.]
1.3.2 Segway
1.3.2.1
Segway ( 1.46)

.
.
- ,
.

1.46 - Segway

, ,
40

 , ,
.

1.47 , .
M ;
m ;
b ;
L ;
J ;
u(t) , ;
x(t) ;
(t) ;
H(t) ;
V(t) ;
g .
:
d2
xt L sin t H t
dt 2

(6)

d2
m 2 L cos t V t mg
dt

(7)

d 2 t
L H t cos t L V t sin t
dt 2

d 2 xt
dxt
u t H t b
2
dt
dt

(8)
(9)
41

 (6) (7), :
mx mL 2 sin mL cos H t

(10)

mL 2 cos mLsin V t mg

(11)

H(t) (10) (9),

:

M mx bx mLcos mL 2 sin ut

(12)

H(t) (10) V(t)

(11) (8), :

J mL mLxcos mgLsin 0
2

(12)

(13)

(13)

.

(12) (13)
.
t 0 , xt 0 , ut 0 ,
sin t cos t (12) (13)
:

M mx bx mL ut

(14)

J mL mLx mgL 0

(15)

, ,
u(t), (t).

(12)

(13)

(s) U(s)
(t) u(t) :

J mL2 g 2
J mL2 g
s M m
2 s b
2 s mLs2 U s
s
s
mL
mL

(16)

:
42

Wp s

s
mLs

2
2 2 3
U s M m J mL m L s b J mL2 s 2 M mmgLs bmgL

(17)

:
.
:
x1 t xt , x2 t x1 t xt , x4 t t , x4 t x3 t t .

(18)

(12) (13) :

M mx2 bx2 mLx4 ut 0,

(19)

J mL x

(20)

mLx2 mgLx3 0.

x2

b J mL2
m 2 gL2 M m
J mL2
x2
x3
u t 0, (21)
J M m MmL2
J M m MmL2
J M m MmL2

x4

bmL
mgLM m
mL
x
x
u t 0. (22)
2 2
2 3
J M m MmL
J M m MmL
J M m MmL2

, :
x Ax Bu t ,

(23)

1
0
x1 t

b J mL2
x t
0 J M m MmL2
2

x
, A
0
x3 t
0
bmL

0
x4 t

J M m MmL2

0
m 2 gL2 M m
J M m MmL2
0
mgLM m
J M m MmL2

0
,
1
0

J mL2

J
M

MmL

.
B

mL

2
J M m MmL

( t xt )
:
y Cx Dut ,

(24)
43

 y t

1 0 0 0

y 1 , C
, D 0.
0 0 1 0

y2 t
1.3.2.2
1.
,

, :

,
,
.
2.


xref x.

1.48
,
1.48, :
(
)

, ,

-.
, .
,
, :
Control System Toolbox
ctrb.

.
1. .

-.
. Control System Toolbox
place.
44

, ,
A=[0, 1; -2; -3], B=[0;1]
: [-5, -6]

2.

J,
:

Q R,
. Control System Toolbox
lqr.
:
A=[0, 1; -2, -3], B=[0; 1], Q=[100, 0; 0, 1]; R=1

1.3.2.3
,
.
- - ,

. - ,
,
, ,
.
- x0
x u.
x0 , e = (xref x)
.
u :
45


p, i, d ,
, .

1.49 -
1.3.2.4

1.50
1.14
2
g = 9.81 (/ )

m = 0.03 ()

R = 0.035 ()

2
2
Jw = mR /2 ( )

M = 0.6 ()

W = 0.14 ()

D = 0.04 ()

H = 0.144 ()

L = H/2 ()

46

J = ML2/3 (2)
J = M(W2+D2)/12 (2)
Jm = 10-5 (2)
Kb = 0.468 (/)
Kt = 0.317 (/)

1.3.2.5 Segway
. , ,
.
, Motobot.
nxtway_app.

1.51

/. , ,
Bluetooth.
.
Reference Generator ,
.

47

 1.52
1.51 Controller ,
NXT. ,
, , Bluetooth, ,
Bluetooth
,
,
.

1.53
1.54 NXTway-GS - , ,
, .
, ,
.
.

48

 1.54

1.54 Actuator ,
, .

1.55
Plant - ,
, .

49

 1.56
Sensor ,
.
, .

.

50

 1.57

1.58 - nxtway_gs_controller.mdl
Data Store Memory ,
.

1.59
task_ts2: .

51

 1.60 task_ts2
task_ts3: .

1.61 - task_ts3
task_ts1: , ;
.
. time_start.

52

 1.62 - task_ts1
.

1.63
, 1.64 Discrete Derivative
Block,
Discrete Integrator Block,
.

53

 1.64 - Discrete Derivative Discrete Integrator Block

1.64 ,
,
,
.

1.65
,
.
,
.

54

 1.66
,
.

1.67
1.3.2.6
NXT GamePad

.
1.68 1.69 ,
.

55

 1.68

1.69
1.3.3
1.3.3.1
( 1.70),
,
56

 .

[1], 1.65.

1.70

. , . m
. Rwm Rss ( 1.71).

1.71

, ,
. =0 t=0,
:
( xs , z s ) ( Rs , zb ) , ( x s , z s ) ( Rs ,0) ,
cos ,L
sin ) .
( xb , zb ) ( x L sin , z L cos ) , ( xb , zb ) ( Rs L
T1,
T2, U :
57

T1

1
1
M s ( x s2 z s2 ) M b ( xb2 zb2 )
2
2

T2

R2
1 2 1
1
1
1
1
1
2 J w 2m J m 2m J s 2s J
2 ( J m J w ) s (
),
J s s J
2
2
2
2
2
2
2
Rw2

U M s gz s M b gzb

.
L :
L T1 T2 U .
.
:
d L L
F ,

dt
d L L

F .
dt

,
:
F Kt i f m m f s , F K t i f m m ,
i ; m k ( ) .
,
[4]:
K
K K
, t , k( t b fm ) .
, F
F ( f s )
Rm
Rm

2
0(sin , cos 1) ,
:

E F G H ,

( M M s ) Rs2 J s J m
E b
M b LRs k 2 J m

0
0
G

M
b gL

M b LRs k 2 J m

M b L2 J k 2 J m

f s
F

H .

x ; u .
T
; u ;
x
, :
x A(t ) x(t ) B(t )u(t )
0
0
A
0

0
0
A(3,2)
A(4,2)

1
0
A(3,3)
A(4,3)

0
0

0
1

,
B (3)
A(3,4)

A(4,4)
B ( 4)

A(3,2) Mb gLE(1,2) / det( E)

A(4,2) M b gLE(1,1) / det( E)

58

A(3,3) [( f s ) E(2,2) E(1,2)] / det( E)

A(4,3) [( f s ) E(1,2) E(1,1)] / det( E)

A(3,4) [ E(2,2) E(1,2)] / det( E)

A(4,4) [ E(1,1) E(1,2)] / det( E)

B(3) [ E(2,2) E(1,2)] / det( E)

B(4) [ E(1,1) E(1,2)] / det( E)

det( E ) E (1,1) E (2,2) E (1,2)2 .

1.3.3.2

.
m
.
.

,
.
.
,
, .
- .

- ( 1.72). C
x.

1.72 - -

- .
-
,
[28]. Q R [29]:

59

0
1
0 6 *10 5

Q 0
0

0
0
0
0

0
0
1
0
0

0 0
0 0
180 2
0 0 , R 6 *103 * (
)

1 0
0 10 3

Q(2,2) ,
, Q(5,5)
,
.

, Matlab
. lqr

[30].
:
kf = [0,015 1,5698 0,027 0,2325], ki = 0,0071.
. ,

: t 0,36 c; 0 .

:
J x, u

T
T
x t Qt xt u t Rt ut dt .

x 0 ,

V xt xT t Pt xt ,
P PT n n ,
: 0 ,
t 0 :
V xt 2V xt .
, :
V xt 2V xt xT t Qt xt uT t Rt ut .
,
:
At xt Bt ut xt T Pt xt xT t Pt At xt Bt ut xt xT t P t xt
xT t Qt xt uT t Rt ut 0 ,
, [31],
:
60

ut R 1t BT t Pt xt .

[31]:
P t At Bt K t I T Pt Pt At Bt K t I K T t Rt K t Qt 0 ,
K t R 1t BT t Pt .
A, B, Q, R ,
,
P ( P 0 ):
A BK I T P PA BK I K T RK Q 0 , K R1BT P .
,
K : K f [0,0117 1,4993 0,0284 0,2432] .
1.3.3.3
1.15
g

9,81

/2

Ms

0,013

Rs

0,026

Js

2 Ms Ms2/3

Mw

0,015

Rw

0,021

Lw

0,022

Jw

Mw Lw2/2

Mb

0,682

0,135

0,135

0,17

Mb L2/3

Jm

0,00001

Rm

6,69

Kb

0,468

1.3.3.4 Ballbot

61


, ,
.
.
1.73 NXT Ballbot,
, , .
,
.
. ,
, , .
, ,
.

1.73 NXT Ballbot

X Y.

1.74
62

 1.75 ,
, .

1.75
1.76 ,
,
.

1.76
Sensor ,
.

63

 1.77
ExpFcnCalls ,
, .

1.78
Data Store Memory ,
.
.
(
)

64

 1.79 task_init
task_ts2-4 , Segway.
.

1.80

.

65

 1.81
,
.
,
.

1.82
,
.
66

 1.83
1.3.3.5
NXT GamePad

.

,
. 1.84 1.85
,
. [4]

1.84 - -
:
(); ()

67


1.85 -
:
(); ()
, nxtOSEK
,
. nxtOSEK

MATLAB/Simulink.

ECRobot
Simulink- .
nxtOSEK
.
-,
/++
- .

68

2 BIOLOID PREMIUM KIT

Bioloid
.
, Dynamixel
,
, [18].
Bioloid LEGO Mindstorms LEGO Vex
Robotics Design System VEX Robotics.
-
[16]. Bioloid
RoboCup [22].
BIOLOID Premium Kit ( 2.1),
.
, ,
,
. BIOLOID
bio+ all+ oid, ,
( 2.1).

69

 2.1 BIOLOID Premium Kit

2.1 Bioloid

( 2.2), ,
,
. Bioloid Dynamixel,
, , 3D
70

 - .
,
.

2.2 Bioloid Premium Kit

CM-510 ATmega2561
DYNAMIXEL ( 2.3):

2.3 -510
2.1 DYNAMIXEL
Robotis Dynamixel Servos
, .

,
71

(,
) .

BIOLOID -510
( 2.4).

2.4 - -510
Dynamixel

,
,

.
Dynamixel

.
16 * 106 *
.
, ,
.
Dynamixel,
72

 ,
( 2.5).

2.5 Dynamixel

Dynamixel ,
,
. Dynamixel

Maxon

RE-max,

0.35-0.07
( 2.6).

73

 2.6 Dynamixel
2.1.1 Dynamixel
,
.

AVR ATmega.
,
Dynamixel
, .

2.7 Dynamixel
-510

, ROBOTIS (
74

 . ).
Dynamixel
RS-232 RS-485 (
2.8).

2.8 RS-232 RS-485

Dynamixel
2.1.2 Dynamixel
: 54.6;
: 32 * 50 * 40;
: 0,29;
: 254: 1;
: 1.5 (12, 1.5);
: 59rpm ( 12);
: 1) 0 ~ 300; 2) ;
: -5 ~ +70;
: 9 ~ 12 ( 11,1);
: ;
:
(8, 1-, );
ID: 254 ID (0 ~ 253);
: 7343bps ~ 1Mbps;

75

, ..;
: .
2.2

.
.
Robotis RoboPlus
( 2.9), : RoboPlus Terminal,
RoboPlus Task, RoboPlus Manager, RoboPlus Motion, RoboPlus Help & eManual.
Robotis OLLO, Bioloid, Expert
kit [23].

2.9 RoboPlus
76

 RoboPlus Manager ( 2.10)

, .
RoboPlus Manager
, .. RoboPlus Manager
,
Robotis.

2.10 RoboPlus Manager

RoboPlus Manager ID
, .
Dynamixel :
, , ,
.. , RoboPlus Manager
Dynamixel, 5,-510,-700 Zigbee.
RoboPlus Terminal ( 2.11) ,
-510
.
,

-510.
Boot Loader.
77

 2.11 RoboPlus Terminal

RoboPlus Motion ( 2.12) ,

.

2.12 - RoboPlus Motion

.

-510

,
, .
78

.
Basic Pose Editor
, ,
. Pose Utility
,
.

, .

[24].
RoboPlus Task ( 2.13)
,
,
, .

2.13 RoboPlus Task

RoboPlus Motion RoboPlus Task

.
79

 Robotis
.

[17].
, - ATmega,
Atmel Studio, WinAVR AVRStudio
. Atmel Studio 6,
AVR Studio 5,
. Microsoft Visual Studio Shell 10
Visual Assist,
C/C++.
Atmel Studio 6 Atmel Software
Framework,
,
, Atmel Studio 6.0 1000
, . Atmel Studio 6
Atmel Software
Framework (ASF Explorer) Qtouch composer,
[25].
: ,
"", , ..
"",
Framework, ,
..
Atmel Studio:
1. Atmel ;
2. ;
3. ;
4.
;
80

5. Atmel Studio .

2.14 Atmel Studio 6.0

. Atmel Studio .hex
.
RoboPlus Terminal:
1) c

COM

57600bps.

:
: ;
-: 1
: 8
2)

shift+3

,
( 2.15):

81

 2.15 CM-510 (ATmega2561)

3) L ( 2.16), "Transmit File"

RoboPlus Terminal, .hex ;

2.16 .hex
4) ,
"GO".

APP/SYS. ,
.
ATmega2561 2.17.

82

 2.17 - ATmega2561
RoboPlus Terminal ,
.txt.

2.3

BIOLOID

ATmega2561 Atmel Studio.
Dynamixel
: serial.h dynamixel.h.
Zigbee libzigbee.h
[17].
CM-510
Dynamixel.

83

2.3.1 CM-510

2.18 - CM-510
2.1 -510

PORTF1 ~ PORTF6
PORTD0
PORTD1 ~ PORTD2
PORTA2 ~ PORTA7
PORTC0 ~ PORTC6
PORTB5
PORTE4 ~ PORTE6
PORTD4 ~ PORTD6

(Tx) (Rx)
( 5
( 2.19))
(, )

(U, D, L, R)

2.19 -510
84

 :
1.

5 ;

2.

VCC (5 );

3.

4.

GND;

5.

NC: .

2.3.2 Dynamixel
( 2.2) Dynamixel,

Dynamixel
.
EEPROM RAM. (RAM)
,
EEPROM ,
.

.
.
. Dynamixel :
(R), ,
(RW), .
. EEPROM
,

.
/ . ,
16 ,
8 .
.
85

 2.2 Dynamixel

0 (0x00)
1 (0x01)
2 (0x02)
3 (0x03)
4 (0x04)
5 (0x05)
6 (0x06)

7 (0x07)

8 (0x08)
E
E
P
R
O
M

9 (0x09)

11
(0x0B)
12
(0x0C)
13
(0x0D)
14
(0x0E)
15
(0x0F)
16
(0x10)
17
(0x11)
18
(0x12)
24
(0x18)
25
(0x19)

Model Number(H)

Version of Firmware

ID
ID
Baud Rate


Return Delay Time
CW Angle Limit(L)

( )
CW Angle Limit(H)

( )
CCW Angle

Limit(L)


( )
CCW Angle

Limit(H)


( )
the Highest Limit

Temperature

the Lowest Limit

Voltage

the Highest Limit

Voltage

Max Torque(L)

Max Torque(H)

Status Return Level

Alarm LED

Alarm Shutdown

Torque Enable
/

LED
/

Model Number(L)

12 (0x0C)

0 (0x00)

RW
RW

1 (0x01)
1 (0x01)

RW
RW

250 (0xFA)
0 (0x00)

RW

0 (0x00)

RW

255 (0xFF)

RW

3 (0x03)

RW

70 (0x46)

RW

60 (0x3C)

RW

140 (0xBE)

RW

255 (0xFF)

RW

3 (0x03)

RW

2 (0x02)

RW

36 (0x24)

RW

36 (0x24)

RW

0 (0x00)

RW

0 (0x00)

86

R
A
M

RW

1 (0x01)

RW

1 (0x01)

RW

32 (0x20)

RW

32 (0x20)

RW

RW

RW

RW

RW

ADD14

RW

ADD15

Present Voltage

Present Temperature

Registered

,
-

0 (0x00)

0 (0x00)

26
(0x1A)

CW Compliance
Margin

27
(0x1B)

CCW Compliance
Margin

28
(0x1C)

CW Compliance
Slope

29
(0x1D)

CCW Compliance
Slope

30
(0x1E)
31
(0x1F)
32
(0x20)
33
(0x21)
34
(0x22)
35
(0x23)
36
(0x24)
37
(0x25)
38
(0x26)
39
(0x27)
40
(0x28)
41
(0x29)
42
(0x2A)
43
(0x2B)
44
(0x2C)

Goal Position(L)

46
(0x2E)

Goal Position(H)
Moving Speed(L)
Moving Speed(H)
Torque Limit(L)
Torque Limit(H)
Present Position(L)
Present Position(H)
Present Speed(L)
Present Speed(H)
Present Load(L)
Present Load(H)

Moving

87

47
(0x2F)
48
(0x30)

49
(0x31)

Lock
Punch(L)

Punch(H)

EEPROM

,

RW

0 (0x00)

RW

32 (0x20)

RW

0 (0x00)

2.3.3
.
C.
.hex
.

#include <avr/io.h>
#include <util/delay.h>
int main(void)
{
DDRC = 0x7F;
PORTC = 0x7E;
while (1)
{
int i;
for(i = 0; i <= 6; i++)
{
PORTC = ~(1<<i);
_delay_ms(250);
}
}
return 1;
}
2.3.4
, ,
/.
.
88

 .
D E, PIND PINE
. PIND PINE
&.

if(~PINE & BTN_UP)

PORTC &= ~LED_MANAGE;
else if(~PINE & BTN_DOWN)
PORTC &= ~LED_AUX;
else if(~PINE & BTN_LEFT)
PORTC &= ~LED_PROGRAM;
else if(~PINE & BTN_RIGHT)
PORTC &= ~LED_PLAY;
else if(~PIND & BTN_START)
PORTC =
~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRA
M|LED_PLAY);
else PORTC =
LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|
LED_PLAY;
2.3.5

2.20

89


.
.
// USART ( -
-)
serial_initialize(57600);
sei(); //
serial_initialize
serial.h, sei() , .

unsigned char ReceivedData = getchar();

if(ReceivedData == 'u')
Value++;
else if(ReceivedData == 'd')
Value--;
printf("%d\r\n", Value);
Value,
1 u 1
d. getchar().

2.21 -

90

2.3.6

.
2.3.
2.3
-510

C
C#
D
D#
E
F
F#
G
G#
A
A#
G

32.7032
34.6478
36.7081
38.8909
41.2034
43.6535
46.2493
48.9994
51.9130
55.0000
58.2705
61.7354

65.4064
69.2957
73.4162
77.7817
82.4069
87.3071
92.4986
97.9989
103.8262
110.0000
116.5409
123.4708

130.8128
138.5913
146.8324
155.5635
164.8138
174.6141
184.9972
195.9977
207.6523
220.0000
233.0819
246.9417

261.6256
277.1826
293.6648
311.1270
329.6276
349.2282
369.9944
391.9954
415.3047
440.0000
466.1638
493.8833

523.2511
554.3653
587.3295
622.2540
659.2551
698.4565
739.9888
783.9909
830.6094
880.0000
932.3275
987.7666

1046.502
1108.731
1174.659
1244.508
1318.510
1396.913
1479.978
1567.982
1661.219
1760.000
1864.655
1975.533

2093.005
2217.461
2349.318
2489.016
2637.020
2793.826
2959.955
3135.963
3322.438
3520.000
3729.310
3951.066

4186.009
4434.922
4698.636
4978.032
5274.041
5587.652
5919.911
6271.927
6644.875
7040.000
7458.620
7902.133

. Start
.

if(~PIND & SW_START)

{
PORTC =
~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRA
M|LED_PLAY);
_delay_ms(1);
PORTB |= 0x20;
_delay_ms(1);
PORTB &= ~0x20;
}
else
{
PORTC =
LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|
LED_PLAY;
PORTB &= ~0x20;
}
91

2.3.7
,
. , ,
, 1 .

if(~PIND & MIC_SIGNAL)

{
PORTC =
~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRA
M|LED_PLAY);
_delay_ms(1000);
}
else PORTC =
LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|
LED_PLAY;
2.3.8

. ,
-,
- .. , ,
-, 1.

92

 2.22 -

serial_initialize(57600);
sei();
ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1); //

ADMUX = ADC_PORT_1; // 1
PORTA &= ~0x80; // -
_delay_us(12); //
ADCSRA |= (1 << ADIF); //

ADCSRA |= (1 << ADSC); //

while( !(ADCSRA & (1 << ADIF)) ); //

PORTA = 0xFC; // -
printf( "%d\r\n", ADC); //
_delay_ms(50);
93

 -
, , ,
.
2.3.9 Dynamixel
Dynamixel

dinamixel.h.
ID 1.

unsigned short GoalPos[2] = {0, 1023};

serial_initialize(57600);
dxl_initialize( 0, 1 );
sei(); //
dxl_initialize(int devIndex, int baudnum)
. devIndex
, baudnum
(
). : 1 0
.
2.4
4
1
3
4
7
9
16
34
103
207

, bps
1000000.0
500000.0
400000.0
250000.0
200000.0
117647.1
57142.9
19230.8
9615.4

bps
1000000.0
0.000 %
500000.0
0.000 %
400000.0
0.000 %
250000.0
0.000 %
200000.0
0.000 %
115200.0
-2.124 %
57600.0
0.794 %
19200.0
-0.160 %
9600.0
-0.160 %

94

//
bMoving = dxl_read_byte( id, P_MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( bMoving == 0 )
{
//
if( index == 0 )
index = 1;
else
index = 0;
//
dxl_write_word( id, P_GOAL_POSITION_L, GoalPos[index] );
}
PrintErrorCode();
//
wPresentPos = dxl_read_word( id, P_PRESENT_POSITION_L );
printf( "%d %d\n",GoalPos[index], wPresentPos );
}
else
PrintCommStatus(CommStatus);
dxl_write_word( int id, int address, value )
; id ID
; address ; value .
.
, ,
,
.
2.3.10

ID 1 3.

Dynamixel.
95

int AmpPos = 512;

serial_initialize(57600);
dxl_initialize( 0, 1);
sei(); // Interrupt Enable
for( i=0; i<NUM_ACTUATOR; i++ )
{
id[i] = i+1;
phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
}
//
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
//
dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );
_delay_ms(1000);

.
.
,
,
. , SYNC WRITE ,

. , 4
, 26 Dynamixel
.
, 143 ,
Dynamixel 143 .
:
ID 0XFE
Length (L+1) X N + 4 (L: , N:
)
Instruction 0X83
Parameter1
96

Parameter2
Parameter3 ID
Parameter4
Parameter5

Parameter L+3 L-
Parameter L+4 ID
Parameter L+5
Parameter L+6

Parameter 2L+4 L-
2.5 :

:
ID 0:
0010 0150
ID 1:

:
0XFF 0XFF 0XFE 0X18 0X83 0X1E
0X04 0X00 0X10 0X00 0X50 0X01
0X01 0X20 0X02 0X60 0X03 0X02
0X30 0X00 0X70

0220 0360
ID 2:
0030 0170
ID 3:
0220 0380
:
//
dxl_set_txpacket_id(BROADCAST_ID);
dxl_set_txpacket_instruction(INST_SYNC_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, 2);
for( i=0; i<NUM_ACTUATOR; i++ )
{
dxl_set_txpacket_parameter(2+3*i, id[i]);
97

GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos);

printf( "%d ", GoalPos );
dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
}
dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
printf( "\n" );
dxl_txrx_packet();
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
PrintErrorCode();
else
PrintCommStatus(CommStatus);
theta += STEP_THETA;
if( theta > 2*PI )
theta -= 2*PI;
_delay_ms(CONTROL_PERIOD);
.

,
.
2.3.11 Zigbee
ZigBee (

(API)

(NWK)),

(MAC) (PHY),
IEEE

802.15.4.

ZigBee

IEEE

802.15.4

(WPAN).

ZigBee

98


[25].

2.23
ZigBee ,

(-, ),
(mesh)
. , ZigBee

,
, ,
, , ,
, .
ZigBee
2,405-2,485

(
07.05.2007 07-20-03-001).
ZigBee
, (

),

99

 ,
.

RC100 ZigBee .
C D. ZigBee
D:

PORTD &= ~0x80; //PORT_LINK_PLUGIN = 0;

PORTD &= ~0x20; //PORT_ENABLE_RXD_LINK_PC = 0;
PORTD |= 0x40; //PORT_ENABLE_RXD_LINK_ZIGBEE = 1;
zgb_initialize( 0 );
sei(); //
Zigbee . Zigbee
, .
0.

if(zgb_rx_check() == 1)
{
RcvData = zgb_rx_data();
if(RcvData & RC100_BTN_1)
PORTC &= ~LED_MANAGE;
else
PORTC |= LED_MANAGE;
if(RcvData & RC100_BTN_2)
PORTC &= ~LED_PROGRAM;
else
PORTC |= LED_PROGRAM;
if(RcvData & RC100_BTN_3)
PORTC &= ~LED_PLAY;
else
PORTC |= LED_PLAY;
}
, ZigBee ,

zgb_rx_data().

100

 zgb_rx_check. , ,
2.6.
2.6 ,

1
2
4
8
16
32
64
128
256
512

RC100_BTN_U - U
RC100_BTN_D - D
RC100_BTN_L - L
RC100_BTN_R - R
RC100_BTN_1 - 1
RC100_BTN_2 - 2
RC100_BTN_3 - 3
RC100_BTN_4 - 4
RC100_BTN_5 - 5
RC100_BTN_6 - 6

2.23 - RC100

CM-510

.
2.3.12 Bioloid

: Bioloid
. (.
2.2):

(),

(),

().
101

 2.24

Bioloid.

.
Bioloid Premium Kit.

2.24 -


. ,
, .
RoboPlus Motion,
dxl_read_word( int id, int address ),
int address=36. ,
.
.

:

#include <avr/io.h>
102

#include <avr/interrupt.h>
#include <stdio.h>
#include <math.h>
#include <util/delay.h>

#include "serial.h"
#include "dynamixel.h"

/// Control table address

#define P_GOAL_POSITION_L

30

#define P_GOAL_POSITION_H

31

#define P_GOAL_SPEED_L

32

#define P_GOAL_SPEED_H

33

#define P_PRESENT_POSITION_L 36
#define P_PRESENT_POSITION_H 37

#define DEFAULT_BAUDNUM

1 // 1Mbps

#define NUM_ACTUATOR

6 // Number of actuator

#define CONTROL_PERIOD

(500) // msec

void PrintCommStatus(int CommStatus);

void PrintErrorCode(void);

int main(void)
{
int id[NUM_ACTUATOR];
int AmpPos = 512;
int GoalPos, i, j, Error, l, CommStatus, wPresentPos;
int u[NUM_ACTUATOR];
103

const int k_hip = 57, k_kn=43, k_an=43;

const int MaxSpeed = 1023;
int buffer = 0;
// ID 11-16
int IDPositions[6][28] = {{363,372, 373, 361, 341, 319, 299, 287, 283,
286, 291, 293, 293, 297, 302, 308, 314, 321, 327, 332, 335,
337, 338, 339, 340, 343, 347, 354},
{721, 715, 709, 702, 696, 691, 688, 686, 685, 684, 683, 680,
676, 669, 660, 651, 650, 662, 682, 704, 724, 736, 740, 737,
732, 730, 730, 726},
{254, 262, 262, 245, 222, 203, 196, 203, 222, 245, 262, 262,
254, 248, 246, 248, 252, 257, 262, 266, 267, 266, 262, 257,
252, 248, 246, 248},
{777, 775, 771, 766, 761, 757, 756, 757, 761, 766, 771, 775,
777, 775, 769, 761, 761, 778, 801, 820, 827, 820, 801, 778,
761, 761, 769, 775},
{675, 675, 677, 682, 685, 681, 669, 649, 627, 607, 595, 596,
605, 614, 621, 625, 628, 629, 630, 631, 633, 636, 641, 647,
654, 660, 666, 671},
{402, 398, 395, 394, 393, 392, 390, 387, 382, 376, 369, 363,
357, 352, 348, 348, 346, 341, 338, 342, 354, 374, 396, 416,
428, 427, 418, 409}};

serial_initialize(57600);
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
sei(); //

ID , 11 16, :
104

for( i=0; i<NUM_ACTUATOR; i++ )

{id[i] = i+11;

//

Set goal position

dxl_write_word( 7, P_GOAL_POSITION_L, 358 );
dxl_write_word( 8, P_GOAL_POSITION_L, 665 );
dxl_write_word( 9, P_GOAL_POSITION_L, 511 );
dxl_write_word( 10, P_GOAL_POSITION_L, 511 );
dxl_write_word( 17, P_GOAL_POSITION_L, 511 );
dxl_write_word( 18, P_GOAL_POSITION_L, 511 );
_delay_ms(1000);

.
:
while(1)
{
for( j=0; j<28; j++ ) // Loop for walk position moving
{
for( i=0; i<6; i++ )
{

wPresentPos = dxl_read_word( id[i], 36 );

if (j==0) l=27;
else l=j-1;

}
//
for( i=0; i<2; i++ )
105

{
GoalPos = IDPositions[i][j];
wPresentPos = dxl_read_word( id[i], P_PRESENT_POSITION_L );
Error = wPresentPos - IDPositions[i][j];
buffer = k_hip * Error;
if(buffer < 0) buffer=-buffer;
if(buffer > MaxSpeed) buffer = MaxSpeed;
else if(buffer < 100) buffer = 100;
u[i] = buffer;
}
//
for( i=2; i<4; i++ )
{
GoalPos = IDPositions[i][j];
wPresentPos = dxl_read_word( id[i], P_PRESENT_POSITION_L );
Error = wPresentPos - IDPositions[i][j];
buffer = k_kn * Error;
if(buffer < 0) buffer=-buffer;
if(buffer > MaxSpeed) buffer = MaxSpeed

//
for( i=4; i<6; i++ )
{
GoalPos = IDPositions[i][j];
wPresentPos = dxl_read_word( id[i], P_PRESENT_POSITION_L );
Error = wPresentPos - IDPositions[i][j];
buffer = k_an * Error;
if(buffer < 0) buffer=-buffer;
106

if(buffer > MaxSpeed) buffer = MaxSpeed

//
dxl_set_txpacket_id(BROADCAST_ID);
dxl_set_txpacket_instruction(INST_SYNC_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, 4);
for( i=0; i<NUM_ACTUATOR; i++ )
{
GoalPos = IDPositions[i][j];
dxl_set_txpacket_parameter(2+5*i, id[i]);
dxl_set_txpacket_parameter(2+5*i+1, dxl_get_lowbyte(GoalPos));
dxl_set_txpacket_parameter(2+5*i+2, dxl_get_highbyte(GoalPos));
dxl_set_txpacket_parameter(2+5*i+3, dxl_get_lowbyte(u[i]));
dxl_set_txpacket_parameter(2+5*i+4, dxl_get_highbyte(u[i]));
}
dxl_set_txpacket_length((4+1)*NUM_ACTUATOR+4);
dxl_txrx_packet();

CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
PrintErrorCode();
else
PrintCommStatus(CommStatus);
_delay_ms(CONTROL_PERIOD);
}
}
return 0;
}
107


PrintCommStatus(int CommStatus)
PrintErrorCode():
//
void PrintCommStatus(int CommStatus)
{
switch(CommStatus)
{
case COMM_TXFAIL:
printf("COMM_TXFAIL: Failed transmit instruction packet!\n");
break;
case COMM_TXERROR:
printf("COMM_TXERROR: Incorrect instruction packet!\n");
break;
case COMM_RXFAIL:
printf("COMM_RXFAIL: Failed get status packet from device!\n");
break;
case COMM_RXWAITING:
printf("COMM_RXWAITING: Now recieving status packet!\n");
break;
case COMM_RXTIMEOUT:
printf("COMM_RXTIMEOUT: There is no status packet!\n");
break;
case COMM_RXCORRUPT:
printf("COMM_RXCORRUPT: Incorrect status packet!\n");
break;
default:
printf("This is unknown error code!\n");
break;
108

}
}
//
void PrintErrorCode()
{
if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
printf("Input voltage error!\n");
if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
printf("Angle limit error!\n");
if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
printf("Overheat error!\n");
if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
printf("Out of range error!\n");
if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
printf("Checksum error!\n");
if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
printf("Overload error!\n");
if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
printf("Instruction code error!\n");
}

( )
2.25 2.30.

109

 2.25
11

2.26
12

2.27
13

110

 2.28
14

2.29
15

2.30
16

,
40, .
2.7

2.31.
.
111

 2.31 : 1 0.12; 2 0.25; 3

0.37; 4 0.48; 5 0.6; 6 0.72; 7 0.84; 8 0.95; 9 1.09; 10 1.2; 11
1.31; 12 1.44; 13 1.56; 14 1.56; 15 1.68; 16 1.8; 17 1.90; 18
2.02


. , ,

,
, .

112


1. .., .., .., ..,
.., .., .. LEGO MINDSTORMS
NXT // . 2011. 1. .103-108.
2. Information Control Problems in Manufacturing, V. 1 -14.
3. Bobtsov A.A., Pyrkin A.A., Borgul A.S., Zimenko K.A. Control Approaches
for Complicated Self-Unstable Plants with Applications for Two-Wheel Mobile
Robot Motobot in Educational Purposes // Preprints of the 9th IFAC
Symposium on Advances in Control Education (ACE2012), Nizhny Novgorod,
Russia,2012, pp. 107-111;
4. Bobtsov A.A., Kolyubin S.A., Pyrkin A.A., Borgul A.S., Zimenko K.A.,
Rabysh E.Y. Mechatronic and Robotic Setups forModern Control Theory
Workshops // Preprints of the 9th IFAC Symposium on Advances in Control
Education (ACE2012), Nizhny Novgorod, Russia,2012, pp. 348-353;
5. .., .., .., ..
//
- , 2011, #75, . 6368;
6. .., .., .., ..
// -
, 2011, #75, . 5863;
7. .., .. NXTOSEK
EMBEDDED CODER ROBOT NXT LEGO
MINDSTORMS NXT //
PACO 2010, .
.1489-1496.
8. .. : //
.: , 2011. 184 .
9. LEGO firmware. http://mindstorms.lego.com
10.leJOS. http://lejos.sourceforge.net
11.nxtOSEK. http://lejos-osek.sourceforge.net
12.OSEK. http://portal.osek-vdx.org
13.ECRobotInstaller. www.mathworks.com/matlabcentral/fileexchange/25207
14.Explorer. www.nxtprograms.com/NXT2/explorer/index.html
15.NXT GamePad http://lejos-osek.sourceforge.net/nxtgamepad.htm.
16.US Navy Academy to Acquire 50 Robotis Bioloid Humanoid Robot Kits from
KumoTek http://www.azonano.com/news.aspx?NewsID=4498
17.

Bioloid

C
http://support.robotis.com/en/software/embeded_c/cm510_cm700.htm
18. ROBOTIS BIOLOID http://www.robotis.com/xe/BIOLOID_main_en
19. Benedettelli D., Casini M., Garulli A., Giannitrapani A., Vicino A.A. LEGO
Mindstorms experimental setup for multi-agent systems // Control Applications,
(CCA) & Intelligent Control, (ISIC), 2009 IEEE, pp. 1230-1235;
113

20. Brigandi S., Yunfeng Wang, Field J. A LEGO Mindstorms NXT based
multirobot system // Advanced Intelligent Mechatronics (AIM), 2010
IEEE/ASME International Conference, pp. 135-139;
21. Akhtaruzzaman M., Shafie A.A. Geometrical analysis on BIOLOID humanoid
system standing on single leg // Mechatronics (ICOM), 2011 4th International
Conference, pp. 1-5;
22. RoboCup. http://www.robocup.org;
23. RoboPlus - BIOLOID.
http://www.wertech.ru/Blog/23-roboplus-bioloid.aspx;
24. Roboforum. http://roboforum.ru/forum67/topic11643.html#p245158;
25. Atmel Studio 6. http://www.atmel.com/microsite/atmel_studio6;
26. ZigBee. http://ru.wikipedia.org/wiki/ZigBee
27. .., .., ..
. .: . , 1981. 448 .
28.Anderson B.D.O., Moore J.B. Optimal control: linear quadratic methods //
Prentice-Hall, 1989. 394 p.
29.Ha Y.-S., Yuta S. Trajectory tracking control for navigation of self-contained
mobile inverse pendulum // In Proc. IEEE/RSJ Intl. Conf. on Intelligent Robots
and Systems, pages 1875-1882, 1994.
30.Linear-Quadratic-Regulator (LQR) design MATLAB [
].

:
http://www.mathworks.com/help/toolbox/control/ref/lqr.html, . .
.
31. ..
[]: . ... - . : 05.13.01 / ..
. . .: [..], 1988. 483 .

114

115

116

117

118

119

120

 Segway

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136


-
- My files
- NXTway-GS,

137