root/branches/atorf/NXC/TestRampMotorControl.nxc @ 542

safecall void MotorBrake(byte port)   {
// Similar to "StopMotor(port, 'brake')", enables active braking for a motor...

    //MotorCmd(port, 0, 0, 0, true, false);

    byte flags = UF_UPDATE_MODE + UF_UPDATE_SPEED; // + UF_UPDATE_RESET_COUNT;

    byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED;
    byte reg  = OUT_REGMODE_SPEED;
    byte state = OUT_RUNSTATE_RUNNING;

    SetOutput(port, Power, 0, OutputMode, mode, RegMode, reg, RunState, state, UpdateFlags, flags);

}//end MotorBrake


safecall void MotorOff(byte port)   {
// Similar to "StopMotor(port, 'off')", turns off power to a motor, enabling COAST mode...

    //MotorCmd(port, 0, 0, 0, true, false);

    byte flags = UF_UPDATE_MODE + UF_UPDATE_SPEED; // + UF_UPDATE_RESET_COUNT;

    byte mode = OUT_MODE_COAST;
    byte reg  = OUT_REGMODE_IDLE;
    byte state = OUT_RUNSTATE_IDLE;

    SetOutput(port, Power, 0, OutputMode, mode, RegMode, reg, RunState, state, UpdateFlags, flags);

}//end MotorOff


safecall void MotorCmdEx(byte port, int pwr, long tacholimit, int turnratio, bool speedreg, bool sync, byte runstate) {
// The actual Motor-Command we use, based on IOOutputMap settings. A bit similar to the
// SetOutputState direct command...

    byte flags = UF_UPDATE_MODE + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT; // + UF_UPDATE_RESET_COUNT;

    byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON;
    byte reg  = OUT_REGMODE_IDLE;
    byte state = runstate;

    if (speedreg) {
        mode = mode + OUT_MODE_REGULATED;
        reg  = OUT_REGMODE_SPEED;
    }//end if

    if (sync) {
        mode = mode + OUT_MODE_REGULATED;
        reg  = OUT_REGMODE_SYNC;
    }//end if

    SetOutput(port, Power, pwr, TachoLimit, tacholimit, TurnRatio, turnratio, OutputMode, mode, RegMode, reg,  RunState, state, UpdateFlags, flags);
}//end MotorCmdEx


// safecall! if a something were interfering, this would be dangerous...
safecall void ResetErrorCorrectionAndBlockCount(byte port) {
// Resets TachoCount of a motor, which also resets internal error correction memory.
// also resets BlockTachoCount with just one single call. Needed for sync driving. If not
// reset, bot will "re-align" itself to last turnratio, which is usually 0, so in this
// case, bot will turn to drive straight again. Nice feature if wanted, but usually
// unexpected and then looks like "dancing" or totally chaotic movements

    byte flags = UF_UPDATE_RESET_COUNT + UF_UPDATE_RESET_BLOCK_COUNT;

    SetOutput(port, UpdateFlags, flags);


}//end ResetErrorCorrection


void DebugPrintPower(byte port) {
    string tmp;
    tmp = NumToStr(MotorPower(port));
    tmp = StrCat("Power=", tmp);
    TextOut(0,LCD_LINE2, tmp);
}//end

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

    int accel = 50;
    int decel = 270;
    byte minpower = 20;

    int distance = 1000;
    
    byte port = OUT_B;
    byte power = 100;
    bool accSpeedreg = false;
    bool speedreg = false;
    bool decSpeedreg = true;
    
    
    bool smoothStart = false;
    
    


    TextOut(0,LCD_LINE1, "", true);
    
    
    MotorOff(port);
    ResetErrorCorrectionAndBlockCount(port);
    
    Wait(1200);
    

// **** precalc
    int accelStartPower;
    if (power > minpower) {
        smoothStart = true;
        accelStartPower = power / 2;
        if (accelStartPower < minpower) {
            accelStartPower = minpower;
        }
    } else { // for smaller values do the "classic" thing
        accelStartPower = power;
    }

// **** Acceleration
    if (smoothStart) {
        MotorCmdEx(port, accelStartPower, accel, 0, false, false, OUT_RUNSTATE_RUNNING);
        MotorCmdEx(port, power, accel, 0, accSpeedreg, false, OUT_RUNSTATE_RAMPUP);
    } else {
        // improve this by merging this running step to the next one...
        MotorCmdEx(port, power, accel, 0, speedreg, false, OUT_RUNSTATE_RUNNING);
    }


    //TextOut(0,LCD_LINE1, "Accel...");
    while(MotorTachoCount(port) < accel) {
        // do nothing
        //DebugPrintPower(port);
    }//end while


// **** Running
    MotorCmdEx(port, power, distance - accel - decel, 0, speedreg, false, OUT_RUNSTATE_RUNNING);
    
    //TextOut(0,LCD_LINE1, "Running...");
    while(MotorTachoCount(port) < distance - decel) {
        // do nothing
    }//end while

// **** Deceleration
    MotorCmdEx(port, 0, decel, 0, decSpeedreg, false, OUT_RUNSTATE_RAMPDOWN);
    
    //TextOut(0,LCD_LINE1, "Decel...");
    while(MotorTachoCount(port) < distance) {
        // do nothing
        //DebugPrintPower(port);
    }//end while

    
    //MotorBrake(port);
    // NXT-G MoveBlock sets TachoLimit=0 for final brake, we do so too now...
    MotorCmdEx(port, 0, 0, 0, true, false, OUT_RUNSTATE_RUNNING);

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


    string tmp = "";
    
    tmp = NumToStr(distance);
    tmp = StrCat("goal=", tmp);
    TextOut(0,LCD_LINE4, tmp);
    
    tmp = NumToStr(MotorTachoCount(port));
    tmp = StrCat("real=", tmp);
    TextOut(0,LCD_LINE5, tmp);
    
    Wait(3000);

}