|
@@ -8,7 +8,7 @@ conn = ArduinoSlave()
|
|
|
class AcusticSensor:
|
|
|
def __init__(self):
|
|
|
self.sonic_speed_left = 0
|
|
|
- self.sonic_speed_rigth = 0
|
|
|
+ self.sonic_speed_right = 0
|
|
|
self.sensor_distance = 450 #in mm
|
|
|
|
|
|
def start(self):
|
|
@@ -29,28 +29,28 @@ class AcusticSensor:
|
|
|
self.sonic_speed_left = statistics.mean(speed)
|
|
|
print("sonic speed left:",self.sonic_speed_left)
|
|
|
|
|
|
- print("Move gondel to far rigth corner")
|
|
|
+ print("Move gondel to far right corner")
|
|
|
input()
|
|
|
speed = list()
|
|
|
for i in range(100):
|
|
|
time_val = [0,1.312]
|
|
|
speed.append(distance/time_val[1])
|
|
|
- self.sonic_speed_rigth = statistics.mean(speed)
|
|
|
- print("sonic speed rigth:",self.sonic_speed_rigth)
|
|
|
+ self.sonic_speed_right = statistics.mean(speed)
|
|
|
+ print("sonic speed right:",self.sonic_speed_right)
|
|
|
|
|
|
def read(self):
|
|
|
return (952, 660)
|
|
|
|
|
|
def calculate_position(self):
|
|
|
- if not self.sonic_speed_rigth or not self.sonic_speed_left:
|
|
|
+ if not self.sonic_speed_right or not self.sonic_speed_left:
|
|
|
print("sensor not calibrated! calibrate now!")
|
|
|
return (0,0)
|
|
|
else:
|
|
|
time_val = self.read()
|
|
|
distance_left = time_val[0] * self.sonic_speed_left / 1000 # seconds -> mseconds
|
|
|
- distance_rigth = time_val[1] * self.sonic_speed_rigth / 1000
|
|
|
- print(distance_left,distance_rigth)
|
|
|
- x = (self.sensor_distance**2 - distance_rigth**2 + distance_left**2) / (2*self.sensor_distance)
|
|
|
+ distance_right = time_val[1] * self.sonic_speed_right / 1000
|
|
|
+ print(distance_left,distance_right)
|
|
|
+ x = (self.sensor_distance**2 - distance_right**2 + distance_left**2) / (2*self.sensor_distance)
|
|
|
y = math.sqrt(distance_left**2 - x**2)
|
|
|
return (x,y)
|
|
|
|