Project Implementation Report

10/28/14

Maxim Olifer

&

Aditya Hendra

1

Table of Contents1Description...............................................................................................................................................32Requirements...........................................................................................................................................3

2.1Proposed Requirements:..................................................................................................................32.2Implemented Requirements:...........................................................................................................3

3Basic Features..........................................................................................................................................33.1Proposed Basic Features:.................................................................................................................33.2Implemented Basic Features:...........................................................................................................4

3.2.1Master Logic.............................................................................................................................53.2.1.1Controller State.................................................................................................................7

3.2.1.1.1Function Init Source Code.........................................................................................83.2.1.1.2Function ChangeMotion Source Code......................................................................83.2.1.1.3PerformAction State.................................................................................................93.2.1.1.4Movement State.....................................................................................................113.2.1.1.5Init_move function source code.............................................................................123.2.1.1.6Non Primitive Movement.......................................................................................13

3.2.1.2Transmitter State............................................................................................................153.2.2Slave Logic...............................................................................................................................17

3.2.2.1Function Init Source Code...............................................................................................184Possible Extensions................................................................................................................................18

4.1Proposed Possible extensions:.......................................................................................................184.2Implemented Possible extensions:.................................................................................................18

5Conclusion..............................................................................................................................................19

Illustration IndexIllustration 3.1: Self Balancing Two Wheel Model.....................................................................................4Illustration 3.2: Decision Maker Block's Content.......................................................................................5Illustration 3.3: Master's Logic Stateflow...................................................................................................6Illustration 3.4: Controller Stateflow..........................................................................................................7Illustration 3.5: PerformAction Stateflow................................................................................................10Illustration 3.6: Movement Stateflow......................................................................................................11Illustration 3.7: Snake Stateflow..............................................................................................................13Illustration 3.8: function SwitchMode Box...............................................................................................14Illustration 3.9: Circle Stateflow...............................................................................................................15Illustration 3.10: Goon Stateflow.............................................................................................................15Illustration 3.11: Vertigo Stateflow..........................................................................................................15Illustration 3.12: Transmitter Stateflow...................................................................................................16Illustration 3.13: Slave Logic's Stateflow..................................................................................................17

2

1 DescriptionThe project aims to design a coordinated movement between robots in form of leaderand 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 permitcommunication 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

Communication shall be implemented over Bluetooth. It is not yet knownwhether 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 bytyping "lego_selfbalance" at matlab console.

We put our implementation into the Decision Maker block which is executed after theinitialization time which is 4 seconds.

Inside the Decision Maker block are the blocks required to implement bluetoothcommunication between Lego bricks and put messages at the Lego brick's LCD as

4

Illustration 3.1: Self Balancing Two Wheel Model

debugging message.

The functionalities to accomodate basic features are implemented in the stateflowcalled "Logic". Since Master and Slave bricks have their own implementation related todata 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 communicationprotocol and moving patterns sequences implementation for Master.The stateflow is divided into two main state: Controller and Transmitter. Controller statefunction is to manage whole brick movement process and Transmitter state is tomanage data communication between Master and Slave.

5

Illustration 3.2: Decision Maker Block's Content

6

Illustration 3.3: Master's Logic Stateflow

3.2.1.1 Controller State

This state decides which movement command that will be executed by both master andslave. 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 calledcommand # (#: 1, 2, ..., 14). These set of commands is further described at 3.2.1.1.3PerformAction state. The commands are being used to create a matrix, called patternsas, shown at 3.2.1.1.1. It consists of pre-arranged sequences of commands that wouldresult in an interesing movement for Lego bricks, number in the list relates to therespective command number.

Master will execute the command number based on the output of ChangeMotionfunction results, see 3.2.1.1.2. Before executing each commands, Master will send thecommand 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, beforecontinuing executing the command. If instead of event TX_DONE, TX_TIMEOUT istriggered then Master will go to Stop state and Lego brick will stop, this would indicatethat there is a bluetooth connection problem.

7

Illustration 3.4: Controller Stateflow

Command is executed as Lego's movement at PerformAction state, after it finishes, itwill check whether Slave has also finished before executing the next command in thelist.

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 actionsRETRANS = 5; % number of retransmissions if ack was not receivedCOM_DELAY = 0.2; % min communication delay between messagesACT_TIMEOUT = 1; % max time required for actionnode = 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 Legobrick 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

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 thesame kind of PerformAction state to execute commands, in order to ensure that theycould move similarly. Each primitive commands corresponds to a different parametersgiven as inputs for Movement state, as shown at illustration 3.5. A different parameterwould 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 inputshould be turning angle in degree, while for backward-forward the input should bedistance in centimeters.

For more complicated movement which is shown as the last six commands atillustration 3.5, each commands is built using a slightly modified Movement state andextra stateflow as shown at 3.2.1.1.6..

In order to create a smooth movement, we need to keep track of the motor outputrotation using the Lego block- Motor Encoder. This block will give the current sum ofmotor rotation in degree at that moment. Both master and slave motors have a slightlydifferent output, so to make a smoother turning movement, we have to use differentsconstant as offset not only master and slave but also left and right direction as seen at3.2.1.1.5. Init_move function source code.

