import pickle
# get the time step of the current world.
# timestep = int(robot.getBasicTimeStep())
TIME_STEP = 64
MAX_SPEED = 6.28
# Dimensiones robot
r = 0.0205
l = 0.0355
a = 0.0355
# create the Robot instance.
robot = Robot()
argc = int(robot.getControllerArguments())
# Enable compass
compass = robot.getCompass("compass")
compass.enable(TIME_STEP)
robot.step(TIME_STEP)
# get a handler to the motors and set target position to infinity (speed control)
leftMotor = robot.getMotor('left wheel motor')
rightMotor = robot.getMotor('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0)
rightMotor.setVelocity(0)
posAct = np.zeros([3])
