debug完成, 加入卡尔曼滤波器,减少线对编码器数值的影响

stm32/HARDWARE/KEY/key.c

@@ -1,8 +1,8 @@
 /*
  * @Author: your name
  * @Date: 2022-04-17 21:03:46
- * @LastEditTime: 2022-04-19 17:45:57
+ * @LastEditTime: 2022-04-19 23:32:17
- * @LastEditors: lishengyin
+ * @LastEditors: Please set LastEditors
  * @Description: 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
  * @FilePath: \undefinede:\项目\兼职项目\MotorPid\stm32\HARDWARE\KEY\key.c
  */
@@ -28,18 +28,13 @@ void KEY_Init(void) //IO<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
 { 
  	GPIO_InitTypeDef GPIO_InitStructure;
  
- 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOC,ENABLE);//ʹ<EFBFBD><EFBFBD>PORTA,PORTEʱ<EFBFBD><EFBFBD>
+ 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC|RCC_APB2Periph_GPIOB,ENABLE);//ʹ<EFBFBD><EFBFBD>PORTA,PORTEʱ<EFBFBD><EFBFBD>
 
-	GPIO_InitStructure.GPIO_Pin  = GPIO_Pin_1 | GPIO_Pin_13 | GPIO_Pin_12;//KEY0-KEY1
 	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU; //<EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
- 	GPIO_Init(GPIOC, &GPIO_InitStructure);//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>GPIOE4,3
+	GPIO_InitStructure.GPIO_Pin  = GPIO_Pin_0 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
-
+	GPIO_Init(GPIOC, &GPIO_InitStructure);
-	//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD> WK_UP-->GPIOA.0	  <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-	GPIO_InitStructure.GPIO_Pin  = GPIO_Pin_0;
-	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; //PA0<EFBFBD><EFBFBD><EFBFBD>ó<EFBFBD><EFBFBD><EFBFBD><EFBFBD>룬Ĭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>	  
-	GPIO_Init(GPIOA, &GPIO_InitStructure);//<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>GPIOA.0
-
 }
+
 //<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 //<EFBFBD><EFBFBD><EFBFBD>ذ<EFBFBD><EFBFBD><EFBFBD>ֵ
 //mode:0,<EFBFBD><EFBFBD>֧<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>;1,֧<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>;
@@ -52,14 +47,14 @@ u8 KEY_Scan(u8 mode)
 {	 
 	static u8 key_up=1;//<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɿ<EFBFBD><EFBFBD><EFBFBD>־
 	if(mode)key_up=1;  //֧<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>		  
-	if(key_up&&(KEY0==0||KEY1==0 || KEY2==0 ||WK_UP==1))
+	if(key_up&&(KEY0==0 || KEY1==0 || KEY2==0 ||WK_UP==0))
 	{
 		delay_ms(10);//ȥ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> 
 		key_up=0;
 		if(KEY0==0)return KEY0_PRES;
 		else if(KEY1==0)return KEY1_PRES;
-		else if(WK_UP==1)return WKUP_PRES;
+		else if(WK_UP==0)return WKUP_PRES;
-		else if(KEY2 == 0) return KEY2_PRES;
+		else if(KEY2 == 0)return KEY2_PRES;
-	}else if(KEY0==1&&KEY1==1&&WK_UP==0&&KEY2==1)key_up=1; 	    
+	}else if(KEY0==1&&KEY1==1&&WK_UP==1&&KEY2==1)key_up=1; 	    
  	return 0;// <EFBFBD>ް<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 }

stm32/HARDWARE/KEY/key.h

@@ -15,22 +15,18 @@
 //////////////////////////////////////////////////////////////////////////////////   	 
 
 
-//#define KEY0 PEin(4)   	//PE4
+#define KEY0  GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_15)//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
-//#define KEY1 PEin(3)	//PE3 
+#define KEY1  GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_14)//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
-//#define WK_UP PAin(0)	//PA0  WK_UP
+#define KEY2  GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_13)
+#define WK_UP GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_0)//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>3(WK_UP) 
 
