68
NXT Ballbot Model-Based Design - Control of self-balancing robot on a ball, built with LEGO Mindstorms NXT -
NXT Ballbot Model-Based Design - Control of self-balancing robot on a ball,
built with LEGO Mindstorms NXT -
Author (First Edition) Yorihisa Yamamoto
Application Engineer
Advanced Support Group 1 Engineering Department
Applied Systems First Division
CYBERNET SYSTEMS CO., LTD.
Revision History
Revision Date Description Author / Editor
1.0 Apr 14, 2009 First edition Yorihisa Yamamoto
The contents and URL described in this document can be changed with no previous notice.
Introduction
NXT Ballbot is a LEGO Mindstorms NXT version of Ballbot that is developed by Ralph Hollis at Carnegie
Mellon University. The Ballbot is designed to balance itself on its single spherical wheel while traveling about.
Please refer the following URL to know more detailed information about Ballbot.
http://en.wikipedia.org/wiki/Ballbot
This document presents Model-Based Design about balance and drive control of NXT Ballbot by using
MATALB / Simulink. The main contents are the following.
Mathematical Modeling
Controller Design
Model Illustration
Simulation and Experimental Results
Preparation
To build NXT Ballbot, read NXT Ballbot Building Instructions.
You need to download Embedded Coder Robot NXT from the following URL because it is used as
Model-Based Design Environment in this document.
http://www.mathworks.com/matlabcentral/fileexchange/13399
Read Embedded Coder Robot NXT Instruction Manual (Embedded Coder Robot NXT Instruction
En.pdf) and test sample models / programs preliminarily. The software versions used in this document are as
follows.
Software Version
Embedded Coder Robot NXT 3.14
nxtOSEK (previous name is LEJOS OSEK) 2.03
Cygwin 1.5.24
GNU ARM 4.0.2
- i -
Required MATLAB Products
Product Version Release
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
You can simulate NXT Ballbot models and generate code from it without the products (N1). Virtual Reality Toolbox
is required to run 3D visualization (nxt_ballbot_vr.mdl).
File List
File Description
iswall.m M-function for detecting wall in map
mywritevrtrack.m M-function for generating map file
nxt_ballbot.mdl NXT Ballbot model (It does not require Virtual Reality Toolbox)
nxt_ballbot_controller.mdl NXT Ballbot controller model (single precision floating-point)
nxt_ballbot_vr.mdl NXT Ballbot model (It requires Virtual Reality Toolbox)
param_controller.m M-script for controller parameters
param_nxt_ballbot.m M-script for NXT Ballbot parameters (It calls param_***.m)
param_plant.m M-script for plant parameters
param_sim.m M-script for simulation parameters
post_sdo_codegen.m M-script for post-process of code generation with Simulink Data Object
pre_sdo_codegen.m M-script for pre-process of code generation with Simulink Data Object
vr_nxt_ballbot.wrl map & NXT Ballbot VRML file
vr_nxt_ballbot_track.bmp map image file
vr_nxt_ballbot_track.wrl map VRML file
- ii -
Table of Contents
Introduction ............................................................................................................................................................... i Preparation................................................................................................................................................................ i Required MATLAB Products ................................................................................................................................. ii File List ..................................................................................................................................................................... ii 1 Model-Based Design ...................................................................................................................................... 1
1.1 What is Model-Based Design?.............................................................................................................. 1 1.2 V-Process ................................................................................................................................................. 2 1.3 Merits of MBD .......................................................................................................................................... 3
2 NXT Ballbot System........................................................................................................................................ 4 2.1 Structure ................................................................................................................................................... 4 2.2 Sensors and Actuators ........................................................................................................................... 5
3 NXT Ballbot Modeling..................................................................................................................................... 6 3.1 Spherical Wheeled Inverted Pendulum Model.................................................................................... 6 3.2 Motion Equations of Spherical Wheeled Inverted Pendulum ........................................................... 8 3.3 State Equations of Spherical Wheeled Inverted Pendulum .............................................................. 9
4 NXT Ballbot Controller Design .................................................................................................................... 11 4.1 Control System ...................................................................................................................................... 11 4.2 Controller Design .................................................................................................................................. 12
5 NXT Ballbot Model ........................................................................................................................................ 15 5.1 Model Summary .................................................................................................................................... 15 5.2 Parameter Files ..................................................................................................................................... 19
6 Plant Model .................................................................................................................................................... 20 6.1 Model Summary .................................................................................................................................... 20 6.2 Actuators ................................................................................................................................................ 21 6.3 Plant ........................................................................................................................................................ 22 6.4 Sensors................................................................................................................................................... 23
7 Controller Model ............................................................................................................................................ 24 7.1 Control Program Summary .................................................................................................................. 24 7.2 Model Summary .................................................................................................................................... 26 7.3 Initialization Task : task_init ................................................................................................................. 30 7.4 4ms Task : task_ts1 .............................................................................................................................. 30 7.5 20ms Task : task_ts2 ............................................................................................................................ 37 7.6 100ms Task : task_ts3 .......................................................................................................................... 37 7.7 Tuning Parameters ............................................................................................................................... 38
8 Simulation....................................................................................................................................................... 39 8.1 How to Run Simulation......................................................................................................................... 39 8.2 Simulation Results ................................................................................................................................ 40 8.3 3D Viewer ............................................................................................................................................... 42
9 Code Generation and Implementation ....................................................................................................... 43 9.1 Target Hardware & Software ............................................................................................................... 43 9.2 How to Generate Code and Download .............................................................................................. 44 9.3 Experimental Results............................................................................................................................ 45
10 Challenges for Readers ........................................................................................................................... 47 Appendix A Modern Control Theory ................................................................................................................ 48
A.1 Stability ..................................................................................................................................................... 48 A.2 State Feedback Control.......................................................................................................................... 48 A.3 Servo Control ........................................................................................................................................... 50
Appendix B Virtual Reality Space .................................................................................................................... 52 B.1 Coordinate System ................................................................................................................................. 52 B.2 Making Map File ...................................................................................................................................... 54 B.3 Distance Calculation and Wall Hit Detection....................................................................................... 55
Appendix C Generated Code ........................................................................................................................... 57 References............................................................................................................................................................. 61
- 1 -
1 Model-Based Design
This chapter outlines Model-Based Design briefly.
1.1 What is Model-Based Design?
Model-Based Design is a software development technique that uses simulation models. Generally, it is
abbreviated as MBD. For control systems, a designer models a plant and a controller or a part of them, and tests
the controller algorithm based on a PC simulation or real-time simulation. The real-time simulation enables us to
verify and validate the algorithm in real-time, by using code generated from the model. It is Rapid Prototyping
(RP) that a controller is replaced by a real-time simulator, and Hardware In the Loop Simulation (HILS) is a plant
version of Rapid Prototyping.
Furthermore, auto code generation products like RTW-EC enables us to generate C/C++ code for embedded
controllers (microprocessor, DSP, etc.) from the controller model. Figure 1-1 shows the MBD concept for control
systems based on MATLAB product family.
Simulink Stateflow Simulink Control Design Simulink Response OptimizationSimulink Parameter EstimationSimMechanics SimPowerSystems SimDriveline SimHydraulics Signal Processing Blockset Simulink Fixed Point
Control SystemDesign & Analysis SimulationEngineering
Problem
Mathematical Modeling
Code Generation
RP
HILS
EmbeddedSystem
Data
Data basedModeling
Prototyping Code (RTW)
Embedded Code (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 Products (Toolbox) Simulink Products
Figure 1-1 MBD for control systems based on MATLAB product family
- 2 -
1.2 V-Process
The V-process shown in Figure 1-2 describes the MBD development process for control systems. The V-process
consists of the Design, Coding, and Test stage. Each test stages correspond to the appropriate Design stages. A
developer makes plant / controller models in the left side of the V-process for early improvement of controller
algorithm, and reuses them in the right side for improvement of code verification and validation.
System Design
Module Design
Unit Design
Coding
Unit Test
Module Test
System Test
Product Test
Plant Modeling
Controller Algorithm Design
Code Generation for Embedded Systems
Verification Static Analysis Coding Rule Check
Early Improvement of Controller Algorithm
Improvement of CodeVerification & Validation
Product Design
Validation
Figure 1-2 V-process for control systems
- 3 -
1.3 Merits of MBD
MBD has the following merits.
Detection about specification errors in early stages of development
Hardware prototype reduction and fail-safe verification with real-time simulation
Efficient test by model verification
Effective communication by model usage
Coding time and error reduction by auto code generation
- 4 -
2 NXT Ballbot System
This chapter describes the structure and the sensors / actuators of NXT Ballbot.
2.1 Structure
Figure 2-1 shows the structure of NXT Ballbot. Hitechnic gyro sensors are used to calculate body pitch angle.
Ultrasonic Sensor
HiTechnic Gyro Sensor
DC Motor Rotary Encoder
Plastic Ball
Figure 2-1 NXT Ballbot
The plastic ball is kept and held by the three wheels and the LEGO parts attached diagonally. The wheels
include two rubber tires attached to DC motors and one freely-spinning wheel. NXT Ballbot can move by
rotating DC motors.
- 5 -
2.2 Sensors and Actuators
Table 2-1 and Table 2-2 show sensor and actuator properties.
(N1) : The heuristic maximum sample rate for measuring accurate distance.
The reference [1] illustrates many properties about DC motors. Generally speaking, sensors and actuators are
different individually. Especially, you should note that gyro offset and gyro drift have big impact on balance
control. Gyro offset is an output when a gyro sensor does not rotate, and gyro drift is time variation of gyro
offset.
Sensor Output Unit Data Type Maximum Sample [1/sec]
Rotary Encoder angle deg int32 1000
Ultrasonic Sensor distance cm int32 50 (N1)
Gyro Sensor angular velocity deg/sec uint16 300
Table 2-1 Sensor properties
Table 2-2 Actuator properties
Actuator Input Unit Data Type Maximum Sample [1/sec]
DC Motor PWM % int8 500
- 6 -
3 NXT Ballbot Modeling
This chapter describes mathematical model and motion equations of NXT Ballbot.
3.1 Spherical Wheeled Inverted Pendulum Model
We assume that the motion in XZ plane and YZ plane are decoupled and that the equations of motion in these
two planes are identical. Under this assumption, NXT Ballbot can be considered as two separate and identical
spherical wheeled inverted pendulum model shown in Figure 3-1 for each planes.
Figure 3-2 shows the coordinate system of spherical wheeled inverted pendulum.
ψ : body pitch angle θ : spherical wheel angle sθ : spherical wheel angle driven by DC motor
Figure 3-1 Spherical wheeled inverted pendulum
Figure 3-2 Coordinate system of spherical wheeled inverted pendulum
Z
θ
ψ
bz
L
ψJMb ,
ss JM ,
bx
R
sθ
sz
sx X
- 7 -
The DC motors can drive the spherical wheel through the rubber tires. Figure 3-3 shows this mechanism.
s
mθ : DC motor angle
We assume there is no slip between the wheels.
ssmw RR θθ =
mθ
Spherical Wheel (Ball)
Rubber tires attached to DC motor
w
θ
Physical parameters of NXT Ballbot are the following.
: Gravity acceleration 81.9=g ]sec/[ 2m
: Spherical wheel weight 013.0=sM ][kg
: Spherical wheel radius 026.0=sR ][m
32 2sss RMJ = : Spherical wheel inertia moment ][ 2kgm
: Motor wheel (rubber tire) weight 015.0=wM ][kg
: Motor wheel (rubber tire) radius 021.0=wR ][m
: Motor wheel (rubber tire) height 022.0=wL ][m
22www LMJ = : Motor wheel (rubber tire) inertia moment ][ 2kgm
: Body weight 682.0=bM ][kg
: Body width 135.0=W ][m
: Body depth 135.0=D ][m
: Distance between body center and spherical wheel center 17.0=L ][m 32LMJ b=ψ : Body pitch inertia moment ][ 2kgm
: DC motor inertia moment 5101 −×=mJ ][ 2kgm
: DC motor resistance 69.6=mR ][Ω
468.0=bK ]sec[ radV : DC motor back EMF constant
317.0=tK ][ ANm : DC motor torque constant
0022.0=mf : Friction coefficient between body and DC motor
0=sf : Friction coefficient between spherical wheel and floor.
We use the values described in reference [2] for . tbm KKR ,,
We use the values that seem to be appropriate for , because it is difficult to measure. smm ffJ ,,
Rs
ssww JM ,JM ,
R
Figure 3-3 Spherical wheel and DC motor
- 8 -
3.2 Motion Equations of Spherical Wheeled Inverted Pendulum
We can derive motion equations of spherical wheeled inverted pendulum by the Lagrangian method based on
the coordinate system in Figure 3-2. If 0=θ at 0=t , each coordinates are given as the following.
( ) ( bsss zRzx ,, )θ= (3.1)
( ) ( )0,, θ&&& sss Rzx = (3.2)
( ) ( )ψψ cos,sin, LzLxzx bb ++= (3.3)
( ) ( )ψψψψθ sin,cos, &&&&& LLRzx sbb −+= (3.4)
The translational kinetic energy , the rotational kinetic energy , the potential energy are 1T 2T U
( ) ( 22221 2
121
bbbsss zxMzxMT &&&& +++= ) (3.5)
( ) ( )22
222
22222
21
21
21
21
21
21
21
ψθψθ
θθψθ
ψ
ψ
&&&&
&&&&
−+++=
+++=
w
swmss
mmmwss
RR
JJJJ
JJJJT (3.6)
bbss gzMgzMU += (3.7)
The Lagrangian L has the following expression.
UTTL −+= 21 (3.8)
We use θ and ψ as the generalized coordinates. Lagrange equations are the following.
θθθFLL
dtd
=∂∂
−⎟⎠⎞
⎜⎝⎛∂∂&
(3.9)
ψψψFLL
dtd
=∂∂
−⎟⎟⎠
⎞⎜⎜⎝
⎛∂∂&
(3.10)
We derive the following equations by evaluating Eqs. (3.9) - (3.10).
( ) ( )[ ] ( )[ ] θψψψψθ FLRMJJkLRMJJkJRMM sbwmsbwmsssb =−+−+++++ sincos 2222 &&&&& (3.11)
( )[ ] ( )[ ] ψψ ψψθψ FgLMJJkJLMJJkLRM bwmbwmsb =−+++++− sincos 222 &&&& (3.12)
where ws RRk = .
- 9 -
In consideration of DC motor torque and viscous friction, the generalized forces are given as the following.
θθθ&&
smmt ffiKF −−= (3.13)
mmt fiKF θψ&+−= (3.14)
where is the DC motor current and is the DC motor angular velocity. i )( ψθθ &&& −= km
We cannot use the DC motor current directly in order to control it because it is based on PWM (voltage)
control. Therefore, we evaluate the relation between current and voltage using DC motor equation. If the
friction inside the motor is negligible, the motor equation is generally as follows.
i v
(3.15) iRKviL mmbm −−= θ&&
Here we consider that the motor inductance is negligible and is approximated as zero. Therefore the current is
m
b
RkKv
i)( ψθ && −−
= (3.16)
From Eq. (3.16), the generalized force can be expressed using the motor voltage.
( ) ψβθβαθ && ++−= sfvF (3.17)
ψβθβαψ && −+−= vF (3.18)
⎟⎟⎠
⎞⎜⎜⎝
⎛+== m
m
bt
m
t fR
KKk
RK
βα , (3.19)
3.3 State Equations of Spherical Wheeled Inverted Pendulum
We can derive state equations based on modern control theory by linearizing the motion equations at a balance point of NXT Ballbot. It means that we consider the limit 0→ψ ( ψψ →sin , 1cos →ψ ) and neglect the
second order term like . The motion equations (3.11) - (3.12) are approximated as the following. 2ψ&
( )[ ] ( ) θψθ FJkLRMJkJRMM msbmsssb =−++++ &&&& 222 (3.20)
( ) ( ) ψψ ψψθ FgLMJkJLMJkLRM bmbmsb =−+++− &&&& 222 (3.21)
- 10 -
These equations can be expressed in the form
HvGFE =⎥⎦
⎤⎢⎣
⎡+⎥
⎦
⎤⎢⎣
⎡+⎥
⎦
⎤⎢⎣
⎡ψθ
ψθ
ψθ
&
&
&&
&& (3.22)
( )
⎥⎦
⎤⎢⎣
⎡−
=
⎥⎦
⎤⎢⎣
⎡−
=
⎥⎦
⎤⎢⎣
⎡−
−+=
⎥⎥⎦
⎤
⎢⎢⎣
⎡
++−−+++
=
αα
ββββ
ψ
H
gLMG
fF
JkJLMJkLRMJkLRMJkJRMM
E
b
s
mbmsb
msbmsssb
000
222
222
Here we consider as state, and as input. indicates transpose of . x u Tx x
(3.23) [ ] vT
== ux ,,,, ψθψθ &&
Consequently, we can derive state equations of spherical wheeled inverted pendulum from Eq. (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(
EEEEEEEB
EEEBEEEAEEEA
EEEfAEEEfA
EgLEMAEgLEMA
s
s
b
b
−=
+−=+=
+−=+=
++=++−=
=−=
αα
ββ
ββββ
The unit of angle is [rad] in Eq.(3.24). We can convert it to [deg] by multiplying α by π180( πα 180×= mt RK ). Please note the unit of angle is [deg] in a plant model and a controller model in consideration of the unit of output from a rotary encoder.
- 11 -
4 NXT Ballbot Controller Design
This chapter describes the controller design of NXT Ballbot (spherical wheeled inverted pendulum) based on
modern control theory.
4.1 Control System
The characteristics as control system are as follows.
Inputs and Outputs
The input to an actuator is PWM duty of the DC motor even though input in Eq. (3.24) is voltage. The outputs from sensors are the DC motor angle
u
mθ and the body pitch angular velocity ψ& .
DC Motor PWM
DC Motor Angle
Body Pitch Angular Velocity
Figure 4-1 Inputs and Outputs
There are two methods to evaluate ψ by using ψ& .
1. Derive ψ by integrating the angular velocity numerically.
2. Estimate ψ by using an observer based on modern control theory.
We use method 1 in the following controller design.
Stability
It is easy to understand NXT Ballbot balancing position is not stable. We have to move NXT Ballbot in the
same direction of body pitch angle to keep balancing. Modern control theory gives many techniques to stabilize
an unstable system. (See Appendix A)
- 12 -
4.2 Controller Design
Eq. (3.22) is a similar equation as mass-spring-damper system. Figure 4-2 shows an equivalent system of
spherical wheeled inverted pendulum interpreted as mass-spring-damper system.
θ
ψSpring
Damper
Figure 4-2 Equivalent mass-spring-damper system
We find out that we can make the spherical wheeled inverted pendulum stable by adjusting spring constants and
damper friction constants in Figure 4-2. It is possible to think that the control techniques described in Appendix
A provides theoretical method to calculate those constants.
We use a servo controller described in Appendix A.3 as NXT Ballbot controller and select the spherical wheel
angle θ as a reference of servo control. It is important that we cannot use variables other than θ as the
reference because the system goes to be uncontrollable. Figure 4-3 shows the block diagram of NXT Ballbot
servo controller.
u+
+
+ θ∫
−
+
−
θCrefx refθ
NXT Ballbot
fk
ik θCx
xfromderivetomatrixoutputanisC θθ
Figure 4-3 NXT Ballbot servo controller block diagram
- 13 -
We calculate feedback gain and integral gain by linear quadratic regulator method. We choose the following weight matrix and by experimental trial and error. Q R
2
3
3
5
180106,
10100000100000100000106000001
⎟⎠⎞
⎜⎝⎛××=
⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢
⎣
⎡
×
×=
πRQ
)2,2(Q is a weight for body pitch angle, and is a weight for time integration of difference between
measured angle and referenced one.
)5,5(Q
param_controller.m calculates linear quadratic regulator and defines other parameters. The gain calculation
part of it is extracted as the following.
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
The result is the following.
-0.0071
]0.2325- 0.027,- 1.5698,- -0.015,[
=
=
i
f
kk
- 14 -
We use same controller algorithm for the motion in XZ plane and YZ plane under the assumption described in
3.1 Spherical Wheeled Inverted Pendulum Model. Consequently, we derive NXT Ballbot controller shown in Figure 4-4. Here is a tuning parameter.
θ&k
NXT Ballbot
θ&k
Speed Reference
PWM Reference Generator
Saturation
Servo Controller
Friction Compensator
State
( )YX PWMPWM ,
( )refYX θθ && ,
( )YX xx ,
Figure 4-4 NXT Ballbot controller block diagram
- 15 -
5 NXT Ballbot Model
This chapter describes summary of NXT Ballbot model and parameter files.
5.1 Model Summary
The nxt_ballbot.mdl and nxt_ballbot_vr.mdl are models of NXT Ballbot control system. Both models are
identical but different in point of including 3D viewer provided by Virtual Reality Toolbox.
Main parts of nxt_ballbot.mdl and nxt_ballbot_vr.mdl are as follows.
Environment subsystem
This subsystem defines environmental parameters. For example, system timer, map data, gyro offset value, and
so on.
Figure 5-1 nxt_ballbot.mdl
Figure 5-2 Environment subsystem
- 16 -
Reference Generator subsystem
This subsystem is a reference signal generator for NXT Ballbot. We can change the speed reference in the
direction of X-axis and Y-axis by using Signal Builder block. The output signal is a 32 byte data which has
dummy data to satisfy the NXT GamePad utility specification.
Figure 5-3 Reference Generator subsystem
Figure 5-4 shows a relation between speed reference value and PC game pad analog sticks.
Forward Run (- GP_MAX)
Backward Run (+ GP_MAX)
Leftward Run (- GP_MAX)
Rightward Run(+ GP_MAX)
GP_MAX is a maximum input from PC game pad.
(We set GP_MAX = 100)
Figure 5-4 Inputs from PC game pad analog sticks
- 17 -
Controller
This block is NXT Ballbot digital controller and references nxt_ballbot_controller.mdl with Model block. Read
7 Controller Model for more details.
Figure 5-5 Controller block (nxt_ballbot_controller.mdl)
The Controller block runs in discrete-time (base sample time = 1 [ms]) and the plant (NXT Ballbot subsystem)
runs in continuous-time (sample time = 0 [s]). Therefore it is necessary to convert continuous-time to
discrete-time and vice versa by inserting Rate Transition blocks between them properly.
Discrete-Time (1 [ms])
Sampling (Cont Disc)
Hold (Disc Cont)
Figure 5-6 Rate conversions between the controller and the plant
- 18 -
NXT Ballbot subsystem
This subsystem is the mathematical model of NXT Ballbot. It consists of sensors, actuators, and linear plant
model. Read 6 Plant Model for more details.
Figure 5-7 NXT Ballbot subsystem
Viewer subsystem
This subsystem includes simulation viewers. nxt_ballbot.mdl includes a position viewer with XY Graph block,
and nxt_ballbot_vr.mdl does 3D viewer provided by Virtual Reality Toolbox.
Figure 5-8 Viewer subsystem (nxt_ballbot.mdl)
- 19 -
5.2 Parameter Files
Table 5-1 shows parameter files for simulation and code generation.
Figure 5-9 Viewer subsystem (nxt_ballbot_vr.mdl)
Table 5-1 Parameter files
File Name Description
param_controller.m M-script for controller parameters
param_nxt_ballbot.m M-script for NXT Ballbot parameters (It calls param_***.m)
param_plant.m M-script for plant parameters
param_sim.m M-script for simulation parameters
param_nxt_ballbot.m calls param_***.m (*** indicates controller, plant, sim) and creates all parameters in base
workspace. Model callback function is used to run param_nxt_ballbot.m automatically when the model is
loaded.
To display model callback function, choose [Model Properties] from the Simulink [File] menu.
- 20 -
6 Plant Model
This chapter describes NXT Ballbot subsystem in nxt_ballbot.mdl / nxt_ballbot_vr.mdl.
6.1 Model Summary
The NXT Ballbot subsystem consists of sensors, actuators, and linear plant model. It converts the data type of
input signals to double, calculates plant dynamics based on double precision floating-point arithmetic, and
outputs the results after quantization.
Double Precision Floating-Point Arithmetic
Integer Value
Physical Value
IntegerValue
Physical Value
Figure 6-1 NXT Ballbot subsystem
Some signals are vectorized for the same calculation in X-direction and Y-direction.
Signal Vectorization (Same calculation to each PWM)
Figure 6-2 Signal vectorization
- 21 -
6.2 Actuators
Actuators subsystem calculates DC motor voltage by using PWM duty derived from the controller.
Considering the coulomb and viscous friction in the driveline, we model it as a dead zone before calculating
voltage.
Dead zone of the coulomb and viscous friction
Maximum voltage calculationwith battery voltage
Figure 6-3 Actuators subsystem
DC motor maximum voltage is necessary to derive the voltage. In the Cal vol_max subsystem, we use the following experimental equation which is a conversion rule between the battery voltage and the maximum
DC motor voltage . BV
maxV
(6.1) 625.0001089.0max −×= BVV
We have derived Eq. (6.1) as the following. Generally speaking, DC motor voltage and rotation speed are
proportional to battery voltage. Figure 6-4 shows an experimental result of battery voltage and motor rotation
speed at PWM = 100% with no load.
Figure 6-4 Experimental result between battery voltage and motor rotation speed at PWM = 100%
Eq. (6.1) is derived by relating Figure 6-4 and the data described in the reference [1].
- 22 -
6.3 Plant
Plant subsystem is a model of the state equation derived in 3.3 State Equations of Spherical Wheeled
Inverted Pendulum. Considering the calibration of gyro offset, it is modeled so as to run calculation after gyro
calibration. Read 7.1 Control Program Summary for more details about the gyro calibration.
State equation in X-direction
State equation in Y-direction
Modeling to start the Enabled Subsystem after gyro calibration
Figure 6-5 Plant subsystem
- 23 -
6.4 Sensors
Sensors subsystem converts the values calculated in plant subsystem to sensor outputs. Because the
computation cost of distance (an output of ultrasonic sensor) calculation and wall hit detection are heavy, we cut
the computation steps by inserting Rate Transition blocks. Read Appendix B.3 for more details about the
distance calculation and wall hit detection.
Figure 6-6 Sensors subsystem
- 24 -
7 Controller Model
This chapter describes control program, task configuration, and model contents of nxt_ballbot_controller.mdl.
7.1 Control Program Summary
State Machine Diagram
Figure 7-1 is a state machine diagram of NXT Ballbot controller. There are two drive modes. One is
autonomous drive mode and the other is remote control drive mode by analog sticks of PC game pad with NXT
GamePad utility. DC motors stop automatically when NXT Ballbot is out of control because of falling or other
reasons.
Balance and Drive Control
Calibration Time Over
Gyro Offset Calibration
ForwardRun
Rightward Run
Obstacle
No Obstacle
Stop
Start Time Over
Autonomous Drive
Remote Control
Backward Run
Obstacle No Obstacle
Remote Control Drive
C
DC Motors Stop
Out of Control
Figure 7-1 State machine diagram of NXT Ballbot controller
- 25 -
Task Configuration
NXT Ballbot controller has four tasks described in Table 7-1.
Table 7-1 NXT Ballbot controller tasks
Task Period Works
task_init Initialization only Initial value setting
task_ts1 4 [ms]
Gyro offset calibration
Balance and drive control
Data logging
DC motors stop when out of control
task_ts2 20 [ms] Obstacle check
task_ts3 100 [ms]
Time check
Out of control check
Battery voltage averaging
We use 4 [ms] period for the balance and drive control task considering max sample numbers of gyro sensor a
second. As same reason, we use 20 [ms] period for the obstacle check task using ultrasonic sensor. The
controller described in 4.2 Controller Design used for the balance and drive control.
Data Type
We use single precision floating-point data for the balance and drive control to reduce the computation error.
There are no FPU (Floating Point number processing Unit) in ARM7 processor embedded in NXT Intelligent
Brick, but we can perform software single precision floating-point arithmetic by using floating-point arithmetic
library provided by GCC.
- 26 -
7.2 Model Summary
nxt_ballbot_controller.mdl is based on Embedded Coder Robot NXT framework.
4ms 20ms 100ms
Device Input Device Output
SchedulerTask Subsystem
Initialization
Shared Data
Figure 7-2 nxt_ballbot_controller.mdl
- 27 -
Device Interface
We can make device interfaces using the sensor and actuator blocks provided by Embedded Coder Robot NXT
library.
Motor Input (rotation angle)
Motor Output (PWM)
Figure 7-3 DC motor interface
Scheduler & Tasks
The ExpFcnCalls Scheduler block has task configuration such as task name, task period, platform, and stack
size. We can make task subsystems by connecting function-call signals from the scheduler to function-call
subsystems.
4ms 20ms 100ms Initialization
Figure 7-4 Scheduler and tasks
- 28 -
Priority
We have to set priority of the device blocks and ExpFcnCalls Scheduler block at root level to sort them in the
order 1. device inputs 2. tasks → 3. device outputs. The low number indicates high priority and negative
numbers are allowed.
→
To display priority, right click the block and choose [Block Properties].
Computation Order
Priority = -1
Priority = 0
Priority = 1
Figure 7-5 Priority setting
Shared Data
We use Data Store Memory blocks as shared data between tasks.
Figure 7-6 Shared data
- 29 -
Signal Vectorization
In the balance and drive control subsystem shown in Figure 7-7, some signals including outputs from DC
motor sensors and gyro sensors are vectorized in order to apply same control algorithm in X-direction and
Y-direction. The controller calculates matrix operations by setting block parameters.
Signal Vectorization
Select Vector Element
Block Parameter Setting for Matrix Multiplication
Figure 7-7 Signal vectorization and matrix operations
- 30 -
7.3 Initialization Task : task_init
This task sets the initial values. We can switch the drive mode. (autonomous drive mode or remote control
drive mode)
Mode Switch
Figure 7-8 task_init subsystem
7.4 4ms Task : task_ts1
This is a task for gyro offset calibration, balance and drive control, data logging, and DC motors stop when out
of control. The balance and drive control starts after the gyro calibration. The operating_state
corresponds to the operation state shown in Figure 7-1.
Gyro Offset Calibration
Balance and Drive Control
DC Motors Stop when Out of Control
Figure 7-9 task_ts1 subsystem
- 31 -
Gyro_Calibration subsystem
This subsystem calculates the initial value of gyro offset using a low path filter. The gyro offset calibration
time is defined as time_start in param_controller.m.
Figure 7-10 Gyro_Calibration subsystem
Stop subsystem
DC motors stop when NXT Ballbot is out of control. The task of out of control check runs in task_ts3
subsystem.
Figure 7-11 Stop subsystem
- 32 -
Balance_and_Drive_Control subsystem
This subsystem is a controller described in 4.2 Controller Design.
Reference Command Digital Controller
(single precision)
Reference Calculation
State Calculation
PWM Calculation
Data Logging
Figure 7-12 Balance_and_Drive_Control subsystem
- 33 -
Cal_Command subsystem
This subsystem calculates the reference command depending on the drive mode and the result of obstacle
check.
Remote Control Drive Mode
Autonomous Drive Mode
Figure 7-13 Cal_Command subsystem
- 34 -
Discrete Derivative and Discrete Integrator
Discrete Derivative block computes a time derivative by backward difference method, and Discrete Integrator
block does a time integral by forward euler method.
Figure 7-14 Discrete Derivative and Discrete Integrator
Reference subsystem
This subsystem calculates the reference using a low path filter to reduce an overshoot induced by a rapid
change of speed reference.
Figure 7-15 Reference subsystem
- 35 -
State subsystem
This subsystem calculates the state using outputs from the sensors. We use a long-time average of gyro output
as gyro offset to remove gyro drift effect, and a low path filter to remove noise in the velocity signal.
Gyro Offset Update
Figure 7-16 State subsystem
PWM subsystem
This subsystem calculates the PWM duty based on the block diagram shown in Figure 4-4 and maximum
motor voltage by using Eq. (6.1).
Friction Compensator
Figure 7-17 PWM subsystem
- 36 -
Data_Logging subsystem
The NXT GamePad ADC Data Logger block is placed in this subsystem for logging the sensor outputs and
calculated results. To change the logging period, change log_count defined in param_controller.m.
The NXT GamePad ADC Data Logger block can log sensor outputs and 8-bit, 16-bit signed integer data. It is
necessary for logging floating-point data to quantize it by dividing appropriate value, because it does not
support floating-point data directly. The denominator of the dividing operation is called Slope or LSB, it
corresponds a value per 1-bit. We can derive an original value by multiplying the logged data and Slope (LSB).
Figure 7-18 Data Logging subsystem
- 37 -
7.5 20ms Task : task_ts2
This is a task to check an obstacle with ultrasonic sensor. NXT Ballbot runs rightward in autonomous drive
mode and does backward in remote control drive mode when finding an obstacle.
Figure 7-19 task_ts2 subsystem
7.6 100ms Task : task_ts3
This is a task to check the transition of operation state and average battery voltage. NXT Ballbot sounds during
sound_dur [ms] when the gyro calibration is finished.
Average Battery Voltage
Gyro Calibration Time Check
Autonomous Start Time Check
Out of Control Time Check
Figure 7-20 task_ts3 subsystem
- 38 -
Timer_Stop subsystem
This subsystem calculates the duration time of out of control. We define out of control as the state that a
difference between reference and measured value on wheel angle or body pitch angle is greater than a threshold
(theta_diff_thr or psi_diff_thr).
Figure 7-21 Timer_Stop subsystem
7.7 Tuning Parameters
All parameters used in nxt_ballbot_controller.mdl are defined in param_controller.m. Table 7-2 shows the
tuning parameters for balance and drive control. You might have to tune these parameters because the parts,
blocks, sensors, and actuators are different individually.
Table 7-2 Tuning parameters
Parameter Description
k_f Servo control feedback gain
k_i Servo control integral gain
k_thetadot Speed reference gain
a_b Low path filter coefficient for averaging battery voltage
a_d Low path filter coefficient for removing velocity noise
a_r Low path filter coefficient for removing overshoot
a_gc Low path filter coefficient for gyro calibration
a_gd Low path filter coefficient for removing gyro drift effect
pwm_gain Friction compensator gain
pwm_offset Friction compensator offset
dst_thr Obstacle avoidance threshold (distance)
- 39 -
8 Simulation
This chapter describes simulation of NXT Ballbot model, its result, and 3D viewer in nxt_ballbot_vr.mdl.
8.1 How to Run Simulation
It is same way as usual models to run simulation. We can change the speed reference by using Signal Builder
block in Reference Generator subsystem.
Figure 8-1 Reference Generator subsystem
- 40 -
8.2 Simulation Results
Stationary Balancing
Figure 8-2 shows a simulation result of stationary balancing at initial value of body pitch angle equals to 4
[deg]. The body pitch angle goes to be 0 [deg] quickly. NXT Ballbot does not run until 1 [s] passes because of
gyro calibration.
Forward and Backward Run
Figure 8-3 shows a simulation result of forward and backward run in X-direction. NXT Ballbot fluctuates at
the time it runs or stops, but it goes to be stable gradually.
Figure 8-2 Position and body pitch angle of stationary balancing (X-direction)
Figure 8-3 Position and body pitch angle of forward and backward run (X-direction)
- 41 -
You can watch the movie of NXT Ballbot simulation at the following URL.
http://www.youtube.com/watch?v=1MfiAZBsWac
Figure 8-4 Movie of NXT Ballbot simulation
- 42 -
8.3 3D Viewer
Using nxt_ballbot_vr.mdl, we can watch 3D simulation with the viewer provided by Virtual Reality Toolbox.
We can change the viewpoint by switching the view mode in Virtual Reality window. nxt_ballbot_vr.mdl has
five view modes.
Top View : Overhead viewpoint
Vista View : Fixed viewpoint
Chaser View 1 : Tracking viewpoint (left side)
Chaser View 2 : Tracking viewpoint (front side)
Robot View : Viewpoint on the ultrasonic sensor
Top View Vista View
Chaser View 1 Chaser View 2
Figure 8-5 View modes
- 43 -
9 Code Generation and Implementation
This chapter describes how to generate code from nxt_ballbot_controller.mdl and download it into NXT
Intelligent Brick, and the experimental results are shown.
9.1 Target Hardware & Software
Table 9-1 shows the target hardware specification of LEGO Mindstorms NXT and the software used in
Embedded Coder Robot NXT.
Table 9-1 LEGO Mindstorms NXT & Embedded Coder Robot NXT specification
Processor ATMEL 32-bit ARM 7 (AT91SAM7S256) 48MHz
Flash Memory 256 Kbytes (10000 time writing guarantee) Hardware
SRAM 64 Kbytes
Actuator DC motor
Sensor Ultrasonic sensor, Touch sensor, Light sensor, Sound sensor
Third party sensor (gyro sensor etc.)
Display 100 * 64 pixel LCD
Interface
Communication Bluetooth
RTOS LEJOS C / LEJOS OSEK
Compiler GCC Software
Library GCC library
You can not download a program when the program size is greater than SRAM or flash memory size (you will
encounter a download error).
- 44 -
9.2 How to Generate Code and Download
You can generate code from the model, build it, and download the program into NXT by clicking the
annotations in nxt_ballbot_controller.mdl shown in Figure 9-1. The procedure is the following.
1. Set Simulink Data Object usage by clicking [SDO Usage]. Simulink Data Object enables us to assign user
variable attributes such as variable name, storage class, modifiers etc. Refer the reference [3] for more
details about Simulink Data Object.
2. Generate code and build it by clicking [Generate code and build the generated code].
3. Connect NXT and PC via USB. Download the program into NXT by clicking [Download (NXT enhanced
firmware)] or [Download (SRAM)] in accordance with the NXT boot mode (enhanced firmware or
SRAM boot).
Step.1 Set Simulink Data Object Usage
Step.2 Code Generation & Build
Step.3a Program Download (NXT enhanced firmware)
Step.3b Program Download (NXT SRAM)
Figure 9-1 Annotations for code generation / build / download
A part of the generated code is described in Appendix C.
- 45 -
9.3 Experimental Results
The experimental results are shown as follows. We have derived similar results as the simulation results.
Stationary Balancing
Figure 9-2 shows an experimental result of stationary balancing. The initial body pitch angle is not zero but
NXT Ballbot is stable over time.
Figure 9-2 Position and body pitch angle of stationary balancing (X-direction)
Forward and Backward Run
Figure 9-3 shows an experimental result of forward and backward run in X-direction.
Figure 9-3 Position and body pitch angle of forward and backward run (X-direction)
- 46 -
You can watch the movie of NXT Ballbot control experiment at the following URL.
http://www.youtube.com/watch?v=f8jxGsg3p0Y
Figure 9-4 Movie of NXT Ballbot control experiment
A plastic ball is very slippy and NXT Ballbot cannot balance on polished floor. You had better choose non-slippy floor like felt or carpet.
- 47 -
10 Challenges for Readers
We provide the following problems as challenges for readers. Try them if you are interested in.
Controller performance improvement (gain tuning, robust control technique, etc.)
Filter improvement for removing gyro drift completely (high path filter etc.)
Proportional sound to the distance between NXT Ballbot and obstacle.
NXT Ballbot hardware mechanism refinement
- 48 -
Appendix A Modern Control Theory
This appendix describes modern control theory used in controller design of NXT Ballbot briefly. Refer
textbooks about modern control theory for more details.
A.1 Stability
We define a system is asymptotically stable when the state goes to zero independent of its initial value if the
input equals to zero. u
(A.1) 0)(lim =∞→
tt
x
A state equation is given as the following
(A.2) )()()( tBtAt uxx +=&
It is necessary and sufficient condition for asymptotically stable that the real part of all eigenvalues of system
matrix A is negative. The system is unstable when it has some eigenvalues with positive real part.
A.2 State Feedback Control
State feedback control is a control technique that multiplies feedback gain K and the difference between reference state value and measured value , and feedbacks it to the system. It is similar to PD control in
classical control theory. Figure A-1 shows block diagram of state feedback control. refx x
The input and state equation of the system shown in Figure A-1 are given as the following
( )reftKt xxu −−= )()( (A.3)
(A.4) ( ) refBKtBKAt xxx +−= )()(&
We can make the system stable by calibrating feed back gain K because we can change the eigenvalues of the
system matrix BKA− .
uxx BA +=&u xrefx +
−K
Figure A-1 Block diagram of state feedback control
- 49 -
It is necessary to use state feedback control that the system is controllability. It is necessary and sufficient condition that a controllability matrix is full rank. (cM nMrank c =)( , is order of ) n x
[ ]BAABBM n
c1,, −= L (A.5)
Control System Toolbox provides ctrb function for evaluating a controllability matrix.
Example
Is the following system controllable? A = [0, 1; -2, -3], B = [0; 1] --> controllable
There are two methods to evaluate feedback gain K .
1. Direct Pole Placement
This method calculates feedback gain K so as to assign the poles (eigenvalues) of system matrix
BKA− at the desired positions. The tuning parameters are the poles and you need to decide appropriate
gain value by trial and error. Control System Toolbox provides place function for direct pole placement.
Example
Calculate feedback gain for the system A = [0, 1; -2, -3], B = [0; 1] so as to assign the poles at [-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. Linear Quadratic Regulator
This method calculates feedback gain K so as to minimize the cost function given as the following
The tuning parameters are weight matrix for state and for input . You need to decide appropriate
gain value by trial and error. Control System Toolbox provides lqr function for linear quadratic regulator.
J
( )∫∞
+=0
)()()()( dttRttQtJ TT uuxx
Q R
- 50 -
Example
Calculate feedback gain for the system A = [0, 1; -2, -3], B = [0; 1] by using Q = [100, 0; 0, 1], R = 1.
A.3 Servo Control
Servo control is a control technique so that an output of a system tracks an expected behavior. PID control and
I-PD control in classical control theory are a kind of servo control. It is necessary to add an integrator in the
closed loop if you want the output tracks step shaped reference. Figure A-2 shows block diagram of servo
control (PID type).
y
We can calculate servo control gains in the same way as feedback control by considering an expansion system.
It has new state that is the difference between the output and referenced value. The servo control gains are
derived as the following.
The state equation and output function are given as the following.
(A.6) )()(
)()()(tCt
tBtAtxy
uxx=
+=&
We use a difference ( )reftCt xxe −= )()( , an integral of the difference , states of the expansion system )(tz
Tttt )](),([)( zxx = , and the orders of and are n , . The state equation of the expansion system is the
following
x u m
uxx BA +=&u x+
+iK C
+∫C
refx refy
−
fK+
−
>> 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
Figure A-2 Block diagram of servo control (PID type)
- 51 -
(A.7) ref
mm
mn
mmmm
mn CI
tB
tt
CA
tt
xuzx
zx
⎥⎦
⎤⎢⎣
⎡−⎥
⎦
⎤⎢⎣
⎡+⎥
⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡
×
×
××
× 0)(
0)()(
00
)()(
&
&
Eq. (A.7) converges to Eq. (A.8) if the expansion system is assumed to be stable.
(A.8) ref
mm
mn
mmmm
mn CI
BCA
xuzx
zx
⎥⎦
⎤⎢⎣
⎡−∞⎥
⎦
⎤⎢⎣
⎡+⎥
⎦
⎤⎢⎣
⎡∞∞
⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡∞∞
×
×
××
× 0)(
0)()(
00
)()(
&
&
The state equation (A.9) is derived by considering the subtraction between Eq. (A.7) and Eq. (A.8).
)()()()(0)(
)(00
)()(
tBtAtdtdt
Btt
CA
tt
eeeemme
e
mm
mn
e
e uxxuzx
zx
+=→⎥⎦
⎤⎢⎣
⎡+⎥
⎦
⎤⎢⎣
⎡⎥⎦
⎤⎢⎣
⎡=⎥
⎦
⎤⎢⎣
⎡
××
×
&
& (A.9)
where , )()( ∞−= xxx te )()( ∞−= zzz te , )()( ∞−= uuu te . We can use feedback control to make the
expansion system stable. The input is
)()()()( tKtKtKt eiefee zxxu −−=−= (A.10)
If it is possible that , , refxx →∞)( 0)( →∞z 0)( →∞u are assumed, we derive the following input . )(tu
(A.11) ( dttCKtKt refireff ∫ −−−−= xxxxu )())(()( )
Modern control theory textbooks usually describe I-PD type expression ( in the first term of Eq. (A.11) equals to zero) as servo control input. However we use PID type expression Eq. (A.11) because of improving the tracking performance.
refx
- 52 -
Appendix B Virtual Reality Space
This appendix describes the virtual reality space used in NXT Ballbot model. For example, coordinate system,
map file, distance calculation, and wall hit detection.
B.1 Coordinate System
We use Virtual Reality Toolbox for 3D visualization of NXT Ballbot. Virtual Reality Toolbox visualizes an
object based on VRML language. The VRML coordinate system is defined as shown in Figure B-1.
x
y
z
z
x
y
MATLAB Graphics VRML
Figure B-1 MATLAB and VRML coordinate system
Figure B-2 shows the coordinate system defined by vr_nxt_ballbot_track.wrl, a map file written in VRML.
z [cm]
x [cm]
0 100 200
0
100
200
Figure B-2 The coordinate system defined by vr_nxt_ballbot_track.wrl
- 53 -
The VRML position of NXT Ballbot is calculated in NXT Ballbot / Sensors / Position subsystem.
Figure B-3 Position subsystem
- 54 -
B.2 Making Map File
You can make vr_nxt_ballbot_track.wrl from vr_nxt_ballbot_track.bmp by using mywritevrtrack.m. Enter the
following command to make vr_nxt_ballbot_track.wrl.
mywritevrtrack.m makes wrl file using the following conversion rules.
1 pixel 1 * 1 [cm2] →
white pixel (RGB = [255, 255, 255]) floor →
gray pixel (RGB = [128, 128, 128]) wall (default height is 20 [cm]) →
black pixel (RGB = [0, 0, 0]) black line →
vr_nxt_ballbot_track.bmp vr_nxt_ballbot_track.wrl
>> mywritevrtrack('vr_nxt_ballbot_track.bmp')
Figure B-4 Making a map file
- 55 -
B.3 Distance Calculation and Wall Hit Detection
The distance between NXT Ballbot and wall, and wall hit detection are calculated with Embedded MATLAB
Function blocks in Sensors subsystem.
Distance Calculation Wall Hit Detection
Figure B-5 Embedded MATLAB Function blocks for the distance calculation and wall hit detection
- 56 -
An error dialog displays when NXT Ballbot hits the wall.
Figure B-6 Wall hit
- 57 -
Appendix C Generated Code
This appendix describes the main code generated from nxt_ballbot_controller.mdl using Simulink Data Object.
The comments are omitted for compactness.
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) <= dst_thr); } void task_ts3(void) { uint32_T rtb_Sum1_d; int8_T rtb_Switch2_n; boolean_T rtb_RelationalOperator_i; battery = (1.0F - a_b) * (real32_T)ecrobot_get_battery_voltage() + a_b * battery; rtb_Sum1_d = (uint32_T)((int32_T)ecrobot_get_systick_ms() - (int32_T) start_time); flag_auto = (rtb_Sum1_d >= 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 -
References
[1] Philo’s Home Page LEGO Mindstorms NXT
http://www.philohome.com/
[2] Ryo’s Holiday LEGO Mindstorms NXT
http://web.mac.com/ryo_watanabe/iWeb/Ryo%27s%20Holiday/LEGO%20Mindstorms%20NXT.html
[3] Real-Time Workshop Embedded Coder document
http://www.mathworks.com/access/helpdesk/help/toolbox/ecoder/
