/*
    Task8 Task
    
*/

/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/motor_rm.h"
#include "component/pid.h"

/* USER INCLUDE END */

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
MOTOR_RM_t motor_6020;
MOTOR_RM_t motor_3508;
MOTOR_RM_Param_t motor_6020_param = {
    .can = BSP_CAN_1,
    .id = 0x205,
    .module = MOTOR_GM6020,
    .reverse = false,
    .gear = false,
};
MOTOR_RM_Param_t motor_3508_param = {
    .can = BSP_CAN_1,
    .id = 0x201,
    .module = MOTOR_M3508,
    .reverse = false,
    .gear = false,
};

KPID_t motor_6020_speed;
KPID_t motor_3508_angle;
KPID_t motor_3508_omega;

KPID_Params_t motor_6020_speed_param = {
  .k = 0.1f,
  .p = 0.2f,
  .i = 0.1f,
  .d = 0.0f,
  .i_limit = 1.0f,
  .out_limit = 1.0f,
  .d_cutoff_freq = 30.0f,
};
KPID_Params_t motor_3508_angle_param = {
   .k = 1.0f,
  .p = 5.0f,
  .i = 0.1f,
  .d = 0.0f,
  .i_limit = 1.0f,
  .out_limit = 1.0f,
  .d_cutoff_freq = 30.0f,
};
KPID_Params_t motor_3508_omega_param = {
    .k = 1.0f,
  .p = 5.0f,
  .i = 0.1f,
  .d = 0.0f,
  .i_limit = 1.0f,
  .out_limit = 1.0f,
  .d_cutoff_freq = 30.0f,
};

float motor_3508_target_angle = 5*M_2PI;
float motor_6020_target_speed = 33.0f;
/* USER STRUCT END */

/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_Task8(void *argument) {
  (void)argument; /* 未使用argument，消除警告 */

  
  /* 计算任务运行到指定频率需要等待的tick数 */
  const uint32_t delay_tick = osKernelGetTickFreq() / TASK8_FREQ;

  osDelay(TASK8_INIT_DELAY); /* 延时一段时间再开启任务 */

  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
  /* USER CODE INIT BEGIN */
  BSP_CAN_Init();
MOTOR_RM_Register( &motor_6020_param);
MOTOR_RM_Register(&motor_3508_param);

PID_Init(&motor_6020_speed, KPID_MODE_CALC_D, TASK8_FREQ, &motor_6020_speed_param);
PID_Init(&motor_3508_angle, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_angle_param);
PID_Init(&motor_3508_omega, KPID_MODE_NO_D, TASK8_FREQ, &motor_3508_omega_param);
  /* USER CODE INIT END */
  
  while (1) {
    tick += delay_tick; /* 计算下一个唤醒时刻 */
    /* USER CODE BEGIN */
  MOTOR_RM_UpdateAll();

    motor_6020.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_6020_param)->motor)) );
    motor_3508.feedback.rotor_abs_angle= MOTOR_RM_GetMotor(&motor_3508_param)->gearbox_total_angle;
    motor_3508.feedback.rotor_speed= (MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&motor_3508_param)->motor)) );


    float motor_6020out,motor_3508_ommega,motor_3508_out;

    motor_6020out = PID_Calc(&motor_6020_speed, motor_6020_target_speed, motor_6020.feedback.rotor_speed, 0.0f,0.0f);

    // motor_3508_ommega = PID_Calc(&motor_3508_omega, motor_3508_target_angle, motor_3508.feedback.rotor_abs_angle, 0.0f,0.0f);
    // motor_3508_out = PID_Calc(&motor_3508_angle, motor_3508_ommega, motor_3508.feedback.rotor_speed, 0.0f,0.0f);

    MOTOR_RM_SetOutput(&motor_6020_param, motor_6020out);
    MOTOR_RM_Ctrl(&motor_6020_param);
    /* USER CODE END */
    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
  }
  
}