def calculate_direction(lon2, lat2): #--- read GPS data ---# try: while True: GPS_data = GPS.readGPS() lat1 = GPS_data[1] lon1 = GPS_data[2] #print(GPS_data) IM920.Send(str(GPS_data)) #print("lat1 = "+str(lat1)) #print("lon1 = "+str(lon1)) #time.sleep(1) if lat1 != -1.0 and lat1 != 0.0: break except KeyboardInterrupt: GPS.closeGPS() print("\r\nKeyboard Intruppted, Serial Closed") except: GPS.closeGPS() print(traceback.format_exc()) #--- calculate angle to goal ---# direction = gps_navigate.vincenty_inverse(lat1, lon1, lat2, lon2) return direction
def transmitdata(): for i in range(3): IM920.Send("M") IM920.Send("M") ack = str(IM920.read()) comData = ack.rsplit(":", 1) ack = comData[1:] if(ack == ['4D']): mode = 0 else: mode = 1 break print(mode) time.sleep(0.5) print(mode) time.sleep(3) return mode
def sendPhoto(photoPath): returnVal = 0 if os.path.exists(photoPath): img = cv2.imread(photoPath) img = cv2.resize(img, (80, 60)) #sendPhotoName = Other.fileName(sendPhotoPath, 'jpg') #cv2.imwrite(sendPhotoName, img) #print("Start") t = time.time() count = 0 for i in range(len(img)): for j in range(len(img[i])): num = int(img[i][j][2]) + int(img[i][j][1]) * (10**3) + int( img[i][j][0]) * (10**6) + j * (10**9) + i * (10**12) sendStatus = IM920.Send(str(num)) print(str(num)) t_send = time.time() while (sendStatus != b'OK\r\n'): if (time.time() - t_send > 3): break else: time.sleep(0.01) sendStatus = IM920.Send(str(num)) #print("Send") count = count + 1 print(num) print(count) time.sleep(0.1) IM920.Send("MF") time.sleep(0.1) IM920.Send("MF") time.sleep(0.1) IM920.Send("MF") print(time.time() - t) print("Finish") returnVal = 1 else: print("File Not Found") returnVal = 0 return returnVal
def LongtransmitPhoto(sendimgName = ""): global t_start photo = "" IM920.Send("G" + str(nLat) + " " + str(nLon)) IM920.Send("G" + str(nLat) + " " + str(nLon)) IM920.Send("G" + str(nLat) + " " + str(nLon)) if sendimgName == "": IM920.Strt("2") #distancemode! time.sleep(1) Motor.motor(15, 15, 0.9) Motor.motor(0, 0, 1) takePhoto() print("Send Photo") sendPhoto.sendPhoto(photoName) print("Send Photo Finished") Other.saveLog(sendPhotoLog, time.time() - t_start, GPS.readGPS(), photoName) IM920.Strt("2") #distancemode time.sleep(1) else: # 1st time transmit print("airphoto") IM920.Strt("2") #distancemode! time.sleep(1) Motor.motor(15, 15, 0.9) Motor.motor(0, 0, 1) print("Send Photo") sendPhoto.sendPhoto(sendimgName) print("Send Photo Finished") Other.saveLog(sendPhotoLog, time.time() - t_start, GPS.readGPS(), sendimgName) IM920.Strt("2") #distancemode time.sleep(1) IM920.Send("G" + str(nLat) + " " + str(nLon)) IM920.Send("G" + str(nLat) + " " + str(nLon)) IM920.Send("G" + str(nLat) + " " + str(nLon))
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
def setup(): global phaseChk global startPosStatus pi.set_mode(17,pigpio.OUTPUT) pi.set_mode(22,pigpio.OUTPUT) pi.write(22,1) #IM920 Turn On IM920.Strt("2") #distance mode pi.write(17,0) #Outcasing Turn Off time.sleep(1) BME280.bme280_setup() BME280.bme280_calib_param() BMX055.bmx055_setup() TSL2561.tsl2561_setup() GPS.openGPS() with open(phaseLog, 'a') as f: pass #if it is End to End Test, then try: phaseChk = int(Other.phaseCheck(phaseLog)) except: phaseChk = 0 #if it is debug #phaseChk = 8 if phaseChk == 0: Other.saveLog(positionLog, "Goal", gLat, gLon, "\t") startPosStatus = 1 else: Other.saveLog(positionLog, "\t") if(Other.positionCheck(positionLog) == [0.0, 0.0]): print("Not Logged Start Position") startPosStatus = 1 else: rsLat, rsLon = Other.positionCheck(positionLog) print(rsLat, rsLon) startPosStatus = 0
sys.path.append('/home/pi/git/kimuralab/Other') import binascii import difflib import pigpio import serial import traceback import time import GPS import IM920 import RunningGPS import Other if __name__ == '__main__': try: GPS.openGPS() while 1: gpsData = GPS.readGPS() IM920.Send("G " + str(gpsData[1]) + " " + str(gpsData[2])) print("G " + str(gpsData[1]) + " " + str(gpsData[2])) #if(RunningGPS.checkGPSstatus(gpsData)): # Other.saveLog("logGPS.txt", time.time(), gpsData) time.sleep(1) except KeyboardInterrupt: GPS.closeGPS() print("\r\nKeyboard Intruppted, Serial Closed") except: GPS.closeGPS() print("\r\nError, Serial Closed") print(traceback.format_exc())
def timer(t): global cond time.sleep(t) cond = False if __name__ == '__main__' try: print('Program Start {0}'.format(time.time())) t_start = time.time() # --- Setup Phase --- # setup() print('Start {0}'.format(phaseChk)) if phaseChk <= 1: IM920.Send('P1S') Other.SaveLog(phaseLog, '1', 'Program Started', time.time() - t_atart) IM920.Send('P1F') # --- Sleep Phase --- # if phaseChk <= 2: Im920.Send('P2S') t_sleep_start = time.time() Other.SaveLog(phaseLog, '2', 'Sleep Phase Started', time.time - t_start) Im920.Send('P2F') # --- Release Phase --- # if phaseChk <= 3: IM920.Send('P3S') Other.SaveLog(phaseLog, '3', 'Release Phase Started', time.time - t_start) t_relase_start = tim.time()
run = pwm_control.Run() run.turn_right_l() time.sleep(0.1) run = pwm_control.Run() run.straight_n() if __name__ == '__main__': try: print('Program Start {0}'.format(time.time())) t_start = time.time() # --- Setup Phase --- # setup() print('Start {0}'.format(phaseChk)) if phaseChk == 1: IM920.Send('P1S') Other.saveLog(phaseLog, '1', 'Program Started', time.time() - t_start) IM920.Send('P1F') phaseChk += 7 print('phaseChk = '+str(phaseChk)) ''' # --- Sleep Phase --- # if phaseChk == 2: IM920.Send('P2S') t_sleep_start = time.time() Other.saveLog(phaseLog, '2', 'Sleep Phase Started', time.time() - t_start) IM920.Send('P2F') phaseChk += 1 print('phaseChk = '+str(phaseChk))
import sys import IM920 if __name__ == "__main__": args = sys.argv if (args[1] == "RDCH"): data = IM920.Rdch() elif (args[1] == "STCH"): data = IM920.Stch(args[2]) elif (args[1] == "ENWR"): data = IM920.Enwr() elif (args[1] == "RDID"): data = IM920.Rdid() #elif(args[1] == "SBRT"): # data = IM920.Sbrt(args[2]) elif (args[1] == "RDRT"): data = IM920.Rdrt() elif (args[1] == "SRID"): data = IM920.Srid(args[2]) elif (args[1] == "RRID"): data = IM920.Rrid() elif (args[1] == "STRT"): data = IM920.Strt(args[2]) elif (args[1] == "RDRT"): data = IM920.Rdrt(args[2]) elif (args[1] == "RPRM"): data = IM920.Rprm() else: data = "None" print(data)
pi.write(22, 1) #IM920 Turn On pi.write(17, 0) Motor.motor(0, 0, 1) Motor.motor_stop() if __name__ == "__main__": try: print("Program Start {0}".format(time.time())) t_start = time.time() #-----setup phase ---------# setup() print("Start Phase is {0}".format(phaseChk)) if (phaseChk <= 1): IM920.Send("P1S") Other.saveLog(phaseLog, "1", "Program Started", time.time() - t_start) IM920.Send("P1F") # ------------------- Sleep Phase --------------------- # if (phaseChk <= 2): Other.saveLog(phaseLog, "2", "Sleep Phase Started", time.time() - t_start) print("Sleep Phase Started {0}".format(time.time() - t_start)) IM920.Send("P2S") #pi.write(22, 0) #IM920 Turn Off t_sleep_start = time.time() # --- Sleep --- # while (time.time() - t_sleep_start <= t_sleep):
def timer(t): global cond time.sleep(t) cond = False if __name__ == '__main__': try: print('Program Start {0}'.format(time.time())) t_start = time.time() # --- Setup Phase --- # setup() print('Start {0}'.format(phaseChk)) if phaseChk == 1: IM920.Send('P1S') Other.saveLog(phaseLog, '1', 'Program Started', time.time() - t_start) IM920.Send('P1F') phaseChk += 1 print('phaseChk = ' + str(phaseChk)) # --- Release Phase --- # if phaseChk == 2: IM920.Send('P2S') Other.saveLog(phaseLog, '2', 'Release Phase Started', time.time() - t_start) t_release_start = time.time() print('Release Phase Started {}'.format(time.time() - t_start)) # --- Release Judgement, 'while' is until timeout --- # while time.time() - t_release_start <= t_release:
def beacon(): IM920.Strt("1") #fastmode convert IM920.Send("B") IM920.Strt("2") # distancemode
pi.write(22, 1) #IM920 Turn On pi.write(17,0) Motor.motor(0, 0, 1) Motor.motor_stop() if __name__ == "__main__": try: print("\nProgram Start {0}".format(time.time())) t_start = time.time() #-----setup phase ---------# setup() print("\n\nStart Phase is {0}".format(phaseChk)) if(phaseChk <= 1): IM920.Send("P1S") Other.saveLog(phaseLog, "1", "Program Started", time.time() - t_start) IM920.Send("P1F") # ------------------- Sleep Phase --------------------- # if(phaseChk <= 2): Other.saveLog(phaseLog, "2", "Sleep Phase Started", time.time() - t_start) print("\nSleep Phase Started {0}".format(time.time() - t_start)) IM920.Send("P2S") pi.write(22, 0) #IM920 Turn Off print("IM920 Turn Off") t_sleep_start = time.time() # --- Sleep --- # while(time.time() - t_sleep_start <= t_sleep): Other.saveLog(sleepLog, time.time() - t_start, GPS.readGPS(), BME280.bme280_read(), TSL2561.readLux(), BMX055.bmx055_read())
def sendphoto(byte): global count i, j, k = 0, 0, 0 with open("/home/pi/log/communicationlog.txt","w")as f: f.write(str(byte)) for i in range(0,len(byte),64): data = IM920.IMSend(byte[i:i+64]) cng = IM920.Reception() for j in range(5): print(count, j) if(cng == ""): data = IM920.IMSend(byte[i:i+64]) cng = IM920.Reception() else: time.sleep(0.1) count+=1 break #print(j) if j == 4: break #print(i,'/',len(byte)) amari=len(byte)-i amari=len(byte)-i IM920.IMSend(byte[i:i+amari]) data=IM920.IMSend(byte[i:i+amari]) cng=IM920.Reception() for k in range(5): print(count,k) if(cng==""): data=IM920.IMSend(byte[i:i+amari]) cng=IM920.Reception() else: break #print(amari,'/',64) #print(str(byte[i:i+amari])) IM920.Send("MFend") time.sleep(1) IM920.Send("MFend") time.sleep(1) IM920.Send("MFend")
if __name__ == "__main__": try: pi.write(22, 1) GPS.openGPS() BME280.bme280_setup() BME280.bme280_calib_param() BMX055.bmx055_setup() TSL2561.tsl2561_setup() for i in range(10): gpsData = GPS.readGPS() luxData = TSL2561.readLux() bmeData = BME280.bme280_read() bmxData = BMX055.bmx055_read() photo = Capture.Capture("photo/photo") IM920.Strt("1") com1Data = IM920.Send("P" + str(1)) IM920.Strt("2") com2Data = IM920.Send("P" + str(2)) print("GPS\t\t", gpsData) print("TSL2561\t\t", luxData) print("BME280\t\t", bmeData) print("BMX055\t\t", bmxData) print("photo\t\t", photo) print("fast mode\t", com1Data) print("distance mode\t", com2Data) print("\n\n") """ for j in range(1): pi.write(22, 1) GPS.openGPS()
def close(): GPS.closeGPS() Motor.motor(0, 0, 2) Motor.motor_stop() pi.write(17, 0) if __name__ == "__main__": try: t_start = time.time() # ------------------- Setup Fhase ------------------- # setup() print("Program Start {0}".format(time.time())) print(phaseChk) if (phaseChk <= 1): IM920.Send("P1S") Other.saveLog(phaseLog, "1", "Program Started", time.time() - t_start) IM920.Send("P1F") # ------------------- Sleep Phase --------------------- # Other.saveLog(phaseLog, "2", "Sleep Phase Started", time.time() - t_start) if (phaseChk <= 2): print("Sleep Phase Started {0}".format(time.time())) IM920.Send("P2S") pi.write(22, 0) #IM920 Turn Off t_wait_start = time.time() while (time.time() - t_wait_start <= t_sleep): bme280Data = BME280.bme280_read() #Read BME280 data bmx055Data = BMX055.bmx055_read() #Read BMX055 data
pi = pigpio.pi() if __name__ == '__main__': try: GPS.openGPS() pi = pigpio.pi() pi.set_mode(22, pigpio.OUTPUT) print('IM920 poweron') pi.write(22, 1) print('Send Start') while 1: gpsdata = GPS.readGPS() #IM920.Srid(19200,'3156') #ペアリング #IM920.Erid(19200) #削除 #IM920.Send('Hello') #文字列送信 IM920.Send(str(gpsdata)) #print(IM920.Reception(19200)) #文字列受信 #IM920.Repeater(19200) #中継機化 #IM920.Rdid(19200) #固有ID #IM920.Stch(19200, '01') #無線通信チャンネルの設定 #IM920.Rdch(19200) #無線通信チャンネルの読み出し #IM920.Rdrs(19200) #RSSI値(現在の信号強度レベル)読み出し #IM920.Stpo(19200, '3') #通信出力の設定 #IM920.Rdpo(19200) #通信出力の読み出し #IM920.Strt(19200, '2') #無線通信速度の設定 #IM920.Rdrt(19200) #無線通信速度の読み出 #IM920.Rprm(19200) #パラメータ一括読み出し #IM920.Sbrt(19200, '4') #ボーレート設定 sleep(3) except: pass
x = 0 amari = 0 img = cv2.imread("/home/pi/git/kimuralab/SensorModuleTest/Camera/photo80.jpg", 0) byte = convertIMG2BYTES.IMGtoBYTES(img) with open("soushinlog.txt", "w") as f: f.write(str(byte)) #print(byte[0:100]) for i in range(0, len(byte), 64): #if i%640==0: #print("sleep") #time.sleep(0.5) if i == 3840: time.sleep(3) data = IM920.IMSend(byte[i:i + 64]) cng = IM920.Reception() if (cng == ""): data = IM920.IMSend(byte[i:i + 64]) cng = IM920.Reception() time.sleep(0.1) count += 1 print(count, data) print(i, '/', len(byte)) # print(str(byte[i:i+63])) amari = len(byte) - i # print(str(amari)) amari = len(byte) - i IM920.IMSend(byte[i:i + amari]) print(amari, '/', 64)
#!/usr/bin/env python # -*- coding: UTF-8 -*- #IM920のデバッグ用プログラム import IM920 #文字列受信 self.imbool = False if IM920.Reception(19200): self.imbool = True print(IM920.Reception(19200)) #文字列送信 IM920.Send(19200, "Hallo")
def timer(t): global cond time.sleep(t) cond = False if __name__ == '__main__': try: print('Program Start {0}'.format(time.time())) t_start = time.time() # --- Setup Phase --- # setup() print('Start {0}'.format(phaseChk)) if phaseChk == 1: IM920.Send('P1S') Other.saveLog(phaseLog, '1', 'Program Started', time.time() - t_start) IM920.Send('P1F') phaseChk += 7 print('phaseChk = ' + str(phaseChk)) ''' # --- Sleep Phase --- # if phaseChk == 2: IM920.Send('P2S') t_sleep_start = time.time() Other.saveLog(phaseLog, '2', 'Sleep Phase Started', time.time() - t_start) IM920.Send('P2F') phaseChk += 1 print('phaseChk = '+str(phaseChk))
def gpsSend(gpsData): IM920.Send('g' + str(gpsData[0]) + ',' + str(gpsData[1]) + ',' + str(gpsData[2]) + ',' + str(gpsData[3]) + ',' + str(gpsData[4]) + ',' + str(gpsData[5]))
run = pwm_control.Run() run.turn_right_l() time.sleep(0.1) run = pwm_control.Run() run.straight_n() if __name__ == '__main__': try: print('Program Start {0}'.format(time.time())) t_start = time.time() # --- Setup Phase --- # setup() print('Start {0}'.format(phaseChk)) if phaseChk == 1: IM920.Send('P1S') Other.saveLog(phaseLog, '1', 'Program Started', time.time() - t_start) IM920.Send('P1F') phaseChk += 1 print('phaseChk = '+str(phaseChk)) # --- Sleep Phase --- # if phaseChk == 2: IM920.Send('P2S') t_sleep_start = time.time() Other.saveLog(phaseLog, '2', 'Sleep Phase Started', time.time() - t_start) IM920.Send('P2F') phaseChk += 1 print('phaseChk = '+str(phaseChk))
#!/usr/bin/env python # -*- coding: UTF-8 -*- #IM920のデバッグ用プログラム import IM920 #ペアリング #IM920.Srid(19200,'52C0') #削除 #IM920.Erid(19200) #文字列送信 #IM920.Send(19200, "hogre") #文字列受信 #print(IM920.Reception(19200)) #中継機化 #IM920.Repeater(19200) #固有ID IM920.Rdid(19200) #ボーレート設定 #IM920.Sbrt(19200, '4')
pi.write(22, 1) #IM920 Turn On pi.write(17, 0) Motor.motor(0, 0, 1) Motor.motor_stop() if __name__ == "__main__": try: print("Program Start {0}".format(time.time())) t_start = time.time() #-----setup phase ---------# setup() print("Start Phase is {0}".format(phaseChk)) if (phaseChk <= 1): IM920.Send("P1S") Other.saveLog(phaseLog, "1", "Program Started", time.time() - t_start) IM920.Send("P1F") # ------------------- Sleep Phase --------------------- # if (phaseChk <= 2): Other.saveLog(phaseLog, "2", "Sleep Phase Started", time.time() - t_start) print("Sleep Phase Started {0}".format(time.time() - t_start)) IM920.Send("P2S") #pi.write(22, 0) #IM920 Turn Off t_sleep_start = time.time() # --- Sleep --- # while (time.time() - t_sleep_start <= t_sleep):
def close(): GPS.closeGPS() pi.write(22, 0) pi.write(17, 0) Motor.motor(0, 0, 1) Motor.motor_stop() if __name__ == "__main__": try: t_start = time.time() # ------------------- Setup Phase --------------------- # print("Program Start {0}".format(time.time())) setup() print(phaseChk) IM920.Send("Start") # ------------------- Waiting Phase --------------------- # Other.saveLog(phaseLog, "2", "Waiting Phase Started", time.time() - t_start) if (phaseChk <= 2): t_wait_start = time.time() while (time.time() - t_wait_start <= t_setup): Other.saveLog(waitingLog, time.time() - t_start, GPS.readGPS(), BME280.bme280_read(), TSL2561.readLux(), BMX055.bmx055_read()) print("Waiting") IM920.Send("Sleep") time.sleep(1) IM920.Send("Waiting Finished")
pi.write(22, 1) #IM920 Turn On pi.write(17, 0) Motor.motor(0, 0, 1) Motor.motor_stop() if __name__ == "__main__": try: print("Program Start {0}".format(time.time())) t_start = time.time() #-----setup phase ---------# setup() print("Start Phase is {0}".format(phaseChk)) if (phaseChk <= 1): IM920.Send("P1S") Other.saveLog(phaseLog, "1", "Program Started", time.time() - t_start) IM920.Send("P1F") # ------------------- Sleep Phase --------------------- # if (phaseChk <= 2): Other.saveLog(phaseLog, "2", "Sleep Phase Started", time.time() - t_start) print("Sleep Phase Started {0}".format(time.time() - t_start)) IM920.Send("P2S") pi.write(22, 1) #IM920 Turn Off t_sleep_start = time.time() # --- Sleep --- #
while (sendStatus != b'OK\r\n'): if (time.time() - t_send > 3): break else: time.sleep(0.01) sendStatus = IM920.Send(str(num)) #print("Send") count = count + 1 print(num) print(count) time.sleep(0.1) IM920.Send("MF") time.sleep(0.1) IM920.Send("MF") time.sleep(0.1) IM920.Send("MF") print(time.time() - t) print("Finish") returnVal = 1 else: print("File Not Found") returnVal = 0 return returnVal if __name__ == "__main__": IM920.Strt("1") photoName = "sendPhoto.jpg" sendPhotoStatus = sendPhoto(photoName) print(sendPhotoStatus)
print('turn left to adjustment') run = pwm_control.Run() run.turn_left_l() time.sleep(0.5) #--- 345 < azimuth <= 360 ---# elif azimuth + 15 > 360: azimuth -= 360 if θ > azimuth + 15: print('turn left to adjustment') run = pwm_control.Run() run.turn_left_l() time.sleep(0.5) #--- stuck detection ---# moved_distance = Stuck.stuck_detection2(longitude_past,latitude_past) if moved_distance >= 15: IM920.Send("Rover is moving now") print('Rover is moving now') stuck_count = 0 location = Stuck.stuck_detection1() longitude_past = location[0] latitude_past = location[1] else: #--- stuck escape ---# IM920.Send("Rover stucks now") print('Stuck!') Stuck.stuck_escape() loop_count -= 1 stuck_count += 1 if stuck_count >= 3: print("Rover can't move any more") break
#IM920のデバッグ用プログラム import IM920 #IM920.Srid(19200,'3156') #ペアリング #IM920.Erid(19200) #削除 IM920.Send(19200, 'Hello') #文字列送信 #print(IM920.Reception(19200)) #文字列受信 #IM920.Repeater(19200) #中継機化 IM920.Rdid(19200) #固有ID #IM920.Stch(19200, '01') #無線通信チャンネルの設定 #IM920.Rdch(19200) #無線通信チャンネルの読み出し IM920.Rdrs(19200) #RSSI値(現在の信号強度レベル)読み出し #IM920.Stpo(19200, '3') #通信出力の設定 IM920.Rdpo(19200) #通信出力の読み出し #IM920.Strt(19200, '2') #無線通信速度の設定 IM920.Rdrt(19200) #無線通信速度の読み出し #IM920.Rprm(19200) #パラメータ一括読み出し IM920.Sbrt(19200, '4') #ボーレート設定