%run 'Set-up.ipynb'
%run 'Loading scenes.ipynb'

%run 'vrep_models/PioneerP3DX.ipynb'

"""
Explanation: Traverse a Square - Part 1 - Simply Does It
In this notebook and the next, you will explore various strategies for programming a robot to traverse a regular two dimensional shape, such as a square.
These strategies will address two different sorts of concern:

how to use programming constructs to create programmes that are concise and easy to maintain;
how to control a robot reliably.
End of explanation
"""


%%vrepsim '../scenes/OU_Pioneer.ttt' PioneerP3DX

# Use the time library to set a wait duration
import time

#Tell the robot to move forward by setting both motors to speed 1
robot.move_forward(1)

#Wait for two seconds
time.sleep(2)

#At the end of the programme the simulation stops
#The robot returns to its original location

"""
Explanation: Driving the Robot - Forwards, Backwards, Turns
The Pioneer P3-DX robot is a general purpose, two wheeled research robot.
<img alt="" src="../images/V-REP_PRO_EDU_-pioneer.png" />
The simulator provides access to two separate motors, defined as rotational joints with velocity control - which means we can set the speed (and direction of rotation) of them.
We have defined a Python class, PioneerP3DX, that contains methods that allow you to drive the robot forwards and backwards, either at a default speed (2.0) or at a specified speed. 

move forwards:  .move_forward() or .move_forward(SPEED)
move backwards: .move_backward() or .move_backward(SPEED)

Remember, if you omit a SPEED value, the default value 2.0 is used. If you use a negative value, the direction of travel is the opposite to the direction given as the method name.
Run the following code cell and watch the behaviour of the robot.
End of explanation
"""


%%vrepsim '../scenes/OU_Pioneer.ttt' PioneerP3DX

# Use the time library to set a wait duration
import time

#YOUR CODE HERE

"""
Explanation: In the code cell below, see if you can write a programme that drives the robot forwards at speed 0.5 for 3 seconds.
End of explanation
"""


%%vrepsim '../scenes/OU_Pioneer.ttt' PioneerP3DX

# Use the time library to set a wait duration
import time

#YOUR CODE HERE

#FOR EXAMPLE, TO DRIVE CLOCKWISE, USE: robot.rotate_right()
#DON'T FORGET TO USE time.wait(TIME_IN_SECONDS) to give the robot time to turn

"""
Explanation: How would you get the robot to drive forwards for 2 seconds and then backwards for 3 seconds? Modify your programme in the cell above and try it out.
Can you get the robot to drive forwards and then backwards using just the .move_forwards() method?
HINT: setting the speed to a negative value reverses the direction of travel.
The forwards and backwards commands are actually implemented by calling another method that sets the speed of each motor separately:
.set_two_motor(SPEED_LEFT_MOTOR, SPEED_RIGHT_MOTOR)
In particular:

the .move_forward(SPEED) command calls .set_two_motor(SPEED, SPEED)
the .move_backward(SPEED) command calls .set_two_motor(-SPEED, -SPEED)

Turning on the Spot
To turn on the spot in a counter-clockwise direction, that is, towards the left, we can set one motor to drive forwards at one speed and the other to drive backwards at the same speed. For example:

.rotate_left(SPEED) calls .set_two_motor(-SPEED, SPEED)

As before, if you omit the SPEED parameter, a default value (again set to 2.0) is used.
How do you think the .rotate_right() method might be defined?
Use the following code cell to write a programme to turn the Pioneer robot slowly on the spot in one direction for 2 seconds, and then faster on the spot and in the opposite direction for a further three seconds.
Also explore what happens if you call the .set_two_motor() method using different speeds for the left and right motors, in either the same, or different, directions.
End of explanation
"""


%%vrepsim '../scenes/OU_Pioneer.ttt' PioneerP3DX
import time

#try to get the robot to draw an L shape: forward, right angle turn, forward



"""
Explanation: A Simple Square
So now you should be able to drive your robot forward, and backwards, and make it turn on the spot in either direction.
Do you think you could get it to describe a square?
In the code cell below, see if you can write a programme to drive the robot forward a short way, turn through ninety degrees, and then move forwards a short distance again.
You may find it takes a few attempts to tune the turn speed and time settings correctly so that the robot turns the correct amount.
To see how right angled the turn actually was, you may find it useful to view the robot from above - change the camera view from the top toolbar.
Don't spend too long on this - you may find that getting exactly 90 degrees every time is virtually impossible.
To see the trace clearly, you may want to pick up the robot and move it out of the way once the programme has finished. Do this by:

select the Pioneer_3dx robot object in the Scene Hierarchy;
select the Object / Item Shift option in the top toolbar (the cube with four arrows coming out of it);
drag the robot out of the way.

Additional resources:

how to change the simulator camera view.
End of explanation
"""


%%vrepsim '../scenes/OU_Pioneer.ttt' PioneerP3DX
import time

#Program to draw a square


"""
Explanation: Once you have have found some values that allow your robot to turn through ninety degrees or so, see if you can get your robot to draw out a square.
Again, don't spend too long on this. An approxi-square that looks like a bent coathanger may be the best you can get.
End of explanation
"""


%%vrepsim '../scenes/OU_Pioneer.ttt' PioneerP3DX
import time

#side 1
robot.move_forward()
time.sleep(1)
#turn 1
robot.rotate_left(1.8)
time.sleep(0.45)
#side 2
robot.move_forward()
time.sleep(1)
#turn 2
robot.rotate_left(1.8)
time.sleep(0.45)
#side 3
robot.move_forward()
time.sleep(1)
#turn 3
robot.rotate_left(1.8)
time.sleep(0.45)
#side 4
robot.move_forward()
time.sleep(1)

"""
Explanation: So how did you do?
And what does your programme look like?
I'm going to hazard a few guesses.
First, calling the trace a square may have been a little hopeful.
Second, the angle turned at each corner may have been different even using the same values, or th robot may have turned different amounts with the same settings on different runs.
Third, your programme was possibly quite long and filled with lots of repeating statements and numbers.
So what happened?
Rubbish Square
Using time to set the turn angle and side length is hazardous. The simulator is doing lots of calculations. Depending on what other activities are going on on your computer, the amount of processor time allocated to the simulator each second may differ from second to second.

Different number of calculations means different number of (calculated) simulator steps, which in turn means different result in the simulated view.
Long Programme With Lots of Steps and Repeated Numbers
You may have found your programme looked something like the one shown below (hopefully with some comments...):
End of explanation
"""


