Trapeziod motion profile working awesome right now, corrected simulator drive polarity