-#define KEY0  GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_1)//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>0
-#define KEY1  GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_13)//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>1
-#define KEY2  GPIO_ReadInputDataBit(GPIOC,GPIO_Pin_12)
-
-#define WK_UP GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0)//<EFBFBD><EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>3(WK_UP) 
-
- 
 #define KEY0_PRES 	1	//KEY0<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 #define KEY1_PRES	  2	//KEY1<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 #define WKUP_PRES   3	//KEY_UP<EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<EFBFBD><EFBFBD>WK_UP/KEY_UP)
 #define KEY2_PRES  4
 
 void KEY_Init(void);//IO<EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
-u8 KEY_Scan(u8);  	//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD>躯<EFBFBD><EFBFBD>					    
+u8 KEY_Scan(u8);  	//<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɨ<EFBFBD>躯<EFBFBD><EFBFBD>
+
+
 #endif

stm32/HARDWARE/TIMER/timer.c

@@ -11,7 +11,6 @@ extern char beam_value_buf[7];
 
 extern struct UserInfo userInfo;
 
-
 /**
  * @description: 设置pwm
  * @param {int} motor
@@ -24,6 +23,26 @@ void Set_Pwm(int motor)
 	PWMA=abs(motor);
 }
 
+
+/**
+ * @description: 卡尔曼滤波器
+ * @param {unsigned long} inData
+ * @return {*}
+ */
+int KalmanFilter(int inData)
+{
+    static float  kalman = 0; //上次卡尔曼值(估计出的最优值)
+    static float  p = 10;
+    float  q = 0.02; //q:过程噪声
+    float  r = 7.0000; //r:测量噪声
+    float  kg = 0; //kg:卡尔曼增益
+    p += q;
+    kg = p / ( p + r ); //计算卡尔曼增益
+    kalman = kalman + (kg * (inData - kalman)); //计算本次滤波估计值
+    p = (1 - kg) * p; //更新测量方差
+    return (int)kalman; //返回估计值
+}
+
 /**
  * @description: 
  * @param {int} Encoder  编码器速度
@@ -33,15 +52,16 @@ void Set_Pwm(int motor)
 int velocity_PID(int Encoder,int Target){
 	static float velocity_KP = 7.7, velocity_KI = 1.1; 
 	static int Last_bias;
-	int Bias, pwm = 0;
+	static int Bias, pwm;
 	Bias = Encoder - Target;  //计算偏差
-	pwm = velocity_KP * Bias  + velocity_KI * (Bias - Last_bias); //增量式PI控制器
+	pwm += velocity_KP * Bias  + velocity_KI * (Bias - Last_bias); //增量式PI控制器
 	if(pwm > 7200) pwm = 7200;
 	if(pwm < -7200) pwm = -7200;
 	Last_bias = Bias; //保存上一次偏差 
 	return pwm;
 }
 
+
 /**
  * @description: 
  * @param {u16} arr
@@ -72,8 +92,7 @@ void TIM3_Int_Init(u16 arr,u16 psc)
 	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 
 	NVIC_Init(&NVIC_InitStructure);  
 
-	TIM_Cmd(TIM3, ENABLE);  
+	TIM_Cmd(TIM3, ENABLE);  				 
-							 
 }
 
 void TIM3_IRQHandler(void)   
@@ -82,7 +101,9 @@ void TIM3_IRQHandler(void)
 	if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) 
 	{
 		userInfo.currentSpeed = Read_Encoder(2);
-		if(userInfo.run) pwm = velocity_PID(userInfo.currentSpeed, userInfo.targetSpeed);
+		userInfo.currentSpeed = KalmanFilter(userInfo.currentSpeed);
+		if(userInfo.run)pwm = velocity_PID(userInfo.currentSpeed, userInfo.targetSpeed);
+		else pwm = 0;
 		userInfo.pwm_val = pwm;
 		Set_Pwm(pwm);
 		TIM_ClearITPendingBit(TIM3, TIM_IT_Update); 

stm32/OBJ/Template.build_log.htm

@@ -5,7 +5,7 @@
 <h2>Tool Versions:</h2>
 IDE-Version: ¦ÌVision V5.25.2.0
 Copyright (C) 2018 ARM Ltd and ARM Germany GmbH. All rights reserved.
-License Information: li Administrator, li, LIC=LSNX7-ESVPU-ARPXW-QH3G4-M33TV-HZM1L
+License Information: sasasa QY, asas, LIC=QF8WF-6I29G-55ZT2-95TJS-AUNX7-U54RF
  
 Tool Versions:
 Toolchain:       MDK-ARM Plus  Version: 5.25.2.0
@@ -21,19 +21,18 @@ Target DLL:      STLink\ST-LINKIII-KEIL_SWO.dll V3.0.1.0
 Dialog DLL:      TCM.DLL V1.35.1.0
  
 <h2>Project:</h2>
-e:\workspace\items\MotorPid-master\motorpid\stm32\USER\Template.uvprojx
+e:\ÏîÄ¿\¼æÖ°ÏîÄ¿\20220418-MotorPid\stm32\USER\Template.uvprojx
-Project File Date:  04/18/2022
+Project File Date:  04/17/2022
 
 <h2>Output:</h2>
 *** Using Compiler 'V5.06 update 6 (build 750)', folder: 'D:\Keil_v5\ARM\ARMCC\Bin'
 Build target 'Target 1'
-compiling key.c...
+compiling timer.c...
-compiling main.c...
+..\HARDWARE\TIMER\timer.c(23): warning:  #223-D: function "abs" declared implicitly
-main.c(113): warning:  #167-D: argument of type "char *" is incompatible with parameter of type "u8 *"
+  	PWMA=abs(motor);
-  	OLED_ShowString(0,0, str, 16);
+..\HARDWARE\TIMER\timer.c: 1 warning, 0 errors
-main.c: 1 warning, 0 errors
 linking...
-Program Size: Code=12802 RO-data=2342 RW-data=392 ZI-data=1344  
+Program Size: Code=13154 RO-data=2342 RW-data=408 ZI-data=1344  
 FromELF: creating hex file...
 "..\OBJ\Template.axf" - 0 Error(s), 1 Warning(s).
 
@@ -49,7 +48,7 @@ Package Vendor: Keil
   D:\Keil_v5\ARM\PACK\Keil\STM32F1xx_DFP\2.3.0\Device\Include
 
 <h2>Collection of Component Files used:</h2>
-Build Time Elapsed:  00:00:03
+Build Time Elapsed:  00:00:01
 </pre>
 </body>
 </html>

stm32/USER/.vscode/c_cpp_properties.json

@@ -3,30 +3,30 @@
         {
             "name": "Target 1",
             "includePath": [
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\LED",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\LED",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\SYSTEM\\delay",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\SYSTEM\\delay",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\SYSTEM\\sys",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\SYSTEM\\sys",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\SYSTEM\\usart",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\SYSTEM\\usart",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\USER",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\USER",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\STM32F10x_FWLib\\inc",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\STM32F10x_FWLib\\inc",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\CORE",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\CORE",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\ESP8266",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\ESP8266",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\DHT11",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\DHT11",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\LCD1602",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\LCD1602",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\ADC",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\ADC",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\TIMER",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\TIMER",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\DS18B20",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\DS18B20",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\cJSON",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\cJSON",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\DS1302",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\DS1302",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\LCD",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\LCD",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\OLED",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\OLED",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\MOTOR",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\MOTOR",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\ENCODER",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\ENCODER",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\HARDWARE\\KEY",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\HARDWARE\\KEY",
                 "D:\\Keil_v5\\ARM\\ARMCC\\include",
                 "D:\\Keil_v5\\ARM\\ARMCC\\include\\rw",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32\\STM32F10x_FWLib\\src",
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32\\STM32F10x_FWLib\\src",
-                "e:\\workspace\\items\\MotorPid-master\\motorpid\\stm32"
+                "e:\\项目\\兼职项目\\20220418-MotorPid\\stm32"
             ],
             "defines": [
                 "STM32F10X_MD",

stm32/USER/.vscode/keil-assistant.log

@@ -24,3 +24,11 @@
 
 [info] Log at : 2022/4/19|17:22:46|GMT+0800
 
+[info] Log at : 2022/4/19|22:29:34|GMT+0800
+
+[info] Log at : 2022/4/20|20:44:57|GMT+0800
+
+[info] Log at : 2022/4/20|20:45:03|GMT+0800
+
+[info] Log at : 2022/4/20|20:45:31|GMT+0800
+

stm32/USER/.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "cstdlib": "c"
+    }
+}

stm32/USER/.vscode/uv4.log

@@ -1,12 +1,2 @@
-*** Using Compiler 'V5.06 update 6 (build 750)', folder: 'D:\Keil_v5\ARM\ARMCC\Bin'
+Load "..\\OBJ\\Template.axf" 
-Build target 'Target 1'
+Erase Done.Programming Done.Verify OK.Flash Load finished at 21:42:22
-compiling key.c...
-compiling main.c...
-main.c(113): warning:  #167-D: argument of type "char *" is incompatible with parameter of type "u8 *"
-  	OLED_ShowString(0,0, str, 16);
-main.c: 1 warning, 0 errors
-linking...
-Program Size: Code=12802 RO-data=2342 RW-data=392 ZI-data=1344  
-FromELF: creating hex file...
-"..\OBJ\Template.axf" - 0 Error(s), 1 Warning(s).
-Build Time Elapsed:  00:00:03

stm32/USER/.vscode/uv4.log.lock

@@ -1 +1 @@
-2022/4/19 17:46:52
+2022/4/20 21:42:23

stm32/USER/Template.uvoptx

@@ -295,7 +295,7 @@
       <GroupNumber>2</GroupNumber>
       <FileNumber>7</FileNumber>
       <FileType>1</FileType>
-      <tvExp>0</tvExp>
+      <tvExp>1</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
       <PathWithFileName>..\HARDWARE\TIMER\timer.c</PathWithFileName>

stm32/USER/main.c

@@ -1,8 +1,8 @@
 /*
  * @Author: your name
  * @Date: 2022-04-17 21:03:48
- * @LastEditTime: 2022-04-19 17:44:21
+ * @LastEditTime: 2022-04-20 21:32:40
- * @LastEditors: lishengyin
+ * @LastEditors: Please set LastEditors
  * @Description: 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
  * @FilePath: \undefinede:\项目\兼职项目\MotorPid\stm32\USER\main.c
  */
