x_motorcontrollerPID.nxc

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/**
* @file motorcontrollerPID.nxc
* \ingroup motorcontroller
*
* @brief Background task and high level routines to run motors with PID regulation.
*
* Originator (core): (c) HaWe 2013      <br>
*
* Edit: Martin Aumair                   <br>
**/

/*
   CHANGELOG:
   TODO:
       controllerOn/Off to idle
   BUGS:
*/
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




/*
#ifndef _MOTORCONTROLLER_PID_
#define _MOTORCONTROLLER_PID_
#endif
*/
/*
whats tprop?

CHANGELOG for nxcBoost
removed aspeed in ctask
removed runstate, use state but in an other way
read to temp var
error checking now at control loop start
changed cont to bool
maxout now global, we do not need this for every motor?
*/


// PID controller: exactly approaching sensor targets by motor control
// ver 1.02


// MOTOR STATES
#define PID_RUNSTATE_IDLE        0   //< Motorcontroller off (nothing)
#define PID_RUNSTATE_EB          1   //< Emergency Braking
#define PID_RUNSTATE_START       2   //< Before move, short initializaton
#define PID_RUNSTATE_RUNNING     3   //< PID-Controller on, move to target
#define PID_RUNSTATE_RUNNINGCONT 4   //< PID-Controller on, hold position
#define PID_RUNSTATE_BRAKE       5   //< Brake motor at target position

// debug mode featuring graphic output
//#define DEBUG_PID_A
//#define DEBUG_PID_B
//#define DEBUG_PID_C

/// \cond

#ifdef DEBUG_PID_A
   #define DEBUG_PORT OUT_A
   #define DEBUG_PID
#endif
#ifdef DEBUG_PID_B
   #define DEBUG_PORT OUT_B
   #define DEBUG_PID
#endif
#ifdef DEBUG_PID_C
   #define DEBUG_PORT OUT_C
   #define DEBUG_PID
#endif

/// Print format at specified position
#define printf1( _x, _y, _format1, _value1) {  \
  string sval1 = FormatVal(_format1, _value1); \
  TextOut(_x, _y, sval1); \
}

// forward
void displayMask();

inline long round(float f) { return (f>0?(f+0.5):(f-0.5)); }

//==============================================================================

// globals
// number of motors (for controlling)
char         _g_nmotors = 1;
// regulation time
unsigned int _g_regtime = 10;
// maximal motor output
const int    _g_maxout = 100;
// mutex for changing motor runstates
mutex        mtx_OUT_A;
mutex        mtx_OUT_B;
mutex        mtx_OUT_C;

// make things easier
#define getMutex(char _outport)                      \
   if(_outport == 0x00)       Acquire(mtx_OUT_A);    \
   else if(_outport == 0x01)  Acquire(mtx_OUT_B);    \
   else if(_outport == 0x02)  Acquire(mtx_OUT_C);    \
   else

#define releaseMutex(char __outport)                 \
   if(__outport == 0x00)      Release(mtx_OUT_A);    \
   else if(__outport == 0x01) Release(mtx_OUT_B);    \
   else if(__outport == 0x02) Release(mtx_OUT_C);    \
   else
   

struct sPID
{                                // custom target values
  long  target;                  // set target
  int   tarpwm;                  // motor target speed
                                 // custom regulation parameters
  float P;                       // P: basic propotional to error
  float I;                       // I: integral: avoid perish
  float D;                       // D: derivative: avoid oscillating
  float precis;                  // error precision to target
  float damp;                    // damp the integral memory
  bool  cont;                    // target: continue or hit once
                                 // internal control variables
  char  state;                   // see runstate defines
  int   outp;                    // PID control output value
  // int   maxout;                  // max output (max motor pwr)
  // long  read;                    // current sensor reading
  float err;                     // current error
  float integr;                  // integral of errors
  float cspeed;                  // current speed
};
sPID pid[];

/// \endcond
//==============================================================================

