sick定位

Core/Src/main.c

@@ -80,6 +80,7 @@ int main(void)
   /* MCU Configuration--------------------------------------------------------*/
 
   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+
   HAL_Init();
 
   /* USER CODE BEGIN Init */

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;

MDK-ARM/.vscode/c_cpp_properties.json

@@ -415,6 +415,114 @@
                 "__vfp_status(x,y)=0"
             ],
             "intelliSenseMode": "${default}"
+        },
+        {
+            "name": "R2",
+            "includePath": [
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Inc",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Include",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\User",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\bsp",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\device",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\task",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Algorithm",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\User\\Module",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\MDK-ARM",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\App",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\USB_DEVICE\\Target",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Inc",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Inc",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\CMSIS\\Lib\\ARM",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Core\\Src",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Drivers\\STM32F4xx_HAL_Driver\\Src",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Core\\Src",
+                "d:\\Desktop\\r2\\R2_NEW\\R2\\Middlewares\\ST\\STM32_USB_Device_Library\\Class\\CDC\\Src"
+            ],
+            "defines": [
+                "USE_HAL_DRIVER",
+                "STM32F407xx",
+                "__CC_ARM",
+                "__arm__",
+                "__align(x)=",
+                "__ALIGNOF__(x)=",
+                "__alignof__(x)=",
+                "__asm(x)=",
+                "__forceinline=",
+                "__restrict=",
+                "__global_reg(n)=",
+                "__inline=",
+                "__int64=long long",
+                "__INTADDR__(expr)=0",
+                "__irq=",
+                "__packed=",
+                "__pure=",
+                "__smc(n)=",
+                "__svc(n)=",
+                "__svc_indirect(n)=",
+                "__svc_indirect_r7(n)=",
+                "__value_in_regs=",
+                "__weak=",
+                "__writeonly=",
+                "__declspec(x)=",
+                "__attribute__(x)=",
+                "__nonnull__(x)=",
+                "__register=",
+                "__breakpoint(x)=",
+                "__cdp(x,y,z)=",
+                "__clrex()=",
+                "__clz(x)=0U",
+                "__current_pc()=0U",
+                "__current_sp()=0U",
+                "__disable_fiq()=",
+                "__disable_irq()=",
+                "__dmb(x)=",
+                "__dsb(x)=",
+                "__enable_fiq()=",
+                "__enable_irq()=",
+                "__fabs(x)=0.0",
+                "__fabsf(x)=0.0f",
+                "__force_loads()=",
+                "__force_stores()=",
+                "__isb(x)=",
+                "__ldrex(x)=0U",
+                "__ldrexd(x)=0U",
+                "__ldrt(x)=0U",
+                "__memory_changed()=",
+                "__nop()=",
+                "__pld(...)=",
+                "__pli(...)=",
+                "__qadd(x,y)=0",
+                "__qdbl(x)=0",
+                "__qsub(x,y)=0",
+                "__rbit(x)=0U",
+                "__rev(x)=0U",
+                "__return_address()=0U",
+                "__ror(x,y)=0U",
+                "__schedule_barrier()=",
+                "__semihost(x,y)=0",
+                "__sev()=",
+                "__sqrt(x)=0.0",
+                "__sqrtf(x)=0.0f",
+                "__ssat(x,y)=0",
+                "__strex(x,y)=0U",
+                "__strexd(x,y)=0",
+                "__strt(x,y)=",
+                "__swp(x,y)=0U",
+                "__usat(x,y)=0U",
+                "__wfe()=",
+                "__wfi()=",
+                "__yield()=",
+                "__vfp_status(x,y)=0"
+            ],
+            "intelliSenseMode": "${default}"
         }
     ],
     "version": 4

MDK-ARM/.vscode/keil-assistant.log

@@ -451,3 +451,15 @@
 
 [info] Log at : 2025/3/28|21:46:16|GMT+0800
 
+[info] Log at : 2025/6/6|21:51:18|GMT+0800
+
+[info] Log at : 2025/6/7|17:45:08|GMT+0800
+
+[info] Log at : 2025/6/7|21:03:23|GMT+0800
+
+[info] Log at : 2025/6/12|16:54:50|GMT+0800
+
+[info] Log at : 2025/6/13|16:30:54|GMT+0800
+
+[info] Log at : 2025/6/14|15:14:16|GMT+0800
+

MDK-ARM/.vscode/settings.json

@@ -96,6 +96,7 @@
     "limits": "c",
     "gimbal_task.h": "c",
     "gimgal.h": "c",
-    "navi.h": "c"
+    "navi.h": "c",
+    "up.h": "c"
   }
 }

MDK-ARM/R2.uvoptx

@@ -103,7 +103,7 @@
         <bEvRecOn>1</bEvRecOn>
         <bSchkAxf>0</bSchkAxf>
         <bTchkAxf>0</bTchkAxf>
-        <nTsel>6</nTsel>
+        <nTsel>3</nTsel>
         <sDll></sDll>
         <sDllPa></sDllPa>
         <sDlgDll></sDlgDll>
@@ -114,7 +114,7 @@
         <tDlgDll></tDlgDll>
         <tDlgPa></tDlgPa>
         <tIfile></tIfile>
-        <pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
+        <pMon>BIN\CMSIS_AGDI.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
         <SetRegEntry>
@@ -140,7 +140,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name>(105=-1,-1,-1,-1,0)</Name>
+          <Name></Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -158,92 +158,22 @@
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>rc_ctrl,0x0A</ItemText>
+          <ItemText>can,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>LD_raw,0x0A</ItemText>
+          <ItemText>chassis,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>can,0x0A</ItemText>
+          <ItemText>UP,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>UP,0x0A</ItemText>
+          <ItemText>NUC_send,0x0A</ItemText>
-        </Ww>
-        <Ww>
-          <count>4</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>cmd_rc,0x0A</ItemText>
-        </Ww>
-        <Ww>
-          <count>5</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>chassis</ItemText>
-        </Ww>
-        <Ww>
-          <count>6</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>nuc_raw,0x0A</ItemText>
-        </Ww>
-        <Ww>
-          <count>7</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>nucbuf</ItemText>
-        </Ww>
-        <Ww>
-          <count>8</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>cmd_fromnuc</ItemText>
-        </Ww>
-        <Ww>
-          <count>9</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>up_cmd,0x0A</ItemText>
-        </Ww>
-        <Ww>
-          <count>10</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>can_out</ItemText>
-        </Ww>
-        <Ww>
-          <count>11</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>Nor_Vx</ItemText>
-        </Ww>
-        <Ww>
-          <count>12</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>Nor_Vy</ItemText>
-        </Ww>
-        <Ww>
-          <count>13</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>a</ItemText>
-        </Ww>
-        <Ww>
-          <count>14</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>b</ItemText>
-        </Ww>
-        <Ww>
-          <count>15</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>count,0x0A</ItemText>
-        </Ww>
-        <Ww>
-          <count>16</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>count</ItemText>
-        </Ww>
-        <Ww>
-          <count>17</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>raw_rx1,0x0A</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>

User/Algorithm/user_math.c

@@ -216,7 +216,21 @@ void abs_limit_fp(fp32 *num, fp32 Limit)
         *num = -Limit;
     }
 }
+//改原始值，限制最大最小
+fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max)
+{
+    if (*num > Limit_max)
+    {
+			  *num=Limit_max;
+        return Limit_max;
+    }
+    else if (*num < Limit_min)
+    {
+			 *num=Limit_min;
+       return Limit_min;
 			
+    }
+}
 
 /* 移动向量 */
 MoveVector_t *mv;

User/Algorithm/user_math.h

@@ -164,5 +164,6 @@ int read_flag_state(uint8_t flag) ;
 void normalize_vector(double x, double y, double *out_x, double *out_y) ;
 bool is_reached(float current, float target, float mistake) ;
 bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) ;
+fp32 abs_limit_min_max_fp(fp32 *num, fp32 Limit_min,fp32 Limit_max);
 
 #endif

User/Module/Chassis.c

@@ -4,243 +4,213 @@
 #include "bsp_buzzer.h"
 #include "bsp_delay.h"
 
-
-
-
-
-
-/*机器人坐标系，向前x，右y，上yaw
-   不同于nuc (前x，左y，上yaw)                          */
 /*
-               x
+机器人坐标系，向前x，右y，上yaw
-               |
+不同于nuc (前x，左y，上yaw)
-                 --y
+            x
+            |
+              --y
+*/
+
+static int8_t Chassis_SetCtrl(Chassis_t *c, CMD_t *ctrl) {
+    if (c == NULL) return CHASSIS_ERR_NULL;
+    if (ctrl->CMD_CtrlType == c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode)
+        return CHASSIS_OK;
+    c->chassis_ctrl.ctrl = ctrl->CMD_CtrlType;
+    c->chassis_ctrl.mode = ctrl->CMD_mode;
+    return CHASSIS_OK;
+}
+
+int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
+    if (c == NULL) return CHASSIS_ERR_NULL;
+    if (can == NULL) return CHASSIS_ERR_NULL;
+    for (uint8_t i = 0; i < 4; i++) {
+        c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
+        c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
+    }
+    for (uint8_t i = 0; i < 4; i++) {
+		
+		c->sick_cali.sick_dis[i]=can->sickfed.raw_dis[i] / 10; //没使用原值
+    }
+			
+    return CHASSIS_OK;
+}
+
+int8_t Chassis_init(Chassis_t *c, const Chassis_Param_t *param, float target_freq) {
+    if (c == NULL) return CHASSIS_ERR_NULL;
+    c->param = param;
+
+    for (int i = 0; i < 4; i++) {
+        PID_init(&(c->pid.chassis_3508VecPID[i]), PID_POSITION_D, &(c->param->M3508_param));
+    }
+    PID_init(&(c->pid.chassis_PICKWzPID_HIGN), PID_POSITION, &(c->param->chassis_PICKWzPID_HIGN_param));
+    PID_init(&(c->pid.chassis_PICKWzPID_LOW), PID_POSITION, &(c->param->chassis_PICKWzPID_LOW_param));
+    PID_init(&(c->pid.SickCaliWzPID), PID_POSITION, &(c->param->Sick_CaliWparam));
+    PID_init(&(c->pid.SickCaliVxPID), PID_POSITION, &(c->param->Sick_CaliXparam));
+    PID_init(&(c->pid.SickCaliVyPID), PID_POSITION, &(c->param->Sick_CaliYparam));
+		
+
+    LowPassFilter2p_Init(&(c->filled[0]), target_freq, 80.0f); // 角加速度滤波
+    LowPassFilter2p_Init(&(c->filled[1]), target_freq, 80.0f); // w滤波
+    LowPassFilter2p_Init(&(c->filled[2]), target_freq, 80.0f); // y滤波
+    LowPassFilter2p_Init(&(c->filled[3]), target_freq, 80.0f); // x滤波
+		
+		c->sick_cali .sickparam=c->param ->sickparam ;
+
+    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] = -Nor_Vx + Nor_Vy + 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;  // 左前
+}
+int8_t Chassis_Control(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) {
+    if (c == NULL) return CHASSIS_ERR_NULL;
+    if (ctrl == NULL) return CHASSIS_ERR_NULL;
+
+    Chassis_SetCtrl(c, ctrl);
+
+    // IMU加滤波修正
+    c->pos088.bmi088.filtered_gyro.z = LowPassFilter2p_Apply(&(c->filled[0]), c->pos088.bmi088.gyro.z);
+
+    /*初始数据*/
+    c->move_vec.Vw = 0;
+    c->move_vec.Vx = 0;
+    c->move_vec.Vy = 0;
+    c->NUC_send.send[0] = 0;
+    c->sick_cali.is_reach = 0;
+	
+    switch (c->chassis_ctrl.ctrl) {
+        case RCcontrol:
+            switch (c->chassis_ctrl.mode) {
+                case Normal:
+                    c->move_vec.Vw = ctrl->Vw * 6000;
+                    c->move_vec.Vx = ctrl->Vy * 6000;
+                    c->move_vec.Vy = ctrl->Vx * 6000;
+                    break;
+                case Pitch:
+                   
+                    break;
+                case UP_Adjust:
+                    c->move_vec.Vw = ctrl->Vw * 6000;
+                    
+                    break;
+                case Chassis_Adjust:
+									
+                    sick_calibration(c, ctrl, out);
+                    c->NUC_send.send[0] = (c->sick_cali.is_reach == 1) ? 1 : 0;
+                    break;
+                default:
+                    
+                    break;
+            }
+            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);
+                    break;
+                case AUTO_MID360_Pitch:
+                    
+                    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;
+                    c->NUC_send.send[0] = 1;
+                    break;
+                default:
+                    
+                    break;
+            }
+            break;
+
+        default:
+            
+            break;
+    }
+
+    // 电机速度限幅
+    abs_limit_fp(&c->move_vec.Vx, 6000.0f);
+    abs_limit_fp(&c->move_vec.Vy, 6000.0f);
+    abs_limit_fp(&c->move_vec.Vw, 6000.0f);
+
+    Chassis_speed_calculate(c, c->move_vec.Vx, c->move_vec.Vy, c->move_vec.Vw);
+
+    for (uint8_t i = 0; i < 4; i++) {
+        c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),
+                                                 c->motorfeedback.rotor_rpm3508[i],
+                                                 c->hopemotorout.OmniSpeedOut[i]);
+        out->motor_CHASSIS3508.as_array[i] = c->final_out.final_3508out[i];
+    }
+
+    c->vofa_send[0] = (fp32)c->sick_cali.sick_dis[1] - c->sick_cali.sick_dis[2];
+    return CHASSIS_OK;
+		
+		
+		
+}
+
+
+
+/*
+sick0,下
+sick1,左2
+sick2,左1
 
 
 */
 
-static int8_t Chassis_SetCtrl(Chassis_t *c,CMD_t *ctrl){
+int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) 
-	if (c == NULL) return CHASSIS_ERR_NULL; /*主结构体不能为空 */
-  if (ctrl->CMD_CtrlType== c->chassis_ctrl.ctrl && ctrl->CMD_mode == c->chassis_ctrl.mode) return CHASSIS_OK; /*模式未改变直接返回*/
-   c->chassis_ctrl.ctrl =ctrl->CMD_CtrlType ;
-   c->chassis_ctrl.mode =ctrl->CMD_mode ;
-   
-   return CHASSIS_OK;
-}    //设置控制模式
-
-
-/*该函数用来更新can任务获得的电机反馈值*/
-
-int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can) {
-  if (c == NULL) return CHASSIS_ERR_NULL;
-  if (can == NULL) return CHASSIS_ERR_NULL;
-  for (uint8_t i = 0; i < 4; i++) 
- {
-	c->motorfeedback.rotor_rpm3508[i] = can->motor.chassis_motor3508.as_array[i].rotor_speed;
-	c->motorfeedback.rotor_current3508[i] = can->motor.chassis_motor3508.as_array[i].torque_current;
- }
- 
- 
-
-  return CHASSIS_OK;
-}
-
-
-int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq)
 {
-   if (c == NULL) return CHASSIS_ERR_NULL;
+    if (c == NULL) return CHASSIS_ERR_NULL;
+    if (ctrl == NULL) return CHASSIS_ERR_NULL;
 
-   c->param = param; 	/*初始化参数 */
     
-		 for(int i =0 ; i < 4 ; i++)
+    fp32 sick0 = c->sick_cali.sick_dis[0] ;
-	{
+    fp32 sick1 = c->sick_cali.sick_dis[1];
-		   PID_init(&(c->pid.chassis_3508VecPID[i]),PID_POSITION_D,&(c->param->M3508_param));  //带D项滤波
+    fp32 sick2 = c->sick_cali.sick_dis[2];
-	}
+    const   sickparam_t *param = &c->sick_cali.sickparam;
 
-	    PID_init(&(c->pid.chassis_PICKWzPID_HIGN),PID_POSITION,&(c->param->chassis_PICKWzPID_HIGN_param));
+    fp32 diff_yaw = -(fp32)(sick1 - sick2);
+    fp32 diff_y  = -(fp32)(sick1 - c->sick_cali.sickparam.y_set);
+    fp32 diff_x  = (fp32)(sick0 - c->sick_cali.sickparam.x_set);
 
-		  PID_init(&(c->pid.chassis_PICKWzPID_LOW),PID_POSITION,&(c->param->chassis_PICKWzPID_LOW_param));
 
+    c->move_vec.Vw = (fabsf(diff_yaw) > param->w_error) ? PID_calc(&(c->pid.SickCaliWzPID), diff_yaw, 0) : 0;
+    c->move_vec.Vx = (fabsf(diff_x)   > param->xy_error) ? PID_calc(&(c->pid.SickCaliVxPID), diff_x, 0) : 0;
+    c->move_vec.Vy = (fabsf(diff_y)   > param->xy_error) ? PID_calc(&(c->pid.SickCaliVyPID), diff_y, 0) : 0;
 		
-	    PID_init(&(c->pid.sick_CaliforYPID),PID_POSITION,&(c->param->Sick_CaliYparam));
+    static uint16_t reach_cnt = 0;
+    if (fabsf(diff_yaw) <= param->w_error &&
+        fabsf(diff_x)   <= param->xy_error &&
+        fabsf(diff_y)   <= param->xy_error) {
+        reach_cnt++;
+        if (reach_cnt >= 50) {
             
-	    PID_init(&(c->pid.sick_CaliforXPID),PID_POSITION,&(c->param->Sick_CaliXparam));
+            c->sick_cali.is_reach = 1;
             
-	    LowPassFilter2p_Init(&(c->filled[0]),target_freq,80.0f);  //给角加速度做滤波
+        }
-	
+    } else {
-	
+        reach_cnt = 0;
-	    LowPassFilter2p_Init(&(c->filled[1]),target_freq,80.0f); //给w 做滤波
+			  c->sick_cali.is_reach = 0;
-	
+    }	
-	    LowPassFilter2p_Init(&(c->filled[2]),target_freq,80.0f);  //给y做滤波
-	
-	    LowPassFilter2p_Init(&(c->filled[3]),target_freq,80.0f); //给x 做滤波
-			
-		//
-	
-	
-	 return CHASSIS_OK;
-}
-
-
-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] = -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;//左后
-//		c->hopemotorout.OmniSpeedOut[3] = Nor_Vx+Nor_Vy+Vw   + c->pos088 .bmi088.gyro.z;//左前
-		c->hopemotorout.OmniSpeedOut[0] = -Nor_Vx+Nor_Vy+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   ;//左前
-
-}
-
-
-int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out)
-{
-	if(c ==NULL)  return CHASSIS_ERR_NULL;
-	if(ctrl ==NULL)  return CHASSIS_ERR_NULL;
-	   
-
-	  Chassis_SetCtrl(c,ctrl);
-		 
-	 //此处对imu加滤波做修正
-	  c->pos088.bmi088.filtered_gyro.z =LowPassFilter2p_Apply(&(c->filled[0]),c->pos088.bmi088.gyro.z);
-	  
-	switch (c->chassis_ctrl .ctrl)
-	{
-	  case RCcontrol: //手动控制
-		
-/*     
-	在cmd里对数据进行处理 包括方向和油门	  
-	  6000为全向轮的倍率,遥控器坐标系和机器人坐标系不同*/
-		if(c->chassis_ctrl .mode == Pitch){
-	    c->move_vec.Vw = 0;
-		  c->move_vec.Vx = 0;
-		  c->move_vec.Vy = 0;
-		}
-		else if(c->chassis_ctrl .mode == UP_Adjust)
-		{
-	    c->move_vec.Vw = ctrl->Vw*6000;
-		  c->move_vec.Vx = 0;
-		  c->move_vec.Vy = 0;			
-		}
-		else {
-	    c->move_vec.Vw = ctrl->Vw*6000;
-		  c->move_vec.Vx = ctrl->Vy*6000;
-		  c->move_vec.Vy = ctrl->Vx*6000;
-		}
-			
-	
-
-	  break;
-
-  case AUTO : //在自动模式下
-		
-		switch(c->chassis_ctrl.mode ){	 
-	 
-
-	  case AUTO_NAVI:  //自动雷达
-//		  //这套是全向轮的方向,一定要注意这里的xy方向
-	    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 ;
-//	    c->move_vec.Vw =LowPassFilter2p_Apply(&(c->filled[1]),c->move_vec.Vw);
-//	    c->move_vec.Vy =LowPassFilter2p_Apply(&(c->filled[2]),c->move_vec.Vy);
-//	    c->move_vec.Vx =LowPassFilter2p_Apply(&(c->filled[3]),c->move_vec.Vx);
-//	
-//	    c->move_vec.Vw =PID_calc(&(c->pid.chassis_NaviWzPID),c->pos088.bmi088.gyro.z,c->move_vec.Vw);
-//	    c->move_vec.Vx =PID_calc(&(c->pid.chassis_NaviVxPID),c->pos088.bmi088.accl.y,c->move_vec.Vx);
-//	    c->move_vec.Vy =PID_calc(&(c->pid.chassis_NaviVyPID),c->pos088.bmi088.accl.x,c->move_vec.Vy);
-//	   //电机速度限幅
-
-  abs_limit_fp(&c->move_vec.Vx,2000.0f);
-
-  abs_limit_fp(&c->move_vec.Vy,2000.0f);
-
-  abs_limit_fp(&c->move_vec.Vw,2000.0f);
-
-	   if(ctrl->status[5] ==1)
-		 {
-			c->move_vec.Vw = c->move_vec.Vw * 0.8f;
-			c->move_vec.Vx = c->move_vec.Vx * 0.5f; 
-			c->move_vec.Vy = c->move_vec.Vy * 0.5f; 	 
-		 }
-
-	    break;
-		 
-		 	case AUTO_PICK:  //自动视觉
-	    
-	    c->move_vec.Vx =ctrl->Vx*6000 ;
-	    c->move_vec.Vy =ctrl->Vy *6000;	
-	    c->move_vec .Vw = -ctrl->cmd_pick  .posx;
-	
-	
-	
-      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->vofa_send[0]=c->move_vec.Vw;
-	      c->vofa_send[1]=-ctrl->cmd_pick .posx;
-		
-
-
-	
-     
-     break ;	
-		
-
-
-		}
-		break ;
-			 
-}
-	
-	
-   //电机速度限幅
-
-  abs_limit_fp(&c->move_vec.Vx,6000.0f);
-
-  abs_limit_fp(&c->move_vec.Vy,6000.0f);
-
-  abs_limit_fp(&c->move_vec.Vw,6000.0f);
-
-  Chassis_speed_calculate(c,c->move_vec.Vx,c->move_vec.Vy,c->move_vec.Vw);
-	
-	
-		for (uint8_t i = 0 ; i <4 ; i++)
-	{
-		 c->final_out.final_3508out[i] = PID_calc(&(c->pid.chassis_3508VecPID[i]),c->motorfeedback.rotor_rpm3508[i],c->hopemotorout.OmniSpeedOut[i]);
-	 
-		 out->motor_CHASSIS3508 .as_array[i] = c->final_out.final_3508out[i];
-	}
-	
-
-//	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;
-
-	return CHASSIS_OK;
 		
+    return CHASSIS_OK;
 }
 
 

User/Module/Chassis.h

@@ -58,7 +58,12 @@ typedef struct {
  AHRS_Eulr_t imu_eulr;//解算后存放欧拉角（弧度制）
 }ChassisImu_t;
 
-
+typedef	struct {
+	int32_t w_error;//角度纠正时的允许误差量
+	int32_t xy_error;//xy允许误差量		
+	int32_t x_set;
+	int32_t y_set;
+}sickparam_t;
 
 /* 该结构体用于存取固定的一些参数 在config.c中更改后不再变化 */
 typedef struct
@@ -74,10 +79,13 @@ typedef struct
   pid_param_t NaviVx_param;
   pid_param_t NaviVy_param;
   pid_param_t NaviVw_param;	
+	
+	pid_param_t Sick_CaliWparam;
 	pid_param_t Sick_CaliYparam;
 	pid_param_t Sick_CaliXparam;
 	
 	
+	sickparam_t sickparam;
 	
 }Chassis_Param_t;
 
@@ -90,7 +98,11 @@ typedef struct
    fp32 Vw;
 	fp32 mul;//油门倍率
 }ChassisMove_Vec;
+typedef struct 
+{
+   fp32 send[4];
 	
+}NUC_send_t;
 
 /**
  *  @brief  
@@ -144,12 +156,10 @@ typedef struct{
 		
 		
 	
-	pid_type_def	chassis_NaviWzPID;
+	  pid_type_def SickCaliWzPID;
-		pid_type_def chassis_NaviVxPID;
+		pid_type_def SickCaliVxPID;
-		pid_type_def chassis_NaviVyPID;
+		pid_type_def SickCaliVyPID;
 		
-	pid_type_def sick_CaliforYPID;
-	pid_type_def sick_CaliforXPID;
 		
 	}pid;
 
@@ -157,44 +167,28 @@ typedef struct{
 	
 	 LowPassFilter2p_t filled[6]; /* 输出滤波器滤波器数组 */
 	 
-	
+	struct {
 	int32_t sick_dis[4];   //获取到的sick激光值
+  sickparam_t sickparam;
+	int is_reach;
+	}sick_cali;
 	
+	NUC_send_t NUC_send;
 
 	
 }Chassis_t;
 
-/**
+int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
- * @brief 
- *
- * @param c 
- * @param param 
- * @param mech_zero 
- * @param wheelPolar 
- * @return 
- */
- int8_t Chassis_init(Chassis_t *c,const Chassis_Param_t *param,float target_freq);
 
 
-/**
- * \brief 
-
- */
 int8_t Chassis_UpdateFeedback(Chassis_t *c, const CAN_t *can);
 
 
-/**
- * \brief 
-
- */
 int8_t Chassis_Control(Chassis_t *c,CMD_t *ctrl,CAN_Output_t *out);
 
 
+int8_t sick_calibration(Chassis_t *c, CMD_t *ctrl, CAN_Output_t *out) ;
 
 
 
-
-/// @brief 
-/// @param c 
-void vesc_current_detection(Chassis_t *c);
 #endif

User/Module/config.c

@@ -95,13 +95,13 @@ static const ConfigParam_t param ={
 
 /*上层其他参数*/
     /*运球*/
-    .DribbleConfig_Config = {
+    .DribbleCfg = {
 		.dribble_flag=0,
-    .m3508_init_angle = 50,
+    .m3508_init_ang = -5,
-    .m3508_translate_angle = -930,
+    .m3508_translate_angle = 1000,
-    .m3508_dribble_Reverse_speed=-3500,
+    .rev_spd=-2000,
-    . m3508_dribble_speed= 4000, // 转动速度
+    .spd= 4200, // 转动速度
-		.m3508_dribble_init_speed=0,
+		.init_spd=0,
 
 		.light_3508_flag=0,//3508平移处的光电，0初始，1到位置
 	  .light_ball_flag=0,//检测球的flag			
@@ -109,15 +109,16 @@ static const ConfigParam_t param ={
 
 },
     /*投球*/
-    .PitchConfig_Config = {
+    .PitchCfg = {
-    .m2006_init_angle =-170,    
+    .m2006_init =-100,    //释放的角度
-	  .m2006_trigger_angle =0,
+	  .m2006_trig =0,       //可以拉住发射的角度
-    .go1_init_position = -50,   
+		.go_init = -50,       //仅用在go上电，初始位置
-	  .go1_release_threshold =-210,
+	  .go_pull_pos =-210,
-		.m2006_Screw_init=0,	
+		.Pitch_angle  =58,
-		.Pitch_angle  =56,
-},
 			
+		.pull_speed  =5,
+
+},
 
 	},
 	 .chassis = {/**/
@@ -131,20 +132,48 @@ static const ConfigParam_t param ={
 		 .out_limit =6000.0f,
 	 },	
 		 /*视觉*/
-		 .chassis_PICKWzPID_HIGN_param ={ //高响应
+		 .chassis_PICKWzPID_HIGN_param ={ 
 			.p = 1.0f,
 			.i = 0.03f,
 			.d = 0.03f,
 			.i_limit = 100.0f,
 			.out_limit =2000.0f,	 
 		 },
-		 	.chassis_PICKWzPID_LOW_param ={ //高精度
+		 	.chassis_PICKWzPID_LOW_param ={ 
 			.p = 0.5f,     //1.0 0.5
 			.i = 0.5f,    //0.01 0.04
 			.d = 0.0f,    //0.02 0.02
 			.i_limit = 50.0f,
 			.out_limit =1000.0f,	 
 		 },
+			.Sick_CaliWparam ={ 
+			.p = 4.5f,     
+			.i = 0.005f,   
+			.d = 0.0f,    
+			.i_limit = 500.0f,
+			.out_limit =1000.0f,	 
+		 },
+			.Sick_CaliYparam ={ 
+			.p = 2.9f,     
+			.i = 0.0051f,   
+			.d = 0.0f,    
+			.i_limit = 500.0f,
+			.out_limit =3000.0f,	 
+		 },		
+			.Sick_CaliXparam ={ 
+			.p = 2.9f,     
+			.i = 0.0065f,   
+			.d = 0.0f,    
+			.i_limit = 500.0f,
+			.out_limit =3000.0f,	 
+		 },	
+			.sickparam={
+				.w_error=5,
+				.xy_error=5,
+				.x_set=927,
+				.y_set=1255,
+				
+			},
 
 				 
    },

User/Module/up.c

@@ -49,19 +49,21 @@ 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) { //检查是否为第一次运行状态机，运球
+	if (!u->DribbleCont .is_init) { //检查是否为第一次运行状态机，运球
-		  u->DribbleContext .DribbleConfig = u->param ->DribbleConfig_Config ;//赋值
+		  u->DribbleCont .DribbleConfig = u->param ->DribbleCfg ;//赋值
-      u->DribbleContext .DribbleState  = DRIBBLE_PREPARE;  
+      u->DribbleCont .DribbleState  = DRIBBLE_PREPARE;  
-      u->DribbleContext .is_initialized = 1;
+      u->DribbleCont .is_init = 1;
 		
     }
 
-	if (!u->PitchContext  .is_initialized) { 
+	if (!u->Pitch_Cfg  .is_init) { 
-		  u->PitchContext .PitchConfig  = u->param ->PitchConfig_Config  ;//赋值
+		  u->Pitch_Cfg .PitchConfig  = u->param ->PitchCfg  ;//赋值
-      u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
+      u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
-      u->PitchContext .is_initialized = 1;
+      u->Pitch_Cfg .is_init = 1;
     }
 
      BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART6_RxCompleteCallback);//注册串口回调函数，bsp层
@@ -93,6 +95,10 @@ int8_t UP_UpdateFeedback(UP_t *u, const CAN_t *can, CMD_t *c) {
 	}	
 
 	u->cmd =c;
+	/*光电状态更新*/
+	u->DribbleCont .DribbleConfig .light_ball_flag =HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port  ,LIGHT_C_Pin);
+	u->DribbleCont .DribbleConfig .light_3508_flag =HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port  ,LIGHT_B_Pin);
+
 
 	return 0;
 }
@@ -162,17 +168,8 @@ int8_t GO_SendData(UP_t *u, float pos, float limit)
 	static int is_calibration=0;
 	static fp32 error=0; //误差量
 
-//	GO_MotorData_t *GO_calibration;//校准前，原始数据
 	   u->motorfeedback .go_data = get_GO_measure_point() ;
-//	if(is_calibration==0)
+
-//	{
-//		is_calibration=HAL_GPIO_ReadPin (GPIOE ,GPIO_PIN_9 );
-//		u->go_cmd.Pos = -50; //上电之后跑		
-//	  error= GO_calibration->Pos ;
-//	}	  
-//    u->motorfeedback .go_data  = GO_calibration;
-//	  u->motorfeedback .go_data ->Pos= error  ;
-//    u->go_cmd.Pos = pos;
 
     // 读取参数
     float tff = u->go_cmd.T;          // 前馈力矩
@@ -208,7 +205,7 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 	DJ_Speed_Control(u,0x201,&u->motorfeedback .DJmotor_feedback[0] ,&u->pid .Dribble_M3508_speed[0],u->motor_target .Dribble_M3508_speed[0]);
 	DJ_Speed_Control(u,0x202,&u->motorfeedback .DJmotor_feedback[1] ,&u->pid .Dribble_M3508_speed[1],u->motor_target .Dribble_M3508_speed[1]);
 	DJ_Speed_Control(u,0x203,&u->motorfeedback .DJmotor_feedback[2] ,&u->pid .Dribble_M3508_speed[2],u->motor_target .Dribble_M3508_speed[2]);
-   DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
+  DJ_Angle_Control(u,0x204,&u->motorfeedback .DJmotor_feedback[3] ,
 	                 &u->pid .Dribble_translate_M3508_angle ,
 	                 &u->pid .Dribble_translate_M3508_speed ,
 	                 u->motor_target .Dribble_translate_M3508_angle );
@@ -225,7 +222,9 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 	u->motorfeedback .Encoder.angle,u->motor_target .Shoot_Pitch_angle))
 	);
   
-	GO_SendData(u,u->motor_target .go_shoot,5 );
+	GO_SendData(u,
+	u->motor_target .go_shoot,
+	u->Pitch_Cfg .PitchConfig .pull_speed );//对应的拉动速度
 
 	
 	for(int i=0;i<4;i++){
@@ -243,16 +242,23 @@ int8_t ALL_Motor_Control(UP_t *u,CAN_Output_t *out)
 }
 
 
-int a=0;
-int b=0;
-
 
 int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 {
 	
 	if(u ==NULL)  return 0;
+		if(out ==NULL)  return 0;
+		if(c ==NULL)  return 0;
+	
+	/*指针简化*/
+	PitchCfg_t *pitch_cfg = &u->Pitch_Cfg.PitchConfig;	
+	DribbleCfg_t *DribbleCont = &u->DribbleCont.DribbleConfig;
+	
+	up_motor_target_t *target=&u->motor_target ;
 	static int is_pitch=1;
 	
+	/*config值限位*/
+		abs_limit_min_max_fp(&u->Pitch_Cfg.PitchConfig.go_release_pos ,-208.0f,2.0f);
 
 
     switch (c->CMD_CtrlType  )
@@ -265,53 +271,51 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 		case Normal :
          /*投篮*/
 		if(is_pitch){
-		u->motor_target .go_shoot =u->PitchContext .PitchConfig .go1_init_position ;
+		target->go_shoot =pitch_cfg->go_init ;
-		u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
+		target->Shoot_Pitch_angle=pitch_cfg->Pitch_angle;
 		is_pitch=0;	
 			} //让初始go位置只读一次，后面按调整模式更改的来,后面稳定了之后，可以跟随调整模式下一块删
-		u->motor_target .Shoot_M2006_angle =u->PitchContext .PitchConfig .m2006_init_angle ;
+		target->Shoot_M2006_angle =u->Pitch_Cfg .PitchConfig .m2006_init ;
-		u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig.m3508_translate_angle;//平行移动 
+		u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
-		u->PitchContext .PitchState   = PITCH_PREPARE;  //状态更新,开始夹球
 
 		    /*运球*/
 		
 		RELAY1_TOGGLE(0);//关闭气缸
 			for(int i=0;i<3;i++){
-	    	u->motor_target.Dribble_M3508_speed[i]=0;
+	    	target->Dribble_M3508_speed[i]=0;
 			}
-    u->motor_target .Dribble_translate_M3508_angle =u->DribbleContext .DribbleConfig .m3508_init_angle;
+        target->Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_translate_angle;
-	  u->DribbleContext .DribbleState  = DRIBBLE_PREPARE; //重置最初状态
+	      u->DribbleCont .DribbleState  = DRIBBLE_PREPARE; //重置最初状态
+			
+			
 
 				break;
 			
 		case Pitch :
-				if (u->PitchContext .PitchState  ==PITCH_PREPARE) 
+				if (u->Pitch_Cfg .PitchState  ==PITCH_PREPARE) 
 				{
-		      u->PitchContext .PitchState=PITCH_START;//置标志位用于启动投篮
+		      u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
 				}
 
-			 Pitch_Process(u,out,c);
+			 Pitch_Process(u,out);
+
 
 				 break ;
 		case UP_Adjust: //测试用
+			  pitch_cfg ->pull_speed=5;
+			  pitch_cfg ->go_release_pos += c->Vx ;
+	      pitch_cfg->Pitch_angle += c->Vy/100;
 	
-				u->PitchContext.PitchConfig.go1_init_position += c->Vx ;
+		    target->go_shoot=u->Pitch_Cfg.PitchConfig.go_release_pos;
-	      u->PitchContext.PitchConfig.Pitch_angle += c->Vy/100;
+		    target->Shoot_Pitch_angle=u->Pitch_Cfg.PitchConfig.Pitch_angle;
-	
-		    u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position;
-		    u->motor_target .Shoot_Pitch_angle=u->PitchContext.PitchConfig.Pitch_angle;
 			break ;
 		case Dribble:
 		{
 
-			if(u->DribbleContext.DribbleState== DRIBBLE_PREPARE){
+			if(u->DribbleCont.DribbleState== DRIBBLE_PREPARE){
-				u->DribbleContext .DribbleState=DRIBBLE_TRANSLATE;			
+				u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;			
 			}
 			//光电状态更新
-			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);
-      a=HAL_GPIO_ReadPin(LIGHT_C_GPIO_Port  ,LIGHT_C_Pin);
-			b=HAL_GPIO_ReadPin(LIGHT_B_GPIO_Port  ,LIGHT_B_Pin);
 			 Dribble_Process(u,out);
 		}break ;
 
@@ -320,7 +324,37 @@ int8_t UP_control(UP_t *u,CAN_Output_t *out,CMD_t *c)
 	default:
 		break;
 	}
+	case AUTO:
+		switch(c-> CMD_mode)
+		{
+			case 	AUTO_MID360_Pitch:
+		      pitch_cfg ->go_init=LowPassFilter2p_Apply(&u->filled[0],c->pos);
 
+			if (u->Pitch_Cfg .PitchState  ==PITCH_PREPARE) 
+				{
+		      u->Pitch_Cfg .PitchState=PITCH_START;//置标志位用于启动投篮
+				}
+				/*根据距离实时计算所需pos*/
+				
+        pitch_cfg ->go_release_pos=c->pos;
+//				if(u->CoContext .CoState == CoPITCH){
+//				u->CoContext.CoState=CoPITCH_MID360;
+//				}
+//				Co_Process(u,out);
+
+			 Pitch_Process(u,out);
+			break ;
+				
+			case AUTO_MID360:
+						target->Shoot_M2006_angle =pitch_cfg ->m2006_init ;
+
+						u->Pitch_Cfg .PitchState   = PITCH_PREPARE;  
+            u->DribbleCont .DribbleState = DRIBBLE_PREPARE;
+			
+			
+				break ;
+			
+		}
 			
 			
 return 0;
@@ -339,20 +373,24 @@ return 0;
 
 
 
