diff --git a/MDK-ARM/R2.uvoptx b/MDK-ARM/R2.uvoptx index d65d7cf..3e2206d 100644 --- a/MDK-ARM/R2.uvoptx +++ b/MDK-ARM/R2.uvoptx @@ -180,6 +180,31 @@ 1 chassis + + 5 + 1 + UP,0x0A + + + 6 + 1 + SendBuffer + + + 7 + 1 + NUC_send + + + 8 + 1 + nuc_raw,0x0A + + + 9 + 1 + a,0x0A + 0 diff --git a/MDK-ARM/R2/R2.axf b/MDK-ARM/R2/R2.axf index 5494a3f..73ecc37 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 576c55b..ff365bd 100644 --- a/User/Module/Chassis.c +++ b/User/Module/Chassis.c @@ -3,6 +3,8 @@ #include "user_math.h" #include "bsp_buzzer.h" #include "bsp_delay.h" +#include "nuc.h" + /* 机器人坐标系，向前x，右y，上yaw 不同于nuc (前x，左y，上yaw) @@ -101,8 +103,8 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z); /*初始数据*/ - - c->NUC_send.send[0] = 0; + + c->sick_cali.is_reach = 0; c->vofa_send[0]= KalmanFilter(&c->extKalman[0] , c->sick_cali.sick_dis[0]); @@ -113,6 +115,17 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { switch (c->chassis_ctrl.ctrl) { case ShootGame_Mode: //投球赛模式 + c->to_nuc.send=0; + switch(c->chassis_ctrl.mode){ + + case ShootRst: + c->move_vec.Vw = ctrl->Vw * 8000; + c->move_vec.Vx = ctrl->Vy * 8000; + c->move_vec.Vy = ctrl->Vx * 8000; + + break; + + default: if (ctrl->Vw != 0 || ctrl->Vy != 0 || ctrl->Vx != 0) { // 有遥控器输出，设置标志位1 osThreadFlagsSet(thread_alert, SHOOT_GAME_FLAG); @@ -128,66 +141,22 @@ int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) { c->move_vec.Vx = ctrl->Vy * 8000; c->move_vec.Vy = ctrl->Vx * 8000; }else { - c->move_vec.Vw = ctrl->cmd_MID360.posw * 1000; - c->move_vec.Vy = -ctrl->cmd_MID360.posy * 1000; + c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; + c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000; c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; abs_limit_fp(&c->move_vec.Vx, 5000.0f); abs_limit_fp(&c->move_vec.Vy, 5000.0f); abs_limit_fp(&c->move_vec.Vw, 5000.0f); } - break; - - case AUTO: - switch (c->chassis_ctrl.mode) { - case AUTO_MID360: - c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; - c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000; - c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; - abs_limit_fp(&c->move_vec.Vx, 5000.0f); - abs_limit_fp(&c->move_vec.Vy, 5000.0f); - abs_limit_fp(&c->move_vec.Vw, 5000.0f); - // c->NUC_send.send[0] = 1; - break; - case AUTO_MID360_Pitch: - c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; - c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000; - c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; - abs_limit_fp(&c->move_vec.Vx, 5000.0f); - abs_limit_fp(&c->move_vec.Vy, 5000.0f); - abs_limit_fp(&c->move_vec.Vw, 5000.0f); - break; - case AUTO_MID360_Adjust: - c->move_vec.Vw = ctrl->Vw * 6000; - c->move_vec.Vx = ctrl->Vy * 6000; - c->move_vec.Vy = ctrl->Vx * 6000; - - break; - - default: - - break; - } - case PASS_BALL: - switch (c->chassis_ctrl.mode) - { - case PB_UP: - c->move_vec.Vw = ctrl->Vw * 6000; - c->move_vec.Vx = ctrl->Vy * 6000; - c->move_vec.Vy = ctrl->Vx * 6000; - break ; - case PB_MID: - case PB_DOWN: - c->move_vec.Vw = -ctrl->cmd_MID360.posw * 1000; - c->move_vec.Vy = ctrl->cmd_MID360.posy * 1000; - c->move_vec.Vx = -ctrl->cmd_MID360.posx * 1000; - abs_limit_fp(&c->move_vec.Vx, 5000.0f); - abs_limit_fp(&c->move_vec.Vy, 5000.0f); - abs_limit_fp(&c->move_vec.Vw, 5000.0f); - break ; - - } - break; + + } + break; + + case REPOSITION: + c->to_nuc.send=1; + break; + case RELAXED: c->move_vec.Vw = 0; c->move_vec.Vx = 0; diff --git a/User/Module/Chassis.h b/User/Module/Chassis.h index 91d56cd..696c69a 100644 --- a/User/Module/Chassis.h +++ b/User/Module/Chassis.h @@ -32,6 +32,7 @@ #include "cmd.h" #include "filter.h" #include "kalman.h" +#include "nuc.h" #define CHASSIS_OK (0) #define CHASSIS_ERR (-1) @@ -103,12 +104,6 @@ typedef struct fp32 Vw; fp32 mul;//油门倍率 }ChassisMove_Vec; -typedef struct -{ - - fp32 send[4]; - -}NUC_send_t; /** * @brief @@ -194,7 +189,9 @@ typedef struct{ int is_reach; }sick_cali; - NUC_send_t NUC_send; + struct { + fp32 send; + }to_nuc; }Chassis_t; diff --git a/User/Module/up.c b/User/Module/up.c index 85abdbe..0f248e8 100644 --- a/User/Module/up.c +++ b/User/Module/up.c @@ -192,6 +192,7 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c) switch (c->CMD_CtrlType ) { case ShootGame_Mode: + UP_AUTO_Control(u,out,c); @@ -364,36 +365,45 @@ int8_t UP_AUTO_Control(UP_t *u,CAN_Output_t *out,CMD_t *c){ LaunchContext_t *LaunchContext = &u->LaunchContext; MID360Context_t *MID360Context=&u->MID360Context; MID360Cfg_t *MID360Cfg = &u->MID360Context.MID360Cfg; - MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.05,3.2,4.3,&u->MID360Context.Curve); + MID360Cfg->go_release_pos=CurveChange(LowPassFilter2p_Apply(&u->filled[0],c->pos)-0.25f,3.2,4.3,&u->MID360Context.Curve); + /* 函数，角度更新*/ if (u->MID360Context.Curve == CURVE_58) { target->Pitch_angle = 58; } else { target->Pitch_angle = 66; - } + /* 上下速度更改*/ } LaunchContext->LaunchCfg.go_up_speed=MID360Cfg->go_up_speed; LaunchContext->LaunchCfg.go_down_speed=MID360Cfg->go_down_speed; + /*检测重置*/ + if(MID360Context->IsLaunch==0){ - - switch(c-> CMD_mode) - { - - case AUTO_MID360_Pitch: - if(MID360Context->IsLaunch==0){ MID360Context->IsLaunch=1; LaunchContext->LaunchState = Launch_PREPARE; } + /*运行*/ Pitch_Launch_Sequence(u,LaunchContext,u->motorfeedback.go_data.Pos,MID360Cfg->go_release_pos,out); - break ; + + switch(c-> CMD_mode) + { + + case Shooting://发射 + - case AUTO_MID360: - target->Shoot_M2006_angle= LaunchContext->LaunchCfg.m2006_init; + + break; + case ShootRst://重置发射 MID360Context->IsLaunch=0; - break ; - default: + target->Shoot_M2006_angle=LaunchContext->LaunchCfg.m2006_init; + + break ; + + + + default: break; } - + return 1; } \ No newline at end of file diff --git a/User/bsp/protocol.h b/User/bsp/protocol.h index c0d7390..7ac82bd 100644 --- a/User/bsp/protocol.h +++ b/User/bsp/protocol.h @@ -14,6 +14,8 @@ extern "C" { #define HEAD (0xFF) #define TAIL (0xFE) + + #define IMU_ID (0x01) #define CMD_ID (0x02) @@ -83,7 +85,7 @@ typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) { - uint8_t status; + float status; } Protocol_UpDataCMD_t; /* 视觉 -> 电控 上层机构数据结构体*/ diff --git a/User/device/cmd.c b/User/device/cmd.c index 267f45d..26d2215 100644 --- a/User/device/cmd.c +++ b/User/device/cmd.c @@ -136,41 +136,74 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) { cmd->key_ctrl_l = rc->dr16.sw_l; cmd->key_ctrl_r = rc->dr16.sw_r ; - if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) { - cmd->CMD_CtrlType =RELAXED; - } - else if(rc->dr16.sw_l==CMD_SW_UP) - { - cmd ->CMD_CtrlType =ShootGame_Mode; - if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左上，右上，无模式 - - if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式 - - if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左上，右上，无模式 - - if(rc->dr16.res > 3000) cmd ->CMD_mode =ShootRst; - if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting; - } - - else if(rc->dr16.sw_l==CMD_SW_MID) - { - - cmd ->CMD_CtrlType =RCcontrol; - if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左中，右中，雷达 - - if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式 - - if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉 - - } - else if(rc->dr16.sw_l==CMD_SW_DOWN) - { - cmd ->CMD_CtrlType =RCcontrol; - if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式 - if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式 - if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式 - } - +// if ((rc->dr16.sw_l == CMD_SW_ERR) || (rc->dr16.sw_r == CMD_SW_ERR)) { +// cmd->CMD_CtrlType =RELAXED; +// } +// else if(rc->dr16.sw_l==CMD_SW_UP) +// { +// cmd ->CMD_CtrlType =ShootGame_Mode; +// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Shooting; //左上，右上，无模式 +// +// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左上，右中，无模式 +// +// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =ShootRst; //左上，右上，无模式 +// +//// if(rc->dr16.res > 3000) cmd ->CMD_mode =ShootRst; +//// if(rc->dr16.res == 660) cmd ->CMD_mode =Shooting; +// } +// +// else if(rc->dr16.sw_l==CMD_SW_MID) +// { +// +// cmd ->CMD_CtrlType =RCcontrol; +// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左中，右中，雷达 +// +// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode =Normal; //左中，右中，无模式 +// +// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左中，右下，视觉 +// +// } +// else if(rc->dr16.sw_l==CMD_SW_DOWN) +// { +// cmd ->CMD_CtrlType =RCcontrol; +// if(rc->dr16.sw_r ==CMD_SW_UP) cmd ->CMD_mode =Normal; //左下，右上，无模式 +// if(rc->dr16.sw_r ==CMD_SW_MID) cmd ->CMD_mode = Normal; //左下，右中，无模式 +// if(rc->dr16.sw_r ==CMD_SW_DOWN) cmd ->CMD_mode =Normal; //左下，右上，无模式 +// } + +if ((rc->dr16.sw_r == CMD_SW_ERR) || (rc->dr16.sw_l == CMD_SW_ERR)) { + cmd->CMD_CtrlType = RELAXED; +} +// 右拨杆控制 CtrlType，左拨杆控制 Mode +else if (rc->dr16.sw_r == CMD_SW_UP) +{ + cmd->CMD_CtrlType = ShootGame_Mode; + + if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右上，左上， + if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = ShootRst; // 右上，左中， + if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右上，左下，无模式 + + // 可选保留遥控旋钮控制 + // if (rc->dr16.res > 3000) cmd->CMD_mode = ShootRst; + // if (rc->dr16.res == 660) cmd->CMD_mode = Shooting; +} +else if (rc->dr16.sw_r == CMD_SW_MID) +{ + cmd->CMD_CtrlType = REPOSITION; + + if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右中，左上， + if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = Normal; // 右中，左中，无模式 + if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右中，左下， +} +else if (rc->dr16.sw_r == CMD_SW_DOWN) +{ + cmd->CMD_CtrlType = RCcontrol; + + if (rc->dr16.sw_l == CMD_SW_UP) cmd->CMD_mode = Normal; // 右下，左上，无模式 + if (rc->dr16.sw_l == CMD_SW_MID) cmd->CMD_mode = Normal; // 右下，左中，无模式 + if (rc->dr16.sw_l == CMD_SW_DOWN) cmd->CMD_mode = Normal; // 右下，左下，无模式 +} + return 0; } diff --git a/User/device/cmd.h b/User/device/cmd.h index b06897e..2534832 100644 --- a/User/device/cmd.h +++ b/User/device/cmd.h @@ -13,7 +13,10 @@ typedef enum{ RCcontrol,//遥控器控制，左按键上，控制上层 + REPOSITION, //重定位 + AUTO, + PASS_BALL,//传球模式 ShootGame_Mode,//投球赛模式 diff --git a/User/device/nuc.c b/User/device/nuc.c index 4d866f7..da602d9 100644 --- a/User/device/nuc.c +++ b/User/device/nuc.c @@ -170,28 +170,34 @@ int8_t NUC_PackIMU(NUC_t *nuc, const AHRS_Quaternion_t *quat, const AHRS_Accl_t int8_t NUC_PackCMD(NUC_t *nuc, const NUC_send_t *send){ nuc->to_nuc.cmd.head = HEAD; nuc->to_nuc.cmd.id = IMU_ID; - nuc->to_nuc.cmd.type = TYPE; nuc->to_nuc.cmd.end = TAIL; - // memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send)); + memcpy((void *)&(nuc->to_nuc.cmd.package.status), (const void *)send,sizeof(*send)); /*在这里添加你们的控制命令*/ return DEVICE_OK; } int8_t NUC_StartSend(NUC_t *nuc, bool cmd_update){ if (cmd_update) { +// if (HAL_UART_Transmit_DMA( +// BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd), +// sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK) +/*仅发送cmd数据*/ if (HAL_UART_Transmit_DMA( BSP_UART_GetHandle(BSP_UART_NUC), (uint8_t *)&(nuc->to_nuc.cmd), - sizeof(nuc->to_nuc.cmd) + sizeof(nuc->to_nuc.imu)) == HAL_OK) + sizeof(nuc->to_nuc.cmd) ) == HAL_OK) + + return DEVICE_OK; else return DEVICE_ERR; - } else { - if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC), - (uint8_t *)&(nuc->to_nuc.imu), - sizeof(nuc->to_nuc.imu)) == HAL_OK) - return DEVICE_OK; - else - return DEVICE_ERR; - } + } +// else { +// if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_NUC), +// (uint8_t *)&(nuc->to_nuc.imu), +// sizeof(nuc->to_nuc.imu)) == HAL_OK) +// return DEVICE_OK; +// else +// return DEVICE_ERR; +// } } diff --git a/User/device/nuc.h b/User/device/nuc.h index b80178b..70dda34 100644 --- a/User/device/nuc.h +++ b/User/device/nuc.h @@ -9,6 +9,11 @@ #include "Algorithm/ahrs.h" #include "Module/Chassis.h" +typedef struct +{ + fp32 send; + +}NUC_send_t; typedef struct { uint8_t id; @@ -25,7 +30,6 @@ typedef struct { typedef struct { uint8_t head; - uint8_t type; // 0x01 控制帧 uint8_t id; Protocol_UpDataCMD_t package; uint8_t end; diff --git a/User/task/chassis_task.c b/User/task/chassis_task.c index 5a09e1f..8997ce1 100644 --- a/User/task/chassis_task.c +++ b/User/task/chassis_task.c @@ -88,7 +88,7 @@ void Task_Chassis(void *argument) osMessageQueueReset(task_runtime.msgq.can.output.chassis3508);//清空队列 osMessageQueuePut(task_runtime.msgq.can.output.chassis3508, &out.motor_CHASSIS3508, 0, 0); //发送数据 osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc_send);//清空队列 - osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.NUC_send, 0, 0); //发送数据 + osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc_send, &chassis.to_nuc, 0, 0); //发送数据 vofa_send[0] = chassis.vofa_send[0]; vofa_send[1] = chassis.vofa_send[1]; vofa_send[2] = chassis.vofa_send[2]; diff --git a/User/task/nuc_task.c b/User/task/nuc_task.c index c66c9ce..3167ddb 100644 --- a/User/task/nuc_task.c +++ b/User/task/nuc_task.c @@ -57,19 +57,19 @@ void Task_nuc(void *argument){ osMessageQueueGet(task_runtime.msgq.nuc.accl, &(accl), NULL, 0); osMessageQueueGet(task_runtime.msgq.nuc.gyro, &(gyro), NULL, 0); - // osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0); + //osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0); bool cmd_update = (osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &(NUC_send), NULL, 0) == osOK); - NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro); +// NUC_PackIMU(&nuc_raw, &quat, &accl, &gyro); if (cmd_update) { NUC_PackCMD(&nuc_raw,&NUC_send); + + NUC_StartSend(&nuc_raw,cmd_update); + } - - // NUC_StartSend(&nuc_raw, cmd_update); - tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/ osDelayUntil(tick); }