Roman Dolejší DU-10 - Distance keeper and calibration framework

lesson_10/homework_distance_keeper.py

@@ -0,0 +1,98 @@
+from time import ticks_ms, ticks_diff
+
+from microbit import button_a
+
+from sonar import Sonar
+from wheel_driver import WheelDriver
+
+#  Naimplementujte adaptivní tempomat s využitím třídy Robot
+#  a dalších vzorových kódů. Využijte zpětné vazby a regulátoru P.
+#
+# Robot by měl
+# - Udržovat minimální vzdálenost 0.2m před překážkou
+# - Zastavit se a případně rozjet vzad, pokud je překážka moc blízko
+# - Omezit maximální rychlost na 0.3 m/s
+if __name__ == "__main__":
+    wheel_driver = WheelDriver(left_pwm_min=60, left_pwm_multiplier=0.1809153, left_pwm_shift=-5.509312,
+                               right_pwm_min=60, right_pwm_multiplier=0.1628194, right_pwm_shift=-5.216122)
+    sonar = Sonar()
+    try:
+        # Tries to maintain the robot around 20 cm in front of an obstacle.
+        # We use Sonar to measure distance to the obstacle and gradually increase
+        # Speed of the robot to 30 cm/s. We also support backing off when too close.
+        # We allow for +/- 2 cm around the target distance to avoid oscillation.
+
+        # The wheels are controlled individually and we are trying to keep the robot
+        # going straight, so we are aiming at keeping speed in radians constant for
+        # both wheels.
+        distance_cm_desired = 20
+        distance_cm_tolerance = 2
+        distance_cm_max = 50
+        # with 30 cm/s the robot was a bit sluggish and was not able to respond well
+        # (I might need to calibrate the wheels, try different batteries or verify the speedometer conversion code)
+        speed_cm_max = 40
+        speed_cm_min = 20
+        # any wheel can provide below conversion
+        speed_rad_max = wheel_driver.wheel_left.speedometer.cm_to_radians(speed_cm_max)
+        speed_rad_min = wheel_driver.wheel_left.speedometer.cm_to_radians(speed_cm_min)
+        speed_change_slope = 2
+
+        # Calculate the constants for rad_sec speed dependency on the distance to ensure
+        # the robot won't go fast when the distance is small.
+        # When the distance is 50 cm, the speed should be 0.4 m/s.
+        # When the distance is 20 cm, the speed should be 0.2 m/s.
+        distance_cm_to_speed_rad_sec = (speed_rad_max - speed_rad_min) / (distance_cm_max - distance_cm_desired)
+
+        info_cycle_length = 1000
+        info_cycle_start = ticks_ms()
+        regulation_cycle_length = 200
+        regulation_cycle_start = ticks_ms()
+
+        distance_cm = None
+        while not button_a.is_pressed():
+            wheel_driver.update()
+
+            if ticks_diff(ticks_ms(), regulation_cycle_start) > regulation_cycle_length:
+                regulation_cycle_start = ticks_ms()
+                distance_cm = sonar.get_distance_cm()
+                if distance_cm > distance_cm_max:
+                    wheel_driver.stop()
+                else:
+                    # the back-off distance is very small, we would normally probably use minimal speed,
+                    # but it might not work due to the wheel calibration, so we still try to use the same error
+                    # correction as for the forward movement. We can use the same mechanism and just indicate
+                    # negative pwm speed to move backwards (thanks to our Wheel implementation).
+                    if (distance_cm > distance_cm_desired + distance_cm_tolerance) or \
+                        (distance_cm < distance_cm_desired - distance_cm_tolerance):
+                        distance_cm_error = distance_cm - distance_cm_desired
+                        desired_speed_rad_sec = abs(distance_cm_error) * distance_cm_to_speed_rad_sec + speed_rad_min
+
+                        current_speed_rad_sec_left = wheel_driver.wheel_left.speedometer.get_speed_radians_per_sec()
+                        speed_error_rad_sec_left = desired_speed_rad_sec - current_speed_rad_sec_left
+                        speed_change_rad_sec_left = speed_change_slope * speed_error_rad_sec_left
+                        # print("Current speed %f, Desired speed %f, Speed change left %f" % (current_speed_rad_sec_left, desired_speed_rad_sec, speed_change_rad_sec_left))
+                        new_speed_rad_sec_left = min(current_speed_rad_sec_left + speed_change_rad_sec_left, speed_rad_max)
+                        new_speed_pwm_left = wheel_driver.wheel_left.rad_per_sec_to_pwm_speed(new_speed_rad_sec_left)
+
+                        current_speed_rad_sec_right = wheel_driver.wheel_right.speedometer.get_speed_radians_per_sec()
+                        speed_error_rad_sec_right = desired_speed_rad_sec - current_speed_rad_sec_right
+                        speed_change_rad_sec_right = speed_change_slope * speed_error_rad_sec_right
+                        new_speed_rad_sec_right = min(current_speed_rad_sec_right + speed_change_rad_sec_right, speed_rad_max)
+                        # print("Current speed %f, Desired speed %f, Speed change right %f" % (current_speed_rad_sec_right, desired_speed_rad_sec, speed_change_rad_sec_right))
+                        new_speed_pwm_right = wheel_driver.wheel_right.rad_per_sec_to_pwm_speed(new_speed_rad_sec_right)
+
+                        if distance_cm_error > 0:
+                            wheel_driver.move_pwm_for_distance(new_speed_pwm_left, new_speed_pwm_right, distance_cm_error)
+                        else:
+                            wheel_driver.move_pwm_for_distance(-new_speed_pwm_left, -new_speed_pwm_right, abs(distance_cm_error))
+                    else:
+                        wheel_driver.stop()
+
+            if ticks_diff(ticks_ms(), info_cycle_start) > info_cycle_length:
+                info_cycle_start = ticks_ms()
+                speed_cm_left = wheel_driver.wheel_left.get_speed_cm_per_sec()
+                speed_cm_right = wheel_driver.wheel_right.get_speed_cm_per_sec()
+                print("Distance: %f cm, speed: L=%.2f cm/s, R=%.2f cm/s" % (distance_cm, speed_cm_left, speed_cm_right))
+        print("Done")
+    finally:
+        wheel_driver.stop()

