def Reception(mybaudrate=19200): global com ''' 受信 アスキーコードから文字列に変換したものを返す mybaudrate:ボーレート ''' try: com.flushInput() #print("a") text = com.readline().decode('utf-8').strip() #受信と空白の削除 #print(text) com.flushOutput() text = text.replace("\r\n", "") #power=text.split(":")[0] #print(text) text = text.split(":")[1] #print(text) text = text.split(",") #print("d") cngtext = "" #print(text) for x in text: cngtext += chr(int(x, 16)) Other.saveLog(receptionLog, cngtext, datetime.datetime.now()) #f.write(text) #f.write("\n") #print(text) except Exception: cngtext = "" print("NO DATA") #com.close() return cngtext
def takePhoto(): global photoName global gpsData photo = "" photo = Capture.Capture(photopath) gpsData = GPS.readGPS() if not(photo == "Null"): photoName = photo Other.saveLog(captureLog, time.time() - t_start, gpsData, BME280.bme280_read(), photoName)
def rotate_control(θ, azimuth, t_start): try: timeout_count = time.time() while azimuth - 30 > θ or θ > azimuth + 30: if 0 <= azimuth < 30: if azimuth - 30 + 360 <= θ <= 360: break if 330 <= azimuth <= 360: if 0 <= θ <= azimuth + 30 - 360: break if azimuth < 180: if azimuth < θ < azimuth + 180: run = pwm_control.Run() run.turn_right_l() time.sleep(1) else: #-- 0 < θ < azimuth or azimuth + 180 < θ < 360 --# run = pwm_control.Run() run.turn_left_l() time.sleep(1) else: #-- 180 < azimuth --# if azimuth - 180 < θ < azimuth: run = pwm_control.Run() run.turn_left_l() time.sleep(1) else: #-- 0 < θ < azimuth - 180 or azimuth < θ < 360 --# run = pwm_control.Run() run.turn_right_l() time.sleep(1) #run = pwm_control.Run() #run.stop() #time.sleep(0.5) #get_magdata_average() get_data() θ = calculate_angle_2D(magx, magy, magx_off, magy_off) Other.saveLog(Calibration_rotate_controlLog, 'Calibration_rotate_control', time.time() - t_start, θ, azimuth) if time.time() - timeout_count >= 60: print('rotate control timeout') break print("rotate control finished") #print(θ) except KeyboardInterrupt: run = pwm_control.Run() run.stop() print("faulted to rotate control to goal direction") finally: #run = pwm_control.Run() #run.stop() pass
def transmitPhoto(): global t_start photoName = Capture.Capture(photopath) Other.saveLog(captureLog, time.time() - t_start, GPS.readGPS(), BME280.bme280_read(), photoName) print("Send Photo") sendPhoto.sendPhoto(photoName) print("Send Photo Finished") Other.saveLog(sendPhotoLog, time.time() - t_start, GPS.readGPS(), photoName)
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
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
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): Other.saveLog(sleepLog, time.time() - t_start, GPS.readGPS(),
mPRight = int(motorPowerMax if mPRight > motorPowerMax else mPRight) mPRight = int(motorPowerMax / 2 if mPRight < motorPowerMax / 2 else mPRight) return [mPLeft, mPRight, mPS] if __name__ == "__main__": try: setup() time.sleep(1) fileCal = Other.fileName(calibrationLog, "txt") Motor.motor(40, 0, 1) Calibration.readCalData(fileCal) Motor.motor(0, 0, 1) ellipseScale = Calibration.Calibration(fileCal) Other.saveLog(fileCal, ellipseScale) gpsInterval = 0 #Get GPS data #print("Getting GPS Data") while(not checkGPSstatus(gpsData)): gpsData = GPS.readGPS() time.sleep(1) print("Runnning Start") while(disGoal >= 5): if(checkGPSstatus(gpsData)): nLat = gpsData[1] nLon = gpsData[2]
import sys sys.append.path("/home/pi/git/kimuralab/SensorModuleTest/BMX055") sys.append.path("/home/pi/git/kimuralab/SensorModuleTest/Motor") sys.append.path("/home/pi/git/kimuralab/Other") import time import traceback import BMX055 import Motor import Other if __name__ == "__main__": BMX055.bmx055_setup() t_start = time.time() while (time.time() - t_start <= 3): lp, lr = Motor.motor(50, 50) Other.saveLog("bmx055data.txt", lp, lr, BMX055.bmx055_read()) t_start = time.time() while (time.time() - t_start <= 3): lp, lr = Motor.motor(0, 0) Other.saveLog("bmx055data.txt", lp, lr, BMX055.bmx055_read())
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 (time.time() - t_sleep_start <= t_sleep): photoName = Capture.Capture(photopath)
break else: returnVal = 0 print(data, power) return returnVal if __name__ == "__main__": try: mode = 0 logPath = "photoRecieveLog.txt" photoPath = "receivePhoto" photoSize = (120, 160) convertedPhotoSize = (320, 240) Other.saveLog(receptionLog, "0", "receiveprogram start", datetime.datetime.now()) print("Ready") while 1: im920data = read(baudrate) mode = receiveData(im920data) if (mode == 1): t_start = time.time() print("Photo Receive") receivePhoto(logPath, photoPath, photoSize, convertedPhotoSize) print(time.time() - t_start) print("MissionFinish") else: print("mode : " + str(mode) + " " + str(datetime.datetime.now()))
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()) #takePhoto()
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)) # --- Release Phase --- #
def receiveData(data): returnVal = 0 #print(data) comdata, power = getCommand(data) if (comdata == ['50,51,53']): Other.saveLog(receptionLog, "1-1", "Program Started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Program Started") elif (comdata == ['50,31,46']): Other.saveLog(receptionLog, "1-2", "Program Started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Program Started2") elif (comdata == ['50,32,53']): Other.saveLog(receptionLog, "2", "Sleep Started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Sleep Started") elif (comdata == ['50,32,44']): Other.saveLog(receptionLog, "2", "Sleep now", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Sleep Now") elif (comdata == ['50,32,46']): Other.saveLog(receptionLog, "2", "Sleep Finished", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Sleep Finished") elif (comdata == ['50,33,53']): Other.saveLog(receptionLog, "3", "Release started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Release Started") elif (comdata == ['50,33,44']): Other.saveLog(receptionLog, "3", "Release judge now", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Release judge now") elif (comdata == ['50,33,46']): Other.saveLog(receptionLog, "3", "Release judge finished", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Release judge finished") elif (comdata == ['50,34,53']): Other.saveLog(receptionLog, "4", "Land started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Land judge started") elif (comdata == ['50,34,44']): Other.saveLog(receptionLog, "4", "Land judge now", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Land judge now") elif (comdata == ['50,34,46']): Other.saveLog(receptionLog, "4", "Land FInished", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Land judge finished") elif (comdata == ['50,35,53']): Other.saveLog(receptionLog, "5", "Melt started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Melt Started") elif (comdata == ['50,35,46']): Other.saveLog(receptionLog, "5", "Melt Finished", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Melt finished") elif (comdata == ['50,36,53']): Other.saveLog(receptionLog, "6", "ParaAvoidance started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("ParaAvoidance started") elif (comdata == ['50,36,46']): Other.saveLog(receptionLog, "6", "ParaAvoidance finished", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("ParaAvoidance finished") elif (comdata == ['50,37,53']): Other.saveLog(receptionLog, "7", "Running Phase Started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Running Phase Started") elif (comdata == ['50,37,46']): Other.saveLog(receptionLog, "7", "Running Phase Finished", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Running Phase Finished") elif (comdata == ['50,38,53']): Other.saveLog(receptionLog, "8", "GoalDetection Phase Started", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("GoalDeetection Phase Started") elif (comdata == ['50,38,46']): Other.saveLog(receptionLog, "8", "GoalDetection Phase Finished", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("GoalDeetection Phase Finished") elif (comdata == ['50,40,46']): Other.saveLog(receptionLog, "10", "all Finished", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("Running Phase Finished") elif (comdata == ['45,4F']): Other.saveLog(receptionLog, "11", "Error Occured", power, datetime.datetime.now()) returnVal = 2 powerread(power) print("EO") elif str('47') in str(comdata): #GPS gps = int(comdata, 16) print(gps) Other.saveLog(receptionLog, "0", "GPS", gps, power, datetime.datetime.now()) elif (comdata == ['4D']): for i in range(3): if (comdata == ['4D']): Send("M", baudrate) Send("M", baudrate) #print(data) time.sleep(0.1) data = read(baudrate) data = getCommand(data) time.sleep(0.2) returnVal = 1 else: returnVal = 0 break else: returnVal = 0 print(data, power) return returnVal
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)) # --- Release Phase --- #
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, 0) #IM920 Turn Off # ------------------- Release Phase ------------------- # Other.saveLog(phaseLog, "3", "Release Phase Started",
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() print('Release Phase Started {}'.format(time.time - t_start)) # --- Release Judgement, 'while' is until timeout --- # while time.time() - t_release_start <= t_release: luxjudge,luxcount = Release.luxdetect() pressjudge,presscount = Release.pressdetect() if luxjudge == 1 or pressjudge == 1: Other.saveLog(releaseLog, 'Release Judge', luxjudge, pressjudge) print('Rover has released') break else: print('Rover is still in the air') IM920.Send('P3D') else: Other.savsLog(releaseLog, 'Release Judge by Timeout') print('Release Timeout') IM920.Send('P3F') # --- Landing Phase --- # if phaseChk <= 4: IM920.Send('P4S') Other.saveLog(phaseLog, '4', 'Landing Phase Started', time.time - t_start) t_landing_start = time.time() print('Landing Phase Started' {}.format(time.time - t_start))
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 luxData = TSL2561.readLux() #Read TSL2561 data gpsData = GPS.readGPS() #Read GPS data
def rotate_control(θ, lon2, lat2, t_start): direction = calculate_direction(lon2, lat2) azimuth = direction["azimuth1"] #--- 0 <= azimuth <= 360 ---# print('goal azimuth = ' + str(azimuth)) #print('θ = '+str(θ)) try: t1 = time.time() while azimuth - 30 > θ or θ > azimuth + 30: if 0 <= azimuth < 30: if azimuth - 30 + 360 <= θ <= 360: break if 330 <= azimuth <= 360: if 0 <= θ <= azimuth + 30 - 360: break #--- use Timer ---# global cond cond = True thread = Thread(target=timer, args=([1])) thread.start() while cond: run = pwm_control.Run() run.turn_right() get_data() θ = math.degrees(math.atan((magy - magy_off) / (magx - magx_off))) #--- -90 <= θ <= 90 ---# if θ >= 0: if magx - magx_off < 0 and magy - magy_off < 0: #Third quadrant θ = θ + 180 #180 <= θ <= 270 if magx - magx_off > 0 and magy - magy_off > 0: #First quadrant pass #0 <= θ <= 90 else: if magx - magx_off < 0 and magy - magy_off > 0: #Second quadrant θ = 180 + θ #90 <= θ <= 180 if magx - magx_off > 0 and magy - magy_off < 0: #Fourth quadrant θ = 360 + θ #270 <= θ <= 360 #--- Half turn ---# θ += 180 if θ >= 360: θ -= 360 #--- 0 <= θ <= 360 ---# print('θ = ' + str(θ)) Other.saveLog(Calibration_rotate_controlLog, 'Calibration_rotate_control', time.time() - t_start, θ, azimuth) run = pwm_control.Run() run.stop() time.sleep(0.5) if time.time() - t1 >= 60: #judge = False print('rotate control timeout') break #judge = True print("rotate control finished") #print(θ) except KeyboardInterrupt: run = pwm_control.Run() run.stop() print("faulted to rotate control to goal direction") finally: run = pwm_control.Run() run.stop() return judge
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) print("Calibration Finished") cal_data = Calibration(fileP) for i in range(4): print(cal_data[i]) Other.saveLog(fileP, cal_data) for i in range(3): dir_data[i] = readDir(cal_data, BMX055.bmx055_read()) time.sleep(0.1) while 1: dir_data[2] = dir_data[1] #Thrid latest data dir_data[1] = dir_data[0] #Second latest data dir_data[0] = readDir(cal_data, BMX055.bmx055_read()) #latest data dir = np.median(dir_data) mP = int(dir * 0.6) if (mP > 30): mP = 30 elif (mP < -30): mP = -30