Now that we have an interface that communicates through the serial port with the Arduino and send messages
containing the commands so the desired position of each joint of the robot in form of a string.
We are going to develop in this lesson the code that needs to be loaded into the Arduino board that
receives such a messages from the serial port then interprets them and also controls the servo motors
of the robot accordingly.
To write the code that will go into the board.
Let's open the Arduino IDE here.
Let's create a new project.
A new sketch.
And since in this script we are going to activate several servo motors that belong to the robot.
Let's include the servo dot h library and then let's create four new instances of the servo class.
The first one is called Base.
Then we have shoulder.
Then we have the elbow.
And finally we have the gripper.
Let's initialize all these four objects that belong to the servo class into the setup function, which
is the one that is automatically executed only once at the startup of the Arduino.
So for the first servo motor, the one that we called base, let's use the function attach to attach
the pin of the Arduino board to which this servo motor is connected.
Let's store this one within a new variable that we are going to call servo Base pin.
And then let's do the same also for the others.
So let's copy it three more times and let's do the same for the shoulder.
For the elbow.
And for the gripper.
So basically here we are indicating to the Arduino, which is the pin of its board, which is connected
to each motor here.
Let's also change the name.
So the variable is the shoulder pin, and this one is the elbow pin.
And this one is the gripper.
And now let's define these four variables that we have used here.
So we can define let's start by defining the first one.
So the servo base pin.
And let's say that we are going to connect the pin of the servo motor that actuates the base of our
robot to the pin number eight of the Arduino.
Then let's define also the remaining ones.
So the one of the shoulder, then the one of the elbow, and finally the one of the gripper.
And let's say that we are going to connect the servo motor of the shoulder to the pin.
Number nine, the one of the elbow to the pin number ten and the other one to the PIN number 11.
And now still in the start up function.
So here, let's ensure that when the robot starts up, all the motors are into their initial configuration.
So the default one, so let's use the function, write on each of the motors.
So let's copy this and let's paste it three more times.
And so let's also set the position for the shoulder, for the elbow and also for the gripper and also
for the initial position of each of these joint.
Let's define some more variables.
So let's define, for example, the base start variable and let's set this 1 to 90.
So this indicates that the on position, the starting position of the joint that actuates the base of
the servo motor that actuates the base is 90 degrees, then let's copy this one.
And also the same for the shoulder is 90 degrees and the same goes for the elbow
and about the gripper.
Let's set instead its initial position to zero.
So this indicates the position in which the gripper is closed.
Now let's use these four variables here.
So within the write function so that at the start up the servo motor will reach their initial configuration.
In the elbow.
And finally also lets move the gripper.
Let's conclude the setup function.
So this one by also initializing the serial communication that you are going to use in order to receive
commands from Ros2.
So let's use the serial begin function and let's use about rate of 115 200 and then let's also use a
timeout.
So with the set timeout, let's use a timeout of one with this.
The setup function is completed and now we can move on to define the logic of the loop function.
So which is executed repeatedly by the controller.
Here we are going to develop the logic that receives new messages from Ros2 through the serial port,
then interprets them and send a position command to the appropriate servomotor.
So let's first check if there is any new message available on the serial port.
So if there is something new available in the serial port, and if this is the case, let's read it
with the function read and this function will only read one single character.
So if you remember, we are sending a full string and this will read.
So this function here will read just the first character.
So the next character of the string and let's store this one into a new variable that let's call it
char.
At this point, we need to interpret this character accordingly to the communication protocol that we
have implemented in Ros two and so in the hardware interface to send commands.
So if the character that we have received is a letter and is equal to the letter B, so.
Or otherwise.
If the character.
So the variable c h r is equal to the letter s otherwise.
So let's copy this one.
If the character is equal to the letter E.
Or again.
If this is equal to the letter G or also the character that we have just received, according to the
communication protocol that we are using, can also be a comma or otherwise if we are receiving any
other character.
So if the character that we have received.
So this one doesn't fall into any of the previous categories, then it must be a number.
So an angle, a position that we want to send to the servo motor to decide how the script should behave.
In each of these cases, let's create two global support variables so we can create them here.
And the first one is an unsigned integer.
And so let's call this 1ADX and let's initialize it to zero.
The second one is also an unsigned integer, so we can copy this one, but let's call this variable
value index.
And so now we can already start using these two variables.
So at this point, if we received here the character B, so the letter B, let's set the variable index
to zero or better.
Let's reset the variable index to zero and also the value index to zero.
Otherwise, if we have received the letter S, then let's copy this one.
So if we have received the letter S, let's set the ID so the index to be the one.
So this basically this index indicates which motor we are going to activate.
So this stands for the motor of the base, this for the one of the shoulder.
Then let's again copy this one and let's change this one to be true.
And this one.
To be the number three.
So the index number three.
And so this index will indicate which motor, which servo motor we want to actuate.
Otherwise, if the character that we have received is a number.
So here in this else statement, then it must be one of the digit that belongs to the number.
So for example, if we send a position of 180, this is composed of three digits, so one, eight and
zero.
So let's create a new support variable that will contain these three digits.
Let's create it here.
And this is a vector of characters that we call value, and this has size of four.
And let's also initialize this to zero.
So as you can see, this has three elements.
But anyway, the size is four.
Since there is also one last element that we cannot see here, but is always the closing element for
a char array.
Now this array will contain the three digits that makes up the desired position of a certain servo motor.
So in the else statement here, let's store the value that we have received.
Within the value array at the value index position.
So let's use this variable here and let's store at this position of this array here.
Let's store the character that we have just received.
And in this case, let's increment the variable value index so that when we receive the next character,
so when we receive the next digit, it will be inserted in the next position of the value array of this
array.
Otherwise, if we have received a separator character, namely a comma here, it means that it's time
to send the value of the composed position.
So this array here is then to send this one to the appropriate motor.
So first let's convert to an integer that we call Val.
So a new variable Val and let's convert with the function A to integer.
Let's convert this array here.
So this contains three digits and let's convert it to an integer.
And now let's check which is the motor that we want to activate.
So if the variable index is equal to zero and this means that we want to activate the motor of the base,
so let's use the function rich goal that you are going to define shortly to say that we want to send
a girl to the motor at the base, so to the motor that activates the base.
So to the servo motor here that we call the base and let's set the value so the angle to which we want
to bring this motor, otherwise, so else if in case the index is equal to one.
So in this case, let's still use the rich girl function this time to move the motor of the shoulder
to the position indicated in the variable value.
Otherwise again.
So let's copy this one.
So if the index is equal to two, this means that we want to activate the motor of the elbow.
And the last one.
So if the index is equal to three, then we want to activate to move the robot of the gripper.
Once the position command has been sent to the correct motor, we can reset the character array.
So the variable.
So the variable that we called value this one, we can reset it to zero.
So let's set the first element of this array to the character zero.
And then let's do the same also for the other characters.
So the first one, then the second one and then the third one.
And then if you remember, this array has a size four and the last element.
So the element number three, let's set this one to the closing character of a string.
And now we are just missing to define the reach goal function that we have used here and that we set
that is going to move the appropriate servo motor by a specified value.
So let's define this function before its usage.
So let's define it here.
And this takes as input the servo motor which we want to actuate.
So it takes a reference to the motor that we want to actuate and also the goal which we want to send
to this motor.
In this function, instead of sending the command directly to the servo motor, which would cause a
sudden movement of the servo motor and thus of the robot to smooth the movements of the servo motor.
Let's send gradual commands to the robot until the desired position is reached.
So let's first check if the goal is bigger than the current status of the motor.
So the current position of the motor that we take from the motor variable using the read function.
So basically we are going to use this if statement in order to decide whether to move clockwise or counterclockwise
the motor.
So this is goal.
So in this case we want the motor to rotate clockwise.
So to ensure that this movement is gradual, let's use a for loop.
And this one goes from the current position of the motor that we called position.
And still we get with the read function and then it goes until the goal position and let's increment
at each iteration the position.
And now within this for loop, we are just going to send this command to the robot.
So let's just use the function write in order to send the new position.
So the partial position.
And so basically this for loop will iterate through all the position.
So through all the angles that are from the current position of the robot until the goal position and
will send one by one all of these to the servo motor.
And also in order to make this movement as smooth as possible, let's add a small delay of five.
Now let's add here.
So else.
So here we are managing the clockwise movement of the robot here.
Let's manage the counter clockwise.
So again, let's use a for loop that goes from the current position of the motor.
So let's use the motor read function at each step.
Let's verify that the position this time is bigger than the goal since we are moving counterclockwise.
And then finally, let's decrement the position by one.
So let's use minus minus within this for loop.
Again, we need to send the position to the motor and also to add a small delay so we can copy exactly
the same two instructions that we used in the other for loop.
With this, we have concluded our Arduino script that receives command from the serial port in form
of a string so interprets each character that it receives and then activates the appropriate servo motors.
Now let's save this script.
So let's save it within our workspace and within our Arduino board firmware package, within the firmware
folder, and let's call this one robot control.
So let's save it.
Let's conclude this lesson by compiling and loading this code within the Arduino board.
So let's first compile it to check that there are no errors.
So the compilation is done.
And now let's connect the Arduino board to our PC through a USB cable.
Then let's specify where the port is.
So here in tools port, we can see that in my case it is connected to the ACM zero port and now we can
click on upload to upload the code.
So this command is compiling once more the code and this also loading it into the Arduino board.