Ramp Up / Ramp Down

[mcsummation]

I put in some code to log out the tick counter, the angle on the motor, and power. Here's the datalogger:

Code: Select all

long steps[3000][4];
long stepcount=0;
void ticky(int phase)
{
    steps[stepcount][0] = phase;
    steps[stepcount][1] = CurrentTick();
    steps[stepcount][2] = MotorRotationCount(OUT_B);
    steps[stepcount][3] = MotorPower(OUT_B);
    stepcount++;
}

I put it in the "wait for idle loop" of each phase of the RURD process:

Code: Select all

void RotateMotorRURD2(byte output,
                     char pwr,
                     long angle,
                     bool UseSpeedControl = true,
                     bool BrakeAtEnd = true)
{
    long l1, l2, l3;
    char power = sign(angle);
    angle = abs(angle);
    power *= pwr;

    if (angle > 720)
    l1 = angle*0.10;
    else
    l1 = angle*0.20;
    l3 = l1;
    l2 = angle-(l1+l3);

    byte om = OUT_MODE_MOTORON|OUT_MODE_BRAKE;
    byte rm = OUT_REGMODE_IDLE;
    if (UseSpeedControl) {
        om += OUT_MODE_REGULATED;
        rm = OUT_REGMODE_SPEED;
    }
    // we want to rotate a total of <angle> degrees
    // we'll rampup from 0 power to specified power through 20% of the angle
    // then run at the specified power for 60% of the angle
    SetOutput(output, OutputModeField, om,
                    TachoLimitField, l1,
                    PowerField, power,
                    RegModeField, rm,
                    RunStateField, OUT_RUNSTATE_RAMPUP,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
    Yield(); // give firmware a chance to process this request to update motor state

    // monitor runstate
    while(MotorRunState(output) <> OUT_RUNSTATE_IDLE)
{ticky(1);
Wait(10);}
    // as soon as it goes idle put the motor into the running state
    SetOutput(output, OutputModeField, om,
                    TachoLimitField, l2,
                    PowerField, power,
                    RegModeField, rm,
                    RunStateField, OUT_RUNSTATE_RUNNING,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
    Yield(); // give firmware a chance to process this request to update motor state
    // monitor runstate

    while(MotorRunState(output) <> OUT_RUNSTATE_IDLE)
{ticky(2);
Wait(10);}

    // as soon as the runstate goes idle we rampdown to zero power
    SetOutput(output, OutputModeField, om,
                    TachoLimitField, l3,
                    PowerField, 0,
                    RegModeField, rm,
                    RunStateField, OUT_RUNSTATE_RAMPDOWN,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
    Yield(); // give firmware a chance to process this request to update motor state
    // monitor runstate
    while(MotorRunState(output) <> OUT_RUNSTATE_IDLE)
{ticky(3);
Wait(10);}


    // now apply powered braking for a little while
    SetOutput(output, OutputModeField, OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED,
                    TachoLimitField, 0,
                    PowerField, 0,
                    RegModeField, OUT_REGMODE_SPEED,
                    RunStateField, OUT_RUNSTATE_RUNNING,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
    Yield(); // give firmware a chance to process this request to update motor state

    // Wait until the motor has stopped turning.
    long rc = MotorRotationCount(output);
    long oldrc = rc+1;
    while (oldrc <> rc) {
        oldrc = rc;
ticky(4);
        Wait(100);  // adjust this wait time to see what impact it has on accuracy.
        rc = MotorRotationCount(output);
    }

    if (!BrakeAtEnd) {
        // finally, go idle
        SetOutput(output, OutputModeField, OUT_MODE_COAST+OUT_MODE_REGULATED,
                        TachoLimitField, 0,
                        PowerField, 0,
                        RegModeField, OUT_REGMODE_SPEED,
                        RunStateField, OUT_RUNSTATE_IDLE,
                        UpdateFlagsField, UF_UPDATE_MODE);
        Yield(); // give firmware a chance to process this request to update motor state
    }
ticky(5);
}

What I found was:
1) The ramp up phase shows the acceleration expected.
2) The speed during the constant run phase is about .7 deg/ms.
3) It continued .7 deg/ms in the ramp down phase, all the way to the end of the requested angle, at which point it slammed on the brakes.

Since the motor is running full blast at the end of the angle, it overshoots by about 40 degrees, then has to bounce a couple of times to get to the proper angle, which it does.

The "power" value wasn't all that interesting because it was -75 through phases 1 & 2, then 0 during phases 3-5.

(In case anyone wonders, I spent about 8 years as a performance analyst with IBM. )

[afanofosc]

I will need to run some tests myself, I guess, to try to understand what you are seeing. It looks like it might have something to do with negative power levels. I'll have to check the firmware source code to see what it does in the RAMPUP/RAMPDOWN runstates when the power value is negative/zero/positive. What happens if you rampdown to -1 or +1 instead of 0? I.e., if power < 0 then rampdown to -1 and if power > 0 then rampdown to +1.

It may be a logic problem between negative and positive power levels where zero is considered to be positive and therefore a greater power level than any negative power level. That might explain the rampdown behavior.

John Hansen

[mcsummation]

Ramp down to the smallest non-zero value would be fine because, at the end of the ramp it goes to "brake" anyway.

I tried ramp to -1 and it was still flying when it got to "target angle". Same thing for -10 and +1.

Edit, I tried it with ramp from +75 to 0. Same failure.

[afanofosc]

I'm puzzled because when I first wrote this ramp up then ramp down function and tested it I could have sworn that it gradually increased speed, ran at constant speed, then gradually decreased speed. I will run some tests tonight. In the meantime maybe you could try this with the standard LEGO 1.31 NXT firmware to see if there are any differences in behavior?

John Hansen

[mcsummation]

Nope, did the same thing with the file called lms_arm_nbcnxc_131.rfw which came from the same zip file that contained lms_arm_nbcnxc_131_20111019_1659.rfw.

[afanofosc]

Both of those files are enhanced NBC/NXC firmwares. You can get the standard LEGO firmware from the LEGO MINDSTORMS support website.

http://cache.lego.com/upload/contentTem ... E9CBE4.zip

Have you used the latest enhanced NBC/NXC firmware from the test releases folder? It is now at version 1.32. All of the very latest test releases and enhanced NBC/NXC firmware builds are here:

http://bricxcc.sourceforge.net/test_releases/

John Hansen

[mcsummation]

I was running with the 2/25 version of the FW - the one where you fixed the fopen problem.

I installed the "official" Lego version of 1.31 that you posted the pointer to. Even though I don't have any of my instrumentation installed (trying to use 2D array causes the dreaded click,click,click). I'm running a bare bones test and I can tell it overshoots because of the jerk/jerk at the end. I get the smooth ramp up, but the end is sudden.

[afanofosc]

It appears that the RAMPDOWN runstate does not work with the regulation mode set to SPEED regulation. If you change change the ramp down phase to use OUT_REGMODE_IDLE as the regulation mode then it ramps down like it should - without speed regulation.

Code: Select all

  // as soon as the runstate goes idle we rampdown to zero power
  SetOutput(output, OutputModeField, om,
                    TachoLimitField, l3,
                    PowerField, 0,
                    RegModeField, OUT_REGMODE_IDLE,
                    RunStateField, OUT_RUNSTATE_RAMPDOWN,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);

I'll look in the firmware source code to see if there is a bug that could be easily fixed or not.

John Hansen

[mcsummation]

I simply set UseSpeedControl=false in the call to the routine and it works much better.

Now, I'll work on modifying the way the mode bytes are set up at the top of the routine to allow speed control for the other phases, but not on that one. It's getting close to my "no experiment" time (else my wife will beat me), so I'll do this tomorrow.

BTW, why is OUT_MODE_BRAKE set for the normal running states. That just seems wrong.

[mcsummation]

Because the speed doesn't ever get to 0 on the ramp down, I added a second phase of "ramp down". I first ramped down to power=10 for 90% of the calculated ramp down distance, then to 0 for the other 10%. Looking at the tires on the robot (with the robot on its back), the end is smooth with little overshoot. Looking at the data shows that it isn't as neat as one would like.

I also changed the fourth piece of data being captured to MotorActualSpeed.

Code: Select all

long steps[3000][4];
long stepcount=0;
void ticky(int phase)
{
    steps[stepcount][0] = phase;
    steps[stepcount][1] = CurrentTick();
    steps[stepcount][2] = MotorRotationCount(OUT_B);
    steps[stepcount][3] = MotorActualSpeed (OUT_B);
    stepcount++;
}

void RotateMotorRURD2(byte output,
                     char pwr,
                     long angle,
                     bool UseSpeedControl = true,
                     bool BrakeAtEnd = true)
{
    long l1, l2, l3, l4;
    char power = sign(angle), p3;
    angle = abs(angle);
    power *= pwr;

    // Calculate ramp up distance.
    if (angle > 720)
        l1 = angle*0.10;
    else
        l1 = angle*0.20;
        
    // Calculate ramp down distance.
    if (abs(power) > 30)
    {
        // If running at high power, ramp down needs to be 2 phase to get it to 0 speed and 0 "distance to go" at the same time.
        l4 = l1 / 10;
        l3 = l1 - l4;
        p3 = 10 * sign(power);
    }
    else
    {
        // Single phase ramp down should work.
        l3 = l1;
        l4 = 0;
        p3 = 0;
    }
    l2 = angle-(l1+l3+l4);

    byte om = OUT_MODE_MOTORON|OUT_MODE_BRAKE;
    byte rm = OUT_REGMODE_IDLE;
    if (UseSpeedControl) {
        om += OUT_MODE_REGULATED;
        rm = OUT_REGMODE_SPEED;
    }
    // we want to rotate a total of <angle> degrees
    // we'll rampup from 0 power to specified power through 20% of the angle
    // then run at the specified power for 60% of the angle
    SetOutput(output, OutputModeField, om,
                    TachoLimitField, l1,
                    PowerField, power,
                    RegModeField, rm,
                    RunStateField, OUT_RUNSTATE_RAMPUP,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
    Yield(); // give firmware a chance to process this request to update motor state

    // monitor runstate
    while(MotorRunState(output) <> OUT_RUNSTATE_IDLE)
{ticky(1);
Wait(10);}
    // as soon as it goes idle put the motor into the running state
    SetOutput(output, OutputModeField, om,
                    TachoLimitField, l2,
                    PowerField, power,
                    RegModeField, rm,
                    RunStateField, OUT_RUNSTATE_RUNNING,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
    Yield(); // give firmware a chance to process this request to update motor state
    // monitor runstate

    while(MotorRunState(output) <> OUT_RUNSTATE_IDLE)
{ticky(2);
Wait(10);}

    // as soon as the runstate goes idle we rampdown to zero power
    SetOutput(output, OutputModeField, om,
                    TachoLimitField, l3,
                    PowerField, p3,
                    RegModeField, OUT_REGMODE_IDLE,        // OUT_REGMODE_SPEED won't work here
                    RunStateField, OUT_RUNSTATE_RAMPDOWN,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
    Yield(); // give firmware a chance to process this request to update motor state
    // monitor runstate
    while(MotorRunState(output) <> OUT_RUNSTATE_IDLE)
{ticky(3);
Wait(10);}

    if (l4 != 0)
    {
        // as soon as the runstate goes idle we rampdown to zero power
        SetOutput(output, OutputModeField, om,
                        TachoLimitField, l4,
                        PowerField, 0,
                        RegModeField, OUT_REGMODE_IDLE,        // OUT_REGMODE_SPEED won't work here
                        RunStateField, OUT_RUNSTATE_RAMPDOWN,
                        UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
        Yield(); // give firmware a chance to process this request to update motor state
        // monitor runstate
        while(MotorRunState(output) <> OUT_RUNSTATE_IDLE)
{ticky(4);
Wait(10);}
    }


    // now apply powered braking for a little while
    SetOutput(output, OutputModeField, OUT_MODE_MOTORON+OUT_MODE_BRAKE+OUT_MODE_REGULATED,
                    TachoLimitField, 0,
                    PowerField, 0,
                    RegModeField, OUT_REGMODE_SPEED,
                    RunStateField, OUT_RUNSTATE_RUNNING,
                    UpdateFlagsField, UF_UPDATE_MODE|UF_UPDATE_SPEED|UF_UPDATE_TACHO_LIMIT);
    Yield(); // give firmware a chance to process this request to update motor state

    // Wait until the motor has stopped turning.
    long rc = MotorRotationCount(output);
    long oldrc = rc+1;
    while (oldrc <> rc) {
        oldrc = rc;
ticky(5);
        Wait(100);  // adjust this wait time to see what impact it has on accuracy.
        rc = MotorRotationCount(output);
    }

    if (!BrakeAtEnd) {
        // finally, go idle
        SetOutput(output, OutputModeField, OUT_MODE_COAST+OUT_MODE_REGULATED,
                        TachoLimitField, 0,
                        PowerField, 0,
                        RegModeField, OUT_REGMODE_SPEED,
                        RunStateField, OUT_RUNSTATE_IDLE,
                        UpdateFlagsField, UF_UPDATE_MODE);
        Yield(); // give firmware a chance to process this request to update motor state
    }
ticky(6);
}

Trying to ramp down to power values less than 10 causes the motor control code to hang up (apparently).

In the data, phase 3 is the initial ramp down to power=10, phase 4 is the final ramp to 0, phase 5 is when it is demanding the target distance, and phase 6 is "done".

This code has a 4 degree overshoot, while the original code (which didn't actually ramp down) had a 40 degree overshoot.