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)