3. 1 Description
The project aims to design a coordinated movement between robots in form of leader
and followers.
2 Requirements
2.1 Proposed Requirements:
At least 2 robots.
Empty space without obstacles.
Robots have a default initial position. All robots are put in the same direction
with enough space within each other for non-interfering movement.
Master shall not check the result of slave movement. User will verify movement
results visually.
2.2 Implemented Requirements:
Only two robots necessary since Lego bluetooth block only permit
communication between two Lego bricks.
Empty space without obstacles.
Robots have a default initial position. All robots are put in the same direction
with enough space within each other for non-interfering movement.
Master shall check if slave has finished moving using acknowledgement message.
User will verify both master and slave end result movements visually.
3 Basic Features
3.1 Proposed Basic Features:
Master shall generate and perform a random sequence of moving patterns.
Moving pattern is a set of motions. Then the robots shall repeat this part of the
"dance". All robots know initial patterns of moving.
3
4. Communication shall be implemented over Bluetooth. It is not yet known
whether some dispatcher should be used or robots can communicate directly.
Special protocol shall be created.
3.2 Implemented Basic Features:
In order to fulfill the proposed basic features, we make use of the "Self-Balancing Two-
Wheel Robot" model from matlab as our base model, this model could be accessed by
typing "lego_selfbalance" at matlab console.
We put our implementation into the Decision Maker block which is executed after the
initialization time which is 4 seconds.
Inside the Decision Maker block are the blocks required to implement bluetooth
communication between Lego bricks and put messages at the Lego brick's LCD as
4
Illustration 3.1: Self Balancing Two Wheel Model
5. debugging message.
The functionalities to accomodate basic features are implemented in the stateflow
called "Logic". Since Master and Slave bricks have their own implementation related to
data communication, each has its own Logic stateflow.
3.2.1 Master Logic
Below, as shown at illustration 3.3, is the content of Logic stateflow for communication
protocol and moving patterns sequences implementation for Master.
The stateflow is divided into two main state: Controller and Transmitter. Controller state
function is to manage whole brick movement process and Transmitter state is to
manage data communication between Master and Slave.
5
Illustration 3.2: Decision Maker Block's Content
7. 3.2.1.1 Controller State
This state decides which movement command that will be executed by both master and
slave. In order to simplify Lego movement execution, we have created eight sets of pre-
defined primitive movements and six sets more complex movements which are called
command # (#: 1, 2, ..., 14). These set of commands is further described at 3.2.1.1.3
PerformAction state. The commands are being used to create a matrix, called patterns
as, shown at 3.2.1.1.1. It consists of pre-arranged sequences of commands that would
result in an interesing movement for Lego bricks, number in the list relates to the
respective command number.
Master will execute the command number based on the output of ChangeMotion
function results, see 3.2.1.1.2. Before executing each commands, Master will send the
command number to the Slave by triggering an event, TX_EVENT in Transmitter state.
Master will then wait for event TX_DONE being triggered by Transmitter state, before
continuing executing the command. If instead of event TX_DONE, TX_TIMEOUT is
triggered then Master will go to Stop state and Lego brick will stop, this would indicate
that there is a bluetooth connection problem.
7
Illustration 3.4: Controller Stateflow
8. Command is executed as Lego's movement at PerformAction state, after it finishes, it
will check whether Slave has also finished before executing the next command in the
list.
3.2.1.1.1 Function Init Source Code
function Init
cnt=0;
pNum=1;
isCompleted=0;
speed=0;
turn=0;
N_ACT = 7; % number of actions
RETRANS = 5; % number of retransmissions if ack was not received
COM_DELAY = 0.2; % min communication delay between messages
ACT_TIMEOUT = 1; % max time required for action
node = Action.Master; % Apply appropriate constants for moving master
patterns=[
[4,9,11,4,4];% forward - snake - circle - back - back
[4,13,4,13,5]; %back - goon - back - goon - forward
[5,10,12,14,5];]; % forward - back snake - back circle - vertigo – forward
This function is used to init every local variables used and define the patterns that Lego
brick could use as matrix.
3.2.1.1.2 Function ChangeMotion Source Code
function r_cmd = ChangeMotion
cnt=cnt+1;
if(cnt>5)
cnt=1;
if(pNum==size(patterns,2))
pNum=1;
else
pNum=pNum+1;
end
end
r_cmd = patterns(pNum,cnt);
8
9. This function will do an endless loop access to every index in matrix patterns in row-
column fashion sequentially and to give the output as commands. Note:
size(patterns,2) should have been size(patterns,1), see section 5 Conclusion.
3.2.1.1.3 PerformAction State
This is stateflow for executing commands into movements, master and slave have the
same kind of PerformAction state to execute commands, in order to ensure that they
could move similarly. Each primitive commands corresponds to a different parameters
given as inputs for Movement state, as shown at illustration 3.5. A different parameter
would result in a different "moving action" being executed by Lego brick.
Parameters =
{ action=[Action.TurnRight/Action.TurnLeft/Action.Move];
direction=[Direction.Backward/Direction.Forward];
distance=[degree(right-left)/ centimeters(backward-forward)] }
Action is used to tell lego's action: turning left, turning right, or straight (move).
Direction is used to tell lego's direction: backward, forward.
Distance is used to tell lego how much the direction should be, for right-left the input
should be turning angle in degree, while for backward-forward the input should be
distance in centimeters.
For more complicated movement which is shown as the last six commands at
illustration 3.5, each commands is built using a slightly modified Movement state and
extra stateflow as shown at 3.2.1.1.6..
In order to create a smooth movement, we need to keep track of the motor output
rotation using the Lego block- Motor Encoder. This block will give the current sum of
motor rotation in degree at that moment. Both master and slave motors have a slightly
different output, so to make a smoother turning movement, we have to use differents
constant as offset not only master and slave but also left and right direction as seen at
3.2.1.1.5. Init_move function source code.
9
11. 3.2.1.1.4 Movement State
The content of Movement state is shown above, to keep track of movement distance,
we need to keep the value of each motors rotation when starting the state. Variable
rot_dif is the actual difference between left and right motor in degree needed in order
to achieve a certain turn_angle (in degree). Variable fwd_rot is the actual rotation
needed for lego motors in order to achive a certain fwd_dist (in centimeters).
Here, the movement control is divided into two parts, FwdControl for forward or
backward and TurnControl for turning left or right.
In FwdControl variable F_B is used to control forward-backward and to achieve the
intended distance, fwd_rot is compared with how many rotation the motors have given
since state Movement.
In TurnControl variable L_R is used to control turning speed of left-right in order to
achieve the intended turn angle, rot_dif is compared with rotation difference between
left and right motors since state Movement starts. Variable F_B is also used to move to
the left or right, not just turning on the same spot. The state move to Finish when
11
Illustration 3.6: Movement Stateflow
12. current rotation difference exceed rot_dif.
The output of both FwdControl and TurnControl are speed variable that control Lego's
speed for straight movement and turn variable that control Lego's speed for rotational
movement.
3.2.1.1.5 Init_move function source code
function Init_move(actions, directions, nodes,distances)
turn_const = 0;
fwd_const = 1;
fwd_dist = 0; % forward distance in cm
turn_angle = 0; % turn angle in degree
L_R = 0;
if(directions==Direction.Backward)
F_B = -1;
else
F_B = 1;
end
switch actions
case Action.TurnLeft
switch nodes
case Action.Slave
turn_const = 138; % rotation diff in degrees to turn 90 deg left
% for Master brick
case Action.Master
turn_const = 135; % rotation diff in degrees to turn 90 deg left
% for Slave brick
end
L_R = 1; % turn speed left direction
turn_angle = distances; % turn angle in degree
case Action.TurnRight
switch nodes
case Action.Slave
turn_const = 137; % rotation diff in degrees to turn 90 deg left
% for Master brick
case Action.Master
turn_const = 136; % rotation diff in degrees to turn 90 deg left
% for Slave brick
end
L_R = -1; % turn speed right direction
12
13. turn_angle = distances; % turn angle in degree
case Action.Move
switch nodes
case Action.Slave
fwd_const = 44; % divider constant to get the approximate real
% distance
case Action.Master
fwd_const = 44; % divider constant to get the approximate real
% distance
end
fwd_dist = distances; % forward distance in cm
end
Because of the different output from each Lego motors, there is a need to use different
turn_const and fwd_const for Master and Slave and also for different direction such as
left or right in order to get the most precise result as possible.
3.2.1.1.6 Non Primitive Movement
Below are stateflows for non-primitive movements, they are basically built on state
Movement with more parameters and configurations.
13
Illustration 3.7: Snake Stateflow
16. This state manages communication protocol implementation for Master brick. When
Controller.idle state gives command to be executed as shown at illustration 3.4, the
same command will first be sent to Slave using mechanism described in Transmitter
state as shown above.
First, Master will send a preamble message to the slave as an indicator that new
command will be sent. This preamble will be sent at for COM_DELAY of time, continued
with sending command message for at most another COM_DELAY or less if Slave has
sent back acknowledgement (ComMsg.Ack) before COM_DELAY expires. If Master has
not received ComMsg.Ack and COM_DELAY expires, this is considered as one fail
attempt and Master will try to repeat sending from preamble again.
If number of fail attempts exceed RETRANS, state will go back to Transmitter.Idle state
and wait for new TX_EVENT. This condition should not happen unless there is a
bluetooth connection problem.
When Master receives ComMsg.Ack, it will send ComMsg.None to notify Slave of end of
command message and trigger event TX_DONE for state Controller as seen at 3.2.1.1
Controller State. Variable isCompleted will be given value true (1) when Slave sends
message that it has finished its movement.
16
Illustration 3.12: Transmitter Stateflow
17. 3.2.2 Slave Logic
Above is the Slave logic stateflow which is divided into two parts, Controller state and
Receiver state.
At Controller state, after event ACTION is triggered, it will execute the received
command using the same PerformAction state that Master uses, as described at
3.2.1.1.3. After completing the execution, it will trigger event COMPLETED to Receiver
state and wait for next event.
At Receiver state, if Slave receives a ComMsg.Preamble message it will move to
RxHeader state, ready to receive command message, when no command message is
received within one second then it will go back to Idle state waiting for next command.
If command message is received and it has value between 1 to 14 (number of pre-
defined movement), then it will trigger event ACTION for Controller state and send
acknowledgement message to Master during execution, this is done in order for Master
not to miss acknowledgement message because Slave has already started moving, so
Master has to start too for a synchronize movement. When event COMPLETED is
17
Illustration 3.13: Slave Logic's Stateflow
18. triggered, Slave will wait for COM_DELAY time to make sure Master has finished its
movement before waiting for the next message at Idle state.
3.2.2.1 Function Init Source Code
function Init
turn=0;
speed=0;
COM_DELAY = 0.1; % min communication delay between messages
node = Action.Slave; % Apply appropriate constants for moving a slave
N_ACT=11;
4 Possible Extensions
4.1 Proposed Possible extensions:
Master can create a new moving pattern and teach other robots.
Extension of the robot model in order to make more attractive "dances" (e.g.,
add hands). Since the robot is balancing on two wheels, it might not be as easy as
it seems first.
Make the master "sense" music. It is possible to use a special pre-recorded
monotonic frequency sound as command input. For ex: a two seconds 440hz
sound for pattern 1, a two seconds 460hz sound for patter 2, etc. Master will be
the only robot equipped with sound sensor, and slaves receive command from
master.
4.2 Implemented Possible extensions:
We did not implement any of the possible extensions because of time and
equipment limitations.
One of the lego motor is not functioning properly and hinders turning
movement.
The sound sensor turn out to be only detecting the loudness of sound not the
frequency making any implementation of music "sensing" not suitable.
18
19. 5 Conclusion
Despite our best effort, Lego does not always move precisely especially for
Master brick. It is also difficult to set a distance in centimeters for forward-
backward movement even with using constants derived from experiments.
The difficulty of debugging bluetooth connection arises because of the need to
test every changes directly with Legos and confirm it visually, it is not know by us
how to simulate the communication of commands and acknowledgement
between Master and Slave using Simulink.
Battery condition affects the motors power output which in the end affects the
precision of movements result.
The condition of Lego construction at demonstration also affects the end result
of movement, loose joints or component could create unnecessary wobble which
in turns disrupt the movements.
During Monday's 27 October 2014 demonstration, Legos exhibited error which
stop the movement sequences after some execution time. After further
discussion we conclude that the cause of it is a bug in the code. At function
ChangeMotion, see 3.2.1.1.2, we mistakenly assigned the wrong dimension for
matrix patterns which is a 3 by 5 matrix.
As shown below, pNum is used to access the first dimension of matrix patterns
and by using size(patterns,2) the value of pNum could exceed the maximum
number of row which is 3, because the colum size is 5, and results in a illegal
memory access of an array (assuming that the code is intrepeted as C, and matrix
is built as an array of sequential memory addresses). The correct code should be
size(patterns,1).
function r_cmd = ChangeMotion
cnt=cnt+1;
if(cnt>5)
cnt=1;
if(pNum==size(patterns,2))
pNum=1;
else
pNum=pNum+1;
end
end
r_cmd = patterns(pNum,cnt);
19