9

10Illustration 3.5: PerformAction Stateflow

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. Variablerot_dif is the actual difference between left and right motor in degree needed in orderto achieve a certain turn_angle (in degree). Variable fwd_rot is the actual rotationneeded 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 orbackward and TurnControl for turning left or right.

In FwdControl variable F_B is used to control forward-backward and to achieve theintended distance, fwd_rot is compared with how many rotation the motors have givensince state Movement.

In TurnControl variable L_R is used to control turning speed of left-right in order toachieve the intended turn angle, rot_dif is compared with rotation difference betweenleft and right motors since state Movement starts. Variable F_B is also used to move tothe left or right, not just turning on the same spot. The state move to Finish when

11

Illustration 3.6: Movement Stateflow

current rotation difference exceed rot_dif.

The output of both FwdControl and TurnControl are speed variable that control Lego'sspeed for straight movement and turn variable that control Lego's speed for rotationalmovement.

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 cmturn_angle = 0; % turn angle in degreeL_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

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 cmend

Because of the different output from each Lego motors, there is a need to use differentturn_const and fwd_const for Master and Slave and also for different direction such asleft 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 stateMovement with more parameters and configurations.

13

Illustration 3.7: Snake Stateflow

14

Illustration 3.8: function SwitchMode Box

3.2.1.2 Transmitter State

15

Illustration 3.9: Circle Stateflow

Illustration 3.10: Goon Stateflow

Illustration 3.11: Vertigo Stateflow

This state manages communication protocol implementation for Master brick. WhenController.idle state gives command to be executed as shown at illustration 3.4, thesame command will first be sent to Slave using mechanism described in Transmitterstate as shown above.

First, Master will send a preamble message to the slave as an indicator that newcommand will be sent. This preamble will be sent at for COM_DELAY of time, continuedwith sending command message for at most another COM_DELAY or less if Slave hassent back acknowledgement (ComMsg.Ack) before COM_DELAY expires. If Master hasnot received ComMsg.Ack and COM_DELAY expires, this is considered as one failattempt and Master will try to repeat sending from preamble again.

If number of fail attempts exceed RETRANS, state will go back to Transmitter.Idle stateand wait for new TX_EVENT. This condition should not happen unless there is abluetooth connection problem.

When Master receives ComMsg.Ack, it will send ComMsg.None to notify Slave of end ofcommand message and trigger event TX_DONE for state Controller as seen at 3.2.1.1Controller State. Variable isCompleted will be given value true (1) when Slave sendsmessage that it has finished its movement.

16

Illustration 3.12: Transmitter Stateflow

3.2.2 Slave Logic

Above is the Slave logic stateflow which is divided into two parts, Controller state andReceiver state.

At Controller state, after event ACTION is triggered, it will execute the receivedcommand using the same PerformAction state that Master uses, as described at3.2.1.1.3. After completing the execution, it will trigger event COMPLETED to Receiverstate and wait for next event.

At Receiver state, if Slave receives a ComMsg.Preamble message it will move toRxHeader state, ready to receive command message, when no command message isreceived 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 sendacknowledgement message to Master during execution, this is done in order for Masternot to miss acknowledgement message because Slave has already started moving, soMaster has to start too for a synchronize movement. When event COMPLETED is

17

Illustration 3.13: Slave Logic's Stateflow

triggered, Slave will wait for COM_DELAY time to make sure Master has finished itsmovement 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 messagesnode = Action.Slave; % Apply appropriate constants for moving a slaveN_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 asit seems first.

Make the master "sense" music. It is possible to use a special pre-recordedmonotonic frequency sound as command input. For ex: a two seconds 440hzsound for pattern 1, a two seconds 460hz sound for patter 2, etc. Master will bethe only robot equipped with sound sensor, and slaves receive command frommaster.

4.2 Implemented Possible extensions:

We did not implement any of the possible extensions because of time andequipment limitations.

One of the lego motor is not functioning properly and hinders turningmovement.

The sound sensor turn out to be only detecting the loudness of sound not thefrequency making any implementation of music "sensing" not suitable.

18

5 Conclusion

Despite our best effort, Lego does not always move precisely especially forMaster 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 totest every changes directly with Legos and confirm it visually, it is not know by ushow to simulate the communication of commands and acknowledgementbetween Master and Slave using Simulink.

Battery condition affects the motors power output which in the end affects theprecision of movements result.

The condition of Lego construction at demonstration also affects the end resultof movement, loose joints or component could create unnecessary wobble whichin turns disrupt the movements.

During Monday's 27 October 2014 demonstration, Legos exhibited error whichstop the movement sequences after some execution time. After furtherdiscussion we conclude that the cause of it is a bug in the code. At functionChangeMotion, see 3.2.1.1.2, we mistakenly assigned the wrong dimension formatrix patterns which is a 3 by 5 matrix. As shown below, pNum is used to access the first dimension of matrix patternsand by using size(patterns,2) the value of pNum could exceed the maximumnumber of row which is 3, because the colum size is 5, and results in a illegalmemory access of an array (assuming that the code is intrepeted as C, and matrixis built as an array of sequential memory addresses). The correct code should besize(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