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)