/**
   \brief Main control task.

   Start the task after initPID!
   <b>start</b> Motor_Controller_Task;
*/
task PID_Controller_Task() {

   // temp values
  float damp, PWMpwr, tprop;
  float readold[], errorold[];
  
  // do we need cmin??
  // overshooting should be abs!
  long  readstart[], cmax[]; //cmin;                         // for monitoring

  long  starttime[], runtime[], clock[];
  long  dtime;              // timer

  // regloop for ending controling
  char  regloop[];
  // for counting in brake mode
  unsigned int cnt[];

  // actual position
  long read;
  
  // graph output
  int timex, oldtx, oldy=15, pwm0=15;     // values for graphic screen
  float scrXratio, scrYratio;
  
  ArrayInit(readold,   0, _g_nmotors);
  ArrayInit(errorold,  0, _g_nmotors);
  ArrayInit(readstart, 0, _g_nmotors);
  ArrayInit(cmax,      0, _g_nmotors);
  ArrayInit(starttime, 0, _g_nmotors);
  ArrayInit(runtime,   0, _g_nmotors);
  ArrayInit(clock,     0, _g_nmotors);
  ArrayInit(regloop,   0, _g_nmotors);
  ArrayInit(cnt,       0, _g_nmotors);
  
  // mainloop
  while(1) {
     long ms = CurrentTick();
     
     // loop through all motors
     for(int m=0; m<_g_nmotors; m++) {
        getMutex(m);
        
        if(pid[m].state == PID_RUNSTATE_IDLE) {
           // controller standby
        }
        else if(pid[m].state == PID_RUNSTATE_EB) {
           Off(m);
        }
        // controller on -> motor cmd call
        else if(pid[m].state == PID_RUNSTATE_START) {
           readstart[m]   = MotorRotationCount(m); // read;
           pid[m].err  = pid[m].target - readstart[m];
           
           regloop[m]     = 1;
           starttime[m] = CurrentTick();    //arr
           
//--------------------------------------------------------------
// init variables for graph output
#ifdef DEBUG_PID
           if(m == DEBUG_PORT) {
              ClearScreen();
              displayMask();

              scrXratio=abs(pid[DEBUG_PORT].err)/8;
              if(pid[DEBUG_PORT].target<50) scrXratio=abs(pid[DEBUG_PORT].err)/4;

              scrYratio=abs(pid[DEBUG_PORT].err)/40;
           }
#endif
//--------------------------------------------------------------

          // initializing done, now start
           pid[m].state = PID_RUNSTATE_RUNNING;
        }
        else if(pid[m].state == PID_RUNSTATE_RUNNING
             || pid[m].state == PID_RUNSTATE_RUNNINGCONT) {
          // get motor position
           read  = MotorRotationCount(m);
           pid[m].err   = pid[m].target - read;

           if (read>cmax[m]) {
              cmax[m] = read;               // monitor overshooting
           }
//         else if (PID_A.read<cmin) {      // monitor overshooting
//            cmin=PID_A.read;
//         }
        
           // core control loop
           dtime      = CurrentTick() - clock[m];
           clock[m]   = CurrentTick();
           runtime[m] = clock[m] - starttime[m];
           tprop      = dtime/20.0;          // whats this, why not dtime?
           
           // check if motor stalling
           if ((pid[m].err==errorold[m])&& (abs(pid[m].err)>pid[m].precis))
              damp=1;    // stalling <- if controll time to low??? TEST THIS ///<<<<
           else
              damp=pid[m].damp;

           pid[m].integr = (damp * pid[m].integr) + pid[m].err;

           // for nxt motors 100
           if((pid[m].integr) > 3*_g_maxout) {
              pid[m].integr = 3*_g_maxout; // cut away
           }
           else if((pid[m].integr) <-3*_g_maxout) {
              pid[m].integr = -3*_g_maxout;
           }

           // core pid calculation
           PWMpwr=   (pid[m].P*pid[m].err)                         // KP*e(t)
                   + (pid[m].I*pid[m].integr)*tprop                // KI*e(t).dt
                   + (pid[m].D*(pid[m].err-errorold[m]))/tprop;    // KD*e(t)/dt

           // speed limits
           if(PWMpwr >  _g_maxout) {
              PWMpwr=  _g_maxout;
           }
           else if(PWMpwr < -_g_maxout) {
              PWMpwr= -_g_maxout;
           }
                           // this is the same es err - errorold?!
           pid[m].cspeed= (read-readold[m])*100/dtime;  // rotat speed [degrees/100ms]

           // user speed limit
           // why not limit pwmpwr? //<< TEST
           // ok this should be the real motor speed/pwm
           if ( abs(pid[m].cspeed) > pid[m].tarpwm)  {
              PWMpwr = sign(PWMpwr)*pid[m].tarpwm;
           }
           pid[m].outp = round(PWMpwr);

#ifdef DEBUG_PID
//..........................................................................
           //  for graph output
           if(m == DEBUG_PORT) {
              timex= runtime[DEBUG_PORT]/scrXratio;
              PointOut(timex,(read-readstart)/scrYratio);
              LineOut(oldtx, oldy, timex, pwm0+pid[DEBUG_PORT].cspeed*0.3);

              oldtx=timex; oldy=pwm0+pid[DEBUG_PORT].cspeed*0.3;

              printf1( 0,48,"%5.0f" , pid[DEBUG_PORT].cspeed);

              printf1(63, 48,"%-6d"   , cmax[DEBUG_PORT]);
             // printf1(63, 40,"%-6d"   , cmin);
              printf1(63, 32,"%6.1f"  , PWMpwr);
          }
//..........................................................................
#endif
           // check end position
           if (abs(pid[m].err)<=pid[m].precis) {
              regloop[m] +=1;
           }
           if( (abs(pid[m].err)>=pid[m].precis) && (regloop[m]<=5) ) {
              // action!
              OnFwd(m, pid[m].outp);
              
              readold[m]  = read;
              errorold[m] = pid[m].err;
              pid[m].state = PID_RUNSTATE_RUNNING;
           }
           else {
              // at target position
              Off(m);
              if(pid[m].cont) {
                 pid[m].state = PID_RUNSTATE_RUNNINGCONT;
              }
              else {
                 pid[m].state = PID_RUNSTATE_BRAKE;
              }
           }
       }
       else if(pid[m].state == PID_RUNSTATE_BRAKE) {
          // brake and recheck position
          Off(m);
          pid[m].outp=0;
          
          // Wait(50) and then recheck if motor is at end pos
          // same as Wait 50
          if( (_g_regtime*cnt[m]) > 50) {
             read = MotorRotationCount(m);
             regloop[m]=1;
             cnt[m] = 0;

             if (read>cmax[m])
                cmax[m] = read;
//           if (read<cmin)
//              cmin=read;

             pid[m].err = pid[m].target-read;

#ifdef DEBUG_PID
             if(m == DEBUG_PORT) {
                printf1(63, 48,"%-6d"   , cmax[DEBUG_PORT]);
                //printf1(63, 40,"%-6d"   , cmin);
                printf1(63, 32,"%6.1f"  , PWMpwr);
             }
#endif

             if ((abs(pid[m].err)>pid[m].precis)) {
                // should solve this with a higher regloop
                // chance to stuck in loop between brake and running?
                pid[m].state = PID_RUNSTATE_RUNNING;
             }
             else {
                // move finished
                pid[m].state = PID_RUNSTATE_IDLE;
#ifdef debug_PID_A
//............................................................................
                PointOut(timex,pid[DEBUG_PORT].read/scrYratio);
                LineOut(oldtx, oldy, timex, pwm0);
                LineOut(timex+2,pid[DEBUG_PORT].target/scrYratio, timex+10, pid[DEBUG_PORT].target/scrYratio);
                LineOut(timex+2, pwm0, timex+10, pwm0);
//............................................................................
#endif
             }
          }
          else {
             cnt[m]++;
          }
       }
       releaseMutex(m);
     } // for

     // wait regtime
     ms += _g_regtime;
     long del = ms-CurrentTick();
     if (del < 1)
         del = 1;
     else if (del > _g_regtime)
         del = _g_regtime;
     Wait(del);
  } // while
}



//==============================================================================
// HIGH LEVEL FUNCTIONS
//==============================================================================

/**
 * \brief  Checks if move is done.
 *
 * \param port   Desired output port: OUT_A, OUT_B, OUT_C
 * \return    True if move is done.
*/
inline bool moveDone(char port)
{
   bool ret = false;
   if(pid[port].state == PID_RUNSTATE_IDLE
   || pid[port].state == PID_RUNSTATE_RUNNINGCONT) {
      ret = true;
   }
   return ret;
}

/**
 * \brief Wait until last move is done.
 *
 * \param port   Desired output port: OUT_A, OUT_B, OUT_C
*/
void waitMove(char port) {
   do {
      Wait(2);
   } while( !moveDone(port) );
}

/**
 * \brief Move motor to absolute position.
 *
 * \param port       Desired output port: OUT_A, OUT_B, OUT_C
 * \param target     Position in degrees. 0°-360°
 * \param speed      Output power 1%-100%
 * \param wait_move  Wait until move is done. (true = wait)
 *
*/
void moveAbs(char port,
             long target,
             float speed
             bool wait_move = true)
{
   // just in case
   if(pid[port].state == PID_RUNSTATE_EB)
      return;

   // allow changing target while moving?
   waitMove(port);
   
   if(pid[port].state == PID_RUNSTATE_RUNNINGCONT) {
      pid[port].state = PID_RUNSTATE_IDLE;
   }
   getMutex(port);
   
   pid[port].target = target;
   pid[port].tarpwm  = speed;

   // now go
   pid[port].state = PID_RUNSTATE_START;
   releaseMutex(port);
   
   if(wait_move)
      waitMove(port);
}

/**
 * \brief Move motor relative.
 *
 * \param port       Desired output port: OUT_A, OUT_B, OUT_C
 * \param degree     Position in degrees. 0°-360°
 * \param speed      Output power 1%-100%
 *                   pwr = 0 will use the last used power.
 * \param wait_move  Wait until move is done. (true = wait)
*/
void moveRel(char port,
             long degree,
             float speed
             bool wait_move = true)
{
    long pos = MotorRotationCount(OUT_A);
    long to_pos = pos + degree;
    moveAbs(port, to_pos, speed, wait_move);
}

/**
 * \brief Get actual motor runstate.
 *
 * See runstate constants for more information.
 *
 * \param port       Desired output port: OUT_A, OUT_B, OUT_C
*/
inline char getRunstate(char port) {
   return pid[port].state;
}

/**
 * \brief Get actual error to target in degrees.
 *
 * \param port       Desired output port: OUT_A, OUT_B, OUT_C
*/
inline long getError(char port) {
   return pid[port].err;
}


/**
 * \brief Reset the motor.
 *
 * Reset the motor by turning until stalling.
 * Be aware of using this function.
 *
 * /param m      Desired output port. Can be a constant or a variable,
 *               see Output port constants.
 * /param pwr    Output power, from -50 to +50, use the sign for direction.
 *
*/
safecall void resetMotor(char port, char pwr)
{
   waitMove(port);
   long tacho_now, tacho_prev;

   // power limiting (anything above this level would be dangerous)
   if(pwr > 50)
      pwr = 50;
   if(pwr < -50)
      pwr = -50;

   // force idle
   getMutex(port);
   pid[port].state = PID_RUNSTATE_IDLE;
   releaseMutex(port);
   Wait(20);
   
   OnFwdEx(port, pwr, RESET_NONE);
   tacho_now = MotorTachoCount(port);
   do {
      Wait(200);
      tacho_prev = tacho_now;
      tacho_now = MotorTachoCount(port);
   } while(tacho_now != tacho_prev);
   ResetTachoCount(port);
   Wait(200);
   
   // start cont mode after next move??
   if(pid[port].cont) {
      // hope changing to cont mode will not cause problems?
      // do not need mutex because we should be in idle mode!
      pid[port].state = PID_RUNSTATE_RUNNINGCONT;
   }
}

/**
 * \brief Stop (all) motors immediately.
 *
*/
void motorEmergencyStop()
{
   PlayTone(440, 500);
   
   for (int m = 0; m<_g_nmotors; m++) {
      getMutex(m);
      pid[m].state = PID_RUNSTATE_EB;
      releaseMutex(m);
   }

   // wait until motor is stable..
   Wait(1500);
   PlayTone(440, 500);
}

/**
 * \brief Set continuous Mode.
 *
 * The continuous mode will regulate the motor with pid to hold position.
 * Do not
 *
 * /param m      Desired output port. Can be a constant or a variable,
 *               see Output port constants.
 * /param pwr    Output power, from -50 to +50, use the sign for direction.
 *
*/
inline void setContMode(char port, bool on) {
   pid[port].cont = on;
   if(pid[port].state == PID_RUNSTATE_IDLE && on) {
      getMutex(port);
      pid[port].state = PID_RUNSTATE_RUNNINGCONT;
      releaseMutex(port);
   } else if(pid[port].state != PID_RUNSTATE_IDLE && !on) {
      waitMove(port);
      pid[port].state = PID_RUNSTATE_IDLE;
   }
}

