if phaseChk == 9:
IM920.Send('P9S')
Other.saveLog(phaseLog, '9', 'Goal Detection Phase Started', time.time() - t_start)
t_GoalDetection_start = time.time()
print('Goal Detection Phase Started {}'.format(time.time() - t_start))
try:
# --- calculate the distance to the goal --- #
direction = Calibration.calculate_direction(lon2,lat2)
goal_distance = direction["distance"]
print('goal_distance = ', goal_distance)
# --- if the distance to the goal is within 5 meters --- #
while goal_distance <= 5:
goal_value[0] = 1
# --- until the goal decision --- #
while goal_value[0] != 0:
goal_value = goaldetection.GoalDetection("/home/pi/photo/photo",200 ,20, 80, 7000)
print("goalflug", goal_value[0], "goalarea",goal_value[1], "goalGAP", goal_value[2], "name", goal_value[3])
Other.saveLog(goalDetectionLog, time.time() - t_start, goal_value, GPS.readGPS())
if goal_value[0] != -1:
# --- if the pixcel error is -30 or less, rotate left --- #
if goal_value[2] <= -30.0:
print('Turn left')
run = pwm_control.Run()
run.turn_left_l()
time.sleep(0.5)
# --- if the pixcel error is 30 or more, rotate right --- #
elif 30 <= goal_value[2]:
print('Turn right')
run = pwm_control.Run()
run.turn_right_l()
time.sleep(0.5)
IM920.Send('P8F')
print('phaseChk = ' + str(phaseChk))
# --- Goal Detection Phase --- #
if phaseChk == 9:
IM920.Send('P9S')
Other.saveLog(phaseLog, '9', 'Goal Detection Phase Started',
time.time() - t_start)
t_GoalDetection_start = time.time()
print('Goal Detection Phase Started {}'.format(time.time() -
t_start))
try:
goalflug = 1
while goalflug != 0:
goalflug, goalarea, goalGAP, photoname = goaldetection.GoalDetection(
"/home/pi/photo/photo", 200, 20, 80, 50000)
print("goalflug", goalflug, "goalarea", goalarea,
"goalGAP", goalGAP, "name", photoname)
if goalGAP <= -30.0:
print('Turn right')
run = pwm_control.Run()
run.turn_right_l()
time.sleep(1)
run.stop()
time.sleep(1)
elif 30 <= goalGAP:
print('Turn left')
run = pwm_control.Run()
run.turn_left_l()
time.sleep(1)