#include "ballTask.hpp"
#include "TopDefine.h"
#include "FreeRTOS.h"
#include "userTask.h"
#include <cmsis_os2.h>
#include "ball.hpp"
#include "remote_control.h"
#include "vofa.h"
extern RC_ctrl_t rc_ctrl;
Ball ball;
float vofa[8];

int abc=0;
extern int speedm;
extern int speedm1;

void FunctionBall(void *argument)
{
    (void)argument; /* 未使用argument，消除警告 */

    const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_BALL;
    uint32_t tick = osKernelGetTickCount();
		
    while(1)
    {
#ifdef DEBUG
     task_struct.stack_water_mark.ball = osThreadGetStackSpace(osThreadGetId());  
#endif          
       abc=HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);
       //abc=HAL_GPIO_ReadPin(STOP_GPIO_Port, STOP_Pin);

       ball.ballHadling(); // 处理摩擦轮转动
      ball.Spin(speedm,speedm1);
      ball.Send_control();

      vofa[0] = speedm; // 发送电机角度数据
      vofa[1] = -ball.hand_Motor[0]->speed_rpm; // 发送电机角度数据
      vofa[2] = ball.hand_Motor[1]->speed_rpm; // 发送电机速度数据
      vofa[3] = ball.hand_Motor[2]->speed_rpm; // 发送电机速度数据

      vofa_tx_main(vofa); // 发送数据到虚拟串口

        tick += delay_tick; /* 计算下一个唤醒时刻 */
        osDelayUntil(tick);
    }
}