roll_target = 89.95
power = 3
integral = 0
#Not actual game values (k)
kp = 0
ki = 0
kd = 0
# Write your program here.
hub.light_matrix.show_image('HAPPY')
wait_for_seconds(3)
while True:
error = roll_target - hub.motion_sensor.get_roll_angle()
integral = integral + error * 0.25
derivative = error - prev_error
prev_error = error
result = error * kp + integral * ki + derivative * kd
print(
"error:",
error,
"integral:",
integral,
"derivative:",
derivative,
"prev_error:",
prev_error,
"result:",
result
)
movement_motors.start(steering=0, speed=result * power)
# %% [markdown]
# # Move shopper Charlie
#
# So far, we've used `move` to move the wheel motors.
# However, when using `move`, the program will not continue until the specified value is reached.
# This is a problem, since we want to move the wheels *and* the arms at the same time.
# Therefore, we will use `start` and `stop`. This comes at the cost of being unable to
# define a stopping condition based on the motors alone (e.g., distance, time).
# We have to stop them at certain point of the program (as we do here) or based on a
# sensory input.
#
# I wonder if there's a way to perform async execution using the vanilla (Micro)Python.
# I'll look into that in the future.
# %%
print("Moving shopper Charlie...")
motors_wheels.start()
wait_for_seconds(1) # This delays moving the arms, but doesn't affect the already moving wheel motors.
for ii in range(0, 5):
motors_arms.move(0.3, unit='seconds', speed=40)
motors_arms.move(0.2, unit='seconds', speed=-40)
motors_wheels.stop()
print("DONE!")
# %%
print("-"*15 + " Execution ended " + "-"*15 + "\n")
"""
from mindstorms import MSHub, Motor, MotorPair, ColorSensor, DistanceSensor, App
from mindstorms.control import wait_for_seconds, wait_until, Timer
from mindstorms.operator import greater_than, greater_than_or_equal_to, less_than, less_than_or_equal_to, equal_to, not_equal_to
import math
import time
# First experiments with a balancing robot... no yet finished.
# See https://medium.com/@janislavjankov/self-balancing-robot-with-lego-spike-prime-ac156af5c2b2 for a more
# complete example.
wheel_radius = 2.8
# Create your objects here.
hub = MSHub()
motor_pair = MotorPair('B', 'F')
motor_pair.set_default_speed(50)
motor_pair.set_motor_rotation(wheel_radius * 2 * math.pi, 'cm')
while True:
r = hub.motion_sensor.get_roll_angle() + 90
motor_pair.start(speed=-r * 40)