def Mission(photo_path1,photo_path2):
photo_path1 = ''
photo_path2 = ''
try:
# --- Save photos in two resolution --- #
camera = picamera.PiCamera()
camera.resolution = (320,240)
camera.capture(photo_path1)
camera.resolution = (640,480)
camera.capture(photo_path2)
# --- Read two images --- #
img1 = cv2.imread(photo_path1)
img2 = cv2.imread(photo_path2)
# --- hight resolution --- #
dst1 = cv2.resize(img1, (img1.shape[1]*4, img1.shape[0]*4), interpolation=cv2.INTER_NEAREST)
dst2 = cv2.resize(img1, (img1.shape[1]*4, img1.shape[0]*4), interpolation=cv2.INTER_LINEAR)
dst3 = cv2.resize(img1, (img1.shape[1]*4, img1.shape[0]*4), interpolation=cv2.INTER_CUBIC)
# --- Save high resolution images --- #
filepath = ''
filepath = Other.fileName('/home/pi/photo/nearest','jpg') # nearest
cv2.imwrite(filepath, dst1)
filepath = Other.fileName('/home/pi/photo/linear','jpg') # linear
cv2.imwrite(filepath, dst2)
filepath = Other.fileName('/home/pi/photo/cubic','jpg') # cubic
cv2.imwrite(filepath, dst3)
# --- Convert to grayscale --- #
gray0 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
gray1 = cv2.cvtColor(dst1, cv2.COLOR_BGR2GRAY)
gray2 = cv2.cvtColor(dst2, cv2.COLOR_BGR2GRAY)
gray3 = cv2.cvtColor(dst3, cv2.COLOR_BGR2GRAY)
# --- Calculate ssim --- #
print("ssim_n: " + str(measurement_nearest(compare_ssim, gray0 = gray0, gray1 = gray1)))
print("ssim_l: " + str(measurement_linear(compare_ssim, gray0 = gray0, gray2 = gray2)))
print("ssim_c: " + str(measurement_cubic(compare_ssim, gray0 = gray0, gray3 = gray3)))
except picamera.exc.PiCameraMMALError:
photo_path = 'Null'
time.sleep(1)
except:
print(traceback.format_exc())
time.sleep(0.1)
return photo_path
def Capture(path, width = 320, height = 240): filepath = "" try: with picamera.PiCamera() as camera: #camera.rotation = 270 camera.resolution = (width,height) filepath = Other.fileName(path,"jpg") camera.capture(filepath) except picamera.exc.PiCameraMMALError: filepath = "Null" time.sleep(0.8) except: print(traceback.format_exc()) time.sleep(0.1) filepath = "Null" return filepath
def calibration():
global ellipseScale
mPL, mPR, mPS = 0, 0, 0
dt = 0.05
roll = 0
time.sleep(1)
fileCal = Other.fileName(calibrationLog, "txt")
print("Calibration Start")
IM920.Send("P7C")
Motor.motor(30, 0, 1)
t_cal_start = time.time()
while(math.fabs(roll) <= 600):
if(time.time() - t_cal_start >= 10):
calData = ellipseScale
Motor.motor(0, 0, 1)
Motor.motor(-60, -60, 2)
print("Calibration Failed")
Other.saveLog(stuckLog, time.time() - t_start, GPS.readGPS(), 3, 0)
Other.saveLog(fileCal, time.time() - t_start, "Calibration Failed")
break
mPL, mPR, mPS, bmx055data = pidControl.pidSpin(-300, 1.0, 1.1, 0.2, dt)
with open(fileCal, 'a') as f:
for i in range(6, 8):
#print(str(bmx055data[i]) + "\t", end="")
f.write(str(bmx055data[i]) + "\t")
#print()
f.write("\n")
roll = roll + bmx055data[5] * dt
Motor.motor(mPL, mPR, dt, 1)
else:
Motor.motor(0, 0, 1)
calData = Calibration.Calibration(fileCal)
Other.saveLog(fileCal, calData)
Other.saveLog(fileCal, time.time() - t_start)
Motor.motor(0, 0, 1)
setup()
print("Calibration Finished")
return calData
time.time() - t_start, GPS.readGPS(), photoName,
paraExsist, paraArea)
Other.saveLog(paraAvoidanceLog,
time.time() - t_start, GPS.readGPS(),
"ParaAvoidance Finished")
IM920.Send("P6F")
# ------------------- Running Phase ------------------- #
if (phaseChk <= 7):
Other.saveLog(phaseLog, "7", "Running Phase Started",
time.time() - t_start)
print("Running Phase Started")
IM920.Send("P7S")
# --- Calibration --- #
fileCal = Other.fileName(calibrationLog, "txt")
Motor.motor(60, 0, 2)
Calibration.readCalData(fileCal)
Motor.motor(0, 0, 1)
ellipseScale = Calibration.Calibration(fileCal)
Other.saveLog(fileCal, ellipseScale)
while (not RunningGPS.checkGPSstatus(gpsData)):
gpsData = GPS.readGPS()
time.sleep(1)
t_calib_origin = time.time()
t_takePhoto_start = time.time()
while (disGoal >= 5):
# --- Get GPS Data --- #
if (RunningGPS.checkGPSstatus(gpsData)):
(bmx055data[6] - calData[0]) / calData[2],
(bmx055data[7] - calData[1]) / calData[3]) * 180 / math.pi
if __name__ == '__main__':
try:
BMX055.bmx055_setup()
targetVal = 300
Kp = 0.9
Ki = 1.1
Kd = 0.2
dt = 0.05
mPL, mPR, mPS = 0, 0, 0
roll = 0
time.sleep(1)
fileP = Other.fileName("calData", "txt")
print("Calibration Start")
Motor.motor(0, 30, 1)
while (math.fabs(roll) <= 720):
mPL, mPR, mPS, bmx055data = pidControl.pidSpin(
targetVal, Kp, Ki, Kd, dt)
with open(fileP, 'a') as f:
for i in range(6, 8):
#print(str(bmx055data[i]) + "\t", end="")
f.write(str(bmx055data[i]) + "\t")
#print()
f.write("\n")
roll = roll + bmx055data[5] * dt
Motor.motor(mPL, mPR, dt, 1)
Motor.motor(0, 0, 1)
