nxt ballbot âéâfâïâxü[âxèjö¡.pdf

NXT Ballbot LEGO Mindstorms NXT

1 1

E-mail : [email protected]

1.0 2009 4

[email protected]

URL

NXT Ballbot Ballbot Ralph Hollis

LEGO Mindstorms NXT Ballbot URL

http://en.wikipedia.org/wiki/Ballbot

MATLAB / Simulink NXT Ballbot

z NXT Ballbot z NXT Ballbot z NXT Ballbot z

NXT BallbotNXT Ballbot

Embedded Coder Robot NXTEmbedded Coder

Robot NXT URLEmbedded Coder Robot NXT Embedded

Coder Robot NXT Instruction En.pdf

http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=13399&objectType=file

Embedded Coder Robot NXT 3.14

nxtOSEKLEJOS OSEK 2.03

Cygwin 1.5.24

GNU ARM 4.0.2

- i -

MATLAB

MATLAB 7.5.0 R2007b

Control System Toolbox 8.0.1 R2007b

Simulink 7.0 R2007b

Real-Time Workshop 7.0 R2007b

Real-Time Workshop Embedded Coder 5.0 R2007b

Virtual Reality Toolbox (N1) 4.6 R2007b

(N1) Virtual Reality Toolbox 3D nxt_ballbot_vr.mdlVirtual

Reality Toolbox 3D nxt_ballbot.mdl

iswall.m M-

mywritevrtrack.m VRML M-

nxt_ballbot.mdl NXT Ballbot Virtual Reality Toolbox 3D

nxt_ballbot_controller.mdl NXT Ballbot

nxt_ballbot_vr.mdl NXT Ballbot Virtual Reality Toolbox 3D

param_controller.m M-

param_ nxt_ballbot.m NXT Ballbot param_***.m

param_plant.m M-

param_sim.m M-

post_sdo_codegen.m Simulink Data Object M-

pre_sdo_codegen.m Simulink Data Object M-

vr_nxt_ballbot.wrl NXT Ballbot VRML

vr_nxt_ballbot_track.bmp

vr_nxt_ballbot_track.wrl VRML

- ii -

.................................................................................................................................................................... i ........................................................................................................................................................................ i MATLAB.................................................................................................................................................... ii ....................................................................................................................................................... ii 1 ............................................................................................................................... 1

1.1 ........................................................................................................................... 1 1.2 ................................................................................................................... 2 1.3 ............................................................................................................... 3

2 NXT Ballbot .......................................................................................................................................... 4 2.1 ........................................................................................................................................................... 4 2.2 ....................................................................................................................... 5

3 NXT Ballbot .............................................................................................................................. 6 3.1 ............................................................................................................................... 6 3.2 ....................................................................................................... 8 3.3 ....................................................................................................... 9

4 NXT Ballbot ............................................................................................................................ 11 4.1 ......................................................................................................................................... 11 4.2 ............................................................................................................................................. 12

5 NXT Ballbot ........................................................................................................................................ 15 5.1 ............................................................................................................................................. 15 5.2 ..................................................................................................................................... 19

6 ............................................................................................................................................. 20 6.1 ............................................................................................................................................. 20 6.2 ..................................................................................................................................... 21 6.3 ................................................................................................................................................. 22 6.4 ..................................................................................................................................................... 23

7 ..................................................................................................................................... 24 7.1 ............................................................................................................................. 24 7.2 ............................................................................................................................................. 26 7.3 task_init....................................................................................................................... 30 7.4 4mstask_ts1 ........................................................................................................................... 30 7.5 20mstask_ts2......................................................................................................................... 37 7.6 100mstask_ts3....................................................................................................................... 37 7.7 ..................................................................................................................... 38

8 ......................................................................................................................................... 39 8.1 ......................................................................................................................... 39 8.2 ......................................................................................................................... 40

8.3 3D .................................................................................................................................................... 42 9 ......................................................................................................................................... 43

9.1 ................................................................................................................................................. 43 9.2 ......................................................................................................................... 44 9.3 ................................................................................................................................................. 45

10 ............................................................................................................................................. 47 A ........................................................................................................................................... 48

A.1 ....................................................................................................................................................... 48 A.2 ....................................................................................................................... 48 A.3 ............................................................................................................................................... 50

B ................................................................................................................................................... 52 B.1 ....................................................................................................................................................... 52 B.2 ........................................................................................................................... 54 B.3 ............................................................................................................................... 55

C .................................................................................................................................. 57 ................................................................................................................................................................. 61 MATLAB.......................................................................................................................... 62

- 1 -

1

1.1

Model Based Design / DevelopmentMBD

MBD

RTW-EC C

C 1-1 MATLAB

MBD

Simulink Stateflow Simulink Control Design Simulink Response Optimization Simulink Parameter Estimation SimMechanics SimPowerSystems SimDriveline SimHydraulics Signal Processing Blockset Simulink Fixed Point

RP

HILS

(RTW)

RP/HILS Rapid Prototyping/Hardware In the Loop Simulation

(RTW-EC)

MATLAB

Data Acquisition Instrument Control OPC

Control System System Identification Fuzzy Logic Robust Control Model Predictive Control Neural Network Optimization Signal Processing Fixed-Point

Real-Time Workshop Real-Time Workshop Embedded Coder Stateflow Coder xPC Target

MATLAB Toolbox Simulink

1-1 MATLAB MBD

- 2 -

1.2

MBD 1-2 V

V

V MBD V

1-2 MBD V

- 3 -

1.3

MBD

z z z z z

- 4 -

2 NXT Ballbot

NXT Ballbot

2.1

NXT Ballbot 2-1

HiTechnic

DC

2-1 NXT Ballbot

3 4

2

X Y

- 5 -

2.2

NXT Ballbot 2-1 2-2

2-1

[1/sec]

deg int32 1000

cm int32 50 ()

deg/sec uint16 300

2-2

[1/sec]

DC PWM % int8 500

[1] DC

- 6 -

3 NXT Ballbot

NXT Ballbot

3.1

XZ YZ

NXT Ballbot

3-1

3-2

: : s : DC

3-1

3-2

Z

bz

L

JMb ,

ss JM ,

bx

R

s

sz

sx X

- 7 -

NXT Ballbot DC

3-3

s m : DC ssmw RR =

m

DC

w

NXT Ballbot

: 81.9=g ]sec/[ 2m : 013.0=sM ][kg : 026.0=sR ][m 32 2sss RMJ = : ][ 2kgm : DC 015.0=wM ][kg : DC 021.0=wR ][m : DC 022.0=wL ][m 22www LMJ = : DC ][ 2kgm : 682.0=bM ][kg : 135.0=W ][m : 135.0=D ][m : 17.0=L ][m 32LMJ b= : ][ 2kgm : DC 5101 =mJ ][ 2kgm : DC 69.6=mR ][ 468.0=bK ]sec[ radV : DC 317.0=tK ][ ANm : DC

0022.0=mf : DC 0=sf :

z [2] tbm KKR ,,z smm ffJ ,,

R

sR

sJM ,JM , sww

3-3 DC

- 8 -

3.2

3-2

0=t 0=

( ) ( bsss zRzx ,, )= (3.1) ( ) ( )0,, &&& sss Rzx = (3.2) ( ) ( ) cos,sin, LzLxzx bb ++= (3.3) ( ) ( ) sin,cos, &&&&& LLRzx sbb += (3.4)

U 1T 2T

( ) ( ) (3.5) 22221 2121 bbbsss zxMzxMT &&&& +++=

( ) ( )222222222

2

21

21

21

21

21

21

21

&&&&

&&&&

+++=

+++=

w

swmss

mmmwss

RR

JJJJ

JJJJT (3.6)

bbss gzMgzMU += (3.7)

L

UTTL += 21 (3.8)

FLL

dtd =

& (3.9)

FLL

dtd =

& (3.10)

(3.9)(3.10)

( ) ( )[ ] ( )[ ] FLRMJJkLRMJJkJRMM sbwmsbwmsssb =++++++ sincos 2222 &&&&& (3.11) ( )[ ] ( )[ ] FgLMJJkJLMJJkLRM bwmbwmsb =+++++ sincos 222 &&&& (3.12)

ws RRk =

- 9 -

DC

&& smmt ffiKF = (3.13) mmt fiKF &+= (3.14)

i DC )( &&& = km

DC PWM

DC iv

DC

(3.15) iRKviL mmbm = &&

DC (3.15)

m

b

RkKv

i)( && = (3.16)

(3.16)(3.13)(3.14)

( ) && ++= sfvF (3.17) && += vF (3.18)

+== m

m

bt

m

t fRKK

kRK , (3.19)

3.3

0 1cos,sin 2 (3.11)(3.12)

2&

( )[ ] ( ) FJkLRMJkJRMM msbmsssb =++++ &&&& 222 (3.20) ( ) ( ) FgLMJkJLMJkLRM bmbmsb =+++ &&&& 222 (3.21)

- 10 -

(3.20)(3.21)

HvGFE =

+

+

&&

&&&&

(3.22)

( )

=

=

+=

+++++=

H

gLMG

fF

JkJLMJkLRMJkLRMJkJRMM

E

b

s

mbmsb

msbmsssb

000

222

222

x u Tx x

(3.23) [ ] vT == ux ,,,, &&

(3.22)

uxx BA +=& (3.24)

=

=

)4()3(

00

,

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

10000100

BB

B

AAAAAA

A (3.25)

( )[ ]( )[ ][ ][ ]

[ ][ ]

2)2,1()2,2()1,1()det()det()2,1()1,1()4(

)det()2,1()2,2()3()det()2,1()1,1()4,4(

)det()2,1()2,2()4,3()det()1,1()2,1()3,4(

)det()2,1()2,2()3,3()det()1,1()2,4(

)det()2,1()2,3(

EEEEEEEBEEEB

EEEAEEEA

EEEfAEEEfA

EgLEMAEgLEMA

s

s

b

b

=+=+=

+=+=

++=++=

==

(3.24)[rad][deg] 180 180= mt RK [deg]

- 11 -

4 NXT Ballbot

NXT Ballbot

4.1

NXT Ballbot

(3.24)u DC PWM PWM NXT Ballbot DC m &

DC PWM

DC

4-1 NXT Ballbot

& 2

1.

2.

1.

NXT Ballbot

NXT Ballbot

NXT Ballbot

A

- 12 -

4.2

(3.22) 4-2

4-2

4-2

A

NXT BallbotA.3

4-3 NXT Ballbot

u++

+ +

Crefx ref

NXT Ballbot

fk

ik C

xC

x

4-3 NXT Ballbot

- 13 -

Q R

2

3

3

5

180106,

10100000100000100000106000001

=

= RQ

)2,2(Q

)5,5(Q

param_controller.m

param_controller.m

param_controller.m % Controller Parameters % Servo Gain Calculation using Optimal Regulator A_BAR = [A, zeros(4, 1); C(1, :), 0]; B_BAR = [B; 0]; Q = [ 1, 0, 0, 0, 0 0, 6e5, 0, 0, 0 0, 0, 1, 0, 0 0, 0, 0, 1, 0 0, 0, 0, 0, 1e3 ]; R = 6e3 * (180 / pi)^2; % R = 6e3 if the unit of angles is [rad]. K = lqr(A_BAR, B_BAR, Q, R); k_f = K(1:4); % feedback gain k_i = K(5); % integral gain

-0.0071

]0.2325- 0.027,- 1.5698,- -0.015,[

==

i

f

kk

- 14 -

NXT Ballbot X Y 4-4 NXT Ballbot

&k

NXT Ballbot

&k

Speed Reference

PWM Reference Generator

Saturation

Servo Controller

Friction Compensator

State

( )YX PWMPWM ,

( )refYX && ,

( )YX xx ,

4-4 NXT Ballbot

- 15 -

5 NXT Ballbot

NXT Ballbot

5.1

nxt_ballbot.mdl nxt_ballbot_vr.mdl NXT Ballbot

Virtual Reality Toolbox 3D

nxt_ballbot.mdl nxt_ballbot_vr.mdl

Environment

5-1 nxt_ballbot.mdl

5-2 Environment

- 16 -

Reference Generator

NXT Ballbot Signal Builder

NXT GamePad

32

5-3 Reference Generator

5-4

(- GP_MAX)

(+ GP_MAX)

(- GP_MAX)

(+ GP_MAX)

GP_MAX GP_MAX = 100

5-4

- 17 -

Controller

NXT Ballbot

nxt_ballbot_controller.mdl7

5-5 Controller (nxt_ballbot_controller.mdl)

Controller TS = 1 [ms]NXT Ballbot

0 [s] Rate

Transition

1 [ms]

5-6

- 18 -

NXT Ballbot

NXT Ballbot

6

5-7 NXT Ballbot

Viewer

nxt_ballbot.mdl XY Graph

nxt_ballbot_vr.mdl Virtual Reality Toolbox 3D

5-8 Viewer (nxt_ballbot.mdl)

- 19 -

5.2

5-1 M-

param_controller.m M-

param_nxt_ballbot.m NXT Ballbot param_***.m

param_plant.m M-

param_sim.m M-

param_nxt_ballbot.m param_***.m*** controller, plant, sim

param_nxt_ballbot.m

5-9 Viewer (nxt_ballbot_vr.mdl)

5-1

[] []

- 20 -

6

NXT Ballbot

6.1

NXT Ballbot

double

6-1 NXT Ballbot

X Y

PWM

6-2

- 21 -

6.2

PWM DC Actuators

6-3 Actuators

PWM DC Cal_vol_max

BV

maxV

(6.1) 625.0001089.0max = BVV

(6.1)DC

6-4 PWM 100%

6-4 PWM=100%

6-4 [1] (6.1)

- 22 -

6.3

Plant3.3

7.1

X

Y

6-5 Plant

- 23 -

6.4

Rate Transition

B.3

6-6 Sensors

- 24 -

7

nxt_ballbot_controller.mdl

7.1

7-1 nxt_ballbot_controller.mdl

PC 2

DC

C

DC

7-1

- 25 -

7-1 4

7-1

task_init

task_ts1 4 [ms]

DC

task_ts2 20 [ms]

task_ts3 100 [ms]

1 4 [ms]

20 [ms]

4.2

LEGO Mindstorms NXT ARM 7 FPU

GCC

- 26 -

7.2

nxt_ballbot_controller.mdl Embedded Coder Robot NXT

4ms 20ms 100ms


- 27 -

Embedded Coder Robot NXT

PWM

7-3 DC

ExpFcnCalls Scheduler

Function-Call Function-Call Subsystem

4ms 20ms 100ms

7-4

- 28 -

[]

-1

0

1

7-5

Data Store Memory

7-6

- 29 -

7-7 DC

X Y

X Y

Gain

7-7

- 30 -

7.3 task_init

or

7-8 task_init

7.4 4ms task_ts1

DC

operating_state

DC

7-9 task_ts1

- 31 -

Gyro_Calibration

param_controller.m time_start

DCStop

DC task_ts3

7-10 Gyro_Calibration

7-11 Stop

- 32 -

Balance_and_Drive_Control

4.2

PWM

7-12 Balance_and_Drive_Control

- 33 -

Cal_Command

7-13 Cal_Command

- 34 -

Discrete Derivative & Discrete Integrator

Euler

7-14 Discrete Derivative & Discrete Integrator

Reference

7-15 Reference

- 35 -

State

7-16 State

PWMPWM

4-4 PWM PWM (6.1)

PWM

7-17 PWM

- 36 -

Data_Logging

NXT GamePad ADC Data Logger

param_controller.m log_count

NXT GamePad ADC Data Logger 8 16

1 LSB

LSB

7-18 Data Logging

- 37 -

7.5 20ms task_ts2

7-19 task_ts2

7.6 100ms task_ts3

sound_dur [ms]

7-20 task_ts3

- 38 -

Timer_Stop

theta_diff_thr psi_diff_thr

7-21 Timer_Stop

7.7

nxt_ballbot_controller.mdl param_controller.m

7-2 LEGO Mindstorms NXT

7-2

k_f k_i

k_thetadot a_b a_d a_r a_gc a_gd

pwm_gain pwm_offset dst_thr

- 39 -

8

NXT Ballbot

nxt_ballbot_vr.mdl 3D

8.1

Simulink Reference Generator Signal Builder

8-1 Reference Generator

- 40 -

8.2

8-2 X 4 [deg]

0 [deg] 1 [s]

NXT Ballbot

8-3 X 20 5 20 5 20

8-2 X

8-3 X

- 41 -

URL

http://www.youtube.com/watch?v=1MfiAZBsWac

8-4

- 42 -

8.3 3D

nxt_ballbot_vr.mdl Virtual Reality Toolbox 3DVirtual Reality

nxt_ballbot_vr.mdl 5

z Top View z Vista View z Chaser View 1 NXT Ballbot z Chaser View 2 NXT Ballbot z Robot View NXT Ballbot

Top View Vista View

Chaser View 1 Chaser View 2

8-5

- 43 -

9

nxt_ballbot_controller.mdl

9.1

9-1 LEGO Mindstorms NXT Embedded Coder Robot NXT

9-1 LEGO Mindstorms NXT & Embedded Coder Robot NXT

ATMEL 32-bit ARM 7 (AT91SAM7S256) 48MHz

256 K 10000

RAM 64 K

DC 3

10064

Bluetooth

RTOS LEJOS C / LEJOS OSEK

GCC GCC C

RAM

- 44 -

9.2


1. [ SDO Usage ] Simulink

Simulink

Simulink

[4]

2. [ Generate code and build the generated code ]

3. NXT PC USB NXT NXT

SRAM [ Download (NXT enhanced firmware) ] [ Download (SRAM) ]

NXT

Step.1 Simulink Data Object

Step.2

Step.3a NXT

Step.3b NXT SRAM

9-1 /

C

- 45 -

9.3

NXT

9-2 0

9-2 X

9-3X 20520520

8-3

9-3 X

- 46 -

URL NXT Ballbot PC

http://www.youtube.com/watch?v=f8jxGsg3p0Y

9-4 NXT Ballbot

LEGO Mindstorms NXT

- 47 -

10

z z z z NXT Ballbot

- 48 -

A

NXT Ballbot

[3]

A.1

u 0

(A.1) 0)(lim =

tt

x

(A.2) )()()( tBtAt uxx +=&

A

A.2

x K

PD A-1 x refx

( )reftKt xxu = )()( (A.3) (A.4) ( ) refBKtBKAt xxx += )()(&

BKA K

uxx BA +=&u xrefx +

K

A-1

- 49 -

[ ]BAABBM nc 1,, = L (A.5) nMrank c =)( n Control System Toolbox ctrb

x

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

K 2

1.

BKA K

Control System Toolbox place

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

>> A = [0, 1; -2, -3]; B = [0; 1]; >> Mc = ctrb(A, B); >> rank(Mc) ans = 2

>> A = [0, 1; -2, -3]; B = [0; 1]; >> poles = [-5, -6]; >> K = place(A, B, poles) K = 28.0000 8.0000

2.

J K

Q R

Control System Toolbox

lqr

( ) += 0 )()()()( dttRttQtJ TT uuxx

- 50 -

A = [0, 1; -2, -3]B = [0; 1] Q = [100, 0; 0, 1]R = 1

A.3

PID I-PD

A-2 PID

y

(A.6) )()(

)()()(tCt

tBtAtxy

uxx=

+=&

( )reftCt xxe = )()( )(tz Tttt )](),([)( zxx = u n x m

(A.7) ref

mm

mn

mmmm

mn CI

tB

tt

CA

tt

xuzx

zx

+

=

0)(0)(

)(00

)()(

&&

uxx BA +=&u x++

iK C+

fK+

refx refyC

>> A = [0, 1; -2, -3]; B = [0; 1]; >> Q = [100, 0; 0, 1]; R = 1; >> K = lqr(A, B, Q, R) K = 8.1980 2.1377

A-2 PID

- 51 -

(A.7)

(A.8) ref

mm

mn

mmmm

mn CI

BCA

xuzx

zx

+

=

0)(0)(

)(00

)()(

&&

)()( = xxx te )()( = zzz te )()( = uuu te (A.7)(A.8)

)()()()(0)(

)(00

)()(

tBtAtdtdt

Btt

CA

tt

eeeemme

e

mm

mn

e

e uxxuzx

zx +=

+

=

&& (A.9)

)()()()( tKtKtKt eiefee zxxu == (A.10)

refxx )( 0)( z 0)( u )(tu

(A.11) ( dttCKtKt refireff = xxxxu )())(()( )

I-PD (A.11) 1 0 PID (A.11) 1

0 refx refx

- 52 -

B

NXT Ballbot

B.1

nxt_ballbot_vr.mdl NXT Ballbot 3D Virtual Reality Toolbox Virtual

Reality Toolbox 3D VRML VRML B-1

x

y

z

z

x

y

MATLAB Graphics VRML

B-1 VRML

VRML vr_nxt_ballbot_track.wrl B-2

z [cm]

x [cm]

0 100 200

0

100

200

B-2 vr_nxt_ballbot_track.wrl

- 53 -

nxt_ballbot.mdl / nxt_ballbot_vr.mdl NXT Ballbot / Sensors / Position NXT Ballbot

VRML

B-3 Position

- 54 -

B.2

mywritevrtrack.m vr_nxt_ballbot_track.bmp VRML

vr_nxt_ballbot_track.wrl vr_nxt_ballbot_track.wrl

>> mywritevrtrack('vr_nxt_ballbot_track.bmp')

mywritevrtrack.m

z 1 pixel 11 [cm2] z pixelRGB = [255, 255, 255] z pixelRGB = [128, 128, 128] 20 [cm] z pixelRGB = [0, 0, 0]

vr_nxt_ballbot_track.bmp vr_nxt_ballbot_track.wrl

B-4 VRML

- 55 -

B.3

NXT Ballbot Sensors Embedded

MATLAB Function

B-5 Embedded MATLAB Function

- 56 -

NXT Ballbot

B-6

- 57 -

C

nxt_ballbot_controller.mdl C

ballbot_app.c #include "ballbot_app.h" #include "ballbot_app_private.h" void task_init(void) { battery = (real32_T)ecrobot_get_battery_voltage(); gyro_offset[0] = (real32_T)ecrobot_get_gyro_sensor(NXT_PORT_S1); gyro_offset[1] = (real32_T)ecrobot_get_gyro_sensor(NXT_PORT_S2); start_time = ecrobot_get_systick_ms(); flag_mode = 1U; } void task_ts1(void) { real32_T rtb_Sum1; real32_T rtb_DataStoreRead4; real32_T rtb_MatrixConcatenate[8]; real32_T rtb_Sum2_b[8]; real32_T rtb_Saturation[2]; int16_T rtb_DataTypeConversion2_o[2]; int8_T rtb_Switch2_e; uint8_T rtb_Sum_d; boolean_T rtb_Switch3; { int32_T tmp; int8_T rtb_MatrixConcatenate_g_idx; real32_T rtb_UnitDelay_idx; real32_T rtb_UnitDelay_idx_0; real32_T rtb_Divide_fh_idx; real32_T rtb_Divide_fh_idx_0; real32_T rtb_Sum_idx; real32_T rtb_Sum_idx_0; real32_T rtb_UnitDelay_d_idx; real32_T rtb_UnitDelay_d_idx_0; real32_T rtb_theta_idx; real32_T rtb_theta_idx_0; real32_T rtb_Sum_a_idx; real32_T rtb_Sum_a_idx_0; real32_T rtb_UnitDelay_p_idx; real32_T rtb_UnitDelay_p_idx_0; real32_T rtb_Sum2_j_idx; real32_T rtb_Sum2_j_idx_0; real32_T rtb_Sum3_h_idx; real32_T rtb_Sum3_h_idx_0; switch (operating_state) { case 1: rtb_Sum1 = 1.0F - a_gc; gyro_offset[0] = (real32_T)ecrobot_get_gyro_sensor(NXT_PORT_S1) * rtb_Sum1 + a_gc * gyro_offset[0]; gyro_offset[1] = (real32_T)ecrobot_get_gyro_sensor(NXT_PORT_S2) * rtb_Sum1 + a_gc * gyro_offset[1]; ecrobot_set_motor_mode_speed(NXT_PORT_B, 1, 0); ecrobot_set_motor_mode_speed(NXT_PORT_C, 1, 0); break;

- 58 -

case 2: ecrobot_read_bt_packet(&(bluetooth_rx[0]), 32); rtb_DataStoreRead4 = battery; rtb_UnitDelay_idx = ud_int_thetadot_ref[0]; rtb_UnitDelay_idx_0 = ud_int_thetadot_ref[1]; rtb_Divide_fh_idx = ts1 * ud_int_thetadot_ref[0] / 1000.0F; rtb_Divide_fh_idx_0 = ts1 * ud_int_thetadot_ref[1] / 1000.0F; if (flag_mode) { rtb_MatrixConcatenate_g_idx = (int8_T)bluetooth_rx[1]; if (flag_avoid) { rtb_Switch2_e = gp_max; } else { rtb_Switch2_e = (int8_T)bluetooth_rx[0]; } rtb_Switch2_e = (int8_T)(-rtb_Switch2_e); } else { if (flag_auto) { if (flag_avoid) { rtb_MatrixConcatenate_g_idx = 0; rtb_Switch2_e = gp_max; } else { rtb_MatrixConcatenate_g_idx = gp_max; rtb_Switch2_e = 0; } } else { rtb_Switch2_e = 0; rtb_MatrixConcatenate_g_idx = 0; } } rtb_Saturation[0] = (real32_T)rtb_Switch2_e; rtb_Saturation[1] = (real32_T)rtb_MatrixConcatenate_g_idx; rtb_Sum1 = 1.0F - a_r; rtb_Sum_idx = rtb_Saturation[0] / (real32_T)gp_max * k_thetadot * rtb_Sum1 + ud_lpf_thetadot_ref[0] * a_r; rtb_Sum_idx_0 = rtb_Saturation[1] / (real32_T)gp_max * k_thetadot * rtb_Sum1 + ud_lpf_thetadot_ref[1] * a_r; rtb_Sum2_b[0] = rtb_Divide_fh_idx; rtb_Sum2_b[4] = rtb_Divide_fh_idx_0; rtb_Sum2_b[1] = 0.0F; rtb_Sum2_b[5] = 0.0F; rtb_Sum2_b[2] = rtb_Sum_idx; rtb_Sum2_b[6] = rtb_Sum_idx_0; rtb_Sum2_b[3] = 0.0F; rtb_Sum2_b[7] = 0.0F; rtb_Sum1 = Rw / Rs; rtb_UnitDelay_d_idx = ud_int_psidot[0]; rtb_UnitDelay_d_idx_0 = ud_int_psidot[1]; rtb_Saturation[0] = ts1 * ud_int_psidot[0] / 1000.0F; rtb_Saturation[1] = ts1 * ud_int_psidot[1] / 1000.0F; rtb_theta_idx = rtb_Sum1 * (real32_T)ecrobot_get_motor_rev(NXT_PORT_B) + rtb_Saturation[0]; rtb_theta_idx_0 = rtb_Sum1 * (real32_T)ecrobot_get_motor_rev(NXT_PORT_C) + rtb_Saturation[1]; rtb_Sum1 = 1.0F - a_d; rtb_Sum_a_idx = rtb_theta_idx * rtb_Sum1 + ud_lpf_theta[0] * a_d; rtb_Sum_a_idx_0 = rtb_theta_idx_0 * rtb_Sum1 + ud_lpf_theta[1] * a_d; rtb_Sum2_j_idx = (real32_T)ecrobot_get_gyro_sensor(NXT_PORT_S1); rtb_Sum2_j_idx_0 = (real32_T)ecrobot_get_gyro_sensor(NXT_PORT_S2); rtb_Sum1 = 1.0F - a_gd; rtb_Sum3_h_idx = rtb_Sum2_j_idx * rtb_Sum1 + a_gd * gyro_offset[0]; rtb_Sum3_h_idx_0 = rtb_Sum2_j_idx_0 * rtb_Sum1 + a_gd * gyro_offset[1]; rtb_Sum2_j_idx -= rtb_Sum3_h_idx; rtb_Sum2_j_idx_0 -= rtb_Sum3_h_idx_0; rtb_MatrixConcatenate[0] = rtb_theta_idx; rtb_MatrixConcatenate[4] = rtb_theta_idx_0; rtb_MatrixConcatenate[1] = rtb_Saturation[0]; rtb_MatrixConcatenate[5] = rtb_Saturation[1];

- 59 -

rtb_MatrixConcatenate[2] = (rtb_Sum_a_idx - ud_drv_theta[0]) * 1000.0F / ts1; rtb_MatrixConcatenate[6] = (rtb_Sum_a_idx_0 - ud_drv_theta[1]) * 1000.0F / ts1; rtb_MatrixConcatenate[3] = rtb_Sum2_j_idx; rtb_MatrixConcatenate[7] = rtb_Sum2_j_idx_0; for (tmp = 0; tmp < 8; tmp++) { rtb_Sum2_b[tmp] -= rtb_MatrixConcatenate[tmp]; } psi_diff[0] = rtb_Sum2_b[1]; psi_diff[1] = rtb_Sum2_b[5]; gyro_offset[0] = rtb_Sum3_h_idx; gyro_offset[1] = rtb_Sum3_h_idx_0; theta_diff[0] = rtb_Sum2_b[0]; theta_diff[1] = rtb_Sum2_b[4]; rtb_UnitDelay_p_idx = ud_int_theta_diff[0]; rtb_UnitDelay_p_idx_0 = ud_int_theta_diff[1]; rtb_Sum3_h_idx = ts1 * ud_int_theta_diff[0] / 1000.0F * k_i; rtb_Sum3_h_idx_0 = ts1 * ud_int_theta_diff[1] / 1000.0F * k_i; { static const int_T dims[3] = { 1, 4, 2 }; rt_MatMultRR_Sgl((real32_T *)rtb_Saturation, (real32_T *)&(k_f[0]), (real32_T *)rtb_Sum2_b, &dims[0]); } rtb_Sum1 = 1.089000027E-003F * rtb_DataStoreRead4 - 0.625F; rtb_Saturation[0] = (rtb_Sum3_h_idx + rtb_Saturation[0]) / rtb_Sum1 * 100.0F; rtb_Saturation[1] = (rtb_Sum3_h_idx_0 + rtb_Saturation[1]) / rtb_Sum1 * 100.0F; rtb_Saturation[0] = pwm_offset * rt_FSGN(rtb_Saturation[0]) + pwm_gain * rtb_Saturation[0]; rtb_Saturation[1] = pwm_offset * rt_FSGN(rtb_Saturation[1]) + pwm_gain * rtb_Saturation[1]; rtb_Saturation[0] = rt_SATURATE(rtb_Saturation[0], -100.0F, 100.0F); rtb_Saturation[1] = rt_SATURATE(rtb_Saturation[1], -100.0F, 100.0F); ecrobot_set_motor_mode_speed(NXT_PORT_B, 1, (int8_T)rtb_Saturation[0]); ecrobot_set_motor_mode_speed(NXT_PORT_C, 1, (int8_T)rtb_Saturation[1]); rtb_Sum_d = ud_flag_log; if (ud_flag_log != 0U) { rtb_Switch3 = 0U; } else { rtb_Switch3 = 1U; } if (rtb_Switch3) { rtb_DataTypeConversion2_o[0] = (int16_T)floor((real_T) (rtb_MatrixConcatenate[1] / lsb) + 0.5); rtb_DataTypeConversion2_o[1] = (int16_T)floor((real_T) (rtb_MatrixConcatenate[5] / lsb) + 0.5); ecrobot_bt_adc_data_logger(0, 0, rtb_DataTypeConversion2_o[0], rtb_DataTypeConversion2_o[1], 0, 0); } rtb_Sum_d = (uint8_T)(1U + (uint32_T)rtb_Sum_d); if (log_count == rtb_Sum_d) { rtb_Sum_d = 0U; } ud_int_thetadot_ref[0] = rtb_Sum_idx + rtb_UnitDelay_idx; ud_int_thetadot_ref[1] = rtb_Sum_idx_0 + rtb_UnitDelay_idx_0; ud_lpf_thetadot_ref[0] = rtb_Sum_idx; ud_lpf_thetadot_ref[1] = rtb_Sum_idx_0; ud_int_psidot[0] = rtb_Sum2_j_idx + rtb_UnitDelay_d_idx; ud_int_psidot[1] = rtb_Sum2_j_idx_0 + rtb_UnitDelay_d_idx_0;

- 60 -

ud_lpf_theta[0] = rtb_Sum_a_idx; ud_lpf_theta[1] = rtb_Sum_a_idx_0; ud_drv_theta[0] = rtb_Sum_a_idx; ud_drv_theta[1] = rtb_Sum_a_idx_0; ud_int_theta_diff[0] = (rtb_Divide_fh_idx - rtb_theta_idx) + rtb_UnitDelay_p_idx; ud_int_theta_diff[1] = (rtb_Divide_fh_idx_0 - rtb_theta_idx_0) + rtb_UnitDelay_p_idx_0; ud_flag_log = rtb_Sum_d; break; default: ecrobot_set_motor_mode_speed(NXT_PORT_B, 1, 0); ecrobot_set_motor_mode_speed(NXT_PORT_C, 1, 0); break; } } } void task_ts2(void) { flag_avoid = (ecrobot_get_sonar_sensor(NXT_PORT_S3) = time_auto); rtb_RelationalOperator_i = (rtb_Sum1_d >= time_start); if ((fabsf(theta_diff[0]) < theta_diff_thr) && (fabsf(theta_diff[1]) < theta_diff_thr) && ((fabsf(psi_diff[0]) < psi_diff_thr) && (fabsf (psi_diff[1]) < psi_diff_thr))) { timer_stop0 = rtb_Sum1_d; } if ((uint32_T)((int32_T)rtb_Sum1_d - (int32_T)timer_stop0) < time_stop) { if (rtb_RelationalOperator_i) { rtb_Switch2_n = control; } else { rtb_Switch2_n = calib; } } else { rtb_Switch2_n = stop; } operating_state = rtb_Switch2_n; if (rtb_RelationalOperator_i > ud_detect_increse) { ecrobot_sound_tone(sound_frq, sound_dur, 70); } ud_detect_increse = rtb_RelationalOperator_i; } void ballbot_app_initialize(void) { operating_state = calib; }

