print(target_info.ticks)
if target_info.ticks > 100:
state = States.AIM_TARGET
else:
state = States.FIND_TARGET
elif state == States.AIM_TARGET:
print("Aiming at target")
# Get obj info from tracker
pos = (target_info.x_for_lowest_y, target_info.lowest_y)
p, t = tracker.centerHeadToPoint(pos)
print(p, t)
dm.headMoveByAngle(p, t)
# Rotate body until pan angle matches shooting angle
dm.walkSetVelocities(-1.5, 0.0, -8.0)
dm.walkStart()
# STATE CHANGE
if p > SHOOTING_PAN:
dm.walkStop()
dm.playMotion(72) # Ready bow motion
dm.motionLoadINI(MOTION_INI)
state = States.SHOOT
keys = pygame.key.get_pressed()
cur_pan = dm.headGetPan()
cur_tilt = dm.headGetTilt()
# Amount to increment in each angle
tilt_i = 0
pan_i = 0
mod = 1
if keys[K_LSHIFT]:
mod = 0.25
if keys[K_j]:
tilt_i += -1 * mod
if keys[K_u]:
tilt_i += +1 * mod
if keys[K_h]:
pan_i += 1 * mod
if keys[K_k]:
pan_i += -1 * mod
if keys[K_o]:
dm.headMoveToHome()
else:
dm.headMoveByAngle(cur_pan + pan_i, cur_tilt + tilt_i)
dm.walkSetVelocities(X_AMPLITUDE, Y_AMPLITUDE, A_AMPLITUDE)
print("X: {}, Y: {}, A: {}".format(X_AMPLITUDE, Y_AMPLITUDE, A_AMPLITUDE))
sleep(0.01)
dm.headMoveByAngle(p, t)
else:
dm.walkStop()
print("Object not detected this iteration.")
continue # Skip this loop
if state == States.RUNNING_FORWARD:
print("walk forward")
if ticks % UPDATE_RATE == 0:
x_speed = updateLinearSpeed(x_speed)
if p > MAX_P:
p = MAX_P
dm.walkSetVelocities(5.0, 0.0, p)
elif p < -MAX_P:
p = -MAX_P
dm.walkSetVelocities(5.0, 0.0, p)
else:
dm.walkSetVelocities(x_speed, 0.0, p)
dm.walkStart()
if area > CROSSED_LINE_OBJ_SIZE:
# Changing State
state = States.RUNNING_BACKWARDS
#dm.walkSetHipPitchOffset(-17.9)
dm.walkStop()
sleep(1)
x_speed = 0.0 # Initial linear walking speed
while True:
pos = tracker.getObjPosFromBGR()
if pos is not None:
dab_ticks = 0
p, t = tracker.centerHeadToPoint(pos)
dm.headMoveByAngle(p, t)
if ticks % UPDATE_RATE == 0:
x_speed = updateLinearSpeed(x_speed)
if p > MAX_P:
p = MAX_P
dm.walkSetVelocities(6.0, 0.0, p)
elif p < -MAX_P:
p = -MAX_P
dm.walkSetVelocities(6.0, 0.0, p)
else:
dm.walkSetVelocities(x_speed, 0.0, p)
dm.walkStart()
else:
dm.walkStop()
print(ticks)
print(x_speed)
ticks += 1
dm.headMoveByAngle(p, t)
else:
dm.walkStop()
print("Object not detected this iteration.")
continue # Skip this loop
if current_state == States.RUNNING_FORWARD:
print("walk forward")
if ticks % UPDATE_RATE == 0:
x_speed = updateLinearSpeed(x_speed)
if p > MAX_P:
p = MAX_P
dm.walkSetVelocities(6.0, 0.0, p)
elif p < -MAX_P:
p = -MAX_P
dm.walkSetVelocities(6.0, 0.0, p)
else:
dm.walkSetVelocities(x_speed, 0.0, p)
dm.walkStart()
if area > CROSSED_LINE_OBJ_SIZE:
# Changing State
current_state = States.RUNNING_BACKWARDS
#dm.walkSetZOffset(40.0)
#dm.walkSetPeriodTime(550.0)
dm.walkStop()
sleep(0.2)