Skip to content

Instantly share code, notes, and snippets.

@peheje

peheje/Lego Mindstorms PID

Last active April 5, 2021 20:01
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save peheje/03bed10d8cb81a255de1d4f10abb1689 to your computer and use it in GitHub Desktop.
Save peheje/03bed10d8cb81a255de1d4f10abb1689 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
Lego Mindstorms PID
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
hub = MSHub()
motors = MotorPair('E', 'A')
roll_target = 89
kp = 15.0
ki = 0.4
kd = 5.0
integral, derivative, error, previous_error, control = 0.0, 0.0, 0.0, 0.0, 0.0
while True:
error = roll_target - hub.motion_sensor.get_roll_angle()
integral += error
derivative = error - previous_error
previous_error = error
control = error * kp + integral * ki + derivative * kd
motors.start_at_power(int(control))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment