An oscillator control system includes an non-linear oscillator structure configured to oscillate about an axis; a driver circuit configured to generate a driving signal to drive the oscillator structure; a detection circuit configured to measure an angle amplitude and a phase error of the oscillator structure; an amplitude controller configured to generate a reference oscillator period based on the measured angle amplitude; a period and phase controller configured to receive the reference oscillator period and the measured phase error from the detection circuit, generate at least one control parameter of the driving signal based on the reference oscillator period and the measured phase error, and determine a driving period of the driving signal based on the reference oscillator period and the measured phase error. The driver circuit is configured to generate the driving signal based on the at least one control parameter and the driving period.