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
