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)