def fuckingRun(mP, thd): suminit = 0 count = 0 for i in range(5): bmxinit = BMX055.bmx055_read() print(i, "init:", bmxinit[0:6]) suminit = suminit + bmxinit[5] aveinit = suminit / 5 print('aveinit', aveinit) Motor.motor(-mP, mP, 1) for i in range(20): bmxnow = BMX055.bmx055_read() print(i, "now:", bmxnow[0:6]) if bmxnow[5] - aveinit < thd: count = count + 1 print('count', count) if count > 5: print("stack") Motor.motor(0, 0, 2) return 1 else: count = 0 time.sleep(1) Motor.motor(0, 0, 2) return 0
def BMXstuckDetection(mP, bmxThd, measureCount, countThd, mode=0): global aveinit, stuckCount returnVal = 0 if mode == 0: for i in range(5): bmxinit = BMX055.bmx055_read() print(i, "init:", bmxinit[0:6]) aveinit = aveinit + bmxinit[5] aveinit = aveinit / 5 #print('aveinit', aveinit) Motor.motor(-mP, mP, 0.5) for i in range(measureCount): bmxnow = BMX055.bmx055_read() print(i, "now:", bmxnow[5]) if math.fabs(bmxnow[5] - aveinit) < bmxThd: stuckCount = stuckCount + 1 #print('stuckCount', stuckCount) if stuckCount > countThd: print("stuck DA YO NE ?") #Motor.motor(0, 0, 2) returnVal = 1 break else: stuckCount = 0 if mode == 0: Motor.motor(0, 0, 2) return returnVal
def pidSpin(targetSpin, KP, KI, KD, deltT): global P, I, D global motorPowerL, motorPowerR, motorPowerS global prebmx bmx055data = BMX055.bmx055_read() bmx055data = BMX055.bmx055_read() P = (targetSpin - bmx055data[5]) / 20 I = I + P * deltT D = -1 * (bmx055data[5] - prebmx) / (20 * deltT) motorPowerS = motorPowerS + int(KP * P) + int(KI * I) + int(KD * D) motorPowerS = 60 if motorPowerS > 60 else motorPowerS motorPowerS = -60 if motorPowerS < -60 else motorPowerS if motorPowerS >= 0: motorPowerL, motorPowerR = 10, motorPowerS elif motorPowerS < 0: motorPowerL, motorPowerR = -motorPowerS, 10 motorPowerL = 60 if motorPowerL > 60 else motorPowerL motorPowerL = -60 if motorPowerL < -60 else motorPowerL motorPowerR = 60 if motorPowerR > 60 else motorPowerR motorPowerR = -60 if motorPowerR < -60 else motorPowerR prebmx = bmx055data[5] #Motor.motor(mPL, mPR, dt, 1) #print(D, bmx055data[5], mPL, mPR, mPS) #spin = spin + bmx055data[5] * dt return motorPowerL, motorPowerR, motorPowerS, bmx055data
def bmxdetect(gyrolandThd): global Mcount global bmxData gyrolandjudge = 0 try: bmxData = BMX055.bmx055_read() gyrox = math.fabs(bmxData[3]) #using gyro gyroy = math.fabs(bmxData[4]) gyroz = math.fabs(bmxData[5]) #print(bmxData) if gyrox < gyrolandThd and gyroy < gyrolandThd and gyroz < gyrolandThd: Mcount += 1 if Mcount > 4: gyrolandjudge = 1 else: Mcount = 0 gyrolandjudge = 0 except: print(traceback.format_exc()) Mcount = 0 gyrolandjudge = 2 finally: #gyrolandjudge = 2 #for debug return gyrolandjudge, Mcount
def get_data(): #--- get bmx055 data ---# try: bmxData = BMX055.bmx055_read() #time.sleep(0.2) except KeyboardInterrupt: print() except Exception as e: print() print(e) #--- get acceralate sensor data ---# global accx, accy, accz accx = bmxData[0] accy = bmxData[1] accz = bmxData[2] #--- get magnet sensor data ---# global magx, magy, magz magx = bmxData[6] magy = bmxData[7] magz = bmxData[8] return magx, magy, magz, accx, accy, accz
def readCalData(filepath): count = 0 while count <= 100: bmx055data = BMX055.bmx055_read() with open(filepath, '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") count = count + 1 time.sleep(0.1) Motor.motor(0, 0, 1)
def get_accdata(): #--- get bmx055 data ---# try: bmxData = BMX055.bmx055_read() #time.sleep(0.2) except KeyboardInterrupt: print() except Exception as e: print() print(e) #--- get acceralate sensor data ---# global accx, accy, accz accx = bmxData[0] accy = bmxData[1] accz = bmxData[2] return accx, accy, accz
def stuckDetection(nLat=0, nLon=0): global oLat global oLon global stuckNum global stuckStatus distance = 0.0 angle1, angle2 = 0.0, 0.0 rollCount = 0 for i in range(10): bmx055data = BMX055.bmx055_read() if (math.fabs(bmx055data[0]) >= 6): rollCount = rollCount + 1 time.sleep(0.05) if (rollCount >= 8): #if rover has rolled over if (not stuckStatus == 1): stuckNum = 0 stuckStatus = 1 stuckNum = stuckNum + 1 elif (not nLon == 0.0): distance, angle1, angle2 = RunningGPS.calGoal(nLat, nLon, oLat, oLon, 0.0) if (distance <= 5): #if rover doesn't move if not stuckStatus == 2: stuckNum = 0 stuckStatus = 2 stuckNum = stuckNum + 1 else: stuckStatus = 0 stuckNum = 0 oLat = nLat oLon = nLon else: stuckStatus = 0 stuckNum = 0 print(stuckStatus, stuckNum, distance) return stuckStatus, stuckNum
# ------------------- 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(), BME280.bme280_read(), TSL2561.readLux(), BMX055.bmx055_read()) photoName = Capture.Capture(photopath) Other.saveLog(captureLog, time.time() - t_start, GPS.readGPS(), BME280.bme280_read(), photoName) time.sleep(1) IM920.Send("P2D") IM920.Send("P2F") # ------------------- Release Phase ------------------- # if (phaseChk <= 3): Other.saveLog(phaseLog, "3", "Release Phase Started", time.time() - t_start) t_release_start = time.time() print("Releasing Phase Started {0}".format(time.time() - t_start)) #IM920.Send("P3S")
def calNAng(calibrationScale, angleOffset): bmx055Data = BMX055.bmx055_read() nowAng = Calibration.readDir(calibrationScale, bmx055Data) - angleOffset nowAng = nowAng if nowAng <= 180.0 else nowAng - 360.0 nowAng = nowAng if nowAng >= -180.0 else nowAng + 360.0 return nowAng
#Calculate angle nAng = calNAng(ellipseScale, angOffset) #Calculate disGoal and relAng relAng[2] = relAng[1] relAng[1] = relAng[0] disGoal, angGoal, relAng[0] = calGoal(nLat, nLon, gLat, gLon, nAng) rAng = np.median(relAng) #Calculate Motor Power mPL, mPR, mPS = runMotorSpeed(rAng, kp, maxMP) Motor.motor(mPL, mPR, 0.001, 1) #Save Log print(nLat, nLon, disGoal, angGoal, nAng, rAng, mPL, mPR, mPS) Other.saveLog(runningLog, time.time(), BMX055.bmx055_read(), nLat, nLon, disGoal, angGoal, nAng, rAng, mPL, mPR, mPS) gpsData = GPS.readGPS() time.sleep(0.1) Motor.motor(0, 0, 2) print("Switch to Goal Detection") close() except KeyboardInterrupt: close() print("Keyboard Interrupt") except: close() print(traceback.format_exc())
if __name__ == "__main__": try: t_start = time.time() # ------------------- Setup Phase --------------------- # print("Program Start {0}".format(time.time())) #{}の中の0はなくてもいい 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", time.time() - t_start) if(phaseChk <= 3): tx1 = time.time() tx2 = tx1 print("Releasing Judgement Program Start {0}".format(time.time() - t_start)) #loopx bme280Data=BME280.bme280_read() while (tx2-tx1<=x):
tsl2561Log = "log/checkTSL2561Log.txt" 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):
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() time.sleep(10) # ------------------- Release Phase ------------------- # if(phaseChk <= 3): Other.saveLog(phaseLog, "3", "Release Phase Started", time.time() - t_start) t_release_start = time.time() print("\nReleasing Phase Started {0}".format(time.time() - t_start)) #IM920.Send("P3S") # --- Release Judgement, "while" is for timeout --- # i = 0 while (time.time() - t_release_start <= t_release): luxreleasejudge,lcount = Release.luxdetect(luxreleaseThd) pressreleasejudge,acount = Release.pressdetect(pressreleaseThd)
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())
import math import pigpio import time import traceback import BMX055 import Motor import Stuck if __name__ == "__main__": try: BMX055.bmx055_setup() flug = 0 count = 0 while flug <= 5: bmx055data = BMX055.bmx055_read() # --- Restart --- # if(count == 0): """ Motor.motor(0, 0, 2) Motor.motor(60, 0, 1.8) Motor.motor(50, 0, 0.2) Motor.motor(48, 0, 0.2) Motor.motor(46, 0, 0.2) Motor.motor(40, 0, 0.2) Motor.motor(38, 0, 0.3) Motor.motor(36, 0, 0.3) """ print(60) Motor.motor(0, 60, 0.5) #print(50)
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) Other.saveLog(captureLog, time.time() - t_start, photoName) Other.saveLog(sleepLog, time.time() - t_start, GPS.readGPS(), BME280.bme280_read(), TSL2561.readLux(), BMX055.bmx055_read()) time.sleep(1) IM920.Send("SLEEP") # ------------------- Release Phase ------------------- # if (phaseChk <= 3): Other.saveLog(phaseLog, "3", "Release Phase Started", time.time() - t_start) t_release_start = time.time() print("Releasing Phase Started {0}".format(time.time() - t_start)) pi.write(22, 1) # time.sleep(2) # IM920.Send("RELEASEJ") # --- Release Judgement, "while" is for timeout --- # while (time.time() - t_release_start <= t_release): #luxjudge,lcount = Release.luxjudge(
# ------------------- 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", time.time() - t_start) if (phaseChk <= 3): tx1 = time.time() tx2 = tx1 print("Releasing Judgement Program Start {0}".format(time.time() - t_start)) #loopx
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 Other.saveLog(sleepLog, time.time() - t_start, gpsData, bme280Data, bmx055Data, luxData) photoName = Capture.Capture(photoPath) Other.saveLog(photoLog, time.time() - t_start, photoName) time.sleep(1) t2 = time.time() # ------------------- Release Fhase ------------------- # Other.saveLog(phaseLog, "3", "Release Phase Started", time.time() - t_start) if (phaseChk <= 3): print("Release Judgement Program Start {0}".format(time.time()))
#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 Motor.motor(mP, -mP, 0.001, 1) print(dir, mP) except KeyboardInterrupt: