|
| 1 | +# SPDX-License-Identifier: MIT |
| 2 | +# Copyright (c) 2020 The Pybricks Authors |
| 3 | + |
| 4 | +""" |
| 5 | +Hardware Module: Any medium-sized drive base with two angular medium motors. |
| 6 | +
|
| 7 | +Description: Verifies data logging capability for a drive base. |
| 8 | +""" |
| 9 | + |
| 10 | +from pybricks.pupdevices import Motor |
| 11 | +from pybricks.tools import wait |
| 12 | +from pybricks.parameters import Port, Direction |
| 13 | +from pybricks.robotics import DriveBase |
| 14 | + |
| 15 | +# Initialize the motor and drive base. |
| 16 | +left_motor = Motor(Port.C, Direction.COUNTERCLOCKWISE) |
| 17 | +right_motor = Motor(Port.D) |
| 18 | +drive_base = DriveBase(left_motor, right_motor, wheel_diameter=44, axle_track=14 * 8) |
| 19 | +drive_base.reset() |
| 20 | + |
| 21 | +# Allocate logs for motors and controller signals. |
| 22 | +DURATION = 6000 |
| 23 | +left_motor.log.start(DURATION) |
| 24 | +right_motor.log.start(DURATION) |
| 25 | +drive_base.distance_control.log.start(DURATION) |
| 26 | +drive_base.heading_control.log.start(DURATION) |
| 27 | + |
| 28 | +# Drive for half a meter. |
| 29 | +drive_base.straight(500) |
| 30 | + |
| 31 | +# Wait so we can also log hold capability, then turn off the motor completely. |
| 32 | +wait(100) |
| 33 | +drive_base.stop() |
| 34 | + |
| 35 | +# Save the data by printing it. Pybricksdev will capture the printed |
| 36 | +# data and save it to the given path relative to this script. |
| 37 | +left_motor.log.save("build/log_drive_base_left_motor.txt") |
| 38 | +right_motor.log.save("build/log_drive_base_right_motor.txt") |
| 39 | +drive_base.distance_control.log.save("build/log_drive_base_distance_control.txt") |
| 40 | +drive_base.heading_control.log.save("build/log_drive_base_heading_control.txt") |
| 41 | + |
| 42 | + |
| 43 | +# The logs can also be read from the user script. As a check, let's print the |
| 44 | +# distance and the estimated speed in a rotated plot. We show only 1 in 5 |
| 45 | +# samples here and scale the speed and angle values for better visibility. |
| 46 | + |
| 47 | +start_distance = drive_base.distance_control.log.get(0)[2] |
| 48 | + |
| 49 | +for i in range(len(drive_base.distance_control.log) / 5): |
| 50 | + |
| 51 | + # Read the (i * 3)th row in the data log. |
| 52 | + values = drive_base.distance_control.log.get(i * 5) |
| 53 | + speed = values[9] |
| 54 | + angle = values[2] - start_distance |
| 55 | + |
| 56 | + # Turn it into a line of spaces with * for speed and + for angle. |
| 57 | + line = [ord(" ")] * 100 |
| 58 | + line[min(speed // 10, 99)] = ord("*") |
| 59 | + line[min(angle // 36, 99)] = ord("+") |
| 60 | + |
| 61 | + # Print it. Looked at it sideways, you should see a trapezoidal graph |
| 62 | + # for speed and a gradual ramp for the angle as it drives forward. |
| 63 | + print(bytes(line)) |
0 commit comments