def paralellMovement(diff, distance):
global angular_direction
spin = angular_direction * abs(diff)
speed = distance
if diff > pi - 0.5:
speed = 0
speed_c = speed / MAX_SPEED
spin_c = spin / MAX_SPIN
if abs(spin) > 0.5:
speed = 0.1
while spin > 1:
spin = spin - 0.5
#print('speed: ', speed, 'spin: ', spin)
if speed_c > spin_c:
speed = speed / speed_c
spin = spin / speed_c
get.postSpeed(spin , speed)
else:
speed = speed / spin_c
spin = spin / spin_c
get.postSpeed(spin , speed)
def singleMovement():
global angular_direction
#MOVE
if abs(diff) < distance / 40:
print('distance ', distance)
get.postSpeed(0, distance)
time.sleep(abs(distance)/10)
get.postSpeed(0, 0)
#ROTATE
else:
print('angular diff ',diff)
get.postSpeed(angular_direction * 0.5, 0)
time.sleep(abs(diff) / 2)
get.postSpeed(0, 0)
robot_position = current_position['Pose']['Position']
deltaX = robot_position['X'] - goal_position['X']
deltaY = robot_position['Y'] - goal_position['Y']
distance = sqrt((deltaX)**2 + (deltaY)**2)
#print('pimary angular diff ',diff * angular_direction)
if distance < 0.8:
i = i + 1
current_instruction = jsonInstruction[i]
goal_position = current_instruction['Pose']['Position']
else:
if starttime is not 0:
print('Started the clock')
starttime = time.time()
diff = (goal_direction - robot_direction)
diff = getDiff(diff)
#print('direction ', math.degrees(robot_direction), ' should be ', math.degrees(goal_direction))
print('%.1f%% of the track' %((i / (len(jsonInstruction) - 1)) * 100))
paralellMovement(diff, distance)
#singleMovement()
print('%.1f%% of the track' %((i / (len(jsonInstruction) - 1)) * 100))
get.postSpeed(0,0)
print('Done!, took %.2f ' %(time.time() - starttime))