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 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")
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 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 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))
#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') #ボーレート設定
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))
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))
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)
print(str(byte[i:i + amari]))
IM920.Send("end")
time.sleep(1)
IM920.Send("end")
time.sleep(1)
IM920.Send("end")
'''
for i in range(0,len(byte)):
IM920.IMSend(byte[i])
print(i,'/',len(byte))
for i in range(0,len(byte),16):
st = str(byte[i:i+15])
IM920.Send(st)
print(i,'/',len(byte))
'''
#IM920.IMSend(byte)
#convertIMG2BYTES.BYTEStoIMG(st)
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())
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 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
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:
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()
BME280.bme280_setup()
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):
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
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
#文字列受信
self.imbool = False
if IM920.Reception(19200):
self.imbool = True
print(IM920.Reception(19200))
#文字列送信
IM920.Send(19200, "Hallo")
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 gpsSend(gpsData):
IM920.Send('g' + str(gpsData[0]) + ',' + str(gpsData[1]) + ',' +
str(gpsData[2]) + ',' + str(gpsData[3]) + ',' +
str(gpsData[4]) + ',' + str(gpsData[5]))
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 --- #
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")
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
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 ---------#
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 --------------------- #
if (phaseChk <= 2):
Other.saveLog(phaseLog, "2", "Sleep Phase Started",
time.time() - t_start)
IM920.Send("P2S")
pi.write(22, 0) #IM920 Turn Off
t_wait_start = time.time()
while (time.time() - t_wait_start <= t_sleep):
Other.saveLog(sleepLog,
time.time() - t_start, GPS.readGPS(),
BME280.bme280_read(), TSL2561.readLux(),