from Coppelia import Coppelia from Pioneer3DX import Pioneer3DX from LaserSensor import LaserSensor import matplotlib.pyplot as plt import time import numpy as np # Load CoppeliaSim Class and Start Run Simulation CoppeliaSim = Coppelia() CoppeliaSim.start_Simulation() # Load Mobile Robot Pioneer 3DX Pioneer3DX = Pioneer3DX(CoppeliaSim.clientID) # Load Laser Scanner Laser = LaserSensor(CoppeliaSim.clientID) # Config plot shouldPlot = True realRobotTraject_x, realRobotTraject_y = [], [] fig, ax = plt.subplots() laserPointsPlot, = ax.plot(realRobotTraject_x, realRobotTraject_y, 'bo', ms=2) realRobotTrajectPlot, = ax.plot(realRobotTraject_x, realRobotTraject_y, 'k-') ax.set_xlim(-6, 6) ax.set_ylim(-6, 6) fig.show() ## Main Routine X_Desired = [0, 0] X_diff = [0, 0] # Start time routine
fig = plt.figure()
ax = fig.add_subplot(111)
ax.set_xlim(-5, 5)
ax.set_ylim(-5, 5)
fig.show()
x, y = [], []
# Load CoppeliaSim Class and Start Run Simulation
CoppeliaSim = Coppelia()
CoppeliaSim.start_Simulation()
# Load Mobile Robot Pioneer 3DX
P = Pioneer3DX(CoppeliaSim.clientID)
# Load Laser Scanner
L = LaserSensor(CoppeliaSim.clientID)
## Main Routine
# Start time routine
startTime = time.time()
while time.time() - startTime < 30:
# Set Position Desired
X_Desired = [-2, 3]
# Get Real Position From Robot
P.get_PositionData()
# Direct kinematic for differencial drive robot
# K = [[cos(theta) -0.15*sin(theta)
# sin(theta) 0.15*cos(theta)]]
K = np.array([[np.cos(P.orientation), -0.15 * np.sin(P.orientation)],
[np.sin(P.orientation), 0.15 * np.cos(P.orientation)]])