void controllerOff()
{
   for(int m=0; m<_g_nmotors; ++m) {
      setContMode(m, false);
   }
}

void controllerOn()
{
   for(int m=0; m<_g_nmotors; ++m) {
      setContMode(m, true);
   }
}

void setPID(char port,
            float P,
            float I,
            float D,
            float prec = NA,
            float damp = NA,
            bool cont =  NA)
{
   pid[port].P = P;              // P: propotional to error
   pid[port].I = I;              // I: avoid perish
   pid[port].D = D;              // D: derivative: avoid oscillating

   if(prec != NA)
      pid[port].precis = prec;      // error precision to target
   if(damp != NA)
      pid[port].damp = damp;  // PID regulation time
   if(cont != NA)
      pid[port].cont = cont;        // damp error integral
}


void initPID(char nmotors,
             float P = 0.40,
             float I = 0.40,
             float D = 10,
             float prec = 1,
             float damp = 0.75,
             int rtime = 10)
{
   // i hope nobody will pass 0 ;)
   _g_nmotors = nmotors;
   _g_regtime = rtime;
   ArrayInit(pid, 0, _g_nmotors);
   Wait(10);
   for (char m=0; m<_g_nmotors; ++m) {
      // p,i,d, precis, reg_time, damp
      setPID(m, P, I, D, prec, damp, rtime);
      pid[m].state  = PID_RUNSTATE_IDLE;
      pid[m].target = 0;
      pid[m].tarpwm = 0;
      pid[m].err    = 0;
      pid[m].integr = 0;
      pid[m].cspeed = 0;
      Wait(1);
   }
}
//==========================================================================
/// \cond

//..........................................................................
// display, monitor, and debug

void displayMask()
{
   printf1(36, 48,"max","" );     // max value
   printf1(36, 40,"min","");      // min value
   printf1(36, 32,"out" ,"");     // last pwm out
}


void _updateDisplay(char port)
{
   switch(port) {
      case OUT_A:
         printf1( 0,56,"run %-2d", pid[port].state);
         printf1(21,24,"%-6d", MotorRotationCount(port));
         printf1(21, 0,"%-6d", pid[port].target);
         break;
      case OUT_B:
         printf1(49,56,"%-2d", pid[port].state);
         printf1(49,24,"%-6d", MotorRotationCount(port));
         printf1(49, 0,"%-6d", pid[port].target);
         break;
      case OUT_C:
         printf1(77,56,"%-2d", pid[port].state);
         printf1(77,24,"%-6d", MotorRotationCount(port));
         printf1(77, 0,"%-6d", pid[port].target);
         break;
   }
}

task Monitor_Display_Task()
{
   while(1) {
      if(_g_nmotors <= 1) {
         _updateDisplay(OUT_A);
      } else if(_g_nmotors <= 2) {
         _updateDisplay(OUT_A);
         _updateDisplay(OUT_B);
      } else if(_g_nmotors <= 3) {
         _updateDisplay(OUT_A);
         _updateDisplay(OUT_B);
         _updateDisplay(OUT_C);
      } else {
      }

      Wait(50);
   }
}

/// \endcond
//..........................................................................

task main() {

  long target;                     // custom values
  int  rotateSpeed;

  initPID(1);
  start PID_Controller_Task;

  setContMode(OUT_A, false);

#ifdef DEBUG_PID
   start Monitor_Display_Task;
#endif

  while(true) {

    target      =  360;                // set new motor rotation target
    rotateSpeed =  100;                // set max rotation speed

    moveRel(OUT_A, target, rotateSpeed);

    PlayTone(TONE_C4,100);
    Wait(100);
    // printf1(0,48,"%s","OK");

    while (BTNCENTER!= getchar()) {
       // give controller task time
       Wait(10);
    }
  }
  
  /*
  moveRel(OUT_A, 2000, 77, false);
  Wait(1000);
  motorEmergencyStop();
  */
  
  Wait(5000);
}