diff --git a/team.py b/team.py index 080da67461c9ce77b7b8fabe9f6b62ed175e2a13..78f3b9bd660fd06db3118d5f3a8ebe9040f50fad 100644 --- a/team.py +++ b/team.py @@ -49,8 +49,8 @@ if(fpDistance.fileCount()>0): if fpAngle.fileCount()>0: angleDatas = [angle for angle in fpAngle.read(-1, False) if angle[1]=='4'] _thread.start_new_thread(fpAngle.removeAllFile,()) - pitch = statistics.mean([float(a) for a in angleDatas[4]])* 2 / 32767 * math.pi - roll = statistics.mean([float(a) for a in angleDatas[5]])* 2 / 32767 * math.pi + pitch = float(angleDatas[-1][4])* 2 / 32767 * math.pi + roll = float(angleDatas[-1][5])* 2 / 32767 * math.pi radian = math.acos(math.sqrt(1 / (1 + math.tan(pitch) * math.tan(pitch) + math.tan(roll) * math.tan(roll)))) height = distance*math.cos(radian) + distance2*math.sin(radian) width = distance2*math.cos(radian)