sensors.py 2.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081
  1. from connection import ArduinoSlave
  2. import time
  3. import statistics
  4. import math
  5. conn = ArduinoSlave()
  6. class AcusticSensor:
  7. def __init__(self):
  8. self.sonic_speed_left = 0
  9. self.sonic_speed_rigth = 0
  10. self.sensor_distance = 450 #in mm
  11. def start(self):
  12. if not conn.isConnected():
  13. conn.open()
  14. conn.addRecvCallback(self._readCb)
  15. def _readCb(self, raw):
  16. print("acc: ", conn.getAcusticRTTs())
  17. def calibrate(self, distance):
  18. print("Move gondel to far left corner")
  19. input()
  20. speed = list()
  21. for i in range(100):
  22. time_val = [1.312,0]
  23. speed.append(distance/time_val[0])
  24. self.sonic_speed_left = statistics.mean(speed)
  25. print("sonic speed left:",self.sonic_speed_left)
  26. print("Move gondel to far rigth corner")
  27. input()
  28. speed = list()
  29. for i in range(100):
  30. time_val = [0,1.312]
  31. speed.append(distance/time_val[1])
  32. self.sonic_speed_rigth = statistics.mean(speed)
  33. print("sonic speed rigth:",self.sonic_speed_rigth)
  34. def read(self):
  35. return (952, 660)
  36. def calculate_position(self):
  37. if not self.sonic_speed_rigth or not self.sonic_speed_left:
  38. print("sensor not calibrated! calibrate now!")
  39. return (0,0)
  40. else:
  41. time_val = self.read()
  42. distance_left = time_val[0] * self.sonic_speed_left / 1000 # seconds -> mseconds
  43. distance_rigth = time_val[1] * self.sonic_speed_rigth / 1000
  44. print(distance_left,distance_rigth)
  45. x = (self.sensor_distance**2 - distance_rigth**2 + distance_left**2) / (2*self.sensor_distance)
  46. y = math.sqrt(distance_left**2 - x**2)
  47. return (x,y)
  48. class MagneticSensor:
  49. def __init__(self):
  50. pass
  51. def start(self):
  52. if not conn.isConnected():
  53. conn.open()
  54. conn.addRecvCallback(self._readCb)
  55. def _readCb(self, raw):
  56. print("mag: ", conn.getMagneticField())
  57. def calibrate(self, x, y):
  58. pass
  59. def read(self):
  60. return (0, 0)
  61. if __name__ == "__main__":
  62. acc = AcusticSensor()
  63. acc.start()
  64. mag = MagneticSensor()
  65. mag.start()
  66. while True:
  67. time.sleep(1)