import vrep
import numpy as np
import time
import numpy as np
import matplotlib.pyplot as plt
import math
from mpl_toolkits import mplot3d
print("Program Started")
vrep.simxFinish(-1)
SimulationTime = 10 # in seconds
angX = 0
angY = 0
angMag = 0
robotMode = 0
# Connecting to V-REP Scene
clientID = vrep.simxStart('
127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print("Connected to remote API server")
# Launching simulation
vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot)
# Simple terminal UI
while True:
robotMode = input("Choose robot mode or quit (q): 0 - Move To Target, 1 - Circular Path: ")
if robotMode == 'q':
break
vrep.simxSetIntegerSignal(clientID, "robotMode", int(robotMode), vrep.simx_opmode_oneshot)
current_x=0
current_y=0
current_z=0
fig = plt.figure(figsize =(14, 9))
ax = plt.axes(projection ='3d')
# Creating color map
my_cmap = plt.get_cmap('hot')
# Mode of robot that allows to control its target angular position
if int(robotMode) == 0:
# Sending signal for choice functionality
vrep.simxSetIntegerSignal(clientID, "angX", 0, vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "angY", 0, vrep.simx_opmode_oneshot)
i=-90
for i in range (-90,90,1):
for j in range (-90,90,1):
#angX = input("Enter target angle around X-axis: ")
angX=i
if angX == 'q':
break
#angY = input("Enter target angle around Y-axis: ")
angY=j
if angY == 'q':
break
# Sending target position to the Robot
vrep.simxSetIntegerSignal(clientID, "angX", int(angX), vrep.simx_opmode_oneshot)
vrep.simxSetIntegerSignal(clientID, "angY", int(angY), vrep.simx_opmode_oneshot)
[r,h]=vrep.simxGetObjectHandle(clientID,'Platform_shape0',vrep.simx_opmode_blocking)
positions=vrep.simxGetObjectPosition(clientID,h,-1,vrep.simx_opmode_oneshot)
print(positions)
xa=np.linspace(current_x,positions[1][0])
current_x=positions[1][0]
ya=np.linspace(current_y,positions[1][1])
current_y=positions[1][1]
za=np.linspace(current_z,positions[1][2])
current_y=positions[1][1]
plt.plot(positions[1][0],positions[1][1],positions[1][2])
#