Tested on 960a3163ed68e3fc46b15fb590b7e60c748979ad
![image](https://user-images.githubusercontent.com/12326241/196248464-04b8691f-c293-4511-88c8-1305b3d5b549.png)
Pybricks MicroPython v3.2.0b3-97-g2270df4e on 2022-10-15; SPIKE Prime Hub with STM32F413VG
1) Program crashes (bad mpy, but keeps running)
Some other variations with bad programs cause a watchdog timeout as well. This variant keeps the hub running, which may help with debugging.
from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Direction
from pybricks.tools import wait, StopWatch
from pybricks.geometry import Axis
# MicroPython imports.
from umath import copysign
from usys import stdin
from uselect import poll
# Initialize motors.
left = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right = Motor(Port.B)
left.reset_angle(0)
right.reset_angle(0)
# Initialize the hub.
hub = InventorHub()
# Control constants.
DUTY_FRICTION = 10
VOLTAGE_NOMINAL = 7400
# Drive speeds in motor degrees per second.
FORWARD = 350
STOPPED = 0
BACKWARD = -350
# Turn speed in motor degrees per second.
LEFT = -300
STRAIGHT = 0
RIGHT = 300
# Drive/turn command for each numpad key.
COMMANDS = {
"1": (BACKWARD, LEFT),
"2": (BACKWARD, STRAIGHT),
"3": (BACKWARD, RIGHT),
"4": (STOPPED, LEFT),
"5": (STOPPED, STRAIGHT),
"6": (STOPPED, RIGHT),
"7": (FORWARD, LEFT),
"8": (FORWARD, STRAIGHT),
"9": (FORWARD, RIGHT),
}
# Register the standard input so we can read keyboard presses.
keyboard = poll()
keyboard.register(stdin)
# Initialize the loop timer.
timer = StopWatch()
# Initialize angle buffer for motor speed calculation.
LOOP_TIME = 0.015
# Motor speed is obtained as the motor position difference across
# a given time span. Time span must be multiple of loop time.
angle_filter_time = 0.150
angle_filter_size = round(angle_filter_time / LOOP_TIME)
angle_filter_buffer = [0] * angle_filter_size
angle_filter_index = 0
2) Fails before it runs
from pybricks.hubs import InventorHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Direction
from pybricks.tools import wait, StopWatch
from pybricks.geometry import Axis
# MicroPython imports.
from umath import copysign
from usys import stdin
from uselect import poll
# Initialize motors.
left = Motor(Port.A, Direction.COUNTERCLOCKWISE)
right = Motor(Port.B)
left.reset_angle(0)
right.reset_angle(0)
# Initialize the hub.
hub = InventorHub()
# Control constants.
DUTY_FRICTION = 10
VOLTAGE_NOMINAL = 7400
# Drive speeds in motor degrees per second.
FORWARD = 350
STOPPED = 0
BACKWARD = -350
# Turn speed in motor degrees per second.
LEFT = -300
STRAIGHT = 0
RIGHT = 300
# Drive/turn command for each numpad key.
COMMANDS = {
"1": (BACKWARD, LEFT),
"2": (BACKWARD, STRAIGHT),
"3": (BACKWARD, RIGHT),
"4": (STOPPED, LEFT),
"5": (STOPPED, STRAIGHT),
"6": (STOPPED, RIGHT),
"7": (FORWARD, LEFT),
"8": (FORWARD, STRAIGHT),
"9": (FORWARD, RIGHT),
}
# Register the standard input so we can read keyboard presses.
keyboard = poll()
keyboard.register(stdin)
# Initialize the loop timer.
timer = StopWatch()
# Initialize angle buffer for motor speed calculation.
LOOP_TIME = 0.015
# Motor speed is obtained as the motor position difference across
# a given time span. Time span must be multiple of loop time.
angle_filter_time = 0.150
angle_filter_size = round(angle_filter_time / LOOP_TIME)
angle_filter_buffer = [0] * angle_filter_size
angle_filter_index = 0
# End-user controls for speed and steering.
user_drive_speed = 0
user_steering_speed = 0
# Initial state values.
target_motor_angle = 0
target_steering = 0
gyro_angle = 0
gyro_average = 0
gyro_filter = 0.01
while True:
# Reset loop timer.
timer.reset()
# Filter the angular speed measurement.
gyro_raw = -hub.imu.angular_velocity(Axis.Y)
gyro_average = gyro_average * (1 - gyro_filter) + gyro_filter * gyro_raw
gyro_filter = max(0.997 * gyro_filter, 0.0005)
# Get angular gyro_speed and estimated angle.
gyro_speed = gyro_raw - gyro_average
gyro_angle += gyro_speed * LOOP_TIME
# Get average motor angle.
motor_angle = (left.angle() + right.angle()) / 2
# Calculate average motor speed.
motor_speed = (motor_angle - angle_filter_buffer[angle_filter_index]) / angle_filter_time
angle_filter_buffer[angle_filter_index] = motor_angle
angle_filter_index = (angle_filter_index + 1) % angle_filter_size
# Update user controls if given.
if keyboard.poll(0):
try:
user_drive_speed, user_steering_speed = COMMANDS[stdin.read(1)]
print("Speed:", user_drive_speed, "steering:", user_steering_speed)
except KeyError:
user_drive_speed, user_steering_speed = 0, 0
# Update target angle and steering based on user input.
target_motor_angle += user_drive_speed * LOOP_TIME
target_steering += user_steering_speed * LOOP_TIME
motor_angle_error = motor_angle - target_motor_angle
# Calculate balance control signal as weighed sum of all four states.
# This has the effect of driving all of them to zero.
duty_balance = (
gyro_speed * 0.1 +
gyro_angle * 15 +
motor_speed * 0.15 +
motor_angle_error * 0.1
)
# Calculate steering control signal
duty_steering = (right.angle() - left.angle() + target_steering) * 0.2
# Combine balance and steering for left motor.
duty_left = duty_balance + duty_steering
duty_left += copysign(DUTY_FRICTION, duty_left)
duty_left *= VOLTAGE_NOMINAL / hub.battery.voltage()
# Combine balance and steering for right motor.
duty_right = duty_balance - duty_steering
duty_right += copysign(DUTY_FRICTION, duty_right)
duty_right *= VOLTAGE_NOMINAL / hub.battery.voltage()
# Apply duty cycle for balancing and steering.
left.dc(duty_left)
right.dc(duty_right)
# Wait for the loop time to complete.
while timer.time() < LOOP_TIME * 1000:
wait(1)