stmbl

/*
* This file is part of the stmbl project.
*
* Copyright (C) 2013-2015 Rene Hopf <renehopf@mac.com>
* Copyright (C) 2013-2015 Nico Stute <crinq@crinq.de>
*
* This program 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.
*
* This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
*/

COMP(pid);

HAL_PIN(pos_ext_cmd) = 0.0; // cmd in (rad)
HAL_PIN(pos_fb) = 0.0; // feedback in (rad)
HAL_PIN(pos_error) = 0.0; // error out (rad)

HAL_PIN(vel_ext_cmd) = 0.0; // cmd in (rad/s)
HAL_PIN(vel_fb) = 0.0; // feedback in (rad/s)
HAL_PIN(vel_cmd) = 0.0; // cmd out (rad/s)
HAL_PIN(vel_error) = 0.0; // error out (rad/s)

HAL_PIN(acc_ext_cmd) = 0.0; // cmd in (rad/s^2)
HAL_PIN(acc_cmd) = 0.0; // cmd out (rad/s^2)

HAL_PIN(torque_ext_cmd) = 0.0; // cmd in (Nm)
HAL_PIN(torque_cmd) = 0.0; // cmd out (Nm)
HAL_PIN(torque_cor_cmd) = 0.0; // corrected cmd out (Nm)

HAL_PIN(friction) = 0.0; // (Nm)
HAL_PIN(damping) = 0.0; // (Nm/rad/s)
HAL_PIN(load) = 0.0; // (Nm)


HAL_PIN(enable) = 0.0;
HAL_PIN(pos_en) = 1.0;
HAL_PIN(vel_en) = 1.0;
HAL_PIN(acc_en) = 1.0;

HAL_PIN(p) = 0.9;

HAL_PIN(pos_p) = 100.0;
HAL_PIN(pos_lp) = 5000.0;

HAL_PIN(vel_p) = 1.0;
HAL_PIN(vel_lp) = 5000.0;
HAL_PIN(vel_ff) = 1.0;

HAL_PIN(acc_p) = 1.0;
HAL_PIN(acc_lp) = 5000.0;
HAL_PIN(acc_pi) = 20.0;
//HAL_PIN(acc_aw) = 1.0;
HAL_PIN(acc_ff) = 1.0;

HAL_PIN(torque_ff) = 1.0;


HAL_PIN(friction_ff) = 0.9;
HAL_PIN(damping_ff) = 0.9;
HAL_PIN(load_ff) = 0.9;

HAL_PIN(j) = 0.0;


// system limits
HAL_PIN(max_vel) = 0.0;
HAL_PIN(max_torque) = 0.0;
HAL_PIN(min_torque) = 0.0;

// user limits
HAL_PIN(max_usr_vel) = 0.0;
HAL_PIN(max_usr_acc) = 0.0;
HAL_PIN(max_usr_torque) = 0.0;

HAL_PIN(vel_sat) = 0.0;
HAL_PIN(acc_sat) = 0.0;
HAL_PIN(torque_sat) = 0.0;
HAL_PIN(saturated) = 0.0;


MEM(float sat) = 0.0;
MEM(float torque_error_sum) = 0.0;

MEM(float velcmd) = 0.0;
MEM(float acccmd) = 0.0;
MEM(float torquecmd) = 0.0;

