diff --git a/Core/Src/usart.c b/Core/Src/usart.c
index f133fad..329c947 100644
--- a/Core/Src/usart.c
+++ b/Core/Src/usart.c
@@ -46,7 +46,7 @@ void MX_USART1_UART_Init(void)
/* USER CODE END USART1_Init 1 */
huart1.Instance = USART1;
- huart1.Init.BaudRate = 115200;
+ huart1.Init.BaudRate = 4000000;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
@@ -104,7 +104,7 @@ void MX_USART6_UART_Init(void)
/* USER CODE END USART6_Init 1 */
huart6.Instance = USART6;
- huart6.Init.BaudRate = 4000000;
+ huart6.Init.BaudRate = 115200;
huart6.Init.WordLength = UART_WORDLENGTH_8B;
huart6.Init.StopBits = UART_STOPBITS_1;
huart6.Init.Parity = UART_PARITY_NONE;
diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx
index 4e7b59f..58ce169 100644
--- a/MDK-ARM/R2.uvoptx
+++ b/MDK-ARM/R2.uvoptx
@@ -223,27 +223,92 @@
13
1
- a
+ b
14
1
- b
+ count,0x0A
15
1
- count,0x0A
+ count
16
1
- count
+ raw_rx1,0x0A
17
1
- raw_rx1,0x0A
+ gyro_kp,0x0A
+
+
+ 18
+ 1
+ nucbuf,0x0A
+
+
+ 19
+ 1
+ vofa_send,0x0A
+
+
+ 20
+ 1
+ SendBuffer,0x10
+
+
+ 21
+ 1
+ NUC_StartSending,0x0A
+
+
+ 22
+ 1
+ PIAN
+
+
+ 23
+ 1
+ pid
+
+
+ 24
+ 1
+ bmi088
+
+
+ 25
+ 1
+ ist8310
+
+
+ 26
+ 1
+ imu_eulr,0x0A
+
+
+ 27
+ 1
+ imu_temp,0x0A
+
+
+ 28
+ 1
+ htim10,0x0A
+
+
+ 29
+ 1
+ pulse
+
+
+ 30
+ 1
+ imu_temp
diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf
index e2feb1d..8272b1a 100644
Binary files a/MDK-ARM/R2/R2.axf and b/MDK-ARM/R2/R2.axf differ
diff --git a/User/Module/Chassis.c b/User/Module/Chassis.c
index bbcc4ec..d1ea42d 100644
--- a/User/Module/Chassis.c
+++ b/User/Module/Chassis.c
@@ -81,13 +81,17 @@ int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
return CHASSIS_OK;
}
-
+fp32 gyro_kp=1.0f;
+fp32 PIAN=0;
void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆运动学的解算,
{
fp64 Nor_Vx,Nor_Vy;//归一化后的数据
normalize_vector(Vx,Vy,&Nor_Vx,&Nor_Vy);
-
+// c->hopemotorout.OmniSpeedOut[0] = -Vx+Vy+Vw;//右前
+// c->hopemotorout.OmniSpeedOut[1] = -Vx-Vy+Vw;//右后
+// c->hopemotorout.OmniSpeedOut[2] = Vx-Vy+Vw;//左后
+// c->hopemotorout.OmniSpeedOut[3] = Vx+Vy+Vw;//左前
// c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右前
// c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//右后
// c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw + c->pos088 .bmi088.gyro.z;//左后
@@ -96,6 +100,14 @@ void Chassis_speed_calculate(Chassis_t *c,fp32 Vx,fp32 Vy,fp32 Vw) //底盘逆
c->hopemotorout.OmniSpeedOut[1] = -Nor_Vx-Nor_Vy+Vw ;//右后
c->hopemotorout.OmniSpeedOut[2] = Nor_Vx-Nor_Vy+Vw ;//左后
c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw ;//左前
+
+// if(!Vw){
+// PIAN = jiuzheng(c->pos088 .imu_eulr .yaw );
+// }
+// c->hopemotorout.OmniSpeedOut[0] += PIAN ;//右前
+// c->hopemotorout.OmniSpeedOut[1] += PIAN ;//右后
+// c->hopemotorout.OmniSpeedOut[2] += PIAN ;//左后
+// c->hopemotorout.OmniSpeedOut[3] += PIAN ;//左前
}
@@ -173,32 +185,32 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
break;
- case AUTO_PICK: //自动视觉
+ case AUTO_NAVI_Pitch: //自动视觉
- c->move_vec.Vx =ctrl->Vx*6000 ;
- c->move_vec.Vy =ctrl->Vy *6000;
- c->move_vec .Vw = -ctrl->cmd_pick .posx;
+ c->move_vec.Vw =0;
+ c->move_vec.Vy =0;
+ c->move_vec.Vx =0 ;
- if(abs_value(ctrl ->cmd_pick .posx )>20)
- {
- c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0);
-
- }
- else if(abs_value(ctrl ->cmd_pick .posx )<0.1)
- {
- c->move_vec.Vw =0;
- }
- else
- c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_LOW),(c->move_vec .Vw) ,0);
+// if(abs_value(ctrl ->cmd_pick .posx )>20)
+// {
+// c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_HIGN),(c->move_vec .Vw) ,0);
+//
+// }
+// else if(abs_value(ctrl ->cmd_pick .posx )<0.1)
+// {
+// c->move_vec.Vw =0;
+// }
+// else
+// c->move_vec.Vw =PID_calc(&(c->pid.chassis_PICKWzPID_LOW),(c->move_vec .Vw) ,0);
- c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
+// c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
-
- c->vofa_send[0]=c->move_vec.Vw;
- c->vofa_send[1]=-ctrl->cmd_pick .posx;
-
+//
+// c->vofa_send[0]=c->move_vec.Vw;
+// c->vofa_send[1]=-ctrl->cmd_pick .posx;
+//
@@ -232,17 +244,54 @@ int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
}
-// c->vofa_send[0]=c->pos088.bmi088.gyro.x;
-// c->vofa_send[1]=c->pos088.bmi088.gyro.y;
-// c->vofa_send[2]=c->pos088.bmi088.gyro.z;
-// c->vofa_send[3]=c->pos088.bmi088.accl.x;
-// c->vofa_send[4]=c->pos088.bmi088.accl.y;
-// c->vofa_send[5]=c->pos088.bmi088.accl.z;
+
+// c->vofa_send[0]=c->pos088.bmi088.gyro.z;
+
+// c->vofa_send[1]=c->motorfeedback .rotor_rpm3508 [0];
+// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [1];
+// c->vofa_send[3]=c->motorfeedback .rotor_rpm3508 [2];
+// c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [3];
+//
+// c->vofa_send[0]=c->hopemotorout .OmniSpeedOut [0];
+// c->vofa_send[1]=c->hopemotorout .OmniSpeedOut [0];
+//// c->vofa_send[2]=c->hopemotorout .OmniSpeedOut [2];
+// c->vofa_send[3]=c->hopemotorout .OmniSpeedOut [3];
+
+//// c->vofa_send[4]=c->motorfeedback .rotor_rpm3508 [0];
+// c->vofa_send[0]=c->motorfeedback .rotor_rpm3508 [0];
+//// c->vofa_send[6]=c->motorfeedback .rotor_rpm3508 [2];
+// c->vofa_send[2]=c->motorfeedback .rotor_rpm3508 [3];
+//
+// c->vofa_send[5]=c->pos088 .imu_eulr .yaw ;
return CHASSIS_OK;
}
+ pid_type_def pid;
+ pid_param_t pid_param={
+ .p = 0.50f,
+ .i = 0.0f,
+ .d = 0.0f,
+ .i_limit = 50.0f,
+ .out_limit = 100.0f
+ };
+fp32 jiuzheng(fp32 yaw)
+{
+ static fp32 pian_yaw=0;
+ static fp32 shang_yaw=0;
+ static int is=0;
-
+
+ if(is==0)
+ {
+ PID_init (&pid,PID_POSITION ,&pid_param);
+ is=1;
+ }
+
+ pian_yaw+= (yaw - shang_yaw);
+ shang_yaw=yaw ;
+
+ return PID_calc(&pid,pian_yaw,0);
+}
diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h
index a6c3e11..919bb3e 100644
--- a/User/Module/Chassis.h
+++ b/User/Module/Chassis.h
@@ -190,6 +190,7 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
+fp32 jiuzheng(fp32 yaw);
diff --git a/User/Module/config.c b/User/Module/config.c
index 9c56d78..65a65cd 100644
--- a/User/Module/config.c
+++ b/User/Module/config.c
@@ -110,7 +110,7 @@ static const ConfigParam_t param ={
},
/*投球*/
.PitchConfig_Config = {
- .m2006_init_angle =-170,
+ .m2006_init_angle =-90,
.m2006_trigger_angle =0,
.go1_init_position = -50,
.go1_Receive_ball = -5, //偏下
diff --git a/User/Module/up.c b/User/Module/up.c
index 5bd5c88..df7dcd1 100644
--- a/User/Module/up.c
+++ b/User/Module/up.c
@@ -49,6 +49,8 @@ int8_t up_init(UP_t *u,const UP_Param_t *param,float target_freq)
}
u->go_cmd =u->param ->go_cmd ;
+ LowPassFilter2p_Init(&(u->filled[0]),target_freq,100.0f);
+
// 初始化上层状态机
if (!u->DribbleContext .is_initialized) { //检查是否为第一次运行状态机，运球
@@ -97,6 +99,7 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
u->DribbleContext .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port ,LIGHT_C_Pin);
u->DribbleContext .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port ,LIGHT_B_Pin);
+
return 0;
}
@@ -257,7 +260,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
static int is_pitch=1;
-
+
switch (c->CMD_CtrlType )
{
case RCcontrol: //在手动模式下
@@ -283,29 +286,31 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
}
u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_translate_angle;
u->DribbleContext .DribbleState = DRIBBLE_PREPARE; //重置最初状态
+
+
break;
case Pitch :
-// if (u->PitchContext .PitchState ==PITCH_PREPARE)
-// {
-// u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
-// }
-
-// Pitch_Process(u,out);
if (u->PitchContext .PitchState ==PITCH_PREPARE)
{
- u->CoContext .CoState =CoTRANSLATE_OUT;
- }
- Co_Process(u,out);
+ u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
+ }
+
+ Pitch_Process(u,out);
+// if (u->PitchContext .PitchState ==PITCH_PREPARE)
+// {
+// u->CoContext .CoState =CoTRANSLATE_OUT;
+// }
+// Co_Process(u,out);
break ;
case UP_Adjust: //测试用
- u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
+ u->PitchContext.PitchConfig.go1_Receive_ball += c->Vx ;
u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
- u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
+ u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_Receive_ball;
u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
break ;
case Dribble:
@@ -323,7 +328,32 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
default:
break;
}
+ case AUTO:
+ switch(c-> CMD_mode)
+ {
+ case AUTO_NAVI_Pitch:
+ u->PitchContext .PitchConfig .go1_Receive_ball=LowPassFilter2p_Apply(&u->filled[0],c->pos);
+ if (u->PitchContext .PitchState ==PITCH_PREPARE)
+ {
+ u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
+ }
+ /*根据距离实时计算所需pos*/
+
+// u->PitchContext .PitchConfig .go1_release_threshold=c->pos;
+ Pitch_Process(u,out);
+ break ;
+
+ case AUTO_NAVI:
+ u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
+
+ u->PitchContext .PitchState = PITCH_PREPARE;
+
+
+
+ break ;
+
+ }
return 0;
@@ -372,14 +402,14 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
u->PitchContext .PitchState=PITCH_LAUNCHING;
}
}
-
-
-
break ;
- case PITCH_LAUNCHING: //等待发射
+ case PITCH_LAUNCHING: //等待发射
+ u->PitchContext .PitchState=PITCH_COMPLETE;
break ;
case PITCH_COMPLETE: //发射完成
+
+
break ;
diff --git a/User/bsp/bsp_usart.c b/User/bsp/bsp_usart.c
index 4843fac..e40ddb8 100644
--- a/User/bsp/bsp_usart.c
+++ b/User/bsp/bsp_usart.c
@@ -9,9 +9,9 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
if (huart->Instance == USART3)
return BSP_UART_REMOTE;
else if (huart->Instance == USART1)
- return BSP_UART_NUC;
- else if (huart->Instance == USART6)
return BSP_UART_RS485;
+ else if (huart->Instance == USART6)
+ return BSP_UART_NUC;
/*
else if (huart->Instance == USARTX)
return BSP_UART_XXX;
@@ -95,9 +95,9 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
case BSP_UART_REMOTE:
return &huart3;
case BSP_UART_RS485:
- return &huart6;
- case BSP_UART_NUC:
return &huart1;
+ case BSP_UART_NUC:
+ return &huart6;
/*
case BSP_UART_XXX:
return &huartX;
diff --git a/User/bsp/bsp_usart.h b/User/bsp/bsp_usart.h
index 4974127..bff764a 100644
--- a/User/bsp/bsp_usart.h
+++ b/User/bsp/bsp_usart.h
@@ -13,6 +13,7 @@ typedef enum {
BSP_UART_REMOTE,
BSP_UART_RS485,
BSP_UART_NUC,
+ BSP_UART_VOFA,
/* BSP_UART_XXX, */
BSP_UART_NUM,
BSP_UART_ERR,
diff --git a/User/bsp/pwm.c b/User/bsp/pwm.c
index d2bd484..eba9c86 100644
--- a/User/bsp/pwm.c
+++ b/User/bsp/pwm.c
@@ -19,11 +19,11 @@ int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
return BSP_OK;
}
+ uint16_t pulse;
int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle) {
if (duty_cycle < 0.0f) duty_cycle = 0.f;
- uint16_t pulse;
/* 通过PWM通道对应定时器重载值和给定占空比，计算PWM周期值 */
switch (ch) {
case BSP_PWM_IMU_HEAT:
@@ -38,7 +38,9 @@ int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle) {
break;
}
} else {
-// BSP_PWM_Stop(ch);
+ pulse =0;
+ __HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, pulse);
+ //BSP_PWM_Stop(ch);
}
return BSP_OK;
}
diff --git a/User/device/cmd.c b/User/device/cmd.c
index 899aad9..eec5297 100644
--- a/User/device/cmd.c
+++ b/User/device/cmd.c
@@ -102,6 +102,8 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
cmd->cmd_MID360.posx = n->navi.vx;
cmd->cmd_MID360.posy = n->navi.vy;
cmd->cmd_MID360.posw = n->navi.wz;
+
+ cmd->pos =n->navi .pos ;
break;
case PICK :
@@ -155,7 +157,7 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式
- if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_PICK; //左中，右下，视觉
+ if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =AUTO_NAVI_Pitch; //左中，右下，视觉
}
else if(rc->dr16.sw_l==CMD_SW_DOWN)
@@ -189,8 +191,9 @@ int8_t CMD_LD(CMD_t *cmd,const CMD_RC_t *rc){
if(cmd ->CMD_CtrlType ==AUTO){
/*自动下的*/
- if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =AUTO_NAVI;
- else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_PICK;
+ if(rc->LD.key_E ==CMD_SW_DOWN ) cmd ->CMD_mode =Normal;
+ else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_NAVI_Pitch;
+ else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_NAVI;
else cmd ->CMD_mode =Normal ;
}
diff --git a/User/device/cmd.h b/User/device/cmd.h
index c5082ab..52d5703 100644
--- a/User/device/cmd.h
+++ b/User/device/cmd.h
@@ -9,6 +9,7 @@
#define MID (0x09)
#define PICK (0x06)
+#define NUC (0x08)
typedef enum{
RCcontrol,//遥控器控制，左按键上，控制上层
@@ -23,7 +24,7 @@ typedef enum{
Normal, //无模式
AUTO_NAVI,
- AUTO_PICK,
+ AUTO_NAVI_Pitch,
UP_Adjust,//上层调整
@@ -41,6 +42,11 @@ typedef struct {
fp32 vx;
fp32 vy;
fp32 wz;
+
+ fp32 pos;
+ fp32 angle;
+ char flag;
+
}navi;
struct
{
@@ -52,6 +58,8 @@ typedef struct {
{
fp32 angle;
}sick_cali;
+
+
} CMD_NUC_t;
/* 拨杆位置 */
typedef enum {
@@ -122,6 +130,8 @@ typedef struct {
fp32 key_ctrl_l;
fp32 key_ctrl_r;
+ fp32 pos;//雷达反馈go位置
+
fp32 Vx;
fp32 Vy;
fp32 Vw;
diff --git a/User/device/nuc.c b/User/device/nuc.c
index 05bc64a..57fce4c 100644
--- a/User/device/nuc.c
+++ b/User/device/nuc.c
@@ -8,6 +8,7 @@ static osThreadId_t thread_alert;
uint8_t nucbuf[31];
+
static void NUC_IdleCallback(void) {
osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
detect_hook(NUC_TOE);
@@ -26,6 +27,35 @@ int8_t NUC_StartReceiving() {
sizeof(nucbuf)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
+}
+ char SendBuffer[19];
+
+int8_t NUC_StartSending(fp32 *data) {
+
+ union
+ {
+ float x[4];
+ char data[16];
+ }instance;
+
+
+ for (int i = 0; i < 4; i++) {
+ instance.x[i] = data[i];
+ }
+
+ SendBuffer[0] = 0xFC; //发送ID
+ SendBuffer[1] = 0x01; //控制帧
+ for(int i = 2; i < 18; i++)
+ {
+ SendBuffer[i] = instance.data[i-2];
+ }
+ SendBuffer[18] = 0xFD; //结束符
+
+ if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC),
+ (uint8_t *)SendBuffer,
+ sizeof(SendBuffer)) == HAL_OK)
+ return DEVICE_OK;
+ return DEVICE_ERR;
}
int8_t NUC_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_NUC));
@@ -42,8 +72,8 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
if(n ==NULL) return DEVICE_ERR_NULL;
union
{
- float x[3];
- char data[12];
+ float x[5];
+ char data[20];
}instance;
if(nucbuf[0]!=HEAD) goto error; //发送ID不是底盘
else{
@@ -51,7 +81,34 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
n->ctrl_status =nucbuf[2];
switch (n->status_fromnuc)
{
- case MID://控制帧0x09
+// case MID://控制帧0x09
+// /* 协议格式
+// 0xFF HEAD
+// 0x09 控制帧
+// 0x01 相机帧
+// vx fp32
+// vy fp32
+// wz fp32
+// 0xFE TAIL
+// */
+//// if(nucbuf[15]!=TAIL)goto error;
+// instance.data[3] = nucbuf[6];
+// instance.data[2] = nucbuf[5];
+// instance.data[1] = nucbuf[4];
+// instance.data[0] = nucbuf[3];
+// n->navi.vx = instance.x[0]; //
+// instance.data[7] = nucbuf[10];
+// instance.data[6] = nucbuf[9];
+// instance.data[5] = nucbuf[8];
+// instance.data[4] = nucbuf[7];
+// n->navi.vy = instance.x[1];//
+// instance.data[11] = nucbuf[14];
+// instance.data[10] = nucbuf[13];
+// instance.data[9] = nucbuf[12];
+// instance.data[8] = nucbuf[11];
+// n->navi.wz = instance.x[2];//
+// break;
+ case MID ://控制帧0x08
/* 协议格式
0xFF HEAD
0x09 控制帧
@@ -61,7 +118,7 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
wz fp32
0xFE TAIL
*/
- if(nucbuf[15]!=TAIL)goto error;
+ if(nucbuf[24]!=TAIL)goto error;
instance.data[3] = nucbuf[6];
instance.data[2] = nucbuf[5];
instance.data[1] = nucbuf[4];
@@ -77,53 +134,21 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
instance.data[9] = nucbuf[12];
instance.data[8] = nucbuf[11];
n->navi.wz = instance.x[2];//
+ instance.data[15] = nucbuf[18];
+ instance.data[14] = nucbuf[17];
+ instance.data[13] = nucbuf[16];
+ instance.data[12] = nucbuf[15];
+ n->navi.pos = instance.x[3];//
+ instance.data[19] = nucbuf[22];
+ instance.data[18] = nucbuf[21];
+ instance.data[17] = nucbuf[20];
+ instance.data[16] = nucbuf[19];
+ n->navi.angle = instance.x[4];//
+
+ n->navi.flag = nucbuf[23];//
break;
- case PICK:
- /* 协议格式
- 0xFF HEAD
- 0x0X 控制帧
- 0x01 相机帧
- cmd 8位
- dis 相机深度值
- posx 相机yaw轴值
- posy 相机pitch轴值
- 0xFE TAIL
- */
-// if(nucbuf[15]!=TAIL)goto error;
- instance.data[3] = nucbuf[6];
- instance.data[2] = nucbuf[5];
- instance.data[1] = nucbuf[4];
- instance.data[0] = nucbuf[3];
- n->pick.posx = instance.x[0]; //距离球中心的角度值
- instance.data[7] = nucbuf[10];
- instance.data[6] = nucbuf[9];
- instance.data[5] = nucbuf[8];
- instance.data[4] = nucbuf[7];
- n->pick.posy = instance.x[1];// 相机yaw轴
- instance.data[11] = nucbuf[14];
- instance.data[10] = nucbuf[13];
- instance.data[9] = nucbuf[12];
- instance.data[8] = nucbuf[11];
- n->pick.posw= instance.x[2];// 暂未用到
- break;
-// case SICK_CAIL:
-// if(nucbuf[15]!=TAIL)goto error;
-// instance.data[3] = nucbuf[14];
-// instance.data[2] = nucbuf[13];
-// instance.data[1] = nucbuf[12];
-// instance.data[0] = nucbuf[11];
-// n->sick_cali.angle = instance.x[0]; //
-// instance.data[7] = nucbuf[10];
-// instance.data[6] = nucbuf[9];
-// instance.data[5] = nucbuf[8];
-// instance.data[4] = nucbuf[7];
-// n->sick_cali.isleft = instance.x[1];//
-// instance.data[11] = nucbuf[14];
-// instance.data[10] = nucbuf[13];
-// instance.data[9] = nucbuf[12];
-// instance.data[8] = nucbuf[11];
-// n->pick.posw= instance.x[2];// 暂未用到
-// break;
+
+
}
return DEVICE_OK;
}
diff --git a/User/device/nuc.h b/User/device/nuc.h
index 9b9cc4b..3f7c0d1 100644
--- a/User/device/nuc.h
+++ b/User/device/nuc.h
@@ -23,12 +23,13 @@ typedef struct {
NUC_UpPackageMCU_t to_nuc; //发送的数据协议
uint8_t status;//控制状态
- uint8_t pit_status;//pit相机朝向
+
} NUC_t;
int8_t NUC_Init(NUC_t *nuc);
int8_t NUC_StartReceiving(void);
+int8_t NUC_StartSending(fp32 *data) ;
bool_t NUC_WaitDmaCplt(void);
int8_t NUC_RawParse(CMD_NUC_t *n);
int8_t NUC_HandleOffline(CMD_NUC_t *cmd);
diff --git a/User/device/rc.c b/User/device/rc.c
index a8cffe5..bebd452 100644
--- a/User/device/rc.c
+++ b/User/device/rc.c
@@ -99,10 +99,25 @@ int8_t LD_ParseRaw( LD_raw_t *raw, LD_Data_t *LD)
raw->ch[3] = 0.5*(raw->ch[3]); //w
- raw->map_ch[0]=map_fp32(raw->ch[0],-719,680,-1,1);
- raw->map_ch[1]=map_fp32(raw->ch[1],-653,746,-1,1);
- raw->map_ch[2]=map_fp32(raw->ch[2],95,1482,0,1);
- raw->map_ch[3]=map_fp32(raw->ch[3],-317,375,-1,1);
+if (raw->ch[0] < 0)
+ raw->map_ch[0] = map_fp32(raw->ch[0], -696, 2, -1, 0);
+else
+ raw->map_ch[0] = map_fp32(raw->ch[0], 2, 704, 0, 1);
+
+// ch[1]
+if (raw->ch[1] < 0)
+ raw->map_ch[1] = map_fp32(raw->ch[1], -638, 5, -1, 0);
+else
+ raw->map_ch[1] = map_fp32(raw->ch[1], 5, 762, 0, 1);
+
+ raw->map_ch[2] = map_fp32(raw->ch[2], 2, 1377, 0, 1);
+
+// ch[3]
+if (raw->ch[3] < 0)
+ raw->map_ch[3] = map_fp32(raw->ch[3], -344, 0, -1, 0);
+else
+ raw->map_ch[3] = map_fp32(raw->ch[3], 0, 348, 0, 1);
+
/*非线性映射*/
raw->map_ch[0]=expo_map(raw->map_ch[0],0.7f);
diff --git a/User/device/vofa.c b/User/device/vofa.c
index 248e161..d1f8b7f 100644
--- a/User/device/vofa.c
+++ b/User/device/vofa.c
@@ -28,13 +28,14 @@ void vofa_tx_main(float *data)
/*在下面使用对应的串口发送函数*/
+// CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
+// osDelay(1);
+// CDC_Transmit_FS( tail, 4);
// HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
// osDelay(1);
-// HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_VOFA), tail, 4);
-// osDelay(1);
- CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
- osDelay(1);
-// CDC_Transmit_FS( tail, 4);
+// HAL_UART_Transmit_DMA(&huart1, tail, 4);
+// osDelay(1);
+
}
diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c
index 98d9c81..cbe6d3d 100644
--- a/User/task/nuc_task.c
+++ b/User/task/nuc_task.c
@@ -11,7 +11,7 @@ static CMD_NUC_t cmd_fromnuc;
#endif
-
+fp32 send[4]={11.0f,45.0,1.f,4.0f};
void Task_nuc(void *argument){
(void)argument; /**/
@@ -29,6 +29,7 @@ void Task_nuc(void *argument){
task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
#endif
NUC_StartReceiving();
+ NUC_StartSending(send);
if (NUC_WaitDmaCplt()){
NUC_RawParse(&cmd_fromnuc);
}
diff --git a/User/task/up_task.c b/User/task/up_task.c
index c784f32..6422d55 100644
--- a/User/task/up_task.c
+++ b/User/task/up_task.c
@@ -70,7 +70,6 @@ void Task_up(void *argument)
ALL_Motor_Control(&UP,&UP_CAN_out);
osDelay(1);
-
/*can上设备数据获取*/
osMessageQueueGet(task_runtime.msgq.can.feedback.UP_CAN_feedback, &can, NULL, 0);
@@ -92,8 +91,8 @@ void Task_up(void *argument)
osMessageQueuePut(task_runtime.msgq.can.output.shoot5065 ,&UP_CAN_out.chassis5065, 0, 0);
- vofa_send [0]=UP.vofa_send [0];
- vofa_send [1]=UP.vofa_send [1];
+// vofa_send [0]=UP.vofa_send [0];
+// vofa_send [1]=UP.vofa_send [1];
vofa_tx_main (vofa_send);
tick += delay_tick;