/*
 * mortor_control.c
 *
 *  Created on: Jun 17, 2023
 *      Author: hht
 */

#include "motor_control.h"

extern float zero_electric_angle;
extern int pole_pairs;
extern float shaft_angle;
extern int dir;
extern float voltage_limit;
extern float voltage_power_supply;
extern int period;

float _normalizeAngle(float angle){
  float a = fmod(angle, 2*M_PI);   //取余运算可以用于归一化，列出特殊值例子算便知
  return a >= 0 ? a : (a + 2*M_PI);
  //三目运算符。格式：condition ? expr1 : expr2
  //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。
  //可以将三目运算符视为 if-else 语句的简化形式。
  //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2M_PI 的符号相反。
  //也就是说，如果 angle 的值小于 0 且 _2M_PI 的值为正数，则 fmod(angle, _2M_PI) 的余数将为负数。
  //例如，当 angle 的值为 -M_PI/2，_2M_PI 的值为 2M_PI 时，fmod(angle, _2M_PI) 将返回一个负数。
  //在这种情况下，可以通过将负数的余数加上 _2M_PI 来将角度归一化到 [0, 2M_PI] 的范围内，以确保角度的值始终为正数。
}

float _electricalAngle(float shaft_angle, int pole_pairs) {
  return _normalizeAngle(((float)(dir * pole_pairs)*shaft_angle)-zero_electric_angle);
}



//开环速度函数
float velocityOpenloop(float target_velocity, float Uq, TIM_TypeDef * TIM_BASE){
//	uint32_t now_us = getCurrentMicros();
//	uint32_t now_us = HAL_GetTick();
//  Provides a tick value in microseconds.

  //计算当前每个Loop的运行时间间隔
//  float Ts = (now_us - open_loop_timestamp) * 1e-3f;
	float Ts=2.5E-3f;

  // 通过乘以时间间隔和目标速度来计算需要转动的机械角度，存储在 shaft_angle 变量中。
  //在此之前，还需要对轴角度进行归一化，以确保其值在 0 到 2π 之间。
  shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
  //以目标速度为 10 rad/s 为例，如果时间间隔是 1 秒，则在每个循环中需要增加 10 * 1 = 10 弧度的角度变化量，才能使电机转动到目标速度。
  //如果时间间隔是 0.1 秒，那么在每个循环中需要增加的角度变化量就是 10 * 0.1 = 1 弧度，才能实现相同的目标速度。
  //因此，电机轴的转动角度取决于目标速度和时间间隔的乘积。

  // Uq is not related to voltage limit


  setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, pole_pairs),TIM_BASE);

//  open_loop_timestamp = now_us;  //用于计算下一个时间间隔

  return Uq;
}





void setPwm(float Ua, float Ub, float Uc, TIM_TypeDef * TIM_BASE) {

//	// 限制上限
	Ua = _constrain(Ua, 0.0f, voltage_limit);
	Ub = _constrain(Ub, 0.0f, voltage_limit);
	Uc = _constrain(Uc, 0.0f, voltage_limit);
	// 计算占空比
	// 限制占空比从0到1
	float dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
	float dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
	float dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );

	//写入PWM到PWM 0 1 2 通道
	TIM_BASE->CCR1 = (uint32_t) roundf(dc_a*period);
	TIM_BASE->CCR2 = (uint32_t) roundf(dc_b*period);
	TIM_BASE->CCR3 = (uint32_t) roundf(dc_c*period);

}

void setPhaseVoltage(float Uq,float Ud, float angle_el, TIM_TypeDef * TIM_BASE) {
  angle_el = _normalizeAngle(angle_el + zero_electric_angle);
  if (Uq <0 ){
	  angle_el+=M_PI;
	  Uq=abs(Uq);
  }
  angle_el =  _normalizeAngle (angle_el+M_PI/2);
  int sector = floor(angle_el / M_PI*3) + 1;
  // calculate the duty cycles
    float T1 = _SQRT3 * sin(sector * M_PI/3 - angle_el) * Uq / voltage_power_supply;
    float T2 = _SQRT3 * sin(angle_el - (sector - 1.0) * M_PI/3) * Uq / voltage_power_supply;
    float T0 = 1 - T1 - T2;


    float Ta, Tb, Tc;
      switch (sector)
      {
      case 1:
        Ta = T1 + T2 + T0 / 2;
        Tb = T2 + T0 / 2;
        Tc = T0 / 2;
        break;
      case 2:
        Ta = T1 + T0 / 2;
        Tb = T1 + T2 + T0 / 2;
        Tc = T0 / 2;
        break;
      case 3:
        Ta = T0 / 2;
        Tb = T1 + T2 + T0 / 2;
        Tc = T2 + T0 / 2;
        break;
      case 4:
        Ta = T0 / 2;
        Tb = T1 + T0 / 2;
        Tc = T1 + T2 + T0 / 2;
        break;
      case 5:
        Ta = T2 + T0 / 2;
        Tb = T0 / 2;
        Tc = T1 + T2 + T0 / 2;
        break;
      case 6:
        Ta = T1 + T2 + T0 / 2;
        Tb = T0 / 2;
        Tc = T1 + T0 / 2;
        break;
      default:
        Ta = 0;
        Tb = 0;
        Tc = 0;
      }
  // 克拉克逆变换
      float Ua = Ta * voltage_power_supply;
       float Ub = Tb * voltage_power_supply;
       float Uc = Tc * voltage_power_supply;
  setPwm(Ua,Ub,Uc,TIM_BASE);
}

