WEBVTT 00:00.540 --> 00:06.900 Now that we have an interface that communicates through the serial port with the Arduino and send messages 00:06.900 --> 00:12.930 containing the commands so the desired position of each joint of the robot in form of a string. 00:12.960 --> 00:18.450 We are going to develop in this lesson the code that needs to be loaded into the Arduino board that 00:18.450 --> 00:25.050 receives such a messages from the serial port then interprets them and also controls the servo motors 00:25.050 --> 00:26.460 of the robot accordingly. 00:27.510 --> 00:30.010 To write the code that will go into the board. 00:30.030 --> 00:33.180 Let's open the Arduino IDE here. 00:33.180 --> 00:34.950 Let's create a new project. 00:34.950 --> 00:36.150 A new sketch. 00:37.540 --> 00:43.000 And since in this script we are going to activate several servo motors that belong to the robot. 00:43.000 --> 00:54.670 Let's include the servo dot h library and then let's create four new instances of the servo class. 00:55.390 --> 00:58.320 The first one is called Base. 00:59.680 --> 01:01.150 Then we have shoulder. 01:02.500 --> 01:05.080 Then we have the elbow. 01:06.280 --> 01:10.600 And finally we have the gripper. 01:10.960 --> 01:18.220 Let's initialize all these four objects that belong to the servo class into the setup function, which 01:18.220 --> 01:23.380 is the one that is automatically executed only once at the startup of the Arduino. 01:23.710 --> 01:31.990 So for the first servo motor, the one that we called base, let's use the function attach to attach 01:31.990 --> 01:36.130 the pin of the Arduino board to which this servo motor is connected. 01:36.160 --> 01:43.670 Let's store this one within a new variable that we are going to call servo Base pin. 01:43.850 --> 01:48.590 And then let's do the same also for the others. 01:48.590 --> 01:53.750 So let's copy it three more times and let's do the same for the shoulder. 01:54.260 --> 01:55.820 For the elbow. 01:58.240 --> 01:59.560 And for the gripper. 02:00.400 --> 02:05.620 So basically here we are indicating to the Arduino, which is the pin of its board, which is connected 02:05.620 --> 02:07.090 to each motor here. 02:07.090 --> 02:08.580 Let's also change the name. 02:08.590 --> 02:17.650 So the variable is the shoulder pin, and this one is the elbow pin. 02:18.070 --> 02:20.050 And this one is the gripper. 02:22.780 --> 02:26.410 And now let's define these four variables that we have used here. 02:26.650 --> 02:32.680 So we can define let's start by defining the first one. 02:32.680 --> 02:34.600 So the servo base pin. 02:34.600 --> 02:40.420 And let's say that we are going to connect the pin of the servo motor that actuates the base of our 02:40.420 --> 02:43.690 robot to the pin number eight of the Arduino. 02:44.140 --> 02:49.030 Then let's define also the remaining ones. 02:49.030 --> 02:56.650 So the one of the shoulder, then the one of the elbow, and finally the one of the gripper. 02:57.520 --> 03:02.650 And let's say that we are going to connect the servo motor of the shoulder to the pin. 03:02.650 --> 03:09.820 Number nine, the one of the elbow to the pin number ten and the other one to the PIN number 11. 03:10.090 --> 03:12.520 And now still in the start up function. 03:12.520 --> 03:19.630 So here, let's ensure that when the robot starts up, all the motors are into their initial configuration. 03:19.630 --> 03:27.710 So the default one, so let's use the function, write on each of the motors. 03:27.710 --> 03:32.810 So let's copy this and let's paste it three more times. 03:33.050 --> 03:42.500 And so let's also set the position for the shoulder, for the elbow and also for the gripper and also 03:42.500 --> 03:44.990 for the initial position of each of these joint. 03:45.020 --> 03:48.020 Let's define some more variables. 03:48.260 --> 03:55.280 So let's define, for example, the base start variable and let's set this 1 to 90. 03:55.310 --> 04:01.220 So this indicates that the on position, the starting position of the joint that actuates the base of 04:01.220 --> 04:06.710 the servo motor that actuates the base is 90 degrees, then let's copy this one. 04:07.190 --> 04:15.980 And also the same for the shoulder is 90 degrees and the same goes for the elbow 04:19.220 --> 04:21.080 and about the gripper. 04:23.810 --> 04:26.870 Let's set instead its initial position to zero. 04:26.870 --> 04:30.860 So this indicates the position in which the gripper is closed. 04:31.040 --> 04:34.490 Now let's use these four variables here. 04:34.490 --> 04:40.790 So within the write function so that at the start up the servo motor will reach their initial configuration. 04:44.650 --> 04:45.460 In the elbow. 04:45.460 --> 04:48.400 And finally also lets move the gripper. 04:48.730 --> 04:51.100 Let's conclude the setup function. 04:51.100 --> 04:57.220 So this one by also initializing the serial communication that you are going to use in order to receive 04:57.220 --> 04:58.660 commands from Ros2. 04:59.590 --> 05:10.870 So let's use the serial begin function and let's use about rate of 115 200 and then let's also use a 05:10.870 --> 05:11.440 timeout. 05:11.440 --> 05:16.390 So with the set timeout, let's use a timeout of one with this. 05:16.390 --> 05:22.270 The setup function is completed and now we can move on to define the logic of the loop function. 05:22.270 --> 05:26.230 So which is executed repeatedly by the controller. 05:26.620 --> 05:32.500 Here we are going to develop the logic that receives new messages from Ros2 through the serial port, 05:32.530 --> 05:37.810 then interprets them and send a position command to the appropriate servomotor. 05:37.960 --> 05:42.850 So let's first check if there is any new message available on the serial port. 05:43.420 --> 05:50.480 So if there is something new available in the serial port, and if this is the case, let's read it 05:51.260 --> 05:56.900 with the function read and this function will only read one single character. 05:56.900 --> 06:01.280 So if you remember, we are sending a full string and this will read. 06:01.280 --> 06:04.040 So this function here will read just the first character. 06:04.040 --> 06:11.810 So the next character of the string and let's store this one into a new variable that let's call it 06:11.810 --> 06:12.380 char. 06:13.290 --> 06:18.270 At this point, we need to interpret this character accordingly to the communication protocol that we 06:18.270 --> 06:22.800 have implemented in Ros two and so in the hardware interface to send commands. 06:23.100 --> 06:32.520 So if the character that we have received is a letter and is equal to the letter B, so. 06:33.640 --> 06:34.990 Or otherwise. 06:37.870 --> 06:39.160 If the character. 06:40.540 --> 06:48.310 So the variable c h r is equal to the letter s otherwise. 06:48.310 --> 06:49.510 So let's copy this one. 06:51.660 --> 06:54.750 If the character is equal to the letter E. 06:56.930 --> 06:58.130 Or again. 06:59.900 --> 07:06.410 If this is equal to the letter G or also the character that we have just received, according to the 07:06.410 --> 07:15.350 communication protocol that we are using, can also be a comma or otherwise if we are receiving any 07:15.350 --> 07:16.280 other character. 07:16.280 --> 07:18.530 So if the character that we have received. 07:18.530 --> 07:24.140 So this one doesn't fall into any of the previous categories, then it must be a number. 07:24.140 --> 07:30.830 So an angle, a position that we want to send to the servo motor to decide how the script should behave. 07:30.860 --> 07:38.060 In each of these cases, let's create two global support variables so we can create them here. 07:38.180 --> 07:41.600 And the first one is an unsigned integer. 07:43.580 --> 07:50.120 And so let's call this 1ADX and let's initialize it to zero. 07:50.390 --> 07:58.370 The second one is also an unsigned integer, so we can copy this one, but let's call this variable 07:59.150 --> 08:01.460 value index. 08:02.190 --> 08:04.980 And so now we can already start using these two variables. 08:04.980 --> 08:13.350 So at this point, if we received here the character B, so the letter B, let's set the variable index 08:13.350 --> 08:14.700 to zero or better. 08:14.700 --> 08:25.260 Let's reset the variable index to zero and also the value index to zero. 08:25.950 --> 08:31.500 Otherwise, if we have received the letter S, then let's copy this one. 08:32.280 --> 08:39.150 So if we have received the letter S, let's set the ID so the index to be the one. 08:39.150 --> 08:44.070 So this basically this index indicates which motor we are going to activate. 08:44.070 --> 08:48.210 So this stands for the motor of the base, this for the one of the shoulder. 08:48.660 --> 08:54.240 Then let's again copy this one and let's change this one to be true. 08:54.900 --> 08:56.340 And this one. 08:59.080 --> 09:00.610 To be the number three. 09:00.610 --> 09:02.260 So the index number three. 09:02.260 --> 09:07.150 And so this index will indicate which motor, which servo motor we want to actuate. 09:08.020 --> 09:11.950 Otherwise, if the character that we have received is a number. 09:11.950 --> 09:19.450 So here in this else statement, then it must be one of the digit that belongs to the number. 09:19.450 --> 09:27.730 So for example, if we send a position of 180, this is composed of three digits, so one, eight and 09:27.730 --> 09:28.210 zero. 09:28.240 --> 09:32.440 So let's create a new support variable that will contain these three digits. 09:33.160 --> 09:34.870 Let's create it here. 09:35.200 --> 09:42.460 And this is a vector of characters that we call value, and this has size of four. 09:42.670 --> 09:45.280 And let's also initialize this to zero. 09:45.280 --> 09:47.920 So as you can see, this has three elements. 09:47.920 --> 09:50.140 But anyway, the size is four. 09:50.140 --> 09:56.590 Since there is also one last element that we cannot see here, but is always the closing element for 09:56.590 --> 09:57.760 a char array. 09:58.330 --> 10:04.690 Now this array will contain the three digits that makes up the desired position of a certain servo motor. 10:04.810 --> 10:10.300 So in the else statement here, let's store the value that we have received. 10:10.540 --> 10:17.020 Within the value array at the value index position. 10:17.410 --> 10:24.520 So let's use this variable here and let's store at this position of this array here. 10:24.610 --> 10:30.190 Let's store the character that we have just received. 10:30.850 --> 10:39.070 And in this case, let's increment the variable value index so that when we receive the next character, 10:39.070 --> 10:45.220 so when we receive the next digit, it will be inserted in the next position of the value array of this 10:45.220 --> 10:45.820 array. 10:46.240 --> 10:53.050 Otherwise, if we have received a separator character, namely a comma here, it means that it's time 10:53.050 --> 10:56.650 to send the value of the composed position. 10:56.650 --> 11:01.960 So this array here is then to send this one to the appropriate motor. 11:02.080 --> 11:08.020 So first let's convert to an integer that we call Val. 11:08.020 --> 11:14.320 So a new variable Val and let's convert with the function A to integer. 11:14.350 --> 11:17.380 Let's convert this array here. 11:17.530 --> 11:21.940 So this contains three digits and let's convert it to an integer. 11:22.120 --> 11:25.900 And now let's check which is the motor that we want to activate. 11:25.900 --> 11:35.630 So if the variable index is equal to zero and this means that we want to activate the motor of the base, 11:35.630 --> 11:42.950 so let's use the function rich goal that you are going to define shortly to say that we want to send 11:42.950 --> 11:48.680 a girl to the motor at the base, so to the motor that activates the base. 11:48.680 --> 11:55.760 So to the servo motor here that we call the base and let's set the value so the angle to which we want 11:55.760 --> 12:05.780 to bring this motor, otherwise, so else if in case the index is equal to one. 12:06.380 --> 12:14.660 So in this case, let's still use the rich girl function this time to move the motor of the shoulder 12:14.660 --> 12:18.920 to the position indicated in the variable value. 12:19.570 --> 12:20.800 Otherwise again. 12:20.800 --> 12:22.420 So let's copy this one. 12:23.410 --> 12:29.860 So if the index is equal to two, this means that we want to activate the motor of the elbow. 12:31.540 --> 12:33.280 And the last one. 12:33.280 --> 12:40.180 So if the index is equal to three, then we want to activate to move the robot of the gripper. 12:41.440 --> 12:48.160 Once the position command has been sent to the correct motor, we can reset the character array. 12:48.190 --> 12:49.300 So the variable. 12:49.480 --> 12:54.640 So the variable that we called value this one, we can reset it to zero. 12:55.000 --> 13:01.480 So let's set the first element of this array to the character zero. 13:01.570 --> 13:05.020 And then let's do the same also for the other characters. 13:05.290 --> 13:10.450 So the first one, then the second one and then the third one. 13:10.600 --> 13:16.210 And then if you remember, this array has a size four and the last element. 13:16.450 --> 13:23.270 So the element number three, let's set this one to the closing character of a string. 13:23.670 --> 13:29.570 And now we are just missing to define the reach goal function that we have used here and that we set 13:29.570 --> 13:33.710 that is going to move the appropriate servo motor by a specified value. 13:33.740 --> 13:37.370 So let's define this function before its usage. 13:37.640 --> 13:39.740 So let's define it here. 13:42.920 --> 13:46.910 And this takes as input the servo motor which we want to actuate. 13:47.660 --> 13:55.700 So it takes a reference to the motor that we want to actuate and also the goal which we want to send 13:55.700 --> 13:56.570 to this motor. 13:57.170 --> 14:02.630 In this function, instead of sending the command directly to the servo motor, which would cause a 14:02.630 --> 14:08.540 sudden movement of the servo motor and thus of the robot to smooth the movements of the servo motor. 14:08.540 --> 14:14.060 Let's send gradual commands to the robot until the desired position is reached. 14:14.540 --> 14:23.510 So let's first check if the goal is bigger than the current status of the motor. 14:23.510 --> 14:30.830 So the current position of the motor that we take from the motor variable using the read function. 14:30.830 --> 14:37.940 So basically we are going to use this if statement in order to decide whether to move clockwise or counterclockwise 14:37.940 --> 14:38.780 the motor. 14:38.870 --> 14:41.990 So this is goal. 14:42.500 --> 14:47.120 So in this case we want the motor to rotate clockwise. 14:47.120 --> 14:52.010 So to ensure that this movement is gradual, let's use a for loop. 14:52.760 --> 14:58.620 And this one goes from the current position of the motor that we called position. 14:58.620 --> 15:09.780 And still we get with the read function and then it goes until the goal position and let's increment 15:09.810 --> 15:12.360 at each iteration the position. 15:13.020 --> 15:18.200 And now within this for loop, we are just going to send this command to the robot. 15:18.210 --> 15:26.040 So let's just use the function write in order to send the new position. 15:26.040 --> 15:28.170 So the partial position. 15:28.170 --> 15:32.310 And so basically this for loop will iterate through all the position. 15:32.310 --> 15:37.830 So through all the angles that are from the current position of the robot until the goal position and 15:37.830 --> 15:40.830 will send one by one all of these to the servo motor. 15:40.950 --> 15:48.630 And also in order to make this movement as smooth as possible, let's add a small delay of five. 15:49.980 --> 15:52.170 Now let's add here. 15:52.170 --> 15:53.220 So else. 15:53.370 --> 15:58.080 So here we are managing the clockwise movement of the robot here. 15:58.080 --> 16:00.330 Let's manage the counter clockwise. 16:00.330 --> 16:06.630 So again, let's use a for loop that goes from the current position of the motor. 16:06.630 --> 16:11.670 So let's use the motor read function at each step. 16:11.670 --> 16:18.780 Let's verify that the position this time is bigger than the goal since we are moving counterclockwise. 16:18.780 --> 16:22.890 And then finally, let's decrement the position by one. 16:22.890 --> 16:27.330 So let's use minus minus within this for loop. 16:27.330 --> 16:33.570 Again, we need to send the position to the motor and also to add a small delay so we can copy exactly 16:33.570 --> 16:37.050 the same two instructions that we used in the other for loop. 16:37.600 --> 16:43.240 With this, we have concluded our Arduino script that receives command from the serial port in form 16:43.240 --> 16:50.350 of a string so interprets each character that it receives and then activates the appropriate servo motors. 16:50.380 --> 16:52.390 Now let's save this script. 16:52.390 --> 16:59.320 So let's save it within our workspace and within our Arduino board firmware package, within the firmware 16:59.320 --> 17:05.130 folder, and let's call this one robot control. 17:05.140 --> 17:06.730 So let's save it. 17:08.960 --> 17:14.460 Let's conclude this lesson by compiling and loading this code within the Arduino board. 17:14.480 --> 17:18.740 So let's first compile it to check that there are no errors. 17:18.740 --> 17:20.600 So the compilation is done. 17:20.600 --> 17:25.220 And now let's connect the Arduino board to our PC through a USB cable. 17:30.080 --> 17:33.890 Then let's specify where the port is. 17:33.890 --> 17:41.540 So here in tools port, we can see that in my case it is connected to the ACM zero port and now we can 17:41.540 --> 17:43.940 click on upload to upload the code. 17:44.760 --> 17:50.220 So this command is compiling once more the code and this also loading it into the Arduino board.