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:
