From b75272ab3601812109a5d565ad86bccb922d86b7 Mon Sep 17 00:00:00 2001 From: Roman Dolejsi Date: Mon, 16 Sep 2024 21:39:24 +0200 Subject: [PATCH] DU-10: Calibration revamped, wheel_calibrator introduced, wheel uses rad_sec as primary speed control, pwm_multiplier and pwm_shift are available to use DU-10: Distance keeper - full implementation, future versions might need to do better wheel-to-wheel compensations --- lesson_10/homework_distance_keeper.py | 98 ++++++++++++ .../homework_gen_wheel_calibration_csv.py | 41 +++++ lesson_10/sonar.py | 64 ++++++++ lesson_10/wheel.py | 150 ++++++++++++++++++ lesson_10/wheel_calibrator.py | 110 +++++++++++++ lesson_10/wheel_driver.py | 61 +++++++ lesson_10/wheel_speedometer.py | 103 ++++++++++++ 7 files changed, 627 insertions(+) create mode 100644 lesson_10/homework_distance_keeper.py create mode 100644 lesson_10/homework_gen_wheel_calibration_csv.py create mode 100644 lesson_10/sonar.py create mode 100644 lesson_10/wheel.py create mode 100644 lesson_10/wheel_calibrator.py create mode 100644 lesson_10/wheel_driver.py create mode 100644 lesson_10/wheel_speedometer.py diff --git a/lesson_10/homework_distance_keeper.py b/lesson_10/homework_distance_keeper.py new file mode 100644 index 0000000..e9c1c33 --- /dev/null +++ b/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() diff --git a/lesson_10/homework_gen_wheel_calibration_csv.py b/lesson_10/homework_gen_wheel_calibration_csv.py new file mode 100644 index 0000000..76fec81 --- /dev/null +++ b/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() diff --git a/lesson_10/sonar.py b/lesson_10/sonar.py new file mode 100644 index 0000000..663dc43 --- /dev/null +++ b/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) diff --git a/lesson_10/wheel.py b/lesson_10/wheel.py new file mode 100644 index 0000000..5abccb0 --- /dev/null +++ b/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() diff --git a/lesson_10/wheel_calibrator.py b/lesson_10/wheel_calibrator.py new file mode 100644 index 0000000..3c802bd --- /dev/null +++ b/lesson_10/wheel_calibrator.py @@ -0,0 +1,110 @@ +from time import ticks_ms, ticks_diff, sleep + +from wheel import Wheel + + +class WheelCalibrator: + def __init__(self, wheel: Wheel): + """Initializes the wheel calibrator.""" + self.wheel = wheel + self.pwm_speed_to_rad_per_sec = [0.0 for _ in range(0, 256)] + self.pwm_speed_min = 0 + self.pwm_speed_max = 0 + self.conversion_table_available = False + + def gather_pwm_to_real_speed_table_all(self): + """Gathers the real forward and reverse speeds for a desired PWM range + based on the speedometer readings. Each value is scanned after half a second. + The calibration fills the conversion table between PWM and rad/s.""" + self.pwm_speed_min = 0.0 + for speed_pwm in range(self.wheel.pwm_min, self.wheel.pwm_max + 1): + self.calibrate_pwm(speed_pwm=speed_pwm, gather_min=True) + self.calculate_pwm_value_multiplier_and_shift() + self.conversion_table_available = True + + def gather_pwm_to_real_speed_table_fast_approx(self): + """Gathers the approximate forward and reverse speeds for a desired PWM range + based on the speedometer readings. The range is sampled from bottom to top + in 10 PWM step intervals to find the lowest moving speed. As a follow-up step, + the top speed is scanned as well to find the highest moving speed. + The calibration fills the conversion table between PWM and rad/s using a linear + approximation. Multiplier (a) and shift (b) are calculated for the equation + y = a * x + b.""" + # minimum speed + speed_pwm = self.wheel.pwm_min + while speed_pwm <= self.wheel.pwm_max: + rad_per_sec, cm_per_sec = self.gather_rad_cm_per_sec_for_pwm(speed_pwm) + self.pwm_speed_to_rad_per_sec[speed_pwm] = rad_per_sec + if rad_per_sec > 0.0: + self.pwm_speed_min = speed_pwm + break + speed_pwm += 10 + if self.pwm_speed_min == 0: + print("No movement detected when going through pwm_min .. pwm_max range, %s wheel calibration failed" + % self.wheel.name) + return -1 + if self.pwm_speed_min > self.wheel.pwm_min: + print("Altering %s wheel pwm_min from %s to %s" % (self.wheel.name, self.wheel.pwm_min, self.pwm_speed_min)) + self.wheel.pwm_min = self.pwm_speed_min + + # maximum speed + speed_pwm = self.wheel.pwm_max + rad_per_sec, cm_per_sec = self.gather_rad_cm_per_sec_for_pwm(speed_pwm) + self.pwm_speed_to_rad_per_sec[speed_pwm] = rad_per_sec + if rad_per_sec > 0.0: + self.pwm_speed_max = speed_pwm + else: + print("No movement detected on pwm_max speed, %s wheel calibration failed" % self.wheel.name) + return -1 + + # linear approximation in radians per second + rad_speed_delta = self.pwm_speed_to_rad_per_sec[self.pwm_speed_max] - self.pwm_speed_to_rad_per_sec[ + self.pwm_speed_min] + a = rad_speed_delta / (self.pwm_speed_max - self.pwm_speed_min) + b = self.pwm_speed_to_rad_per_sec[self.pwm_speed_min] - a * self.pwm_speed_min + for speed_pwm in range(self.wheel.pwm_min, self.wheel.pwm_max + 1): + speed_write = a * speed_pwm + b + self.pwm_speed_to_rad_per_sec[speed_pwm] = speed_write + self.calculate_pwm_value_multiplier_and_shift() + self.conversion_table_available = True + + def calibrate_pwm(self, speed_pwm, gather_min): + rad_per_sec, cm_per_sec = self.gather_rad_cm_per_sec_for_pwm(speed_pwm) + self.pwm_speed_to_rad_per_sec[speed_pwm] = rad_per_sec + if gather_min and rad_per_sec > 0.0 and self.pwm_speed_min == 0: + self.pwm_speed_min = speed_pwm + + def gather_rad_cm_per_sec_for_pwm(self, speed_pwm): + """Moves the wheel forward using given PWM speed for half a second, + returns the speed in rad/s and cm/s.""" + start_time = ticks_ms() + self.wheel.move_pwm(speed_pwm) + while ticks_diff(ticks_ms(), start_time) <= 500: + self.wheel.update() + sleep(0.001) + rad_per_sec = self.wheel.get_speed_radians_per_sec() + cm_per_sec = self.wheel.get_speed_cm_per_sec() + self.wheel.stop() + return rad_per_sec, cm_per_sec + + def calculate_pwm_value_multiplier_and_shift(self): + """ + Calculates pwm multiplier and shift using least squares regression (from valid pwm range). + multiplier = (count()*sum(pwd*rad) - sum(pwm)*sum(rad)) / (count()*sum(pwm*pwm) - sum(pwm)*sum(pwm)) + shift = (sum(rad) - multiplier*sum(pwm)) / count() + """ + count = self.wheel.pwm_max - self.wheel.pwm_min + 1 + sum_pwm = 0 + sum_pwm_mult_pwm = 0 + sum_rad = 0 + sum_pwm_mult_rad = 0 + for pwm in range(self.wheel.pwm_min, self.wheel.pwm_max + 1): + sum_pwm += pwm + sum_pwm_mult_pwm += pwm * pwm + sum_rad += self.pwm_speed_to_rad_per_sec[pwm] + sum_pwm_mult_rad += pwm * self.pwm_speed_to_rad_per_sec[pwm] + self.wheel.pwm_multiplier = (count * sum_pwm_mult_rad - sum_pwm * sum_rad) / \ + (count * sum_pwm_mult_pwm - sum_pwm * sum_pwm) + self.wheel.pwm_shift = (sum_rad - self.wheel.pwm_multiplier * sum_pwm) / count + print("Altering %s wheel multiplier: %s, shift: %s" % + (self.wheel.name, self.wheel.pwm_multiplier, self.wheel.pwm_shift)) diff --git a/lesson_10/wheel_driver.py b/lesson_10/wheel_driver.py new file mode 100644 index 0000000..6dedb7b --- /dev/null +++ b/lesson_10/wheel_driver.py @@ -0,0 +1,61 @@ +from microbit import pin14, pin15, i2c + +from wheel import Wheel +from wheel_calibrator import WheelCalibrator + + +class WheelDriver: + """Handles the movement of the whole robot + (forward, backward, turning). Activities are either + indefinite or conditional based on ticks, time + or real speed measured by the speedometer on wheel level.""" + I2C_ADDRESS = 0x70 + + def __init__(self, + left_pwm_min=60, left_pwm_max=255, left_pwm_multiplier=0, left_pwm_shift=0, + right_pwm_min=60, right_pwm_max=255, right_pwm_multiplier=0, right_pwm_shift=0): + """Initializes the wheel driver.""" + i2c.init(freq=100000) + i2c.write(self.I2C_ADDRESS, b"\x00\x01") + i2c.write(self.I2C_ADDRESS, b"\xE8\xAA") + self.wheel_left = Wheel(name="left", i2c_address=self.I2C_ADDRESS, + motor_fwd_cmd=5, motor_rwd_cmd=4, sensor_pin=pin14, + pwm_min=left_pwm_min, pwm_max=left_pwm_max, + pwm_multiplier=left_pwm_multiplier, pwm_shift=left_pwm_shift) + self.wheel_right = Wheel(name="right", i2c_address=self.I2C_ADDRESS, + motor_fwd_cmd=3, motor_rwd_cmd=2, sensor_pin=pin15, + pwm_min=right_pwm_min, pwm_max=right_pwm_max, + pwm_multiplier=right_pwm_multiplier, pwm_shift=right_pwm_shift) + self.wheel_calibrator_left = WheelCalibrator(wheel=self.wheel_left) + self.wheel_calibrator_right = WheelCalibrator(wheel=self.wheel_right) + + def move_pwm(self, speed_pwm_left, speed_pwm_right): + """Moves the robot with the PWM given speed for each wheel.""" + self.wheel_left.move_pwm(speed_pwm_left) + self.wheel_right.move_pwm(speed_pwm_right) + + def move_pwm_for_ticks(self, speed_pwm_left, speed_pwm_right, + distance_ticks_left, distance_ticks_right): + """Moves robot using PWM speed to given distance in ticks, for each wheel.""" + self.wheel_left.move_pwm_for_ticks(speed_pwm_left, distance_ticks_left) + self.wheel_right.move_pwm_for_ticks(speed_pwm_right, distance_ticks_right) + + def move_pwm_for_time(self, speed_pwm_left, speed_pwm_right, time_ms): + """Moves robot using PWM speed for each wheel for given time.""" + self.wheel_left.move_pwm_for_time(speed_pwm_left, time_ms) + self.wheel_right.move_pwm_for_time(speed_pwm_right, time_ms) + + def move_pwm_for_distance(self, speed_pwm_left, speed_pwm_right, distance_cm): + """Moves robot using PWM speed for each wheel to given distance.""" + self.wheel_left.move_pwm_for_distance(speed_pwm_left, distance_cm) + self.wheel_right.move_pwm_for_distance(speed_pwm_right, distance_cm) + + def stop(self): + """Stops the robot.""" + self.wheel_left.stop() + self.wheel_right.stop() + + def update(self): + """Updates the wheel driver, propagating the changes to the hardware.""" + self.wheel_left.update() + self.wheel_right.update() diff --git a/lesson_10/wheel_speedometer.py b/lesson_10/wheel_speedometer.py new file mode 100644 index 0000000..ac0b879 --- /dev/null +++ b/lesson_10/wheel_speedometer.py @@ -0,0 +1,103 @@ +from time import ticks_ms, ticks_diff + + +class WheelSpeedometer: + """Speedometer is able to monitor the wheel movement precisely + and provide the actual wheel rotation speed over the ticks + measured for X consecutive constant time intervals.""" + PI = 3.14159265359 + + def __init__(self, sensor_pin): + """Initializes the wheel speedometer.""" + self.sensor_pin = sensor_pin + self.sensor_value = 0 + self.tick_last_time = 0 + self.tick_sampling_ms = 10 + self.tick_counter = 0 + self.tick_history = [] + self.tick_history_time = [] + self.tick_history_length = 5 + self.tick_history_interval_ms = self.tick_history_length * self.tick_sampling_ms + self.tick_history_sum = 0 + self.ticks_per_wheel = 40 + self.radians_per_wheel = 2 * self.PI + self.radians_per_tick = self.radians_per_wheel / self.ticks_per_wheel + self.wheel_radius_cm = 3.75 + self.cm_per_wheel = 2 * self.PI * self.wheel_radius_cm + self.speed_radians = 0 + self.speed_cm = 0 + self.on_tick_counter = 0 + + def reset(self): + """Resets the speedometer state.""" + self.tick_last_time = 0 + self.tick_counter = 0 + self.tick_history = [] + self.tick_history_time = [] + + def update(self): + """Updates the speedometer state based on the ongoing command.""" + """Retrieves the sensor value, checks for change and updates the wheel state + based on the ongoing command.""" + sensor_value_now = self.sensor_pin.read_digital() + if sensor_value_now == self.sensor_value: + return False + self.sensor_value = sensor_value_now + self.on_tick() + return True + + def on_tick(self): + """Updates the speedometer state based on a new tick.""" + self.tick_counter += 1 + self.on_tick_counter += 1 + now = ticks_ms() + time_delta = ticks_diff(now, self.tick_last_time) + # if the last tick was too long ago, we reset the history + history_too_old = time_delta >= self.tick_history_interval_ms * self.tick_history_length + if self.tick_last_time == 0 or history_too_old: + self.tick_last_time = now + self.tick_history_time = [] + self.tick_history_time.append(now) + self.tick_history = [] + self.tick_history.append(self.tick_counter) + self.tick_history_sum = self.tick_counter + elif time_delta >= self.tick_history_interval_ms: + self.tick_history_time.append(now) + self.tick_history.append(self.tick_counter) + self.tick_history_sum += self.tick_counter + if len(self.tick_history) > self.tick_history_length: + self.tick_history_time.pop(0) + tick_count_obsolete = self.tick_history.pop(0) + self.tick_history_sum -= tick_count_obsolete + self.tick_counter = 0 + self.tick_last_time = now + + def get_speed_radians_per_sec(self): + """Returns the current wheel speed in radians/s.""" + if len(self.tick_history) == 0: + return 0 + # print("history %s" % self.tick_history) + elapsed_time = ticks_diff(self.tick_history_time[-1], self.tick_history_time[0]) + sec_fragment = (1000 / elapsed_time) if elapsed_time > 0 else 1 + speed = (self.tick_history_sum / self.ticks_per_wheel) * self.radians_per_wheel * sec_fragment + return speed + + def get_speed_cm_per_sec(self): + """Returns the current wheel speed in cm/s.""" + return (self.get_speed_radians_per_sec() / self.radians_per_wheel) * self.cm_per_wheel + + def cm_to_radians(self, cm): + """Converts cm to radians.""" + return cm / self.cm_per_wheel * self.radians_per_wheel + + def radians_to_cm(self, radians): + """Converts radians to cm.""" + return radians / self.radians_per_wheel * self.cm_per_wheel + + def get_ticks_per_cm(self): + """Returns the number of ticks per cm.""" + return self.ticks_per_wheel / self.cm_per_wheel + + def __str__(self): + params = (self.get_speed_cm_per_sec(), self.tick_history) + return "speed: %.2f cm/s, tick history: %s" % params