translated[1] = translated[0] * math.sin(
rotation) + translated[1] * math.cos(rotation)
return translated
# main loop
if use_marvelmind: # first, calibrate Realsense by traveling forward for one meter
print("Calibrating....")
time.sleep(5)
position_snapshot() # get current positions
start = np.array([real_x, real_y, marvel_x, marvel_y])
speed = 2000
distance = 1000
rc.SpeedDistanceM1M2(address, speed - steering_nudge,
distance * tickdistanceL, speed + steering_nudge,
distance * tickdistanceR,
1) # go forward 1m at speed 2000
buffers = (0, 0, 0)
while (buffers[1] != 0x80
and buffers[2] != 0x80): #Loop until distance command has completed
buffers = rc.ReadBuffers(address)
print("Calibrating...")
time.sleep(5)
position_snapshot() # get current positions
finish = np.array([real_x, real_y, marvel_x, marvel_y])
calibrate_realsense(start,
finish) # save affine transformation matrix elements
print("Scale:", round(scale, 3), "Translation X ", round(translation_x, 2),
"Y", round(translation_y, 2), "Rotation", round(rotation, 2))
try:
#sleep(0.3)
#motor_direction = ""
elif (motor_direction == "right"):
print ("L")
roboclaw.ForwardM2(address,35)
roboclaw.BackwardM1(address,35)
#sleep(0.3)
#motor_direction = ""
elif (motor_direction == "unsure"):
print("rotating")
roboclaw.BackwardM1(address,40)
roboclaw.ForwardM2(address,40)
elif motor_direction == "neither":
print("Neutral")
print (distance)
roboclaw.SpeedDistanceM1M2(address,0,0,0,0,1)
if (distance >35):
roboclaw.SpeedDistanceM1M2(0x81,-150,10,-150,10,1)#replace with something more bulletfproof
if (distance > 20 and distance <35):
print ('pickup object')
#roboclaw.BackwardM1M2(0x81,64)
#distanceMove = float(distance/35) * 150 #integrate a better value for how far
roboclaw.SpeedDistanceM2(0x80,-1000,500,1)
roboclaw.SpeedDistanceM1M2(0x81,-150,300,-150,300,1)
sleep(3)
roboclaw.SpeedM1M2(0x80,0,0)
roboclaw.SpeedDistanceM2(0x80,1000,500,1)
#rest of the code
sleep(3)
roboclaw.SpeedM1M2(0x80,0,0)
roboclaw.SpeedM1M2(0x81,0,0)
