stmbl

/*
* This file is part of the stmbl project.
*
* Copyright (C) 2013-2016 Rene Hopf <renehopf@mac.com>
* Copyright (C) 2013-2016 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/>.
*/

HAL_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(torque_sum) = 0.0; 

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(torque_en) = 1.0;

HAL_PIN(pos_p) = 100.0; // (1/s)

HAL_PIN(vel_p) = 2000.0; // (1/s)
HAL_PIN(vel_i) = 10.0;
HAL_PIN(vel_g) = 1.0;

HAL_PIN(acc_g) = 1.0;

HAL_PIN(g) = 0.99; // model limit scaling

HAL_PIN(j) = 0.0; // (kgm^2)


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

// user limits
HAL_PIN(max_usr_vel) = 0.0; // (rad/s)
HAL_PIN(max_usr_acc) = 0.0; // (rad/s^2)
HAL_PIN(max_usr_torque) = 0.0; // (Nm)

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


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

RT(
   float j = MAX(PIN(j), 0.0);
   float g = CLAMP(PIN(g), 0.0, 1.0);

   float torque_ext_cmd = PIN(torque_ext_cmd);
   float torque_min = MAX(PIN(min_torque) * g, -PIN(max_usr_torque));
   float torque_max = MIN(PIN(max_torque) * g, PIN(max_usr_torque));
   float torque_cmd;
   float torque_sat;

   float acc_ext_cmd = PIN(acc_ext_cmd);
   float acc_min = -PIN(max_usr_acc);//MAX(torque_min / j, -PIN(max_usr_acc));
   float acc_max = PIN(max_usr_acc);//MIN(torque_max / j, PIN(max_usr_acc));
   float acc_cmd;
   float acc_sat;

   float vel_ext_cmd = PIN(vel_ext_cmd);
   float vel_fb = PIN(vel_fb);
   float vel_min = MAX(-PIN(max_vel) * g, -PIN(max_usr_vel));//MAX(MAX(velfb + accmin * period, -PIN(max_vel) * p) , -PIN(max_usr_vel));
   float vel_max = MIN(PIN(max_vel) * g, PIN(max_usr_vel));//MIN(MIN(velfb + accmax * period, PIN(max_vel) * p), PIN(max_usr_vel));
   float vel_cmd;
   float vel_error;
   float vel_sat;

   float pos_ext_cmd = PIN(pos_ext_cmd);
   float pos_fb = PIN(pos_fb);
   float pos_error = minus(pos_ext_cmd, pos_fb);

   float pos_p = MAX(PIN(pos_p), 0.0);
   float vel_p = MAX(PIN(vel_p), 0.0);
   float vel_i = MAX(PIN(vel_i), 0.0);
   float vel_g = CLAMP(PIN(vel_g), 0.0, 1.0);
   float acc_g = CLAMP(PIN(acc_g), 0.0, 1.0);

   
   float pos_en = PIN(pos_en);
   float vel_en = PIN(vel_en);
   float acc_en = PIN(acc_en);
   float torque_en = PIN(torque_en);

   if(PIN(enable) > 0.0){
      if(torque_en <= 0.0){
         pos_en = 0.0;
         vel_en = 0.0;
         acc_en = 0.0;
         torque_ext_cmd = 0.0;
      }
      if(acc_en <= 0.0){
         pos_en = 0.0;
         vel_en = 0.0;
         acc_g = 0.0;
         acc_ext_cmd = 0.0;
      }
      if(vel_en <= 0.0){
         pos_en = 0.0;
         pos_error = 0.0;
         vel_ext_cmd = 0.0;
         vel_fb = 0.0;
         torque_sum = 0.0;
      }
      if(pos_en <= 0.0){
         pos_p = 0.0;
         pos_error = 0.0;
      }
      
      // pos -> vel
      vel_cmd = pos_error * pos_p; // p
      //vel_cmd = CLAMP(vel_cmd, vel_fb + acc_min * period, vel_fb + acc_max * period); // adaptive loop clamping
      vel_cmd = CLAMP(vel_cmd, vel_min * vel_g, vel_max * vel_g); // p clamping
      vel_cmd += vel_ext_cmd; // ff
      vel_sat = SAT2(vel_cmd, vel_min * 0.99, vel_max * 0.99);
      vel_cmd = CLAMP(vel_cmd, vel_min, vel_max); // clamping

      // vel -> acc
      vel_error = vel_cmd - vel_fb;
      acc_cmd = vel_error * vel_p; // p
      acc_cmd = CLAMP(acc_cmd, acc_min * acc_g, acc_max * acc_g); // p clamping
      acc_cmd += acc_ext_cmd; // ff
      acc_sat = SAT2(acc_cmd, acc_min * 0.99, acc_max * 0.99);
      acc_cmd = CLAMP(acc_cmd, acc_min, acc_max); // clamping

      // acc -> torque
      torque_cmd = acc_cmd * j + torque_ext_cmd; // ff
      torque_cmd = CLAMP(torque_cmd, torque_min, torque_max); // clamping
      torque_sum += vel_error * period * vel_i; // i
      torque_sum = CLAMP(torque_sum, torque_min - torque_cmd, torque_max - torque_cmd); // dynamic integral clamping
      torque_cmd += torque_sum;
      torque_sat = SAT2(torque_cmd, torque_min * 0.99, torque_max * 0.99);
      
      
      if(vel_i == 0.0){
         torque_sum = 0.0;
      }
      
      PIN(torque_cmd) = torque_cmd;
      if(vel_fb != 0.0){ // friction ff
         torque_cmd += SIGN(vel_fb) * PIN(friction);
      }
      else{
         torque_cmd += SIGN(torque_cmd) * PIN(friction);
      }
      torque_cmd += vel_fb * PIN(damping); // damping ff
      torque_cmd -= PIN(load); // load ff
      torque_sat += 2 * SAT2(torque_cmd, torque_min, torque_max);
      torque_cmd = CLAMP(torque_cmd, torque_min, torque_max); // min/max clamping

      // sat
      if(ABS(vel_sat) + ABS(acc_sat) + ABS(torque_sat) > 0.0){
         sat += period;
      }
      else{
         sat -= period;
      }
      sat = MAX(sat, 0.0);
   }
   else{
      torque_sum = 0.0;
      pos_error = 0.0;
      vel_cmd = 0.0;
      vel_error = 0.0;
      acc_cmd = 0.0;
      torque_cmd = 0.0;
      vel_sat = 0.0;
      acc_sat = 0.0;
      torque_sat = 0.0;
      sat = 0.0;
   }

   PIN(pos_error) = pos_error;
   PIN(vel_cmd) = vel_cmd;
   PIN(vel_error) = vel_error;
   PIN(acc_cmd) = acc_cmd;
   PIN(torque_cor_cmd) = torque_cmd;
   PIN(torque_sum) = torque_sum;

   PIN(vel_sat) = vel_sat;
   PIN(acc_sat) = acc_sat;
   PIN(torque_sat) = torque_sat;
   PIN(saturated) = sat;
);

ENDCOMP;