Ocean. Hill. Beach. Fog. Drizzle. Wildflowers.
Perfect.
(Best viewed on a big screen).
/**
* Public domain. Use at your own risk!
*/
let TARGET_ANGLE = 155
let KE = 420
let KD = 8
let KI = 5
let motor_left_bias = -2;
let motor_right_bias = 2;
function control_loop() {
motor_coast()
let motor_out = 0;
let vertical_count = 0;
let motor_on = false
let est_angle = read_accel_tilt_angle()
let start_time = input.runningTime()
let last_time = start_time
let last_err = 0;
let i_err = 0;
while (true) {
let current_time = input.runningTime()
let delta_t = current_time - last_time
last_time = current_time
est_angle = updateAngle(est_angle, delta_t)
if (motor_on && (est_angle > 3000 || est_angle < -3000)) {
motor_coast()
motor_on = false
}
let err = est_angle - TARGET_ANGLE
if (motor_on) {
let d_err = (err - last_err) * 1000 / delta_t
last_err = err
i_err = i_err + err * delta_t
let u = err * KE + d_err * KD + i_err * KI
motor_out = Math.clamp(-90, 90, u / 10000)
motor_move(motor_out + motor_left_bias, motor_out + motor_right_bias)
} else if (err <= 500 && err >= -500) {
vertical_count = vertical_count + 1
if (vertical_count > 100) {
vertical_count = 0
last_err = 0
i_err = 0
est_angle = read_accel_tilt_angle()
motor_on = true
}
} else {
vertical_count = 0
}
basic.pause(1)
}
}
function read_gyro_angle_rate(): number {
InvMPU.read_gyro()
return -InvMPU.gyro_y
}
function read_accel_tilt_angle(): number {
InvMPU.read_accel()
return Math.round(Math.atan2(InvMPU.accel_z, -InvMPU.accel_x) * 18000 / Math.PI)
}
function updateAngle(old_angle: number, delta_t_ms: number): number {
let accel_angle = read_accel_tilt_angle()
let gyro_angle_rate = read_gyro_angle_rate() * 200000 / 32768
let gyro_angle_change = gyro_angle_rate * delta_t_ms / 1000
return (49 * (old_angle + gyro_angle_change) + accel_angle) / 50
}
function motor_coast() {
pins.digitalWritePin(DigitalPin.P15, 0)
pins.digitalWritePin(DigitalPin.P16, 0)
}
function motor_move(left: number, right: number) {
pins.servoWritePin(AnalogPin.P15, 90 - right)
pins.servoWritePin(AnalogPin.P16, 90 + left)
}
input.onButtonPressed(Button.A, function on_button_pressed_a() {
TARGET_ANGLE = TARGET_ANGLE + 5
})
input.onButtonPressed(Button.B, function on_button_pressed_b() {
TARGET_ANGLE = TARGET_ANGLE - 5
})
input.onButtonPressed(Button.AB, function on_button_pressed_ab() {
basic.showNumber(TARGET_ANGLE)
})
if (InvMPU.find_mpu()) {
basic.showIcon(IconNames.Happy)
InvMPU.reset_mpu(5, 5)
basic.pause(100)
InvMPU.set_gyro_bias(5, 1, -13)
control_loop()
} else {
basic.showIcon(IconNames.No)
}