[jump to content]

niHP/ blog/ position control test

position control test

This is a position control test I made for the Mindboards forum:

Download video.

It uses the work in progress position control algorithm implemented for NXT Improved Firmware and a really simple NXC program:

void
PositionRotatePID (long angle, byte kp, byte ki, byte kd)
{
    SetOutput (OUT_A,
               OutputMode, OUT_MODE_MOTORON + OUT_MODE_BRAKE + OUT_MODE_REGULATED,
               RegMode, 4,
               RunState, OUT_RUNSTATE_RUNNING,
               Power, 50,
               TurnRatio, 0,
               TachoLimit, angle,
               RegPValue, kp, RegIValue, ki, RegDValue, kd,
               UpdateFlags, UF_UPDATE_TACHO_LIMIT + UF_UPDATE_SPEED + UF_UPDATE_MODE + UF_UPDATE_PID_VALUES);
}

inline void
PositionRotate (long angle)
{
    PositionRotatePID (angle, 96, 0, 32);
}

task main ()
{   
    /* Turn to 90 degree. */
    PositionRotate(90);
    Wait (1500);
    /* Go back to 0. */
    PositionRotate(0);
    Wait (1500);
    /* Turn to 90 degree again. */
    PositionRotate(90);
    Wait (1500);
    /* Now follow motor B. */
    while (1)
      {
        PositionRotate(MotorTachoCount (OUT_B));
        Wait (1);
      }
}