def main():
print "Start"
# cmd.forward(speed=6000, duration = 5)
cmd.forward(speed=5000)
time.sleep(10)
print "Done"
cmd.stop()
def moveDistance(direction, distance):
""" @param direction: 1 -> forward and -1 -> backward """
initleft = readMotorTicks()[0]
initright = readMotorTicks()[1]
cmd.forward(speed=BOT_SPEED)
distDelta = DELTA * BOT_SPEED
while (True):
temp = readMotorTicks()
left = temp[0]
right = temp[1]
distanceMoved = ((left - initleft) +
(right - initright)) * TICK_TO_DIST / 2
print distanceMoved
if (distanceMoved > dis - distDelta):
break
cmd.stop()
cmd.stop()
cmd.stop()
def moveTowardsTarget(Rvec, Tvec):
# Move towards the object for CALIBRATION_SLEEP_TIME
CALIBRATION_TURN_AMT = 0.3
CALIBRATION_SLEEP_TIME = 0.3
# Get the orientation of the object using Rvec and Tvec
R = cv2.Rodrigues(Rvec)
euler_angles = rotationMatrixToEulerAngles(R[0])
z_angle = euler_angles[1]
print "Z-angle: ", z_angle
calibrate_angle_list.append(z_angle)
# Find sign of z_angle
l = len(calibrate_angle_list)
if (l < 7):
return
global calibrationDirection
if (calibrate_angle_list[l - 1] > calibrate_angle_list[l - 7]):
calibrationDirection = -calibrationDirection
cmd.turn(calibrationDirection * CALIBRATION_TURN_AMT)
cmd.turn(calibrationDirection * CALIBRATION_TURN_AMT)
cmd.turn(calibrationDirection * CALIBRATION_TURN_AMT)
time.sleep(CALIBRATION_SLEEP_TIME)
# if(z_angle < -THRESHOLD_ANGLE):
# print "Calibration-> Right Turn"
# cmd.turn(CALIBRATION_TURN_AMT)
# cmd.turn(CALIBRATION_TURN_AMT)
# cmd.turn(CALIBRATION_TURN_AMT)
# time.sleep(CALIBRATION_SLEEP_TIME)
# elif(z_angle > THRESHOLD_ANGLE):
# print "Calibration-> Left Turn"
# cmd.turn(-CALIBRATION_TURN_AMT)
# cmd.turn(-CALIBRATION_TURN_AMT)
# cmd.turn(-CALIBRATION_TURN_AMT)
# time.sleep(CALIBRATION_SLEEP_TIME)
cmd.forward(speed=BOT_SPEED)
time.sleep(CALIBRATION_SLEEP_TIME)
return
def rotate(angle):
"""
angle -> signed double value in degree
direction: 1 -> left and -1 -> right
"""
sign = lambda a: (a > 0) - (a < 0)
direction = sign(angle)
angle = abs(angle)
initYaw = mc.getYaw()
cmd.forward(speed=BOT_SPEED)
if (direction == 1):
cmd.turn(0.5)
elif (direction == -1):
cmd.turn(-0.5)
while (True):
yaw = mc.getYaw()
print yaw
if (abs(initYaw - yaw) > angle - angleDelta):
break
cmd.stop()
cmd.stop()
def main():
global calibrateMode
global turnMode
rbt.connect(motorcontroller())
mc.initsocket()
print mc.getYaw()
exit(0)
#cs.write_servo(0)
undetectedIterations = 0
turnState = 0
turnId = -1
doneList = set()
prev_id = -1
# # Get rid of initial lag
# print "Initial"
# (frame, markers) = getMarkersFromCurrentFrame()
# if len(markers) > 0:
# markers[0].draw(frame, np.array([255, 0, 0]), 10, True)
# cv2.imshow("frame", frame)
# cv2.waitKey(10)
# time.sleep(5)
print "Start bot"
cmd.forward(speed=BOT_SPEED)
time.sleep(SLEEP_TIME)
while (True):
print "Start loop"
# get an image
# markers = findImageArucoParams('ar2.png')
(frame, markers) = getMarkersFromCurrentFrame()
# Process markers
print "Markers detected", len(markers)
if len(markers) > 0:
undetectedIterations = 0
marker = markers[0]
for m in markers:
if not (m in doneList):
marker = m
marker.calculateExtrinsics(marker_size, camparam)
# min_d = markers[0].Tvec[2][0]
# marker = markers[0]
# for m in markers:
# m.calculateExtrinsics(marker_size, camparam)
# current_distance = m.Tvec[2][0]
# if (current_distance < min_d):
# min_d = current_distanceexit(0)
# marker = m
# # Draw marker on observedframe image
# marker.draw(frame, np.array([255, 0, 0]), 10, True)
print "Id:", marker.id
# print "Rvec:\n", marker.Rvec
# print "Tvec:\n", marker.Tvec
print "Distance:\n", marker.Tvec[2][0]
if calibrateMode:
# ---------------- CALIBRATE MODE ---------------
# take action according to distance and id of detected marker
marker_distance = marker.Tvec[2][0]
if (marker_distance < CUTOFF_DISTANCE):
print "******************"
print "Detected threshold"
print "******************"
calibrateMode = False
turnMode = True
else:
moveTowardsTarget(marker.Rvec, marker.Tvec)
if turnMode:
# ---------------- TURN MODE ---------------
# turnId = marker.id
while True:
toBreak = False
rotateAndMove(bool(turnState))
(frame, markers) = getMarkersFromCurrentFrame()
global calibrate_angle_list
calibrate_angle_list = []
doneList.add(marker.id)
print "Added to doneList: ", marker.id
print doneList
if len(markers) > 0:
for marker_curr in markers:
print "Seeing current marker_curr"
marker_curr.calculateExtrinsics(
marker_size, camparam)
R = cv2.Rodrigues(marker_curr.Rvec)
euler_angles = rotationMatrixToEulerAngles(R[0])
z_angle = euler_angles[1]
if (not (marker_curr.id in doneList)
and z_angle < 0.15):
print "Detected new marker"
rotateAndMove(bool(turnState))
toBreak = True
break
else:
print "Undetected", undetectedIterations
undetectedIterations = undetectedIterations + 1
if (undetectedIterations > 30):
cmd.stop()
if (toBreak):
break
turnMode = False
calibrateMode = True
print "Exited turn mode"
cmd.forward(speed=BOT_SPEED)
# Change turnstate for next turn
turnState = turnState ^ 1
print "Changed turnstate to: ", turnState
else:
print "Undetected", undetectedIterations
undetectedIterations = undetectedIterations + 1
if (undetectedIterations > 20):
cmd.stop()
# # show frame
# cv2.imshow("frame", frame)
# cv2.waitKey(int(SLEEP_TIME * 1000))
time.sleep(SLEEP_TIME)
def moveForward():
cmd.forward(speed=BOT_SPEED)