pxt-ev3/docs/examples/line-follower-pid.md

50 lines
1.3 KiB
Markdown
Raw Normal View History

2018-01-08 23:36:34 +01:00
# Gradien follower PID + calibration
2018-01-08 23:06:39 +01:00
```blocks
let lasterror = 0
let D = 0
let I = 0
let P = 0
let error = 0
2018-01-08 23:36:34 +01:00
let setpoint = 0
let max = 0
let min = 0
let v = 0
v = sensors.color3.light(LightIntensityMode.Reflected)
min = v
max = v
setpoint = v
while (!(brick.buttonEnter.wasPressed())) {
brick.clearScreen()
brick.showString("Move robot on terrain", 1)
brick.showString("Press ENTER when done", 2)
v = sensors.color3.light(LightIntensityMode.Reflected)
min = Math.min(min, v)
max = Math.max(max, v)
setpoint = (max + min) / 2
brick.showValue("v", v, 3)
brick.showValue("min", min, 4)
brick.showValue("max", v, 5)
brick.showValue("setpoint", setpoint, 6)
loops.pause(100)
}
2018-01-08 23:06:39 +01:00
loops.forever(function () {
2018-01-08 23:36:34 +01:00
brick.clearScreen()
v = sensors.color3.light(LightIntensityMode.Reflected)
brick.showValue("light", v, 1)
error = v - setpoint
brick.showValue("error", error, 2)
2018-01-08 23:06:39 +01:00
P = error * 5
2018-01-08 23:36:34 +01:00
brick.showValue("P", P, 3)
2018-01-08 23:06:39 +01:00
I = I + error * 0.01
2018-01-08 23:36:34 +01:00
brick.showValue("I", I, 4)
2018-01-08 23:06:39 +01:00
D = (error - lasterror) * 0.2
2018-01-08 23:36:34 +01:00
brick.showValue("D", D, 5)
2018-01-08 23:06:39 +01:00
motors.largeBC.steer(P + (I + D), 100)
lasterror = error
2018-01-08 23:36:34 +01:00
if (brick.buttonEnter.wasPressed()) {
motors.largeBC.setSpeed(0)
brick.buttonDown.pauseUntil(ButtonEvent.Bumped)
2018-01-08 23:36:34 +01:00
}
2018-01-08 23:06:39 +01:00
})
```