def straightDrive(Robot, v, l, n=150):
Robot.setTimeStep(l / v)
# Definiere Folge von Bewegungsbefehle:
motionCircle = np.zeros((n, 2))
for i in range(n):
motionCircle[i] = [v / n, 0]
for t in range(n):
# Bewege Roboter
motion = motionCircle[t]
#print("v = ", motion[0], "omega = ", motion[1]*180/np.pi)
Robot.move(motion)
return
def curveDrive(Robot, v, r, deltaTheta, n=150):
omega = v / r * np.sign(deltaTheta)
deltaTheta = abs(deltaTheta / 180 * np.pi)
# Anzahl Zeitschritte n mit jeweils der Laenge T = 0.1 sec definieren.
# T laesst sich ueber die Methode myRobot.setTimeStep(T) einstellen.
# T = 0.1 sec ist voreingestellt.
Robot.setTimeStep(deltaTheta / abs(omega))
# Definiere Folge von Bewegungsbefehle:
motionCircle = np.zeros((n, 2))
for i in range(n):
motionCircle[i] = [v / n, omega / n]
# Bewege Roboter
for t in range(n):
# Bewege Roboter
motion = motionCircle[t]
#print("v = ", motion[0], "omega = ", motion[1]*180/np.pi)
Robot.move(motion)
return
from math import *
import numpy as np
from Robot_Simulator_V2 import emptyWorld
from Robot_Simulator_V2 import Robot
from PoseEstimator import PlotUtilities
from PoseEstimator import OdometryPoseEstimator
# time step and number of time steps:
# T = 0.1 sec default value
n = 141
# Set a robot in a world
myWorld = emptyWorld.buildWorld()
myRobot = Robot.Robot()
poseStart = [2, 5, pi / 2]
myWorld.setRobot(myRobot, poseStart)
# Movement definition:
motionCircle = [[1, -24 * pi / 180] for i in range(n)]
n4 = int(n / 4)
motionCircleCurve = [[1, -12 * pi / 180] for i in range(n4)]
motionCircleCurve.extend([[1, 12 * pi / 180] for i in range(n4)])
motionCircleCurve.extend(motionCircleCurve)
motion90RightTurn = [[1, -180 * pi / 180] for i in range(25)]
motionSegment = [[1, 0] for i in range(n4 - 25)]
motionSquare = []
for i in range(4):
motionSquare.extend(motionSegment)
