Do you need to implement control loops in software?
"Applied Control Theory for Embedded Systems" gives you just what it says.
See details at http://www.wescottdesign.com/actfes/actfes.html
try 2 comarators set the trigger at mid point then cros feed F/B to the input the results should be the difference want DC rectify and se the dc either leading or lagging