root/branches/atorf/NXC/MotorControl2/NextGenMotor.nxc @ 588

/*
%
% The NXT Program MotorControl enables precise motor movement via direct
% commands. It listens to "NXT BT messages", interprets their content (own user-
% defined "protocol", if you will), and carries out highly precise motor actions
% in a different thread for each motor, allowing the upper level program (in
% this case, MATLAB) to carry on with execution...
%
% Signature
%   Author: Linus Atorf (see AUTHORS)
%   Date: 2009/04/02
%   Copyright: 2007-2009, RWTH Aachen University
%
%
% ***********************************************************************************************
% *  This file is part of the RWTH - Mindstorms NXT Toolbox.                                    *
% *                                                                                             *
% *  The RWTH - Mindstorms NXT Toolbox is free software: you can redistribute it and/or modify  *
% *  it under the terms of the GNU General Public License as published by the Free Software     *
% *  Foundation, either version 3 of the License, or (at your option) any later version.        *
% *                                                                                             *
% *  The RWTH - Mindstorms NXT Toolbox is distributed in the hope that it will be useful,       *
% *  but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS  *
% *  FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.             *
% *                                                                                             *
% *  You should have received a copy of the GNU General Public License along with the           *
% *  RWTH - Mindstorms NXT Toolbox. If not, see <http://www.gnu.org/licenses/>.                 *
% ***********************************************************************************************
*/



#include "MotorFunctions.nxc"
#include "SpeedFromPosLookup.nxc"
#include "SpeedFromPosLookup.nxc"
#include "SpeedMonitor.nxc"



#define OUTBOX 1

#define SPEEDHISTORY 2

#define LASTSTEPBRAKINGDEG 2
#define ENDGAMEBRAKINGSTARTDIST 10


// CONSTANT LOOP TIMING
#define LOOP_DURATION 24


// Driving "stages" or "phases" during controlled motor loop...
#define STAGE_RAMPUP 1
#define STAGE_DRIVING 2
#define STAGE_BRAKING 3
#define STAGE_ENDGAME 4


// PID parameters
#define PIDUPSCALING 10000
// HAVE TO BE UPDATED ALL THE TIME MANUALLY, depend on PIDUPSCALING!!!
#define HARDCODED_KP -500
#define HARDCODED_KD -2500
/*  // this was the "original formula"
    long KP =  -1 * PIDUPSCALING / 20;  // -1/40 to -1/60 ????
    long KD =  -1 * PIDUPSCALING /  4;  // -1/2  to -1/3
*/



// --- DEBUGGING OPTIONS -----------
#define ENABLEDEBUGGING_LCD
#define ENABLEDEBUGGING_REMOTE
// ---------------------------------



//TODO optimize message-parsing (ReceiveRemoteNumber, etc?)

//TODO remember what happens when CHANGING SPEED during RUNTIME?!?!!?!!



inline long GetAbsBrakingDistFromAbsSpeed(long absSpeed) {
    // absolute max realistic speed with full batteries (not akkus) is 1100 deg per sec
    return absSpeed / 4;
}//end function

inline long GetSgnBrakingDistFromSgnSpeed(long sgnSpeed) {
    // absolute max realistic speed with full batteries (not akkus) is 1100 deg per sec
    return sgnSpeed / 4;
}//end function



void RunMotor(byte port, int power, long tacholimit, bool speedreg, bool endbrake) {

// **** THINGS NOT IN THIS FUNCTION
// - decision if NO CONTROLLED LOOP is needed (classic way?!)
// - defer as much decision etc as possible to MATLAB, see below

// **** More lookups needed:
// - Brakingdist from Speed
// - Absolute acceleration power during rampup (in discrete steps like 10, 20, etc)


// --- Some DECLARATIONS -- not all of them, this is timecritical
    long i;

    int  powerSgn       = sign(power);
    long tachoTarget    = powerSgn * tacholimit;


// --- Reset counters etc
    ResetErrorCorrectionAndBlockCount(port);


// --- Decide if SIMPLE FUNCTION needed?
//     * For small powers!
//     * For (very?) short distances???

// --- Start driving (important that its EARLY in this function -- for direct commands)
//     * Runstate RUNNING
//     * power = +-1
//     * tacholimit already...
    MotorCmdSingle(port, powerSgn, tacholimit, speedreg);


// --- Declare and Initialize various variables
//     * STAGE is RAMPUP
    int  curStage = STAGE_RAMPUP;

    int  absPower = abs(power);

    long absBrakingDist;
    long brakingStartSpeed;
    long brakingStartTacho;

    long loopStartTick;
    long waitEndTick;

    //PID stuff
    long err_0 = 0;  // current error (t =   0)
    long err_1 = 0;  // prev.   error (t = - T)
    long curPIDPowerUpscaled = absPower * PIDUPSCALING;
    long newAbsPower;

    // speed stuff
    long curSpeed;
    int  SpeedLogIndex = 0;
    long TachoCountLog[SPEEDHISTORY];
    long TickCountLog[SPEEDHISTORY];

    // init speedlog
    for(i = 0; i < SPEEDHISTORY; i++) {
        TachoCountLog[i] = 0; // should still be 0, was reset just a moment ago
        TickCountLog[i]  = CurrentTick(); //careful, check if this works...
    }//end for


#ifdef ENABLEDEBUGGING_LCD
    long loopCount = 0;
    long lastTick  = CurrentTick();
    long lastLoopTime;
#endif// - - - - - - - - - -

#ifdef ENABLEDEBUGGING_REMOTE
    string tmp;
    string tmp1;
    string tmp2;
    string tmp3;
#endif// - - - - - - - - - -



// --- MAIN LOOP ---------------------
    while(true) {
    
//    * record time
        loopStartTick = CurrentTick();
        
        #ifdef ENABLEDEBUGGING_LCD
            loopCount++;
            lastLoopTime = CurrentTick() - lastTick;
            lastTick     = CurrentTick();
        #endif// - - - - - - - - - -
        

//    * check STOP CONDITIONS?
//      - already there?
//      - direct command stopped us?
        if ( abs(tachoTarget - MotorTachoCount(port)) >= 2 ) {
            // we are almost already there :-)
            // or even too far :-/
        }//end if
        
        if ( MotorPower(port) == 0 ) {
            // direct command must have stopped us!
        }//end if
        

//    * RECORD SPEED
//      - with this long loop execution time, use speed history of 2
//      - 2 vars, toggle between history...
        curSpeed = ((MotorTachoCount(port) - TachoCountLog[SpeedLogIndex]) * 1000) / (CurrentTick() - TickCountLog[SpeedLogIndex]);

        //TODO optimize speed logging by using only 2 history values and toggling between them

        // record speedlog data to previous index...
        i = SpeedLogIndex - 1;
        if (i < 0) i = SPEEDHISTORY-1;
        TachoCountLog[i] = MotorTachoCount(port);
        TickCountLog[i]  = CurrentTick();

        // move current index for next reading
        SpeedLogIndex++;
        if (SpeedLogIndex > SPEEDHISTORY-1) SpeedLogIndex = 0;


//    * if RAMPUP STAGE:
//      - increase power (set according value from lookup / count loop-passes)
//      - enter NEXT STAGE when power is reached...
//      - maybe enable SPEED REG if not already done?
        if (curStage == STAGE_RAMPUP) {
        
            //TODO do a real manual nice rampup here
            // for now we set to fullspeed right away
            UpdatePower(port, power);
            curStage = STAGE_DRIVING;


        }//end if
        
//    * if DRIVING STAGE:
//      - look up current braking distance from current speed
//      - decide: stage change? time to brake?
//      - enter braking stage, set braking dist!
//      - disable speed reg if necessary...
        if (curStage == STAGE_DRIVING) {
        
            absBrakingDist = GetAbsBrakingDistFromAbsSpeed(abs(curSpeed));
            if ( abs(tachoTarget -  MotorTachoCount(port)) <= absBrakingDist ) {
                // ok, we're close enough, start braking!
                curStage = STAGE_BRAKING;
                // record this for later down (for speed lookup)
                brakingStartSpeed = curSpeed;
                brakingStartTacho = powerSgn * (tacholimit - absBrakingDist);
                //TODO disable SPEED REG if needed!!!
            }//end if


        }//end if
        
//    * if BRAKING STAGE:
//     ( - check endbraking special mode conditions )
//     (    . maybe skip PID if time for endgame?   )
//      - PID control
//        . calc current error
//        . calc new setpoint
//        . use KP and KD constants as MACROS (#define), not VARS!
//        . clip power and set
        if (curStage == STAGE_BRAKING) {

            // shift/calculate all errors for current cycle
            err_1 = err_0;
            err_0 = curSpeed - GetIdealSpeedFromPos(abs(MotorTachoCount(port) - brakingStartTacho), brakingStartSpeed, absBrakingDist);


            curPIDPowerUpscaled = curPIDPowerUpscaled
                                + (KD * (err_0 - err_1))
                                + (KP * err_1);


            //TODO clip these values!
            newAbsPower = curPIDPowerUpscaled / PIDUPSCALING;
            if (newAbsPower > 100) {
                newAbsPower = 100;
            } else if (newAbsPower < 1) {
                newAbsPower = 1;
            }//end if
            
            //finally set new power!
            UpdatePower(port, sgn * newAbsPower);
            
            

            #ifdef ENABLEDEBUGGING_REMOTE
                tmp1 = NumToStr(err_0);
                tmp2 = NumToStr(idealSpeed);
                tmp3 = NumToStr(curSpeed);
                tmp = StrCat(tmp1, "|", tmp2, "|", tmp3);
                SendMessage(OUTBOX, tmp);
            #endif// - - - - - - - - - -
            
        }//end if

//    * WAIT
//      - burn constant CPU time by waiting for max delay...
//      - keep on checking tachocount for ENDGAME MODE?
//      - (MAYBE keep on checking direct command cancellation?)
        waitEndTick = loopStartTick + LOOP_DURATION;
        while (CurrentTick() < waitEndTick) {

            if ( abs(tachoTarget - MotorTachoCount(port)) <  ENDGAMEBRAKINGSTARTDIST) {
               curStage = STAGE_ENDGAME;
               break;
            }//end if
            
        }//end while

//    * if SPECIAL ENDGAME BRAKING
//      - FAST & SIMPLE INNER LOOP!!!
//        . check position
//        . enable HARD BRAKE if necessary, jump out of loop
        if (curStage == STAGE_ENDGAME) {
        
            // fast inner loop now!
            // don't forget TIMEOUT or something, avoid possible lock up
            
        }//end if



    }//end while
// --- END MAIN LOOP ---------------------

// --- if loop exited manually by direct command, exit void!

// --- monitor braking (as in old MotorControl)
//     * release when necessary at full stop

// --- Motor off / idle, over & out?


}//end void