lesson_10/homework_gen_wheel_calibration_csv.py

@@ -0,0 +1,41 @@
+from wheel_driver import WheelDriver
+from wheel_calibrator import WheelCalibrator
+
+
+# Naimplemenetujte funkci kalibrace() motoru,
+# která změří závislost mezi PWM a rad/s a
+# proloží ji přímkou
+def kalibrace_plna(wheel_calibrator: WheelCalibrator):
+    wheel_calibrator.gather_pwm_to_real_speed_table_all()
+
+
+# Stejna funkce, ale rychla varianta -
+# scanuje nejmensi PWM od zvoleneho minima (60) po 10. Po nalezeni prvniho
+# nenuloveho pohybu zmeri i nejrychlejsi pohyb.
+def kalibrace_approx(wheel_calibrator: WheelCalibrator):
+    wheel_calibrator.gather_pwm_to_real_speed_table_fast_approx()
+
+
+def calibration_table_to_csv(wheel_name: str, wheel_calibrator):
+    wheel = wheel_calibrator.wheel
+    print("CSV calibration data for %s wheel (pwm_min = %s, multiplier = %s, shift = %s):"
+          % (wheel_name, wheel.pwm_min, wheel.pwm_multiplier, wheel.pwm_shift))
+    print("pwm,rad_sec,cm_sec")
+    for pwm in range(wheel.pwm_min, wheel.pwm_max + 1):
+        #  We just print the calibration data for the border values (min 5, max 5)
+        if pwm < wheel.pwm_min + 5 or pwm > wheel.pwm_max - 5:
+            rad = wheel_calibrator.pwm_speed_to_rad_per_sec[pwm]
+            cm = wheel.speedometer.radians_to_cm(rad)
+            print("%d,%f,%f" % (pwm, rad, cm))
+
+
+if __name__ == "__main__":
+    wheel_driver = WheelDriver()
+    try:
+        kalibrace_approx(wheel_driver.wheel_calibrator_left)
+        calibration_table_to_csv(wheel_name="left", wheel_calibrator=wheel_driver.wheel_calibrator_left)
+
+        kalibrace_approx(wheel_driver.wheel_calibrator_right)
+        calibration_table_to_csv(wheel_name="right", wheel_calibrator=wheel_driver.wheel_calibrator_right)
+    finally:
+        wheel_driver.stop()

lesson_10/sonar.py

@@ -0,0 +1,64 @@
+from microbit import pin1, pin8, pin12, button_a
+from machine import time_pulse_us
+from time import sleep
+
+class Sonar:
+    """Handles the ultrasonic sensor HC-SR04 to measure distance in cm.
+    Based on https://cdn.sparkfun.com/datasheets/Sensors/Proximity/HCSR04.pdf,
+    The sensor operates at freq 40Hz and supports ranges from 2cm to 400cm,
+    with 0.3cm resolution and 3mm precision, providing a viewing angle of 15 degrees.
+    The sensor uses the speed of sound in the air to calculate the distance.
+    Trigger Input Signal 10uS TTL pulse is used to start the ranging,
+    and the module will send out an 8 cycle burst of ultrasound at 40 kHz
+    and raise its echo. The Echo Output Signal is an input TTL lever signal
+    and the range in proportion to the duration of the echo signal."""
+    SOUND_SPEED = 343  # m/s
+    SERVO_MIN = 20  # right
+    SERVO_MAX = 128  # left
+    SERVO_STEP = (SERVO_MAX - SERVO_MIN) / 180
+
+    def __init__(self, trigger_pin=pin8, echo_pin=pin12, angle_pin=pin1):
+        self.trigger_pin = trigger_pin
+        self.trigger_pin.write_digital(0)
+        self.echo_pin = echo_pin
+        self.echo_pin.read_digital()
+        self.angle_pin = angle_pin
+        self.set_angle(0)
+
+    def set_angle(self, angle):
+        """Sets sonar angle, use -90 to 90"""
+        angle = angle if angle >= -91 else -90
+        angle = angle if angle <= 90 else 90
+        servo_value = self.SERVO_MAX - self.SERVO_STEP * (angle + 90)
+        print("Setting sonar angle to %d (value %d)" % (angle, servo_value))
+        self.angle_pin.write_analog(servo_value)
+
+    def get_distance_cm(self):
+        self.trigger_pin.write_digital(1)
+        self.trigger_pin.write_digital(0)
+
+        measured_time_us = time_pulse_us(self.echo_pin, 1)
+        if measured_time_us < 0:
+            return measured_time_us
+
+        measured_time_sec = measured_time_us / 1000000
+        distance_cm = 100 * measured_time_sec * self.SOUND_SPEED / 2
+        return distance_cm
+
+
+if __name__ == "__main_test_sonar_angles__":
+    sonar = Sonar()
+    for angle in range(-90, 90):
+        sonar.set_angle(angle)
+        sleep(0.25)
+
+if __name__ == "__main_get_distance__":
+    sonar = Sonar()
+    while not button_a.is_pressed():
+        distance_cm = sonar.get_distance_cm()
+        if distance_cm < 0:
+            # Error processing the distance, stop the movement
+            print("Error %f while getting distance value" % distance_cm)
+        else:
+            print("Distance %f" % sonar.get_distance_cm())
+        sleep(0.25)

lesson_10/wheel.py

@@ -0,0 +1,150 @@
+from time import ticks_ms, ticks_diff
+
+from microbit import i2c
+
+from wheel_speedometer import WheelSpeedometer
+
+
+class Wheel:
+    """Handles single wheel capable of moving forward or backward
+    with given (variable) speed and stop immediately or conditionally
+    based on distance and time."""
+
+    def __init__(self, name, i2c_address, motor_fwd_cmd, motor_rwd_cmd, sensor_pin,
+                 pwm_min=60, pwm_max=255, pwm_multiplier=0, pwm_shift=0):
+        """Initializes the wheel."""
+        self.name = name
+        self.i2c_address = i2c_address
+        self.motor_fwd_cmd = motor_fwd_cmd
+        self.motor_rwd_cmd = motor_rwd_cmd
+        self.distance_remain_ticks = 0
+        self.distance_req_time_ms = 0
+        self.distance_start_time_ms = 0
+        self.speed_pwm = 0
+        self.speedometer = WheelSpeedometer(sensor_pin=sensor_pin)
+        self.pwm_min = pwm_min
+        self.pwm_max = pwm_max
+        self.pwm_multiplier = pwm_multiplier
+        self.pwm_shift = pwm_shift
+
+    def move_pwm(self, speed_pwm):
+        """Moves the wheel using given PWM speed (indefinite ticks, time).
+        The wheel will continue to move until stop() is called.
+        The PWM speed is a value between -255 and 255, where 0 means stop."""
+        self.set_speed_pwm(speed_pwm)
+        self.distance_remain_ticks = -1
+        self.distance_req_time_ms = -1
+
+    def move_pwm_for_ticks(self, speed_pwm, distance_ticks):
+        """Moves the wheel forward using given PWM speed for the given distance
+        in sensor ticks. If the motor is already moving, the asked distance is added
+        to the remaining distance and the motor continues until no distance remains."""
+        self.set_speed_pwm(speed_pwm)
+        self.distance_remain_ticks += distance_ticks
+
+    def move_pwm_for_time(self, speed_pwm, distance_time_ms):
+        """Moves the wheel forward using given PWM speed for the given time.
+        If the motor is already moving, the distance in time is added to the current
+        distance and the motor continues to move until the total time is reached."""
+        self.set_speed_pwm(speed_pwm)
+        self.distance_req_time_ms += distance_time_ms
+        if self.distance_start_time_ms == 0:
+            self.distance_start_time_ms = ticks_ms()
+
+    def move_pwm_for_distance(self, speed_pwm, distance_cm):
+        """Moves the wheel forward using given PWM speed for given distance in cm."""
+        ticks_for_distance = int(distance_cm * self.speedometer.get_ticks_per_cm())
+        self.move_pwm_for_ticks(speed_pwm, ticks_for_distance)
+
+    def set_speed_pwm(self, speed_pwm):
+        """Sets the wheel PWM speed (and direction). Does not affect the remaining
+        distance or time previously set to perform. If the wheel was going
+        in the other direction, resets the H-bridge other direction first."""
+        if speed_pwm == 0:
+            # print("Stopping %s wheel" % self.name)
+            i2c.write(self.i2c_address, bytes([self.motor_fwd_cmd, 0]))
+            i2c.write(self.i2c_address, bytes([self.motor_rwd_cmd, 0]))
+            return
+        speed_pwm = int(max(-255, min(255, speed_pwm)))
+        if (self.speed_pwm < 0 < speed_pwm) or (self.speed_pwm > 0 > speed_pwm):
+            # if we are changing the direction, we need to reset the motor first
+            motor_reset_cmd = (self.motor_rwd_cmd
+                               if speed_pwm >= 0 else self.motor_fwd_cmd)
+            # print("Changing %s wheel direction" % self.name)
+            i2c.write(self.i2c_address, bytes([motor_reset_cmd, 0]))
+        motor_set_cmd = self.motor_fwd_cmd if speed_pwm > 0 else self.motor_rwd_cmd
+        # print("Setting %s wheel speed_pwm %d" % (self.name, speed_pwm))
+        i2c.write(self.i2c_address, bytes([motor_set_cmd, abs(speed_pwm)]))
+        self.speed_pwm = speed_pwm
+
+    def get_speed_pwm(self):
+        """Returns the current PWM speed of the wheel."""
+        return self.speed_pwm
+
+    def get_speed_cm_per_sec(self):
+        """Returns the current speed of the wheel."""
+        return self.speedometer.get_speed_cm_per_sec()
+
+    def get_speed_radians_per_sec(self):
+        """Returns the current speed of the wheel in radians per second."""
+        return self.speedometer.get_speed_radians_per_sec()
+
+    def rad_per_sec_to_pwm_speed(self, rad_per_sec):
+        """Returns the PWM speed for the given rad/s speed.
+        We use the multiplier and shift values to calculate the PWM speed using formula:
+        rad_per_sec = pwm * multiplier + shift, for us: pwm = (rad_per_sec - shift) / multiplier."""
+        if self.pwm_multiplier == 0:
+            print("error: wheel %s pwm_multiplier is 0" % self.name)
+            return 0
+        return int((rad_per_sec - self.pwm_shift) / self.pwm_multiplier)
+
+    def cm_per_sec_to_pwm_speed(self, cm_per_sec):
+        """Returns the PWM speed for the given cm/s speed.
+        We just scan the existing table to find the closest speed instead
+        of creating a reverse table due to the lack of memory."""
+        rad_per_sec = self.speedometer.cm_to_radians(cm_per_sec)
+        return self.rad_per_sec_to_pwm_speed(rad_per_sec)
+
+    def get_pwm_ticks_for_distance_using_cm_per_sec(self, speed_cm_per_sec, distance_cm):
+        """Moves the wheel forward using given cm/s speed for given distance in cm.
+        Please note: this method can be used just if the wheel has been calibrated."""
+        speed_pwm = self.cm_per_sec_to_pwm_speed(speed_cm_per_sec)
+        distance_ticks = int(distance_cm * self.speedometer.get_ticks_per_cm())
+        return speed_pwm, distance_ticks
+
+    def stop(self):
+        """Stops the wheel immediately."""
+        self.set_speed_pwm(0)
+        self.distance_remain_ticks = -1
+        self.distance_req_time_ms = -1
+        self.speedometer.reset()
+
+    def stop_on_no_work(self):
+        """Stops the wheel if the remaining distance in ticks or time is reached."""
+        stop_due_to_ticks = False
+        if self.distance_remain_ticks == 0:
+            stop_due_to_ticks = True
+        stop_due_to_time = False
+        if self.distance_req_time_ms > 0:
+            time_delta = ticks_diff(ticks_ms(), self.distance_start_time_ms)
+            if time_delta >= self.distance_req_time_ms:
+                stop_due_to_time = True
+        # we stop only if both conditions are met
+        # otherwise we keep the other condition finish as well
+        if stop_due_to_ticks and stop_due_to_time:
+            self.stop()
+
+    def on_tick(self):
+        """Updates the wheel state based on a new tick,
+        checks the remaining distance in ticks."""
+        self.speedometer.on_tick()
+        if self.distance_remain_ticks > 0:
+            self.distance_remain_ticks -= 1
+            if self.distance_remain_ticks == 0:
+                self.stop_on_no_work()
+
+    def update(self):
+        """Updates the speedometer and general wheel state."""
+        if self.speedometer.update() is True:
+            self.on_tick()
+        self.stop_on_no_work()