@@ -60,23 +60,25 @@ void processingTasks(void)
 	{
 	case WKUP_PRES:
 		userInfo.diversion = ~userInfo.diversion;
+		// 方向
+		if(userInfo.diversion && userInfo.targetSpeed > 0) userInfo.targetSpeed =  0 - userInfo.targetSpeed;
+		else if(userInfo.diversion == 0) userInfo.targetSpeed = abs(userInfo.targetSpeed);
 		break;
 	case KEY1_PRES:
 		userInfo.targetSpeed++;
 		if(userInfo.targetSpeed > 20) userInfo.targetSpeed = 20;
 		break;
 	case KEY0_PRES:
-		if(userInfo.targetSpeed > 0)userInfo.targetSpeed--;
+		userInfo.targetSpeed--;
+		if(userInfo.targetSpeed < -20) userInfo.targetSpeed = -20;
 		break;
 	case KEY2_PRES:
-		userInfo.run = ~userInfo.run;
+		if(userInfo.run == 0) userInfo.run = 1;
+		else userInfo.run = 0;
 		break;
 	default:
 		break;
 	}
-	// 方向
-	if(userInfo.diversion) AIN1 = 1, AIN1 = 0;
-	else AIN1 = 0, AIN1 = 1;
 }
 
 /**
@@ -97,9 +99,11 @@ void Sys_Init(void)
 	OLED_Clear();
 	// 初始化为0
 	userInfo.targetSpeed = 0;
-	userInfo.run = 0;
+	userInfo.run = 1;
 } 
 
+
+
 /**
  * @description: Oled显示
  * @param {*}
@@ -107,9 +111,9 @@ void Sys_Init(void)
  */
 void OLED_Display(void)
 {
-	char str[30];
+	char str[50];
-	if(userInfo.diversion) sprintf(str, "speed:-%d,pwd_val:%d,target:%d   CW", abs(userInfo.currentSpeed), abs(userInfo.pwm_val), userInfo.targetSpeed);
+	if(userInfo.diversion) sprintf(str, "speed:%d,pwm_val:%d,target:%d   CW      ", abs(userInfo.currentSpeed), abs(userInfo.pwm_val), userInfo.targetSpeed);
-	else sprintf(str, "speed:-%d,pwd_val:%d,target:%d  REV", abs(userInfo.currentSpeed), abs(userInfo.pwm_val), userInfo.targetSpeed);
+	else sprintf(str, "speed:%d,pwm_val:%d,target:%d   REV      ", abs(userInfo.currentSpeed), abs(userInfo.pwm_val), userInfo.targetSpeed);
 	OLED_ShowString(0,0, str, 16);
 }