from pymorse import Morse
import sys
import math
try:
from morse.builder import *
except ImportError:
pass
morse = Morse()
motion = morse.robot.motion
pose = morse.robot.pose
goal_x = 5
goal_y = 5
print("At first ,the robot is currently at: %s" % pose.get())
morse.sleep(1)
motion.publish({'x': 5.0, 'y': 5.0, 'z': 0.0, 'tolerance': 0.1, 'speed': 2.0})
morse.sleep(10)
print("The robot is currently at: %s" % pose.get())
import math
try:
from morse.builder import *
except ImportError:
pass
morse = Morse()
motion = morse.robot.motion
pose = morse.robot.pose
goal_x = 5
goal_y = -5
print("At first ,the robot is currently at: %s" % pose.get())
morse.sleep(0.1)
while True:
position = pose.get()
current_x = position['x']
current_y = position['y']
current_theta = position['yaw']
delta_x = goal_x - current_x
delta_y = goal_y - current_y
dis_goal = (delta_x**2 + delta_y**2)**0.5
if delta_y >= 0.0:
temp_theta = math.acos(delta_x / dis_goal)
else:
temp_theta = -math.acos(delta_x / dis_goal)
