Upload
zohaibshabir
View
23
Download
0
Embed Size (px)
Citation preview
NXT Ballbot LEGO Mindstorms NXT
1 1
E-mail : [email protected]
1.0 2009 4
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
7-2 nxt_ballbot_controller.mdl
- 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
9-1 nxt_ballbot_controller.mdl
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
4 NXT Ballbot4.1 4.2
5 NXT Ballbot5.1 5.2
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