stmbl

COMP(p2uvw);

HAL_PIN(pwm) = 0.0;
HAL_PIN(magpos) = 0.0;
HAL_PIN(volt) = 0.0;
HAL_PIN(poles) = 0.0;
HAL_PIN(pwm_max) = 0.0;

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

HAL_PIN(abpwm) = 0.0;

RT_CALC(
  float pos = PIN(magpos) * (int)PIN(poles);
  float pwm_ = LIMIT(PIN(pwm), PIN(pwm_max));
  float volt_ = PIN(volt);
  float u_;
  float v_;
  float w_;
  
  u_ = (sinf(pos + 2.0 * M_PI / 3.0 * 0.0) * pwm_ + 1.0) / 2.0;
  v_ = (sinf(pos + 2.0 * M_PI / 3.0 * 1.0) * pwm_ + 1.0) / 2.0;
  w_ = (sinf(pos + 2.0 * M_PI / 3.0 * 2.0) * pwm_ + 1.0) / 2.0;
  
  if(PIN(abpwm) > 0){
	  float  ofs;
	  if (u_ > w_) {
		  if      (v_ > u_)  ofs = u_/2;
		  else if (v_ > w_)  ofs = v_/2;
		  else     ofs = w_/2;
	  }
	  else {
		  if      (u_ > v_)  ofs = u_/2;
		  else if (w_ > v_)  ofs = v_/2;
		  else     ofs = w_/2;
	  }

	  u_ += ofs;
	  v_ += ofs;
	  w_ += ofs;
  }

  PIN(u) = u_ * volt_;
  PIN(v) = v_ * volt_;
  PIN(w) = w_ * volt_;
);

ENDCOMP;
/*float volt = CLAMP(read_hal_pin(&pwm),-1.0,1.0);
static float pos = 0.0;

if(read_hal_pin(&sync) > 0.0){
	pos = read_hal_pin(&mag_pos) * read_hal_pin(&pole_count);
}
else{
	float vel = read_hal_pin(&mag_vel) * read_hal_pin(&mag_vel_p);
	if(vel > 0){
		vel = MAX(vel, read_hal_pin(&mag_vel_min));
	}
	else{
		vel = MIN(vel, -read_hal_pin(&mag_vel_min));
	}
	pos += vel * read_hal_pin(&pole_count) * period;
}

pos = mod(pos);

switch((int)read_hal_pin(&phase_count)){
	case 0:
		PWM_U = 0;
		PWM_V = 0;
		PWM_W = 0;
		break;
	case 1:
		PWM_U = (sinf(pos) * pwm_scale * volt + 1.0) * mag_res / 2.0;//PD12 PIN1
		PWM_V = mag_res / 2.0;//PD13 PIN2
		PWM_W = 0;//PD15 PIN3
		break;
	case 2:
		PWM_U = (sinf(pos) * pwm_scale * volt + 1.0) * mag_res / 2.0;//PD12 PIN1
		PWM_V = (sinf(pos + M_PI) * pwm_scale * volt + 1.0) * mag_res / 2.0;//PD13 PIN2
		PWM_W = 0;//PD15 PIN3
		break;
	case 3:
		PWM_U = (sinf(pos) * pwm_scale * volt + 1.0) * mag_res / 2.0;
		PWM_V = (sinf(pos + 2.0 * M_PI / 3.0) * pwm_scale * volt + 1.0) * mag_res / 2.0;
		PWM_W = (sinf(pos + 2.0 * M_PI / 3.0 * 2.0) * pwm_scale * volt + 1.0) * mag_res / 2.0;
		break;
	default:
		PWM_U = 0;
		PWM_V = 0;
		PWM_W = 0;
}*/