-
+int8_t Pitch_Process(UP_t *u, CAN_Output_t *out)
-
-int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
 {
 	
-	switch(u->PitchContext .PitchState){
+	PitchCfg_t *cfg = &u->Pitch_Cfg.PitchConfig;
+	PitchState_t *state =&u->Pitch_Cfg .PitchState;
+  up_motor_target_t *target=&u->motor_target ;
+	
+	switch(*state){
 	
 		case PITCH_START:
-			u->motor_target .go_shoot = u->PitchContext .PitchConfig .go1_release_threshold;
+//			u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 
+      cfg->pull_speed=10;
+			target->go_shoot = cfg->go_pull_pos;
 		
 		if(u->motorfeedback .go_data ->Pos < -209){ //检测go位置到达最上面，这里的检测条件可以更改
-			u->motor_target .Shoot_M2006_angle = u->PitchContext .PitchConfig .m2006_trigger_angle ;//设置2006角度，0
+			target->Shoot_M2006_angle = cfg->m2006_trig ;//设置2006角度，0
-		
+		  u->Pitch_Cfg .PitchConfig .pull_speed=6;
-			u->PitchContext .PitchState=PITCH_PULL_TRIGGER;
+			*state=PITCH_PULL_TRIGGER;
 }//更改标志位
 
 			break ;
@@ -361,11 +399,21 @@ int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c)
 		
 			if( u->motorfeedback .DJmotor_feedback [4].total_angle>-1)  //当2006的总角度小于1，可以认为已经勾上,误差为1
 			{
-         u->motor_target .go_shoot=u->PitchContext.PitchConfig.go1_init_position; 
+         target->go_shoot=cfg->go_release_pos; 
+				if(is_reached (u->motorfeedback .go_data ->Pos ,target->go_shoot ,1.0f))
+			  {
+				*state=PITCH_LAUNCHING;			
+			  }
 			}
-		
 			break ;
 		
+		case PITCH_LAUNCHING: //等待发射	
+//			*state=PITCH_COMPLETE;	
+		break ;
+		case PITCH_COMPLETE:  //发射完成
+			
+		
+	  break ;
 		
 	
 	}
@@ -385,39 +433,45 @@ return 0;
 int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
 {
   static int common_speed_flag=0;//是否共速
+  static int common_speed_reverse_flag=0;//是否共速
 	
-	switch (u->DribbleContext.DribbleState) {
+	DribbleCfg_t *DribbleCfg = &u->DribbleCont.DribbleConfig;
+  up_motor_target_t *target=&u->motor_target ;
+  DribbleState_t *DribbleState=&u->DribbleCont.DribbleState;
+
+	switch (u->DribbleCont.DribbleState) {
 
 		case DRIBBLE_TRANSLATE:
-		if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleContext .DribbleConfig.m3508_translate_angle,1.0f))
+		target->Dribble_translate_M3508_angle =DribbleCfg->m3508_translate_angle;//平行移动 
+
+		if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle < -500)//速度为0认为卡主
 		{
-			u->DribbleContext .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
+			u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，转移状态
 		}
 			break;
 		case DRIBBLE_PROCESS_DOWN:
 			
-			u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+			target->Dribble_M3508_speed[0]=DribbleCfg->spd;
-			u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+			target->Dribble_M3508_speed[1]=DribbleCfg->spd;
-			u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_speed;
+			target->Dribble_M3508_speed[2]=DribbleCfg->spd;
 
 		  if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
-				u->motorfeedback .DJmotor_feedback [1].rpm,
+				                     u->motorfeedback .DJmotor_feedback [1].rpm,
-		   	u->motorfeedback .DJmotor_feedback [2].rpm,
+		   	                     u->motorfeedback .DJmotor_feedback [2].rpm,
-			  u->motor_target.Dribble_M3508_speed [0],
+			                       DribbleCfg->spd,
-			  u->motor_target.Dribble_M3508_speed [1],
+			                       DribbleCfg->spd,
-				u->motor_target.Dribble_M3508_speed[2],	
+				                     DribbleCfg->spd,	50.0f,50)
-			  50.0f,50)
 			) {
    		  	RELAY1_TOGGLE(1);  //速度达到后打开气缸
 				  common_speed_flag =1;
 			}
 			if(common_speed_flag){
-				if(u->DribbleContext .DribbleConfig .light_ball_flag == 1){//球下落检测，反转
+				if(u->DribbleCont .DribbleConfig .light_ball_flag == 0){//球下落检测，反转
-				u->motor_target.Dribble_M3508_speed[0]=u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
+				   target->Dribble_M3508_speed[0]=DribbleCfg->rev_spd;
-				u->motor_target.Dribble_M3508_speed[1]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
+				   target->Dribble_M3508_speed[1]=DribbleCfg->rev_spd;
-				u->motor_target.Dribble_M3508_speed[2]=-u->DribbleContext .DribbleConfig.m3508_dribble_Reverse_speed;
+				   target->Dribble_M3508_speed[2]=DribbleCfg->rev_spd;
 
-			  u->DribbleContext .DribbleState=DRIBBLE_PROCESS_UP;
+			  *DribbleState=DRIBBLE_PROCESS_UP;
 
 				}
 				
@@ -426,25 +480,31 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
 			
 			break;
 		case DRIBBLE_PROCESS_UP:
-			  if((u->motorfeedback .DJmotor_feedback [0].rpm<0)&&			
+				 common_speed_flag =0;
-			     (u->motorfeedback .DJmotor_feedback [1].rpm>0)&&
+
-			     (u->motorfeedback .DJmotor_feedback [2].rpm>0) 
+			  if((u->motorfeedback .DJmotor_feedback [0].rpm<-500)&&			
-			   ){
+			     (u->motorfeedback .DJmotor_feedback [1].rpm<-500)&&
+			     (u->motorfeedback .DJmotor_feedback [2].rpm<-500) 
+			   ){
+		        common_speed_reverse_flag=1;
 				  
-				   if(u->DribbleContext .DribbleConfig .light_ball_flag == 0){
-			     u->DribbleContext .DribbleState=DRIBBLE_DONE;
-				   RELAY1_TOGGLE(0);  //关闭气缸
-				  }
 				}
+				 if(common_speed_reverse_flag){
+					 if(DribbleCfg->light_ball_flag == 1){
+			     *DribbleState=DRIBBLE_DONE;
+				    RELAY1_TOGGLE(0);  //关闭气缸
+					 }
+				 }
 				 break ;
 		case DRIBBLE_DONE:
+			common_speed_reverse_flag=0;
 			for(int i=0;i<3;i++){
-			  u->motor_target.Dribble_M3508_speed[i]=u->DribbleContext .DribbleConfig.m3508_dribble_init_speed ;//三摩擦速度归0
+			  target->Dribble_M3508_speed[i]=DribbleCfg->init_spd ;//三摩擦速度归0
 			}
 		
         /*标志位清零*/
-		    u->DribbleContext.DribbleConfig.light_ball_flag=0;
+		    DribbleCfg->light_ball_flag=0;
-		    u->DribbleContext.DribbleConfig.light_3508_flag=0;
+		    DribbleCfg->light_3508_flag=0;
 		
 		
 			break;
@@ -454,5 +514,98 @@ int8_t Dribble_Process(UP_t *u, CAN_Output_t *out)
 	}
     return 0;
 }
+//    CoPREPARE,            // 准备阶段
+//	  CoTRANSLATE_OUT,//上方平移,去运球
+//	  CoDRIBBLE,       //运球阶段
+//	  CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
+//	  CoPITCH,       //发射
+//	  CoDONE,        
+//int8_t Trans_Process	(UP_t *u, CAN_Output_t *out) 
+//{
+//	
+//}
+int8_t Co_Process(UP_t *u, CAN_Output_t *out)
+{
+	switch(u->CoContext .CoState )
+	{
+		case CoPREPARE:
+			
+		break ;
+		case CoTRANSLATE_OUT:
+			u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 
+
+		if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 500)//速度为0认为卡主
+			{
+				u->DribbleCont .DribbleState=DRIBBLE_PROCESS_DOWN;//到达位置后，运球转移状态
+				u->Pitch_Cfg .PitchState = PITCH_START;
+
+				u->CoContext .CoState =CoDRIBBLE;
+			}
+		break;
+
+		case CoDRIBBLE:
+			 Dribble_Process(u,out);  //状态停在DRIBBLE_DONE
+			 Pitch_Process(u,out);    //状态停在PITCH_LAUNCHING
+		//状态停在对应位置时,平移去给发射送球
+		if(u->DribbleCont .DribbleState ==DRIBBLE_DONE && u->Pitch_Cfg.PitchState ==PITCH_LAUNCHING)
+		{
+			u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig .m3508_init_ang;			
+		}
+		if(is_reached(u->motorfeedback.DJmotor_feedback[3].total_angle ,u->DribbleCont .DribbleConfig.m3508_init_ang,1.0f))
+		{
+			u->motor_target.Dribble_M3508_speed[0]=1000;
+			u->motor_target.Dribble_M3508_speed[1]=1000;
+			u->motor_target.Dribble_M3508_speed[2]=1000;
+
+			u->CoContext .CoState =CoTRANSLATE_IN;
+		}
+		
+    break ;	
+		case CoTRANSLATE_IN:
+			
+				if(is_reached_multiple(u->motorfeedback .DJmotor_feedback [0].rpm,
+				u->motorfeedback .DJmotor_feedback [1].rpm,
+		   	u->motorfeedback .DJmotor_feedback [2].rpm,
+			  u->motor_target.Dribble_M3508_speed [0],
+			  u->motor_target.Dribble_M3508_speed [1],
+				u->motor_target.Dribble_M3508_speed[2],	
+			  50.0f,100)
+			) {
+   		  	RELAY1_TOGGLE(1);  //速度达到后打开气缸,送给发射
+			}
+			if(u->DribbleCont .DribbleConfig.light_ball_flag == 0)
+			{
+			  u->motor_target.Dribble_M3508_speed[0]=0;
+			  u->motor_target.Dribble_M3508_speed[1]=0;
+			  u->motor_target.Dribble_M3508_speed[2]=0;	
+				u->motor_target .Dribble_translate_M3508_angle =u->DribbleCont .DribbleConfig.m3508_translate_angle;//平行移动 
+			
+			}
+			
+		  if(u->motorfeedback.DJmotor_feedback[3].rpm==0 && u->motorfeedback.DJmotor_feedback[3].total_angle > 900)//速度为0认为卡主,卡到最右端
+			{
+				
+				u->CoContext .CoState =CoPITCH;
+				
+			}
+    break ;
+			
+    case CoPITCH:
 					
 		     
+
+		case CoPITCH_MID360:
+			
+			u->motor_target .go_shoot=u->Pitch_Cfg.PitchConfig.go_init;
+		
+    break ;			
+			break ;
+
+	}
+	
+	
+	
+
+
+}
+