// -------------------------------------------------
// main function
task main(){


    byte port = OUT_B;
    byte power = 100;
    bool speedreg = false;

    // total dist to move
    long TotalDist = 1000;
    // length of braking process = when to start braking
    long BrakingDist = 220;
    // how long should the braking take?
    long BrakingTime = 3000;

    long MinPower = 0;
    long MaxPowerOffset = 0;



    // saving performance:
    long NormalDrivingDist = (TotalDist - BrakingDist);

    // Start of deceleration in ticks...
    long DecelStart;
    long DecelStop;


    long t; // braking since...
    long x; // current pos...

    int NewSpeed;

    string tmp = "";
    string tmp1 = "";
    string tmp2 = "";
    string tmp3 = "";

    long curPIDPower;
    long curPIDPowerUpscaled;

    long err_0 = 0;  // current error (t =   0)
    long err_1 = 0;  // prev.   error (t = - T)

    //
    long KP =  -1 * PIDUPSCALING / 20;  // -1/40 to -1/60 ????
    long KD =  -1 * PIDUPSCALING /  4;  // -1/2  to -1/3
    
    
    /* stable working PD with wait(4) one thread...
    long P =  -1 * PIDUPSCALING /  3;  // -1/2  to -1/3
    long I =  -1 * PIDUPSCALING / 30;  // -1/40 to -1/60 ????
    long D =  -0 * PIDUPSCALING /  3;  // -1 to -2
    /*


    /* working slow set for looptime = 18
    long P =  (-1 * PIDUPSCALING) /  6;  // -1/2  to -1/3
    long I =  (-1 * PIDUPSCALING) / 15;  // -1/40 to -1/60 ????
    long D =  (-1 * PIDUPSCALING) /  4;  // -1 to -2
    */

    /*
    long P =  (-1 * PIDUPSCALING) /  (3 * 18);  // -1/2  to -1/3
    long I =  (-1 * PIDUPSCALING) / (25 * 18);  // -1/40 to -1/60 ????
    long D =  (-2 * PIDUPSCALING) /  (3 * 18);  // -1 to -2
    */


    long loopCount = 0;


    int SpeedLogIndex = 0;
    long TachoCountLog[SPEEDHISTORY];
    long TickCountLog[SPEEDHISTORY];

    long tmpTachoCount;
    long tmpTickCount;

    long initSpeed = -1;
    long curSpeed = 0;
    long curTick;
    long LastTick;
    long LastLoopTime;


    long idealSpeed = 0;

    int i;

    // performance-stuff
    //int SPEEDHISTORY_MINUS1 = SPEEDHISTORY - 1;


// **** Init
    //SetPosPredictionParams(BrakingDist, BrakingTime);
    InitSpeedFromPosLUT();

    start SpeedMonitor_A_;
    start SpeedMonitor_B_;
    start SpeedMonitor_C_;
    Wait(20);


// **** Init / Reset motor
    MotorOff(port);
    ResetErrorCorrectionAndBlockCount(port);

    Wait(1100);


// **** Start moving!
    MotorCmdEx(port, power, TotalDist, 0, speedreg, false, OUT_RUNSTATE_RUNNING);





// **** Normal drive, wait for braking
    while(MotorTachoCount(port) <= (TotalDist - BrakingDist)) {

        // record position and time once at brakingdist before braking starts...
        if ((initSpeed == -1) && (MotorTachoCount(port) >= (TotalDist - 2*BrakingDist))) {
            tmpTachoCount = MotorTachoCount(port);
            tmpTickCount = CurrentTick();
            initSpeed = 0;
        }//end if

    }//end while

    /*
    // calc real init speed
    if (CurrentTick() - tmpTickCount) > 0 {
        initSpeed = ((MotorTachoCount(port) - tmpTachoCount) * 1000) / (CurrentTick() - tmpTickCount);
    }//end if
    */

    // init speed history log
    for(i = 0; i < SPEEDHISTORY; i=i+1) {
        TachoCountLog[i] = tmpTachoCount;
        TickCountLog[i] = tmpTickCount;
    }//end for


    // initSpeed was used as flag before, now a real value:
    // TODO FIXME: check against *** div by 0 ***
    initSpeed = ((MotorTachoCount(port) - tmpTachoCount) * 1000) / (CurrentTick() - tmpTickCount);


// **** Send new output command for debug info
    //MotorCmdEx(port, power, BrakingDist , 0, speedreg, false, OUT_RUNSTATE_RUNNING);

// ****


// **** Deceleration



    DecelStart = CurrentTick();
    LastTick = CurrentTick() - 20;

    // deceleration control loop
    t = 0;
    curPIDPower = power;
    curPIDPowerUpscaled = curPIDPower * PIDUPSCALING;
    
    while(MotorTachoCount(port) < (TotalDist - 0)) {

        loopCount++;
        LastLoopTime = CurrentTick() - LastTick;
        LastTick = CurrentTick();

        // calc vars we need...
        //curTick = CurrentTick();
        x = MotorTachoCount(port) - NormalDrivingDist;

        // calc speed
        //TODO WARNING FIXME: make sure div by 0 never happens...
        curSpeed = ((MotorTachoCount(port) - TachoCountLog[SpeedLogIndex]) * 1000) / (CurrentTick() - TickCountLog[SpeedLogIndex]);


        // record speedlog data to previous index...
        i = SpeedLogIndex - 1;
        if (i < 0) i = SPEEDHISTORY-1;
        TachoCountLog[i] = MotorTachoCount(port);
        TickCountLog[i] = CurrentTick();


        // move current index for next reading
        SpeedLogIndex++;
        if (SpeedLogIndex > SPEEDHISTORY-1) SpeedLogIndex = 0;




        idealSpeed = GetIdealSpeedFromPos(x, initSpeed, BrakingDist);
        //idealSpeed = GetIdealSpeedFromPos(x, 100, BrakingDist);


        // shift/calculate all errors for current cycle
        err_1 = err_0;
        err_0 = curSpeed - idealSpeed;


        // calculate new output
        //TODO try to leave a version of curPIDPower always upscaled, to avoid 1 multiplcation
        /*
        curPIDPower = (curPIDPower * PIDUPSCALING
              +  (    KD * (err_0 - err_1)
                    + KP * err_1
                  )
                ) / PIDUPSCALING;
         */
         
        
        curPIDPowerUpscaled = curPIDPowerUpscaled
                            + (KD * (err_0 - err_1))
                            + (KP * err_1);







        tmp1 = NumToStr(err_0);
        tmp2 = NumToStr(idealSpeed);
        tmp3 = NumToStr(curSpeed);
        tmp = StrCat(tmp1, "|", tmp2, "|", tmp3);
        SendMessage(OUTBOX, tmp);




        NewSpeed = curPIDPowerUpscaled / PIDUPSCALING;
        //NewSpeed = idealSpeed;


        //TODO decided wether NewSpeed 0 or 1 is ok...?
        if (NewSpeed > 100) {
            NewSpeed = 100;
            //curPIDPower = 100;
        } else if (NewSpeed < 1) {
            NewSpeed = 1;
            //curPIDPower = 0;
        }//end if

        /* but be careful...
        if (NewSpeed == 0) {
            NewSpeed = 0;
        } else if (NewSpeed > 100) {
            NewSpeed = 100;
        } else if (NewSpeed < 1) {
            NewSpeed = 0;
        }//end if
        */


        // finally, set that power value:
        UpdatePower(port, NewSpeed);


        if (MotorTachoCount(port) >= (TotalDist - LASTSTEPBRAKINGDEG)) {
            // jump into final fast waiting loop...
            break;
        }//end if

        // check if braking time is exceeded...
        if (t >= BrakingTime) {
            break;
        } else {
            //tmp = NumToStr(t);
            //TextOut(0,LCD_LINE1, tmp);
        }//end if

        //Wait(4);

    }//end while


// **** Hard stop
    // wait the final tick...
    while((MotorTachoCount(port) < TotalDist)) {
        // nothing...
    }//end while

    DecelStop = CurrentTick();

    MotorBrake(port);

    //TextOut(0,LCD_LINE1, "Braked...");
    Wait(1000);

    MotorOff(port);

// **** Debug statistics about precision

    tmp = NumToStr(loopCount);
    tmp = StrCat("loops=", tmp);
    TextOut(0,LCD_LINE1, tmp);

    tmp = NumToStr((DecelStop - DecelStart) / loopCount);
    tmp = StrCat("t/loop=", tmp);
    TextOut(0,LCD_LINE2, tmp);

    tmp = NumToStr((loopCount * 1000) / (DecelStop - DecelStart));
    tmp = StrCat("loops/s=", tmp);
    TextOut(0,LCD_LINE3, tmp);

    tmp = NumToStr(LastLoopTime);
    tmp = StrCat("lastloop=", tmp);
    TextOut(0,LCD_LINE4, tmp);

    tmp = NumToStr(TotalDist);
    tmp = StrCat("goal=", tmp);
    TextOut(0,LCD_LINE6, tmp);

    tmp = NumToStr(MotorTachoCount(port));
    tmp = StrCat("real=", tmp);
    TextOut(0,LCD_LINE7, tmp);


    until (ButtonPressed(BTNCENTER, false));

}//end main task