Optimal way of making timed code

Projects Programming
18

look this over


char s [190];

// -----------------------------------------------------------------------------
struct Motor {
    const byte    PinStep;
    const byte    PinDir;
    const char   *desc;

    int           dir;
    int           pos;
    int           posTarg;

    unsigned long msecStep;
    unsigned long msec;
} motor [] = {
    { 10, 7, "A" },
    { 11, 8, "B" },
    { 12, 9, "C" },
};

const int Nmotor = sizeof(motor)/sizeof(Motor);

enum { For = 1, Rev = -1 };

// -------------------------------------
struct Motion {
    int             pos [Nmotor];
    unsigned long   msec;
} motion [] = {
    {{  10,  20, 30 },  800 },
    {{  15,  15, 32 },  300 },
    {{   5,  35, 27 },  700 },
};

int Nmotion = sizeof(motion)/sizeof(Motion);
int idx = 0;

// -----------------------------------------------------------------------------
void
setDir (
    int   mtr,
    int   dir )
{
    digitalWrite (motor [mtr].PinDir, 0 < dir ? HIGH : LOW);
}

// -------------------------------------
void
step (
    int   mtr )
{
#if 0
    Serial.print ("    step: "); Serial.println (motor [mtr].desc);
#endif

    digitalWrite (motor [mtr].PinStep, HIGH);
    delayMicroseconds (5);
    digitalWrite (motor [mtr].PinStep, LOW);

    motor [mtr].pos += motor [mtr].dir;
}

// -----------------------------------------------------------------------------
unsigned long msec;
boolean done;

// step as needed
void
stepChk ()
{
    for (int m = 0; m < Nmotor; m++)  {
#if 0
        sprintf (s, " stepChk: %d, targ %6d, pos %6d"
                    ", msec %8ld, mtr msec %8ld, msecStep %8ld",
                    m, motor [m].posTarg, motor [m].pos,
                    msec, motor [m].msec, motor [m].msecStep);
        Serial.println (s);
#endif

        done = true;
        if (motor [m].pos != motor [m].posTarg)  {
            done = false;

            if (msec - motor [m].msec >= motor [m].msecStep)  {
                motor [m].msec += motor [m].msecStep;
                step (m);
            }
        }
    }
}

// -----------------------------------------------------------------------------

void
loop ()
{
    msec = millis ();

    stepChk ();

    // update targets after each motion
    if (done)  {
        if (Nmotion > idx)  {
#if 0
            sprintf (s, "   loop: update idx %d", idx);
            Serial.println (s);
#endif

            for (int m = 0; m < Nmotor; m++)  {
                motor [m].posTarg  = motion [idx].pos [m];
                int nSteps         = abs(motor [m].posTarg - motor [idx].pos);
                motor [m].msecStep = motion [idx].msec / nSteps;

                motor [m].msec     = msec;
                motor [m].dir      = motor [m].posTarg > motor [idx].pos
                                                            ? For : Rev;

                sprintf (s, "  update: idx %d m %d", idx, m);
                Serial.print   (s);

                sprintf(s,", mot pos %6d, mtr pos %6d",
                                    motion [idx].pos [m], motor [m].pos);
                Serial.print   (s);

                sprintf(s,", mot msec %4ld, nSteps %3d, msecStep %3ld",
                        motor [m].msec, nSteps, motor [m].msecStep);
                Serial.println (s);

                setDir (m, motor [m].dir);
                step (m);
            }

            idx++;
        }
    }
}

// -------------------------------------
void setup ()
{
    Serial.begin (1000000);

    for (int m = 0; m < Nmotor; m++)  {
        pinMode        (motor [m].PinDir,  OUTPUT);
        pinMode        (motor [m].PinStep, OUTPUT);
        digitalWrite   (motor [m].PinStep, LOW);
     // Serial.println (motor [m].desc);
    }
}
1 Like