User/Module/up.h

@@ -26,16 +26,25 @@
 运行函数，switch(状态) 运行相应函数
 
 
-
-
-
-
-
-
-
-
-
 */
+/*配合过程状态*/
+typedef enum {
+    CoPREPARE,            // 准备阶段
+	  CoTRANSLATE_OUT,//上方平移,去运球
+	  CoDRIBBLE,       //运球和蓄力阶段,再平移回去
+	  CoTRANSLATE_IN,//上方平移，送球发射，回到运球位置
+	  CoPITCH,       //发射
+	  CoPITCH_MID360, //雷达发射
+	  CoDONE,        
+	  
+
+}CoState_t;
+/*总配合上下文*/
+typedef struct {
+    CoState_t CoState;
+
+    uint8_t is_init ;
+} CoCon_t;
 
 
 
@@ -46,33 +55,39 @@ typedef enum {
 	  PITCH_START,            //启动，拉扳机
     PITCH_PULL_TRIGGER,  
     PITCH_LAUNCHING,        // 发射中
+	
     PITCH_COMPLETE          // 完成
+	
 } PitchState_t;
 
 /* 投球参数配置 */
 typedef struct {
-    fp32 m2006_init_angle;      // M2006初始角度-140
+    fp32 m2006_init;      // M2006初始角度
-	  fp32 m2006_trigger_angle;   // M2006触发角度0,用来合并扳机
+	  fp32 m2006_trig;   // M2006触发角度0,用来合并扳机
-    fp32 go1_init_position;     // GO电机触发位置,0,初始位置
+    fp32 go_init;     // GO电机触发位置,初始位置，后续更改用于发射的位置
-	  fp32 go1_release_threshold; // go上升去合并扳机，扳机位置
+	  fp32 go_pull_pos; // go上升去合并扳机，扳机位置
-	  fp32 m2006_Screw_init;
 	  fp32 Pitch_angle;
-} PitchConfig_t;
 	
+	  fp32 pull_speed;//go速度
+	  fp32 go_release_pos;//go松开，发射位置
+	
+} PitchCfg_t;
+//fp32 go_pull_pos;
+//    fp32 m2006_init;      // M2006初始角度
+//    fp32 m2006_trig;      // M2006触发角度
+//    fp32 go_init;         // GO初始位置
+//    fp32 go_release;      // GO释放阈值
+//    fp32 go_recv;         // GO接球位置
+//    fp32 screw_init;      // 螺杆初始值
+//    fp32 pitch_ang;       // 投球角度
+//    fp32 pull_spd;        // 拉动速度
 /* 投球控制上下文 */
 typedef struct {
     PitchState_t PitchState;
-	  PitchConfig_t PitchConfig;
+	  PitchCfg_t PitchConfig;
-
-    uint8_t is_initialized ;
-} PitchContext_t;
-
-
-
-
-
-
 
+    uint8_t is_init ;
+} Pitch_Cfg_t;
 
 /*运球*/
 typedef enum {
@@ -88,29 +103,27 @@ typedef enum {
 typedef struct {
 	int8_t dribble_flag;//当移动三摩擦后为1，否则为0，防止发射撞到
 	   
-    fp32 m3508_init_angle;     // 平移前位置
+    fp32 m3508_init_ang;     // 3508平移前位置
     fp32 m3508_translate_angle;     // 平移后位置
-	  fp32 m3508_dribble_init_speed;
+	  fp32 init_spd;
-    fp32 m3508_dribble_speed;  // 转动速度
+    fp32 spd;  // 转动速度
-    fp32 m3508_dribble_Reverse_speed;
+    fp32 rev_spd;
 
 	
 	/*光电标志位，初始值均为0，触发为1*/
 		int light_3508_flag;//3508平移处的光电，0初始，1到位置
 	  int light_ball_flag;//检测球的flag
 	
-} DribbleConfig_t;
+} DribbleCfg_t;
 
 /* 状态机上下文 */
 typedef struct {
     DribbleState_t DribbleState;
-	  DribbleConfig_t DribbleConfig;
+	  DribbleCfg_t DribbleConfig;
 	   
-    uint8_t is_initialized;
+    uint8_t is_init;
 
-	
+} DribbleCont_t;
-
-} DribbleContext_t;
 
 
 typedef struct {
@@ -141,57 +154,13 @@ typedef struct
 	pid_param_t Dribble_translate_M3508_angle_param;//平移用的角度环
 	 
 	
-	DribbleConfig_t DribbleConfig_Config;
+	DribbleCfg_t DribbleCfg;
-	PitchConfig_t  PitchConfig_Config;
+	PitchCfg_t  PitchCfg;
 	
 	GO_MotorCmd_t go_cmd;
 }UP_Param_t;
