stmbl

COMP(motsim);

HAL_PIN(u) = 0.0;
HAL_PIN(v) = 0.0;
HAL_PIN(w) = 0.0;

HAL_PIN(pos) = 0.0;
HAL_PIN(vel) = 0.0;
HAL_PIN(acc) = 0.0;
HAL_PIN(torq) = 0.0;
HAL_PIN(ind) = 0.0;
HAL_PIN(cur) = 0.0;

HAL_PIN(r) = 0.0;
HAL_PIN(l) = 0.0;
HAL_PIN(nm_a) = 0.0;
HAL_PIN(v_rps) = 0.0;

HAL_PIN(poles) = 0.0;
HAL_PIN(ac) = 0.0;

HAL_PIN(friction) = 0.0;
HAL_PIN(damping) = 0.0;
HAL_PIN(innertia) = 0.0;

HAL_PIN(load_force) = 0.0;
HAL_PIN(load_friction) = 0.0;
HAL_PIN(load_damping) = 0.0;
HAL_PIN(load_innertia) = 0.0;



MEM(float pos_) = 0.0;
MEM(float vel_) = 0.0;
MEM(float cur_) = 0.0;

FRT(
  float poles_ = PIN(poles);
  float volt;
  float ind_;
  float torq_
  float fric = PIN(friction) + PIN(load_friction);
  float acc_;

  if(PIN(ac) > 0.0){
    volt = 2/3 * (-sinf(pos_ * poles_) * PIN(u) - sinf(pos_ * poles_ - 2/3 * M_PI) * PIN(v) - sinf(pos_ * poles_ + 2/3 * M_PI) * PIN(w));
  }
  else{
    volt = PIN(u) - PIN(v);
  }

  ind_ = vel_ * PIN(v_rps);
  cur_ += (volt - ind_ - cur_ * PIN(r)) / PIN(l) * period;
  //cur_ = CLAMP(state.cur, -elec_spec.max_i, elec_spec.max_i);
  //state.cur = (state.volt - state.ind) / elec_spec.r;
  torq_ = cur_ * PIN(nm_a) - (PIN(damping) + PIN(load_damping)) * vel_ + PIN(load_force);
  if(abs(torq_) < fric_){
    torq_ = 0.0;
  }
  else{
    torq_ = torq_ - SIGN(torq_) * fric_;
  }

  acc_ = torq_ / (PIN(inertia) + PIN(load_inertia));
  vel_ += acc_ * period;
  pos_ += vel_ * period;
  pos_ = mod(pos_);

  PIN(pos) = pos_;
  PIN(vel) = vel_;
  PIN(acc) = acc_;
  PIN(torq) = torq_;
  PIN(ind) = ind_;
  PIN(cur) = cur_;
);



ENDCOMP;