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 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 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
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())
x_csv = x_csv / 70 * myoutput.beta[1]
y_csv = y_csv / 70 * myoutput.beta[0]
cal_data = [x_ave, y_ave, myoutput.beta[1] / 100, myoutput.beta[0] / 100]
return cal_data
def readDir(calData, bmx055data):
return math.atan2(
(bmx055data[6] - calData[0]) / calData[2],
(bmx055data[7] - calData[1]) / calData[3]) * 180 / math.pi
if __name__ == '__main__':
try:
BMX055.bmx055_setup()
targetVal = 300
Kp = 0.9
Ki = 1.1
Kd = 0.2
dt = 0.05
mPL, mPR, mPS = 0, 0, 0
roll = 0
time.sleep(1)
fileP = Other.fileName("calData", "txt")
print("Calibration Start")
Motor.motor(0, 30, 1)
while (math.fabs(roll) <= 720):
mPL, mPR, mPS, bmx055data = pidControl.pidSpin(
targetVal, Kp, Ki, Kd, dt)