-
-typedef struct
-{	
-	  fp32 ecd;
-	  fp32 rpm;
-		uint8_t id;
-    fp32 orig_angle;     
-    fp32 last_angle;     
-    int32_t round_cnt;    
-    int init_cnt;   
-	  fp32 total_angle;	
-}DJmotor_feedback_t;
-
-
 typedef struct{
 
-   uint8_t up_task_run; 
-   const	UP_Param_t *param;  
-		/*运球过程*/
-  DribbleContext_t DribbleContext;
-    /*投篮过程*/
-	PitchContext_t PitchContext;
-		  
-	 CMD_t *cmd;	
-
-	 struct{	
-		 
-	 struct{	
-	  fp32 VESC_5065_M1_rpm;
-	  fp32 VESC_5065_M2_rpm;
-	 }VESC;
-
-	  GO_MotorData_t *go_data;//存放go电机数据
-
-    DJmotor_feedback_t DJmotor_feedback[6];
-		
-    struct { 
-	   uint32_t ecd;
-	   float angle;
-     }Encoder;	
-	}motorfeedback;
- 
-
-	
-	 struct{
-
 	  fp32 VESC_5065_M1_rpm;
 	  fp32 VESC_5065_M2_rpm;
 
@@ -204,9 +173,9 @@ typedef struct{
 
 		fp32 Shoot_Pitch_angle;
 		 
-	}motor_target;
+	}up_motor_target_t;
 
-	 struct{
+typedef struct{
 		 
 		 
 		 pid_type_def VESC_5065_M1;
@@ -226,12 +195,56 @@ typedef struct{
 		 pid_type_def Dribble_translate_M3508_angle;//平移用的角度环
 	 
 		 
-	}pid;
+	}up_pid_t;
- 	   GO_MotorCmd_t go_cmd;
+typedef struct
+{	
+	  fp32 ecd;
+	  fp32 rpm;
+		uint8_t id;
+    fp32 orig_angle;     
+    fp32 last_angle;     
+    int32_t round_cnt;    
+    int init_cnt;   
+	  fp32 total_angle;	
+}DJmotor_feedback_t;
+
+
+typedef struct{
+	
+   uint8_t up_task_run; 
+   const	UP_Param_t *param;  
+   CoCon_t	CoContext;
+	/*运球过程*/
+  DribbleCont_t DribbleCont;
+    /*投篮过程*/
+	Pitch_Cfg_t Pitch_Cfg;
+		  
+	 CMD_t *cmd;	
+
+	 struct{	
+		 
+	 struct{	
+	  fp32 VESC_5065_M1_rpm;
+	  fp32 VESC_5065_M2_rpm;
+	 }VESC;
+	  GO_MotorData_t *go_data;//存放go电机数据
+
+    DJmotor_feedback_t DJmotor_feedback[6];
+		
+    struct { 
+	   uint32_t ecd;
+	   float angle;
+     }Encoder;	
+	}motorfeedback;
  
 
 	
 
+  up_pid_t pid;
+  up_motor_target_t motor_target;
+
+ 	GO_MotorCmd_t go_cmd;
+	
 		/*经PID计算后的实际发送给电机的实时输出值*/
     struct 
 	{
@@ -246,8 +259,6 @@ typedef struct{
 	
 	fp32 vofa_send[8];
 	
-
-	
 }	UP_t;
 
 
@@ -262,7 +273,8 @@ int8_t DJ_Angle_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Angle
 int8_t DJ_Speed_Control(UP_t *u,int id,DJmotor_feedback_t *f,pid_type_def *Speed_pid,fp32 target_speed);
 
 int8_t Dribble_Process(UP_t *u, CAN_Output_t *out);
-int8_t Pitch_Process(UP_t *u, CAN_Output_t *out,CMD_t *c);
+int8_t Pitch_Process(UP_t *u, CAN_Output_t *out);
+int8_t Co_Process(UP_t *u, CAN_Output_t *out);
 
 
 #endif

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;

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,

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

User/device/can_use.c

@@ -1,3 +1,4 @@
+
 /*
   CAN总线上的设备1到7
   将所有CAN总线上挂载的设备抽象成单个设备进行管理和控制
@@ -32,7 +33,7 @@
 #define CAN_M3508_MAX_ABS_CUR (20)
 #define CAN_M2006_MAX_ABS_CUR (10)
 
-#define CAN_ENCODER_RESOLUTION (32768) //欧艾迪编码器分辨率
+#define CAN_ENCODER_RESOLUTION (4096) //欧艾迪编码器分辨率
 
  CAN_RawRx_t raw_rx1;//原始的can数据
  CAN_RawRx_t raw_rx2;
@@ -56,31 +57,20 @@ static void CAN_DJIMotor_Decode(CAN_MotorFeedback_t *feedback,
   feedback->torque_current =
   raw_current * CAN_GM6020_MAX_ABS_CUR / (float)CAN_MOTOR_CUR_RES;														 
 }
-static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id,
+static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,
 	                            const uint8_t *raw) {
 	if (feedback == NULL || raw == NULL) return;	
-  switch 	(id)
+		static	uint16_t sick_rec[4];
-	{
-		case CAN_SICK_ID_1:
-			feedback->raw_dis[0] = (uint16_t)(raw[0] << 8| raw[1]);	
 
-			break ;
+  sick_rec[0] = (uint16_t)(raw[0] << 8 | raw[1]);
+  sick_rec[1] = (uint16_t)(raw[2] << 8 | raw[3]);
+  sick_rec[2] = (uint16_t)(raw[4] << 8 | raw[5]);
+  sick_rec[3] = (uint16_t)(raw[6] << 8 | raw[7]);
 	
-		case CAN_SICK_ID_2:
+		feedback ->raw_dis [0]= sick_rec[0];
-			feedback->raw_dis[1] = (uint16_t)(raw[0] << 8| raw[1]);	
+		feedback ->raw_dis [1]= sick_rec[1];
-			
+		feedback ->raw_dis [2]= sick_rec[2];
-			break ;
+		feedback ->raw_dis [3]= sick_rec[3];
-						
-		case CAN_SICK_ID_3:
-			feedback->raw_dis[2] = (uint16_t)(raw[0] << 8| raw[1]);	
-
-			break ;	
-						
-		case CAN_SICK_ID_4:
-			feedback->raw_dis[3] = (uint16_t)(raw[0] << 8| raw[1]);	
-
-			break ;
-	}		
 }
 void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
                              const uint8_t *raw) {
@@ -93,7 +83,8 @@ void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
 	    { 
 				case 0x01:
 				feedback->ecd =raw[3]|raw[4]<<8|raw[5]<<16|raw[6]<<24;
-				feedback->angle=(fp32)feedback->ecd*360/CAN_ENCODER_RESOLUTION-151.0f;
+				feedback->angle=(fp32)(2048-(fp32)feedback->ecd)/(2048-1797)*(70.0f-48.0f)+48.0f;
+				
 				break;
 			}
 		}
@@ -383,19 +374,10 @@ int8_t CAN_StoreMsg(CAN_t *can, CAN_RawRx_t *can_rx) {
         CAN_DJIMotor_Decode(&(can->motor.chassis_motor3508.as_array[index]), can_rx->rx_data);
         detect_hook(CHASSIS3508M1_TOE + index);
         break;
-        case CAN_SICK_ID_1:
+        case CAN_SICK_ID:
-					   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_1, can_rx->rx_data);	
+					   CAN_Sick_Receive(&(can->sickfed), can_rx->rx_data);	
         break;
-        case CAN_SICK_ID_2:
-					   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_2, can_rx->rx_data);		
-        break;
-        case CAN_SICK_ID_3:
-					   CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_3, can_rx->rx_data);		
-        break;
-        case CAN_SICK_ID_4:
-             CAN_Sick_Receive(&(can->sickfed),CAN_SICK_ID_4, can_rx->rx_data);
 
-        break;
       default:
         break;
     }

User/device/can_use.h

@@ -18,10 +18,8 @@ typedef enum {
   CAN_UP_M3508_M5_ID = 0x205, /* 1 */
   CAN_UP_M3508_M6_ID = 0x206, /* 2 */
 	
-	CAN_SICK_ID_1=0x301,
+	CAN_SICK_ID=0x305,
-	CAN_SICK_ID_2=0x302,
+
-	CAN_SICK_ID_3=0x303,
-	CAN_SICK_ID_4=0x304,
 	
 	CAN_Encoder_ID=0x01,
 	
@@ -200,6 +198,6 @@ void CAN_Encoder_Control(CAN_t *can);
 
 void CAN_Encoder_Decode(CAN_EncoderFeedback_t *feedback,
                              const uint8_t *raw) ;
-static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,uint16_t id,
+static void CAN_Sick_Receive(CAN_SickFeedback_t *feedback,
 	                            const uint8_t *raw) ;
 #endif

User/device/cmd.c

@@ -99,15 +99,17 @@ int8_t CMD_ParseNuc(CMD_t *cmd,CMD_NUC_t *n){
     switch(cmd->cmd_status){
 
 			case MID:
-			cmd->cmd_MID360.posx = n->navi.vx;
+			cmd->cmd_MID360.posx = n->MID360.vx;
-			cmd->cmd_MID360.posy = n->navi.vy;
+			cmd->cmd_MID360.posy = n->MID360.vy;
-			cmd->cmd_MID360.posw = n->navi.wz;
+			cmd->cmd_MID360.posw = n->MID360.wz;
+			
+			cmd->pos =n->MID360 .pos ;
 			break;
 			
-			case PICK :
+			case VISION :
-				cmd ->cmd_pick .posx =n->pick .posx ;
+				cmd ->cmd_pick .posx =n->camera.data1 ;
-				cmd ->cmd_pick .posy =n->pick .posy ;
+				cmd ->cmd_pick .posy =n->camera.data2 ;
-				cmd ->cmd_pick .posw =n->pick .posw ;
+				cmd ->cmd_pick .posw =n->camera.data3 ;
 
 			break;
 						
@@ -151,11 +153,11 @@ int8_t CMD_DR16(CMD_t *cmd,const CMD_RC_t *rc) {
 	{
 		
 		cmd ->CMD_CtrlType =AUTO;		
-		if(rc->dr16.sw_r ==CMD_SW_UP) 	cmd ->CMD_mode =AUTO_NAVI; //左中，右中，雷达	
+		if(rc->dr16.sw_r ==CMD_SW_UP) 	cmd ->CMD_mode =AUTO_MID360; //左中，右中，雷达	
 		
 		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_MID360_Pitch; //左中，右下，视觉
 		
 	}
 	else if(rc->dr16.sw_l==CMD_SW_DOWN)
@@ -189,16 +191,22 @@ 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; 
+	if	(rc->LD.key_C  ==CMD_SW_UP) cmd ->CMD_mode =AUTO_MID360_Adjust;
-	else if(rc->LD.key_E ==CMD_SW_UP) cmd ->CMD_mode =AUTO_PICK;
+	else {
-	else cmd ->CMD_mode =Normal ;
 		
+	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_MID360_Pitch;
+	else if(rc->LD .key_E ==CMD_SW_MID ) cmd ->CMD_mode =AUTO_MID360;		
+	else cmd ->CMD_mode =Normal ;
+	    }
 	}
 	else if(cmd ->CMD_CtrlType ==RCcontrol){
 /*手动下的*/	
 	if(rc->LD.key_G ==CMD_SW_DOWN )  cmd ->CMD_mode=Dribble ;
 	else if(rc->LD.key_G ==CMD_SW_UP) cmd ->CMD_mode =Pitch;
 	else if(rc->LD .key_H==CMD_SW_UP)	cmd ->CMD_mode =UP_Adjust;
+	else if(rc->LD.key_C == CMD_SW_UP) cmd->CMD_mode =Chassis_Adjust;
 	else cmd ->CMD_mode =Normal;
 	}
 }	
+

User/device/cmd.h

@@ -8,7 +8,8 @@
 #include <string.h>
 
 #define MID (0x09)
-#define PICK (0x06)
+#define VISION (0x02)
+#define NUC (0x08)
 
 typedef enum{
 	RCcontrol,//遥控器控制，左按键上，控制上层
@@ -22,10 +23,12 @@ typedef  enum{
      
 		Normal,    //无模式
 	
-	  AUTO_NAVI,
+	  AUTO_MID360,
-	  AUTO_PICK,
+	  AUTO_MID360_Pitch,
+	  AUTO_MID360_Adjust,//雷达调整
 	
 	  UP_Adjust,//上层调整
+	Chassis_Adjust,
 
     Dribble ,  //运球
 	  Pitch,     //投篮,底盘锁定
@@ -41,17 +44,19 @@ typedef struct {
   fp32 vx;
   fp32 vy;
   fp32 wz;
-  }navi;
+		
+	fp32 pos;
+	fp32 angle;
+	char  flag;	
+		
+  }MID360;
  struct 
  {
-	 fp32 posx;
+	 fp32 data1;
-   fp32 posy;
+   fp32 data2;
-   fp32 posw;
+   fp32 data3;
- }pick;
+	 }camera;//相机
-  struct
+
-	{
-		fp32 angle;
-	}sick_cali;
 } CMD_NUC_t;
 /* 拨杆位置 */
 typedef enum {
@@ -122,6 +127,8 @@ typedef struct {
    fp32 key_ctrl_l;
    fp32 key_ctrl_r;
 	
+	fp32 pos;//雷达反馈go位置
+	
 	 fp32 Vx;
    fp32 Vy;
    fp32 Vw;

User/device/nuc.c

@@ -6,7 +6,9 @@ static volatile uint32_t drop_message = 0;
 
 static osThreadId_t thread_alert;
 
-uint8_t nucbuf[31];
+static uint8_t nucbuf[31];
+static char SendBuffer[19];
+
 
 static void NUC_IdleCallback(void) {	
 	osThreadFlagsSet(thread_alert,SIGNAL_NUC_RAW_REDY);
@@ -27,13 +29,40 @@ int8_t NUC_StartReceiving() {
     return DEVICE_OK;
   return DEVICE_ERR;
 }
+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));
   __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_NUC));
   return DEVICE_OK;
 }
 bool_t NUC_WaitDmaCplt(void) {
-  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,500) ==
+  return (osThreadFlagsWait(SIGNAL_NUC_RAW_REDY, osFlagsWaitAll,100) ==
           SIGNAL_NUC_RAW_REDY);
 }
 
@@ -42,8 +71,8 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
 	if(n ==NULL) return DEVICE_ERR_NULL;
  	union 
 	{
-		float x[3];
+		float x[5];
-		char data[12];
+		char data[20];
 	}instance;
   if(nucbuf[0]!=HEAD) goto error;  //发送ID不是底盘
   else{
@@ -51,7 +80,35 @@ int8_t NUC_RawParse(CMD_NUC_t *n){
 		n->ctrl_status =nucbuf[2];
     switch (n->status_fromnuc)
     {
-		case MID://控制帧0x09
+		case VISION://控制帧0x02
+				/* 协议格式 
+		   0xFF  HEAD
+		   0x02  控制帧
+		   0x01  相机帧
+		   vx    fp32
+		   vy    fp32
+		   wz    fp32
+		   0xFE  TAIL
+		  */
+
+
+		instance.data[3] = nucbuf[3];
+       instance.data[2] = nucbuf[4];
+       instance.data[1] = nucbuf[5];
+       instance.data[0] = nucbuf[6];
+      n->camera .data1  = instance.x[0]; //
+       instance.data[7] = nucbuf[7];
+       instance.data[6] = nucbuf[8];
+       instance.data[5] = nucbuf[9];
+       instance.data[4] = nucbuf[10];
+      n->camera .data2 = instance.x[1];//
+       instance.data[11] = nucbuf[11];
+       instance.data[10] = nucbuf[12];
+       instance.data[9] = nucbuf[13];
+       instance.data[8] = nucbuf[14];
+      n->camera .data3 = instance.x[2];//
+      break;
+		case MID ://控制帧0x08
 				/* 协议格式 
 		   0xFF  HEAD
 		   0x09  控制帧
@@ -61,69 +118,37 @@ 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];
        instance.data[0] = nucbuf[3];
-      n->navi.vx = instance.x[0]; //
+      n->MID360.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];//
+      n->MID360.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];//
+      n->MID360.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->MID360.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->MID360.angle  = instance.x[4];//
+
+      n->MID360.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;
   }

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

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);
+if (raw->ch[0] < 0)
-		raw->map_ch[1]=map_fp32(raw->ch[1],-653,746,-1,1);
+    raw->map_ch[0] = map_fp32(raw->ch[0], -696, 2, -1, 0);
-		raw->map_ch[2]=map_fp32(raw->ch[2],95,1482,0,1);
+else
-		raw->map_ch[3]=map_fp32(raw->ch[3],-317,375,-1,1);
+    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);

User/device/vofa.c

@@ -5,36 +5,61 @@
 //用来对需要的数据进行串口绘图  （未添加接收函数）
 
 
+//float vofa_send[8];
+
+////发送函数
+//void vofa_tx_main(float *data)
+//{
+//  float fdata[8]={0};
+//	uint8_t tail[4];
+//	tail[0]=0x00;
+//	tail[1]=0x00;
+//	tail[2]=0x80;
+//	tail[3]=0x7f;
+//	/*在下面添加发送的数据*/
+//	fdata[0] = data[0];
+//	fdata[1] = data[1];
+//	fdata[2] = data[2];
+//	fdata[3] = data[3];
+//	fdata[4] = data[4];
+//	fdata[5] = data[5];
+//	fdata[6] = data[6];
+//	fdata[7] = data[7];
+//	
+//		
+//	/*在下面使用对应的串口发送函数*/
+////	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(&huart1, tail, 4);
+//	osDelay(1);
+
+//		
+//}
+
 float vofa_send[8];
 
 //发送函数
 void vofa_tx_main(float *data)
 {
-  float fdata[8]={0};
+    float fdata[8] = {0};
-	uint8_t tail[4];
+    uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7f};
-	tail[0]=0x00;
-	tail[1]=0x00;
-	tail[2]=0x80;
-	tail[3]=0x7f;
-	/*在下面添加发送的数据*/
-	fdata[0] = data[0];
-	fdata[1] = data[1];
-	fdata[2] = data[2];
-	fdata[3] = data[3];
-	fdata[4] = data[4];
-	fdata[5] = data[5];
-	fdata[6] = data[6];
-	fdata[7] = data[7];
 
    
-	/*在下面使用对应的串口发送函数*/
+    for (int i = 0; i < 8; i++) {
-//	HAL_UART_Transmit_DMA(&huart1, ( uint8_t *)fdata, sizeof(fdata));
+        fdata[i] = data[i];
-//	osDelay(1);
+    }
-//	HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_VOFA), tail, 4);
+
-//  osDelay(1);
+ 
-	CDC_Transmit_FS( ( uint8_t *)fdata, sizeof(fdata));
+    uint8_t packet[8 * sizeof(float) + 4];
-	osDelay(1);
+    memcpy(packet, fdata, 8 * sizeof(float));         
-//	CDC_Transmit_FS( tail, 4);
+    memcpy(packet + 8 * sizeof(float), tail, 4);      
+
+    /*在下面使用对应的串口发送函数*/
+//		HAL_UART_Transmit_DMA(&huart6, packet, sizeof(packet));
+//    CDC_Transmit_FS( packet, sizeof(packet));
+//      osDelay(1);
    
 }
-

User/task/can_task.c

@@ -47,7 +47,6 @@ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CAN;
       
     
 		 //一问一答sick数据指令
-//	  CAN_Sick_Control(&can);
     CAN_Encoder_Control(&can);
 
 		/*can设备数据存入队列*/

User/task/chassis_task.c

@@ -56,6 +56,8 @@ void Task_Chassis(void *argument)
 		{
 #ifdef DEBUG
     task_runtime.stack_water_mark.chassis = osThreadGetStackSpace(osThreadGetId());
+			task_runtime.freq.chassis=TASK_FREQ_CHASSIS;
+			task_runtime.last_up_time.chassis=tick;
 #endif
 			
 	/*imu数据获取*/					
@@ -85,13 +87,14 @@ 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);  //发送数据		
 	vofa_send[0] = chassis.vofa_send[0];
-   	vofa_send[1] = chassis.vofa_send[1];
+  vofa_send[1] = chassis.vofa_send[1];
 	vofa_send[2] = chassis.vofa_send[2];
 	vofa_send[3] = chassis.vofa_send[3];
 	vofa_send[4] = chassis.vofa_send[4];
-   	vofa_send[5] = chassis.vofa_send[5];
+  vofa_send[5] = chassis.vofa_send[5];
 	vofa_send[6] = chassis.vofa_send[6];
 	vofa_send[7] = chassis.vofa_send[7];
 		 

User/task/cmd_task.c

@@ -30,8 +30,9 @@ void Task_cmd(void *argument){
    while(1){
 #ifdef DEBUG
     /*记录任务所需要的栈空间*/
-    task_runtime.stack_water_mark.cmd =
+    task_runtime.stack_water_mark.cmd =osThreadGetStackSpace(osThreadGetId());	 
-        osThreadGetStackSpace(osThreadGetId());	 
+		 task_runtime.last_up_time.cmd=tick;
+		 task_runtime.freq.cmd=TASK_FREQ_CTRL_CMD;
 #endif
 
     osKernelLock(); /*锁住RTOS内核调度*/

User/task/init.c

@@ -85,6 +85,8 @@ void Task_Init(void *argument) {
 	
    task_runtime.msgq.cmd.raw.nuc =
       osMessageQueueNew(3u,sizeof(CMD_NUC_t), NULL);	 
+   task_runtime.msgq.cmd.raw.nuc_send =
+	    osMessageQueueNew(3u,sizeof(NUC_send_t), NULL);	
 			
   osKernelUnlock();
   osThreadTerminate(osThreadGetId()); /* 结束自身 */

User/task/nuc_task.c

@@ -5,18 +5,20 @@
 
 NUC_t nuc_raw;
 CMD_NUC_t cmd_fromnuc;
+
+NUC_send_t NUC_send;
 #else 
 static NUC_t nuc_raw;
 static CMD_NUC_t cmd_fromnuc;
-
+NUC_send_t NUC_send;
 
 #endif
+fp32 send[4]={11.0f,45.0,1.f,4.0f};
 
 void Task_nuc(void *argument){
  (void)argument; /**/
 
 //	 osDelay(TASK_INIT_DELAY_NUC); 
-	
   const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_NUC;
 
    NUC_Init(&nuc_raw);
@@ -27,8 +29,13 @@ void Task_nuc(void *argument){
 		 {	 
 #ifdef DEBUG
     task_runtime.stack_water_mark.nuc= osThreadGetStackSpace(osThreadGetId());
+		task_runtime .freq.nuc = TASK_FREQ_NUC;
+		task_runtime.last_up_time.nuc=	tick;
+    
 #endif
 			NUC_StartReceiving();
+			NUC_StartSending(NUC_send.send);
+//			 NUC_StartSending (send);
       if (NUC_WaitDmaCplt()){ 
          NUC_RawParse(&cmd_fromnuc);
       }
@@ -37,6 +44,7 @@ void Task_nuc(void *argument){
       }
      osMessageQueueReset(task_runtime.msgq.cmd.raw.nuc);
      osMessageQueuePut(task_runtime.msgq.cmd.raw.nuc,&(cmd_fromnuc),0,0);
+		 osMessageQueueGet(task_runtime.msgq.cmd.raw.nuc_send, &NUC_send, NULL, 0);
 	
      
 		 tick += delay_tick; /* 计算下一个唤醒时刄1ķ*/

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 [0]=UP.vofa_send [0];
-   vofa_send [1]=UP.vofa_send [1];
+//   vofa_send [1]=UP.vofa_send [1];
 	 vofa_tx_main (vofa_send);
 	 
 	tick += delay_tick;

User/task/user_task.h

@@ -58,9 +58,9 @@ typedef struct {
     struct {
       struct {
         osMessageQueueId_t rc;
-        osMessageQueueId_t nuc;
+        osMessageQueueId_t nuc; //
         osMessageQueueId_t eulr;/*姿态结算得到的欧拉角*/
-				
+				osMessageQueueId_t nuc_send; //给nuc发
 		 
       }raw;
 			
@@ -68,8 +68,6 @@ typedef struct {
       osMessageQueueId_t UP_cmd_ctrl_t;
 			osMessageQueueId_t CHASSIS_cmd_ctrl_t;
 			
-				
-			
       osMessageQueueId_t status;
     } cmd;
         /* can任务放入、读取，电机或电容的输入输出 */