- 61 -

[1] Philos Home Page LEGO Mindstorms NXT

http://www.philohome.com/

[2] Ryos Holiday LEGO Mindstorms NXT

http://web.mac.com/ryo_watanabe/iWeb/Ryo%27s%20Holiday/LEGO%20Mindstorms%20NXT.html

[3] EE 2004

[4] RTW-EC

http://www.cybernet.co.jp/matlab/library/library/detail.php?id=TT036

- 62 -

MATLAB

>> help or >> doc

>> helpwin

MATLAB MATLAB [ ] [ MATLAB ]

MATLAB

- 63 -

MATLAB http://www.cybernet.co.jp/matlab/

FAQ

http://www.mathworks.com/search/advanced_search.html

MATLAB

- 64 -

MATLABThe MathWorks,Inc.LEGO

101-0022 3 14 Tel03-5297-3565() 03-5297-3546() Fax03-5297-3648 460-0003 1-6-26 3 Tel052-219-5197() 052-219-5198() Fax052-219-5970 542-0028 1-3-8 FN Tel06-6940-3613() 06-6940-3611() Fax06-6940-3602

1 http://www.cybernet.co.jp/MATLAB 1 E-mail: [email protected] 1 E-mail: [email protected]

2009 4 NXT Ballbot LEGO Mindstorms NXT

MATLAB1 1.1 1.2 1.3

2 NXT Ballbot2.1 2.2

3 NXT Ballbot3.1 3.2 3.3



6 6.1 6.2 6.3 6.4

7 7.1 7.2 7.3 task_init7.4 4mstask_ts17.5 20mstask_ts27.6 100mstask_ts37.7

8 8.1 8.2 8.3 3D

9 9.1 9.2 9.3

10 ABCMATLAB

nxt ballbot âéâfâïâxü[âxèjö¡.pdf

Documents