Exercise: Square Test.

You are going to make a program for describing a square trajectory with the robot.

Instead of starting to code from scratch, you are going to reuse the code that you developed for the distance and turning exercises.

1. Starting position

For a better visual understanding of the task, it is recommended that the robot starts at the center of the room.

You can easily relocate the robot there by simply restarting the simulation.

2. Initialization

After restarting the simulation, the robot needs to be initialized.



In [ ]:

    
import packages.initialization
import pioneer3dx as p3dx
p3dx.init()

3. Program

The code is structured in three parts:

The first part is a function for moving forward: you must copy and paste the code inside the body of the function template, in the following cell.
The second part is a similar function for turning.
Finally, the third part is the main code, consisting of a loop that calls the previous functions four times. The code also displays the pose of the robot (position and orientation) before and after the motion.



In [ ]:

    
def forward():
    # copy and paste your code here
    ...



In [ ]:

    
def turn():
    # copy and paste your code here
    ...



In [ ]:

    
print('Pose of the robot at the start')
p3dx.pose()
for _ in range(4):
    forward()
    turn()
print('Pose of the robot at the end')
p3dx.pose()

Solution



In [ ]:

    
def forward():
    target = 2.0      # target distance
    r = 0.1953 / 2    # wheel radius
    initialEncoder = p3dx.rightEncoder
    distance = 0
    while distance < target:
        p3dx.move(2.5,2.5)
        angle = p3dx.rightEncoder - initialEncoder
        distance = angle * r
    p3dx.move(0,0)



In [ ]:

    
def turn():
    target = 3.1416/2 # target angle in radians
    r = 0.1953 / 2    # wheel radius
    L = 0.33          # axis length
    initialEncoder = p3dx.leftEncoder
    robotAngle = 0
    while robotAngle < target:
        p3dx.move(1.0,-1.0)
        wheelAngle = p3dx.leftEncoder - initialEncoder
        robotAngle = 2 * r * wheelAngle / L
    p3dx.move(0,0)



In [ ]:

    
print('Pose of the robot at the start')
p3dx.pose()
for _ in range(4):
    forward()
    turn()
print('Pose of the robot at the end')
p3dx.pose()

The trajectory is:



In [ ]:

    
%matplotlib inline
import matplotlib.pyplot as plt
x, y = p3dx.trajectory()
plt.plot(x,y)

Try-a-Bot: an open source guide for robot programming

Developed by: