97 lines
2.0 KiB
C++
97 lines
2.0 KiB
C++
#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;
|
||
|
||
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=85;
|
||
}
|
||
if(rc_ctrl.sw[6]==1800)
|
||
{
|
||
angle1=10;
|
||
|
||
}
|
||
|
||
//运球
|
||
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();
|
||
ball.Extend_control(angle1);
|
||
|
||
// 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);
|
||
}
|
||
}
|