Micropython EV3 Functions
unknown
python
2 years ago
4.8 kB
4
Indexable
# General Purpose Functions #!usr/bin/env pybricks-micropython from pybricks.hubs import EV3Brick from pybricks.ev3devices import (Motor, ColorSensor, GyroSensor) from pybricks.robotics import DriveBase from pybricks.parameters import Port, Color, Stop # Objects robot = DriveBase(Motor(Port.A), Motor(Port.B), wheel_diameter = 142.24, axle_track = 152) color_sensor = ColorSensor(Port.S1) color_sensor2 = ColorSensor(Port.S2) gyro_sensor = GyroSensor(Port.S3) left_wheel = Motor(Port.A) right_wheel = Motor(Port.B) ev3 = EV3Brick() # PROGRAM ## Color Sensor ### PID Line Follower def follow_line_pid(threshold): integral = 0 last_error = 0 kp = 2 ki = .09 kd = .5 while True: error = threshold - color_sensor.reflection() integral += error derivative = error - last_error proportional_gain = error * kp integral_gain = integral * ki derivative_gain = derivative * kd correction = proportional_gain + integral_gain + derivative_gain robot.drive(50, correction) error = last_error ### PID Double Line Follower def double_follow_line_pid(): integral = 0 last_error = 0 kp = 2 ki = 0.09 kd = 0.5 while True: error = color_sensor.reflection() - color_sensor2.reflection() integral += error derivative = error - last_error proportional_gain = error * kp integral_gain = integral * ki derivative_gain = derivative * kd correction = proportional_gain + integral_gain + derivative_gain robot.drive(50, correction) last_error = error ## Gyro Sensor ### Proportional Gyrostrate def gyrostraight_proportional(angle, distance): gyro_sensor.reset_angle(angle) while robot.distance() != distance: direction = gyro_sensor.angle() * -10 robot.drive(50, direction) right_wheel.hold() left_wheel.hold() robot.drive(0, 0) ### PID Gyrostrate def gyrostraight_pid(angle, distance): gyro_sensor.reset_angle(angle) integral = 0 last_error = 0 kp = 2 ki = 0.09 kd = 0.5 while robot.distance() != distance: error = angle - gyro_sensor.angle() derivative = error - last_error integral += error proportional_gain = error * kp integral_gain = integral * ki derivative_gain = derivative * kd correction = proportional_gain + integral_gain + derivative_gain robot.drive(100, correction) robot.drive(0, 0) right_wheel.hold() left_wheel.hold() ### PID Perfect Turn def perfect_turn(turn_value): gyro_sensor.reset_angle(0) last_error = 0 kp = 2 ki = 0 kd = 0.49 if turn_value > 0: while not gyro_sensor.angle() > (turn_value - 1): error = turn_value - gyro_sensor.angle() derivative = error - last_error proportional_gain = error * kp derivative_gain = derivative * kd correction = proportional_gain + derivative_gain left_wheel.run(correction + 5) right_wheel.run(-correction - 5) error = last_error robot.drive(0, 0) right_wheel.hold() left_wheel.hold() else: while not gyro_sensor.angle() < (turn_angle + 1): error = turn_value - gyro_sensor.angle() derivative = error - last_error proportional_gain = error * kp derivative_gain = derivative * kd correction = proportional_gain + derivative_gain left_wheel.run(-correction - 5) right_wheel.run(correction + 5) error = last_error robot.drive(0, 0) right_wheel.hold() left_wheel.hold() ### 2nd PID Perfect Turn def turn(turn_value): robot.reset() gyro_sensor.reset_angle(0) right_wheel.reset_angle() left_wheel.reset_angle() integral = 0 last_error = 0 kp = 2 ki = 0.0085 kd = 0.5 while abs(gyro_sensor.angle() - turn_value) >= 1: error = turn_value - gyro_sensor.angle() derivative = error - last_error last_error = error integral += error proportional_gain = error * kp integral_gain = integral * ki derivative_gain = derivative * kd correction = proportional_gain + integral_gain + derivative_gain right_wheel.run(-correction - 9) left_wheel.run(correction + 9) robot.drive(0, 0) left_wheel.hold() right_wheel.hold()
Editor is loading...