MotorManager.nxc

#include "Lib/LineLib.c"
#include "Lib/LibV2.c"
#include "Lib/TachoNav.c"

void mmTurnToHeading(int newHeading)
{
    float degreeTurn = newHeading - tnFlagsReadOnly.heading;
    degreeTurn %= 360;
    if(degreeTurn > 180.0)
        degreeTurn -= 360.0;
    bool turnLeft = 1 == sign(degreeTurn);
    while(abs(tnFlagsReadOnly.heading - newHeading) >= 0.5)
    {
         int delta = tnFlagsReadOnly.heading - newHeading;
         delta %= 360;
         if(delta > 180)
             delta -=360;
         if(abs(delta) > 30)
             delta = sign(delta) * 30;
         char pwr = delta * 3 + sign(delta) * 10;
         OnFwdReg(OUT_A, pwr, OUT_REGMODE_POS);
         OnFwdReg(OUT_C, -pwr, OUT_REGMODE_POS);
         Wait(10);
    }
}



void mmDualPowerMoveForwardDist(float distance)
{
    Vector2f endPos =
        v2Add(tnFlagsReadOnly.position, v2Mult(
           v2FromAngle(tnFlagsReadOnly.heading),
           distance));
    while(v2Dist(tnFlagsReadOnly.position, endPos) > 3.0)
    {
         float curHeading = tnFlagsReadOnly.heading;
         Vector2f curPos = tnFlagsReadOnly.position;
         float angularError =
            v2Angle(v2Dif(endPos, curPos)) - curHeading;
         if(abs(angularError) > 15)
            mmTurnToHeading(v2Angle(v2Dif(endPos, curPos)));
         char pwr;
         if(distance > 30)
            pwr = 100;
         else
            pwr = 3 * distance + 10;
         OnFwdSync(OUT_AC, pwr, 2 * angularError);
    }
    Off(OUT_AC);
}