RT(
   float j = MAX(PIN(j), 0.0000001);
   float p = PIN(p);

   float torqueextcmd = PIN(torque_ext_cmd);
   float torquemin = MAX(PIN(min_torque) * p, -PIN(max_usr_torque));
   float torquemax = MIN(PIN(max_torque) * p, PIN(max_usr_torque));
   float torquesat;

   float accextcmd = PIN(acc_ext_cmd);
   float accmin = -PIN(max_usr_acc);//MAX(torquemin / j, -PIN(max_usr_acc));
   float accmax = PIN(max_usr_acc);//MIN(torquemax / j, PIN(max_usr_acc));
   float accsat;

   float velextcmd = PIN(vel_ext_cmd);
   float velfb = PIN(vel_fb);
   float velmin = MAX(-PIN(max_vel) * p, -PIN(max_usr_vel));//MAX(MAX(velfb + accmin * period, -PIN(max_vel) * p) , -PIN(max_usr_vel));
   float velmax = MIN(PIN(max_vel) * p, PIN(max_usr_vel));//MIN(MIN(velfb + accmax * period, PIN(max_vel) * p), PIN(max_usr_vel));
   float velerr;
   float velsat;

   float posextcmd = PIN(pos_ext_cmd);
   float posfb = PIN(pos_fb);
   float poserr = minus(posextcmd, posfb);

   float posp = MAX(PIN(pos_p), 0.0);
   float poslp = LP_HZ(PIN(pos_lp));
   float velp = MAX(PIN(vel_p), 0.0);
   float vellp = LP_HZ(PIN(vel_lp));
   float accp = MAX(PIN(acc_p), 0.0);
   float accpi = MAX(PIN(acc_pi), 0.0);
   //float accaw = MAX(PIN(acc_aw), 0.0);
   float acclp = LP_HZ(PIN(acc_lp));

   float old_velcmd = velcmd;
   float old_acccmd = acccmd;
   float old_torquecmd = torquecmd;

   if(PIN(enable) > 0.0){
      if(PIN(pos_en) == 0.0){
         posp = 0.0;
      }
      if(PIN(vel_en) == 0.0){
         posp = 0.0;
         velp = 0.0;
      }
      if(PIN(acc_en) == 0.0){
         posp = 0.0;
         velp = 0.0;
         accp = 0.0;
         //    accaw = 0.0;
         torque_error_sum = 0.0;
      }

      // pos -> vel
      velcmd = posp * poserr + PIN(vel_ff) * velextcmd;  // vel = p * pos_error + ff1
      velcmd = velcmd * poslp + (1.0 - poslp) * old_velcmd; // lowpass
      velsat = SAT2(velcmd, velmin, velmax);
      velcmd = CLAMP(velcmd, velmin, velmax); // min/max clamping

      // vel -> acc
      velerr = velcmd - velfb;
      acccmd = velerr / period * velp + PIN(acc_ff) * accextcmd; // acc = p * vel_error / period + ff2
      acccmd = acccmd * vellp + (1.0 - vellp) * old_acccmd; // lowpass
      accsat = SAT2(acccmd, accmin, accmax);
      acccmd = CLAMP(acccmd, accmin, accmax); // min/max clamping

      // acc -> torque
      torquecmd = acccmd * j * accp + PIN(torque_ff) * torqueextcmd; // torque = acc * inertia + ext cmd
      torque_error_sum += velerr * period * accp * accpi; // integrate vel_error
      //torque_error_sum = CLAMP(torque_error_sum, torquemin, torquemax); // clamp integral
      torque_error_sum = CLAMP(torque_error_sum, torquemin - torquecmd, torquemax - torquecmd); // clamp integral
      if(accpi == 0.0){
         torque_error_sum = 0.0;
      }
      torquecmd += torque_error_sum;  // add integral
      torquecmd = torquecmd * acclp + (1.0 - acclp) * old_torquecmd; // lowpass
      PIN(torque_cmd) = CLAMP(torquecmd, torquemin, torquemax);
      if(velfb != 0.0){ // friction ff
         torquecmd += SIGN(velfb) * PIN(friction) * PIN(friction_ff);
      }
      else{
         torquecmd += SIGN(torquecmd) * PIN(friction) * PIN(friction_ff);
      }
      torquecmd += velfb * PIN(damping) * PIN(damping_ff); // damping ff
      torquecmd -= PIN(load) * PIN(load_ff); // load ff
      torquesat = SAT2(torquecmd, torquemin, torquemax);
      torquecmd = CLAMP(torquecmd, torquemin, torquemax); // min/max clamping

      // sat
      if(ABS(velsat) + ABS(accsat) + ABS(torquesat) > 0.0){
         sat += period;
      }
      else{
         sat = 0.0;
      }
   }
   else{
      torque_error_sum = 0.0;
      velcmd = 0.0;
      velerr = 0.0;
      acccmd = 0.0;
      torquecmd = 0.0;
      velsat = 0.0;
      accsat = 0.0;
      torquesat = 0.0;
      sat = 0.0;
   }

   PIN(pos_error) = poserr;
   PIN(vel_cmd) = velcmd;
   PIN(vel_error) = velerr;
   PIN(acc_cmd) = acccmd;
   PIN(torque_cor_cmd) = torquecmd;

   PIN(vel_sat) = velsat;
   PIN(acc_sat) = accsat;
   PIN(torque_sat) = torquesat;
   PIN(saturated) = sat;
);

ENDCOMP;