Changeset 663

Show
Ignore:
Timestamp:
07/15/09 06:26:14 (4 years ago)
Author:
atorf
Message:

Bugfixing in MotorControl 2.0:

  • Fixed the strange "motor runs too slow with speed regulation although power seems high" bug
  • Added support for all "direct command like" instructions via NXC_MotorControl call from MATLAB
  • Overwriting a currently running motor WITHOUT tacholimit with a new speed is now possible…
  • For synced driving, motors have to be stopped before every time, otherwise it doesn't work!
  • Tweaked some timeout constants
  • Still one bug missing: With many motors running at the same time, a task can lock up, but where? Can't be stopped with direct command, why?
Location:
branches/atorf/NXC/MotorControl2
Files:
4 modified

Legend:

Unmodified
Added
Removed

  • branches/atorf/NXC/MotorControl2/Controller.nxc

    r660 r663  
    6464// lower this time cautiously, if the motor stalls during this loop, probably 
    6565// for a good reason 
    66 #define ENDGAME_INNERLOOP_TIMEOUT 600           //PARAM 
     66#define ENDGAME_INNERLOOP_TIMEOUT 500           //PARAM 
    6767 
    6868// enable this to free/sleep 1ms of CPU time every waiting-loop iteration 
    … …  
    9898// before, rampup is finished. Multiply this with LOOP_DURATION to get the maximum 
    9999// starting/rampup time for the power=100 case... 
    100 #define SMOOTHSTARTSTEPS 20 
     100#define SMOOTHSTARTSTEPS 18 
    101101 
    102102 

  • branches/atorf/NXC/MotorControl2/ControllerCore.nxc

    r658 r663  
    135135//     * tacholimit already... 
    136136 
    137     // do not yet enable speedreg 
     137 
     138    //TODO why exactly do we only power up to +/-1 here? couldn't it be full speed already? 
     139    // maybe it's because for speed monitor initialization, we use TachoCount and need it to remain 
     140    // constant etc? 
    138141    #ifdef RUNMOTOR2_SYNCMODE 
     142        //avoid already synced motors (that doesn't work as we know...) 
     143        until((MotorRegulation(port) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) { 
     144            // repeatedly setting this is not nice, but so 
     145            // we don't need a timeout...! 
     146            MotorOff(port); 
     147            MotorOff(port2); 
     148            // make sure VM applies our settings 
     149            Wait(1); 
     150        }//end until 
    139151        MotorCmdDoubleReset(port, powerSgn, tacholimit, port2); 
    140152    #else 
    141         MotorCmdSingleReset(port, powerSgn, tacholimit, false); 
     153        MotorCmdSingleReset(port, powerSgn, tacholimit, speedreg); 
    142154    #endif 
    143155     
    … …  
    241253//      - direct command stopped us? 
    242254        // this is like an emergency-check to see if endgame should be enabled? 
    243         //TODO seems obsolete! 
     255        //TODO do we need this below? 
     256        /* 
    244257        if ( abs(tachoTarget - MotorTachoCount(port)) >= 2 ) { 
    245258            // we are almost already there :-) 
    246259            // or even too far :-/ 
    247260        }//end if 
    248  
     261        */ 
    249262         
    250263        if ( MotorPower(port) == 0 ) { 
    … …  
    291304                // when rampup is done 
    292305                if (rampupLoopCount >= SMOOTHSTARTSTEPS) { 
    293                     UpdatePower(port, power); 
    294                     #ifdef RUNMOTOR2_SYNCMODE 
    295                         UpdatePower(port2, power); 
    296                     #endif 
    297                     if (speedreg) { EnableSpeedRegWhithMotorOn(port); } 
     306                    if (speedreg) { 
     307                        //there's no speedreg for synced driving... 
     308                        UpdatePowerAndEnableSpeedReg(port, power); 
     309                    } else { 
     310                        UpdatePower(port, power); 
     311                        #ifdef RUNMOTOR2_SYNCMODE 
     312                            UpdatePower(port2, power); 
     313                        #endif 
     314                    }//end if 
    298315                    curStage = STAGE_DRIVING; 
    299316                } else { 
    … …  
    302319                    // if power < 100 we might be done sooner: 
    303320                    if (newPower > absPower) { 
    304                         UpdatePower(port, power); 
    305                         #ifdef RUNMOTOR2_SYNCMODE 
    306                             UpdatePower(port2, power); 
    307                         #endif 
    308                         if (speedreg) { EnableSpeedRegWhithMotorOn(port); } 
     321                        if (speedreg) { 
     322                            //there's no speedreg for synced driving... 
     323                            UpdatePowerAndEnableSpeedReg(port, power); 
     324                        } else { 
     325                            UpdatePower(port, power); 
     326                            #ifdef RUNMOTOR2_SYNCMODE 
     327                                UpdatePower(port2, power); 
     328                            #endif 
     329                        }//end if 
    309330                        curStage = STAGE_DRIVING; 
    310331                    } else { 
    … …  
    320341             
    321342                // full power right away 
    322                 UpdatePower(port, power); 
    323                 #ifdef RUNMOTOR2_SYNCMODE 
    324                     UpdatePower(port2, power); 
    325                 #endif 
    326                 //there's no speedreg for synced driving... 
    327                 if (speedreg) { EnableSpeedRegWhithMotorOn(port); } 
     343                if (speedreg) { 
     344                    //there's no speedreg for synced driving... 
     345                    UpdatePowerAndEnableSpeedReg(port, power); 
     346                } else { 
     347                    UpdatePower(port, power); 
     348                    #ifdef RUNMOTOR2_SYNCMODE 
     349                        UpdatePower(port2, power); 
     350                    #endif 
     351                }//end if 
     352 
    328353                curStage = STAGE_DRIVING; 
    329354                 

  • branches/atorf/NXC/MotorControl2/MotorControl20.nxc

    r660 r663  
    324324 
    325325 
    326 task main() { 
     326task TESTmain() { 
    327327 
    328328     byte port = OUT_B; 
    … …  
    371371 
    372372 
    373 task NEWmain(){ 
     373task main(){ 
    374374 
    375375    // parameter initialization 
    … …  
    629629                if (port <= 2) { 
    630630                 
    631                     //ResetErrorCorrection(port); 
    632                     //TODO check if motor is ready 
    633                     MotorCmdSingleReset(port, power, angle, speedreg); 
     631                    // if no current tacholimit and no new one, allow speed change 
     632                    if ((MotorTachoLimit(port) == 0) && (angle == 0)) { 
     633                        MotorCmdSingleReset(port, power, angle, speedreg); 
     634                    } else { 
     635                        if (IsMotorReady(port)) { 
     636                            MotorCmdSingleReset(port, power, angle, speedreg); 
     637                        } else { 
     638                            TaskBusySignal(); 
     639                        }//end if 
     640                    }//end if 
    634641 
    635642                } else { // Otherwise (OUT_AB, OUT_AC, OUT_BC, OUT_ABC?) 
    … …  
    650657                    }//end switch 
    651658 
    652                     if (IsMotorReady(port1) && IsMotorReady(port2)) { 
    653                         //needed? shouldn't make any difference actually 
    654                         //MotorOff(port1); 
    655                         //MotorOff(port2); 
     659 
     660                    // if no current tacholimit and no new one, allow speed change 
     661                    if ((MotorTachoLimit(port1) == 0) && (MotorTachoLimit(port2) == 0)  && (angle == 0)) { 
     662                            //~~~~BEGIN COPY PASTE CODE~~~~~~~~~~~~~~~~~~ 
     663                            //avoid already synced motors (that doesn't work as we know...) 
     664                            until((MotorRegulation(port1) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) { 
     665                                // repeatedly setting this is not nice, but so 
     666                                // we don't need a timeout...! 
     667                                MotorOff(port1); 
     668                                MotorOff(port2); 
     669                                // make sure VM applies our settings 
     670                                Wait(1); 
     671                            }//end until 
     672                            MotorCmdDoubleReset(port1, power, angle, port2); 
     673                            //~~~~END COPY PASTE CODE~~~~~~~~~~~~~~~~~~ 
     674                    } else { 
     675                        if (IsMotorReady(port1) && IsMotorReady(port2)) { 
     676                            //~~~~BEGIN COPY PASTE CODE~~~~~~~~~~~~~~~~~~ 
     677                            //avoid already synced motors (that doesn't work as we know...) 
     678                            until((MotorRegulation(port1) == OUT_REGMODE_IDLE) && (MotorRegulation(port2) == OUT_REGMODE_IDLE)) { 
     679                                // repeatedly setting this is not nice, but so 
     680                                // we don't need a timeout...! 
     681                                MotorOff(port1); 
     682                                MotorOff(port2); 
     683                                // make sure VM applies our settings 
     684                                Wait(1); 
     685                            }//end until 
     686                            MotorCmdDoubleReset(port1, power, angle, port2); 
     687                            //~~~~END COPY PASTE CODE~~~~~~~~~~~~~~~~~~ 
     688                        } else { 
     689                           TaskBusySignal(); 
     690                        }//end if 
    656691                         
    657                         //ResetErrorCorrectionAndBlockCount(port1); 
    658                         //ResetErrorCorrectionAndBlockCount(port2); 
    659                         MotorCmdDoubleReset(port1, power, angle, port2); 
    660                          
    661                     } else { 
    662                        TaskBusySignal(); 
    663692                    }//end if 
    664693 

  • branches/atorf/NXC/MotorControl2/MotorFunctions.nxc

    r660 r663  
    4040*/ 
    4141 
    42 #define MOTORSTOPPED_TIMEOUT 700 //in ms 
     42#define MOTORSTOPPED_TIMEOUT 800 //in ms 
    4343//TODO this can / should be lowered! very important to reduce 
    4444// total execution time of motor controlling task! 
    4545// this period is basically "wasted" at the end of EACH AND EVERY SINGLE 
    4646// movement, so it really is a sort of "full blackout" for a motor... 
    47 #define MOTORSTOPPED_RESTINGPERIOD 250 //in ms 
     47#define MOTORSTOPPED_RESTINGPERIOD 200 //in ms 
    4848 
    4949 
    … …  
    342342}//end UpdatePower 
    343343 
     344inline void UpdatePowerAndEnableSpeedReg(const byte &port, const int &pwr) { 
     345    SetOutput(port, Power, pwr, OutputMode, OUT_MODE_BRAKE + OUT_MODE_MOTORON + OUT_MODE_REGULATED, RegMode, OUT_REGMODE_SPEED, UpdateFlags, UF_UPDATE_SPEED + UF_UPDATE_MODE); 
     346}//end UpdatePowerAndEnableSpeedReg 
     347 
     348 
    344349inline void UpdatePowerSync(const byte &port, const int &pwr) { 
    345350    SetOutput(port, Power, pwr, RunState, OUT_RUNSTATE_RUNNING, UpdateFlags, UF_UPDATE_SPEED); // + UF_UPDATE_MODE); 
    346 }//end UpdatePower 
     351}//end UpdatePowerSync 
    347352 
    348353