def rotateTowardsPoint(self, coordinates: Coordinates):
xCat = coordinates.x - self.__pos.get()['x']
selfPosition = Coordinates(self.__pos.get()['x'],
self.__pos.get()['y'])
Hyp = self.getDistance(selfPosition, coordinates)
angle = math.acos(xCat / Hyp)
if (coordinates.y - selfPosition.y) < 0:
angle = -angle
self.rotateInTheMoreConvenient(angle)
def triangulateTarget2(self) -> Coordinates:
self.checkTrilaterationSpace()
cPose = self.__pos.get()
cDist = self.__prox.get()['near_objects']['target']
print("cx = %s cy = %s" % (cPose['x'], cPose['y']))
# Turn the robot up
self.rotateInTheMoreConvenient(Pi / 2, 0.1, 0.005)
# Reach PY
self.moveFWD()
time.sleep(self.__triangDistance)
self.stop()
yPose = self.__pos.get()
yDist = self.__prox.get()['near_objects']['target']
print("Yx = %s Yy = %s" % (yPose['x'], yPose['y']))
# Get back to PC
self.moveBWD()
time.sleep(self.__triangDistance)
self.stop()
# Reach PX
self.rotateInTheMoreConvenient(0, 0.1, 0.005)
cPose = self.__pos.get()
cDist = self.__prox.get()['near_objects']['target']
print("cx = %s cy = %s" % (cPose['x'], cPose['y']))
self.moveFWD()
time.sleep(self.__triangDistance)
self.stop()
xPose = self.__pos.get()
xDist = self.__prox.get()['near_objects']['target']
print("Xx = %s Xy = %s" % (xPose['x'], xPose['y']))
# Damn calculations
xPoint = (xPose['x'], xPose['y'])
yPoint = (yPose['x'], yPose['y'])
cPoint = (cPose['x'], cPose['y'])
side = abs(xPoint[0] - cPoint[0])
targetCoordinates = findVertex2(cPoint, yPoint, xPoint, cDist, xDist,
yDist, side)
self.__target = Coordinates(targetCoordinates[0], targetCoordinates[1])
print("Tx = %s Ty = %s" % (targetCoordinates[0], targetCoordinates[1]))
self.moveBWD()
time.sleep(self.__triangDistance)
self.stop()
return self.__target
def getRelativeAngles(self, coordinates: Coordinates) -> float(2):
xCat = coordinates.x - self.__pos.get()['x']
selfPosition = Coordinates(self.__pos.get()['x'],
self.__pos.get()['y'])
Hyp = self.getDistance(selfPosition, coordinates)
try:
angle = math.acos(xCat / Hyp)
except:
angle = 4
if (coordinates.y - selfPosition.y) < 0:
angle = -angle
if angle > 0:
return (angle - Pi, angle)
else:
return (angle, angle + Pi)
def getPositionCoordinates(self) -> Coordinates:
position = Coordinates(self.__pos.get()['x'], self.__pos.get()['y'])
return position