Merge branch 'master' of gitlab.utwente.nl:s1779397/cps_project_group13

mappingbot/encoder.py

 import RPi.GPIO as GPIO
 import time
+import math
 
 GPIO.setmode(GPIO.BCM)
 print(GPIO.VERSION)
 
 class Encoder:
-    
+
     def __init__(self, name, encoder_pin: int):
-        global pulse_count
+        # Initialize some stuff
+        global pulse_count, start_time, time_passed, old_time_passed, old_pulse_count
         self.name = name
         self.encoder_pin = encoder_pin
         pulse_count = 0
+        start_time = time.time()
+        time_passed = 0
+        old_time_passed = 0
+        old_pulse_count = 0
 
         GPIO.setup(self.encoder_pin, GPIO.IN, GPIO.PUD_DOWN)
-        
+
     def callback_encoder(self, channel):
-        global pulse_count
+        # Interrupt handler, increases pulse_count and updates time
+        global pulse_count, start_time, time_passed
+        time_passed = time.time() - start_time
         pulse_count += 1
 
     def get_pulse_count(self) -> int:
+        # return pulse_count
         global pulse_count
         return pulse_count
 
     def reset_pulse_count(self):
+        # reset pulse_count
         global pulse_count
         pulse_count = 0
 
-    def init_callback(self):
-        GPIO.add_event_detect(self.encoder_pin, GPIO.BOTH, callback = self.callback_encoder, bouncetime = 10)
+    def reset_time(self):
+        # reset time_passed and start_time
+        global time_passed, start_time
+        time_passed = 0
+        start_time = time.time()
+
+    def get_current_pulse_speed(self) -> float:
+        # Get average speed since last call in pulses/s
+        global pulse_count, time_passed, old_time_passed, old_pulse_count
+        if (time_passed - old_time_passed):
+            speed = (pulse_count - old_pulse_count) / (time_passed - old_time_passed)
+        else:
+            speed = 0
+        old_pulse_count = pulse_count
+        old_time_passed = time_passed
+        return speed
 
+    def get_speed(self) -> float:
+        # Convert pulse speed to cm/s
+        pulse_per_second = self.get_current_pulse_speed()
+        return pulse_per_second * (6 * 2 * math.pi) / 40
+
+    def init_callback(self):
+        # Initialize the interrupt
+        GPIO.add_event_detect(self.encoder_pin, GPIO.BOTH, callback=self.callback_encoder, bouncetime=10)
 
 try:
     encoder1 = Encoder(name="encoder1", encoder_pin=4)
     encoder1.init_callback()
     while True:
         print(encoder1.get_pulse_count())
+        print(encoder1.get_speed())
         time.sleep(1)
 except KeyboardInterrupt:  # Exit by pressing CTRL + C
     GPIO.cleanup()