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_cmd_ext) = 0.0;
//HAL_PIN(pos_fb) = 0.0;
HAL_PIN(pos_error) = 0.0;

HAL_PIN(vel_cmd) = 0.0;
HAL_PIN(vel_ext_cmd) = 0.0;
HAL_PIN(vel_fb) = 0.0;
HAL_PIN(vel_error) = 0.0;

HAL_PIN(acc_cmd) = 0.0;
HAL_PIN(acc_ext_cmd) = 0.0;

HAL_PIN(force_cmd) = 0.0;
HAL_PIN(force_ext_cmd) = 0.0;

HAL_PIN(cur_cmd) = 0.0;
HAL_PIN(cur_ext_cmd) = 0.0;
HAL_PIN(cur_fb) = 0.0;
HAL_PIN(cur_error) = 0.0;

HAL_PIN(volt_cmd) = 0.0;

HAL_PIN(pwm_cmd) = 0.0;


HAL_PIN(enable) = 0.0;

HAL_PIN(pos_en) = 1.0;
HAL_PIN(pos_p) = 100.0;
HAL_PIN(pos_lp) = 0.8;

HAL_PIN(vel_en) = 1.0;
HAL_PIN(vel_p) = 1.0;
HAL_PIN(vel_i) = 80.0;
HAL_PIN(vel_d) = 0.0;
HAL_PIN(vel_ff) = 1.0;
HAL_PIN(vel_lp) = 0.3;

HAL_PIN(acc_en) = 1.0;
HAL_PIN(acc_p) = 0.1;
HAL_PIN(acc_ff) = 0.0;

HAL_PIN(force_en) = 1.0;
HAL_PIN(force_p) = 3.667;

HAL_PIN(cur_p) = 0.0;
HAL_PIN(cur_ff) = 15.0;
//HAL_PIN(cur_i) = 0.0;
HAL_PIN(cur_d) = 0.0;
HAL_PIN(cur_ind) = 0.57;
HAL_PIN(cur_lp) = 1.0;

HAL_PIN(volt) = 130;


HAL_PIN(vel_max) = 62.9;
HAL_PIN(vel_e_max) = 1.25;
HAL_PIN(acc_max) = 1200.0;
HAL_PIN(force_max) = 100.0;
HAL_PIN(cur_max) = 6.0;
HAL_PIN(volt_max) = 130.0;
HAL_PIN(pwm_max) = 0.9;


HAL_PIN(saturated) = 0.0;

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

MEM(float vel_error_sum) = 0.0;
MEM(float vel_err_old) = 0.0;
MEM(float cur_cmd_old) = 0.0;
MEM(float sat) = 0.0;

RT_PID(
  float poserr = PIN(pos_error);

  float velextcmd = PIN(vel_ext_cmd);
  float velfb = PIN(vel_fb);
  float velerr;

  float accextcmd = PIN(acc_ext_cmd);

  float forcecmd;
  float forceextcmd = PIN(force_ext_cmd);

  float curcmd;
  float curextcmd = PIN(cur_ext_cmd);
  float curfb = PIN(cur_fb);
  float curerr;

  float pwmcmd;


  float en = PIN(enable);

  float posen = PIN(pos_en);
  float posp = PIN(pos_p);
  float poslp = PIN(pos_lp);

  float velen = PIN(vel_en);
  float velp = PIN(vel_p);
  float veli = PIN(vel_i);
  float veld = PIN(vel_d);
  float velff = PIN(vel_ff);
  float vellp = PIN(vel_lp);

  float accen = PIN(acc_en);
  float accp = PIN(acc_p);
  float accff = PIN(acc_ff);

  float forceen = PIN(force_en);
  float forcep = PIN(force_p);

  float curp = PIN(cur_p);
  float curff = PIN(cur_ff);
  float curd = PIN(cur_d);
  float curind = PIN(cur_ind);
  float curlp = PIN(cur_lp);

  float vlt = PIN(volt);

  float velmax = PIN(vel_max);
  float velerrsummax = PIN(vel_e_max);
  float accmax = PIN(acc_max);
  float forcemax = PIN(force_max);
  float curmax = PIN(cur_max);
  float vltmax = PIN(volt_max);
  float pwmmax = PIN(pwm_max);


  if(en > 0.0){
    // pos -> vel
    if(posen == 0.0){
      posp = 0.0;
    }
    velcmd = LIMIT(posp * poserr + velff * velextcmd, velmax) * poslp + velcmd * (1 - poslp);

    // vel -> acc
    velerr = velcmd - velfb;
    if(velen == 0.0){
      velp = 0.0;
      veli = 0.0;
      veld = 0.0;
    }
    acccmd = velp * velerr + veld * (velerr - vel_err_old) / period + accff * accextcmd;
    vel_err_old = velerr;
    if(veli > 0.0){
      //vel_error_sum = LIMIT(vel_error_sum + velerr * period, velerrsummax);
      vel_error_sum = LIMIT(vel_error_sum  + velerr * period, (accmax - ABS(acccmd)) / veli);
    }
    else{
      vel_error_sum = 0.0;
    }
    acccmd = LIMIT(acccmd + veli * vel_error_sum, accmax) * vellp + acccmd * (1 - vellp);

    // acc -> force
    if(accen == 0.0){
      accp = 0.0;
    }
    forcecmd = LIMIT(accp * acccmd + forceextcmd, forcemax);

    // force -> current
    if(forceen == 0.0){
      forcep = 0.0;
    }
    curcmd = LIMIT(forcep * forcecmd + curextcmd, curmax);

    // current -> volt
    curerr = curcmd - curfb;
    vltcmd = LIMIT(curp * curerr + curff * curcmd + curd * (curcmd - cur_cmd_old) / period + curind * velfb, vltmax)  * curlp + vltcmd * (1 - curlp);
    cur_cmd_old = curcmd;

    // volt -> pwm
    if(vlt == 0.0){
      pwmcmd = 0.0;
    }
    else{
      pwmcmd = vltcmd / vlt;
    }

    if(pwmcmd >= pwmmax || pwmcmd <= -pwmmax){
      sat += period;
    }
    else{
      sat = 0.0;
    }
    pwmcmd = LIMIT(pwmcmd, pwmmax);
  }
  else{
    velcmd = 0.0;
    velerr = 0.0;
    acccmd = 0.0;
    forcecmd = 0.0;
    curcmd = 0.0;
    velerr = 0.0;
    vltcmd = 0.0;
    pwmcmd = 0.0;

    vel_error_sum = 0.0;

    cur_cmd_old = 0.0;

    sat = 0.0;
  }

  PIN(vel_cmd) = velcmd;
  PIN(vel_error) = velerr;
  PIN(acc_cmd) = acccmd;
  PIN(force_cmd) = forcecmd;
  PIN(cur_cmd) = curcmd;
  PIN(cur_error) = curerr;
  PIN(volt_cmd) = vltcmd;
  PIN(pwm_cmd) = pwmcmd;
  PIN(saturated) = sat;
);

ENDCOMP;