#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];


// C键 sw[5]👆 1800   中1000  👇200
// D键 sw[6]👆 200 外伸   👇1800 归位  //三摩擦架子
// H键 sw[7]👆 200   👇1800  //取球

int angle1=34;
int angle2=23;

int abc=0;

int a1=0;

int speed_set=0;
int speed_feedback=0;

extern int speedm;


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);


   if(rc_ctrl.sw[6] == 200)
    {
        angle1=75;
    }
    if(rc_ctrl.sw[6]==1800)
    {
        angle1=0;
      
    }

     //运球
    if(rc_ctrl.sw[5]==200)
    {
        //👇
        ball.ballHadling(); // 处理摩擦轮转动
    }
     //下球     
    if(rc_ctrl.sw[5]==1800)
    {
        //👆
        ball.ballDown();
    }
    //中间
    if(rc_ctrl.sw[7]==200)
    {
         if(rc_ctrl.sw[5]==1000)
        {
            //中
            ball.ballStop();
            ball.ballHadling(); // 处理摩擦轮转动
        }
    }

    if(rc_ctrl.sw[7]==1800)
    {
        //ball.ballTake();

    }

	// CAN_XiaoMi(1,&ball.JZ_xiaomi,&hcan2);

    ball.Spin(speedm);
    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);
    }
}