电机三环控制完成，初步PID参数整定完成 ， 采用中断方式读取角度， pwm 触发电机电流采样

Device/AngleSensor.c

@@ -13,11 +13,14 @@ uint8_t g_spiRxBuff1[DATA_SIZE] = {0};
 
 static float angle_offset = 0.0;
 static float angle_raw = 0.0;
+static float enc_angle = 0.0;
 
 /*************<prototype>**************/
-void SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara);
-ERROR_Type SPI_TransmitReceivePoll_V2(SPI_Type *SPIx, uint8_t *rxBuffer, const uint8_t *txBuffer, uint32_t length, uint32_t timeout);
-uint8_t SPI_CRC8(uint8_t *message, uint8_t Bytelength );
+static void SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara);
+static uint8_t SPI_CRC8(uint8_t *message, uint8_t Bytelength );
+static float Parse_EncAngle(void);
+
+extern void Process_MotorControl(float angle);
 
 void AngleSensor_Init(void)
 {
@@ -33,7 +36,7 @@ void AngleSensor_Init(void)
 	/*清零配置结构体变量.*/
 	memset(&spiConfig, 0x00, sizeof(spiConfig));
 	
-	/*初始化SPI参数,波特率 = 0.8Mbps = (F_BCLK / (SCK_LOW+1 + SCK_HIGH+1)).*/
+	/*初始化SPI参数,波特率 = 2Mbps = (F_BCLK / (SCK_LOW+1 + SCK_HIGH+1)).*/
 	spiConfig.csSetup = 4;/*片选建立时间  = (CS_SETUP + 1) * CLK_PERIOD.*/
 	spiConfig.csHold  = 4;/*片选保持时间  = (CS_HOLD + 1) * CLK_PERIOD.*/
 	spiConfig.sckHigh = 5;/*SCK高电平时间 = (SCK_HIGH + 1) * CLK_PERIOD.*/
@@ -45,8 +48,8 @@ void AngleSensor_Init(void)
 	spiConfig.frmSize 		= SPI_FRAME_SIZE_16BITS;
 	spiConfig.rxMsbFirstEn	= ENABLE;//选择从最高位开始接收
 	spiConfig.txMsbFirstEn	= ENABLE;//选择从最高位开始发送
-	spiConfig.csOutputEn	= ENABLE;//CS有SPI硬件控制
-	spiConfig.continuousCSEn= ENABLE;//片选连续模式
+	spiConfig.csOutputEn	= DISABLE;//CS有SPI硬件控制
+	spiConfig.continuousCSEn= DISABLE;//片选连续模式
 	spiConfig.dmaRxEn		= DISABLE;//禁止使用DMA接收数据
 	spiConfig.dmaTxEn		= DISABLE;//禁止使用DMA发送数据
 	spiConfig.modeFaultEn	= DISABLE;//模式故障禁止
@@ -59,9 +62,9 @@ void AngleSensor_Init(void)
 	spiConfig.modeFaultInterruptEn = DISABLE;//模式故障中断
 	SPI_Init(SPI0, &spiConfig);
 	
-//	NVIC_SetPriority(SPI0_IRQn, 1);
-//  NVIC_ClearPendingIRQ(SPI0_IRQn);
-//  NVIC_EnableIRQ(SPI0_IRQn);
+	NVIC_SetPriority(SPI0_IRQn, 1);
+  NVIC_ClearPendingIRQ(SPI0_IRQn);
+  NVIC_EnableIRQ(SPI0_IRQn);
 	
 
 }
@@ -71,11 +74,9 @@ void AngleSensor_Setoffset(float offset)
 	angle_offset = offset;
 }
 
-float AngleSensor_GetAngleInt(void)
-{
-	uint16_t angle_value;
-	float angle = 0.0;
-			
+
+void AngleSensor_GetAngleInt(void)
+{	
 	g_spiTxBuff1[0] = 0x21;
 	g_spiTxBuff1[1] = 0x80;
 	g_spiTxBuff1[2] = 0xFF;
@@ -83,43 +84,21 @@ float AngleSensor_GetAngleInt(void)
 	g_spiTxBuff1[4] = 0xFF;
 	g_spiTxBuff1[5] = 0xFF;
 	
-	SPI_TransmitReceiveInt(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
-	while(!(SPI_GetTransmitReceiveStatus(SPI0) & SPI_STATUS_RX_FINISH_MASK))//等待读取完毕
-	{
-	
-	};
-	SPI_MasterReleaseCS(SPI0);
-
-	printf("g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", g_spiRxBuff1[0], g_spiRxBuff1[1]);
-	printf("g_spiRxBuff1[2]:0x%x, g_spiRxBuff1[3]:0x%x \r\n", g_spiRxBuff1[2], g_spiRxBuff1[3]);
-	printf("g_spiRxBuff1[4]:0x%x, g_spiRxBuff1[5]:0x%x \r\n", g_spiRxBuff1[4], g_spiRxBuff1[5]);
-//	
-	angle_value = g_spiRxBuff1[3]&0x7F;
-	angle_value = (angle_value<<8)|g_spiRxBuff1[2];
-	angle = (360.0*angle_value)/32768;
-
-	if((g_spiRxBuff1[5]&0x30) != 0x30){
-		angle = -180.0;
-		printf("angle_raw 111 : %f \r\n", angle);
-	}else{
-		angle_raw = angle;
-		printf("angle_raw 222 : %f \r\n", angle);
-		
-		angle += angle_offset;
-		if(angle > 360.0){
-			angle -= 360.0;
-		}
-		
-		if(angle < 0.0){
-			angle += 360.0;
-		}
-		
-	}
+	memset(g_spiRxBuff1, 0x00, 6);
 	
-	return angle;
+	ENC_CS_L;
+	SPI_TransmitReceiveInt(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
+//	while(!(SPI_GetTransmitReceiveStatus(SPI0) & SPI_STATUS_RX_FINISH_MASK));//等待读取完毕
+//	SPI_MasterReleaseCS(SPI0);
+//	ENC_CS_H;
 	
+//	printf("22g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", g_spiRxBuff1[0], g_spiRxBuff1[1]);
+//	printf("22g_spiRxBuff1[2]:0x%x, g_spiRxBuff1[3]:0x%x \r\n", g_spiRxBuff1[2], g_spiRxBuff1[3]);
+//	printf("22g_spiRxBuff1[4]:0x%x, g_spiRxBuff1[5]:0x%x \r\n", g_spiRxBuff1[4], g_spiRxBuff1[5]);
+
 }
 
+
 float AngleSensor_GetAnglePolling(void)
 {
 	uint16_t angle_value;
@@ -137,8 +116,7 @@ float AngleSensor_GetAnglePolling(void)
 	
 	memset(g_spiRxBuff1, 0x00, 6);
 	
-	ret = SPI_TransmitReceivePoll_V2(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6, 2000);
-	//ret = SPI_TransmitReceivePoll(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
+	ret = SPI_TransmitReceivePoll(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
 	
 	if(SUCCESS == ret){
 		
@@ -151,9 +129,9 @@ float AngleSensor_GetAnglePolling(void)
 			crc = SPI_CRC8(g_spiTxBuff1, 4);
 			if(g_spiRxBuff1[4] == crc){
 				
-//				printf("crc22 %x: g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", crc, g_spiRxBuff1[0], g_spiRxBuff1[1]);
-//				printf("g_spiRxBuff1[2]:0x%x, g_spiRxBuff1[3]:0x%x \r\n", g_spiRxBuff1[2], g_spiRxBuff1[3]);
-//				printf("g_spiRxBuff1[4]:0x%x, g_spiRxBuff1[5]:0x%x \r\n", g_spiRxBuff1[4], g_spiRxBuff1[5]);
+				printf("crc22 %x: g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", crc, g_spiRxBuff1[0], g_spiRxBuff1[1]);
+				printf("g_spiRxBuff1[2]:0x%x, g_spiRxBuff1[3]:0x%x \r\n", g_spiRxBuff1[2], g_spiRxBuff1[3]);
+				printf("g_spiRxBuff1[4]:0x%x, g_spiRxBuff1[5]:0x%x \r\n", g_spiRxBuff1[4], g_spiRxBuff1[5]);
 //				
 				if((g_spiRxBuff1[5]&0x30) != 0x30){
 //					printf("angle_raw 111 : %f \r\n", angle);
@@ -177,7 +155,9 @@ float AngleSensor_GetAnglePolling(void)
 					}
 				}
 			
-			}	
+			}else{
+				angle = -180.0;
+			}
 		
 	}else{
 		angle = -180.0;
@@ -186,10 +166,25 @@ float AngleSensor_GetAnglePolling(void)
 	return angle;
 }
 
+void AngleSensor_Read(void)
+{
+	g_spiTxBuff1[0] = 0x21;
+	g_spiTxBuff1[1] = 0x80;
+	g_spiTxBuff1[2] = 0xFF;
+	g_spiTxBuff1[3] = 0xFF;
+	g_spiTxBuff1[4] = 0xFF;
+	g_spiTxBuff1[5] = 0xFF;
+	
+	memset(g_spiRxBuff1, 0x00, 6);
+	
+	ENC_CS_L;
+	SPI_TransmitReceiveInt(SPI0, g_spiRxBuff1, g_spiTxBuff1, 6);
+	
+}
+
 float AngleSensor_GetAngle(void)
 {
-	//return AngleSensor_GetAngleInt();
-	return AngleSensor_GetAnglePolling();
+	return enc_angle;
 }
 
 float AngleSensor_GetAngleRaw(void)
@@ -199,7 +194,7 @@ float AngleSensor_GetAngleRaw(void)
 
 void AngleSensor_PrintInfo(void)
 {
-	printf("AngleSensor angle:%f \r\n", AngleSensor_GetAnglePolling());
+	printf("AngleSensor angle:%f \r\n", AngleSensor_GetAngle());
 
 }
 
@@ -208,6 +203,67 @@ void AngleSensor_DeInit(void)
 	SPI_DeInit(SPI0);
 }
 
+void Angle_Correct(float angle)
+{
+	if(angle < 0){
+		angle_raw = angle;
+		enc_angle = angle;
+		
+	}else{
+		
+		angle_raw = angle;
+		enc_angle = angle + angle_offset;
+		if(enc_angle > 360.0){
+			enc_angle -= 360.0;
+		}
+		
+		if(enc_angle < 0.0){
+			enc_angle += 360.0;
+		}
+	
+	}
+	
+}
+
+float Parse_EncAngle(void)
+{
+	uint16_t angle_value;
+	uint8_t crc;
+	float angle = 0.0;
+	
+		//复用txbuff
+		g_spiTxBuff1[0] = 0x80;
+		g_spiTxBuff1[1] = 0x21;
+		g_spiTxBuff1[2] = g_spiRxBuff1[3];
+		g_spiTxBuff1[3] = g_spiRxBuff1[2];
+	
+		crc = SPI_CRC8(g_spiTxBuff1, 4);
+	
+//		printf("crc %x: g_spiRxBuff1[0]:0x%x, g_spiRxBuff1[1]:0x%x \r\n", crc, g_spiRxBuff1[0], g_spiRxBuff1[1]);
+//		printf("g_spiRxBuff1[2]:0x%x, g_spiRxBuff1[3]:0x%x \r\n", g_spiRxBuff1[2], g_spiRxBuff1[3]);
+//		printf("g_spiRxBuff1[4]:0x%x, g_spiRxBuff1[5]:0x%x \r\n", g_spiRxBuff1[4], g_spiRxBuff1[5]);
+		
+	 if(g_spiRxBuff1[4] == crc){
+			
+			if((g_spiRxBuff1[5]&0x30) != 0x30){ //safeword  
+				angle = -180.0;
+			}else{
+				
+				angle_value = g_spiRxBuff1[3]&0x7F;
+				angle_value = (angle_value<<8)|g_spiRxBuff1[2];
+				angle = (360.0*angle_value)/32768;
+				
+			}
+		
+		}else{
+			angle = -180.0;
+		}
+		
+		
+		return angle;
+		
+}
+
 /**
 * @prototype SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara)
 *
@@ -218,6 +274,8 @@ void AngleSensor_DeInit(void)
 */
 void SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara)
 {
+	//printf("SPI0_Callback wpara=0x%x, lpara=0x%x  \r\n", wpara, lpara);
+	
 	if (wpara & SPI_STATUS_TXUF_Msk)
 	{
 		//TX下溢处理
@@ -227,150 +285,25 @@ void SPI0_Callback(void *device, uint32_t wpara, uint32_t lpara)
 		//RX溢出处理
 	}
 	
-}
+	if(SPI_GetTransmitReceiveStatus(SPI0) & SPI_STATUS_RX_FINISH_MASK) //等待读取完毕
+	{
+		//printf("SPI0_Callback SPI_STATUS_RX_FINISH_MASK  \r\n");
 
-/*!
-* @brief Clear SPI Tx under flow and Rx over flow status
-*
-* @param[in] SPIx: SPI type pointer,x can be 0 to 1
-* @return Function status
-*            0: ERROR, occour Tx under flow or Rx over flow flag
-*            1: SUCCESS, no Tx under flow and Rx over flow flag
-*/
-static ERROR_Type MY_SPI_ClearTxUFRxOF(SPI_Type *SPIx)
-{
-    ERROR_Type ret = SUCCESS;
-
-    /* Clear Tx under flow flag */
-    if (SPI_IsTxUF(SPIx))
-    {
-        SPI_ClearTxUF(SPIx);
-        ret = ERROR;
-    }
-    else
-    {
-        /* Do nothing */
-    }
-
-    /* Clear Rx over flow flag */
-    if (SPI_IsRxOF(SPIx))
-    {
-        SPI_ClearRxOF(SPIx);
-        ret = ERROR;
-    }
-    else
-    {
-        /* Do nothing */
-    }
-
-    return ret;
+		ENC_CS_H;
+		Angle_Correct(Parse_EncAngle());
+		
+		Process_MotorControl(enc_angle);
+	}
+	
 }
 
 
-/*!
-* @brief SPI transmission,reception by polling
-*
-* @param[in] SPIx: SPI type pointer,x can be 0 to 1
-* @param[in] rxBuffer: point to the receive data
-* @param[in] txBuffer: point to the send data
-* @param[in] length: transfer data length
-* @param[in] timeout: timeout us 
-* @return Function status
-*            0: ERROR, length is 0 or rdbuff is NULL or txBuffer is NULL
-*            1: SUCCESS
-*/
-ERROR_Type SPI_TransmitReceivePoll_V2(SPI_Type *SPIx, uint8_t *rxBuffer, const uint8_t *txBuffer, uint32_t length, uint32_t timeout)
-{
-    uint32_t  i = 0;
-		uint32_t  _time_us = 0;
-    ERROR_Type ret = SUCCESS;
-
-    DEVICE_ASSERT(IS_SPI_PERIPH(SPIx));
-
-    if ((length == 0) || (rxBuffer == NULL) || (txBuffer == NULL))
-    {
-        ret = ERROR;
-    }
-    else
-    {
-        /* Disable Tx/Rx only mode */
-        SPI_SetTxOnly(SPIx, DISABLE);
-        SPI_SetRxOnly(SPIx, DISABLE);
-
-        if ((SPI_FrameSizeType)SPI_GetFRMSize(SPIx) > SPI_FRAME_SIZE_8BITS)  /* FrameSize is 9 bits ~ 16 bits */
-        {
-            if (((uint32_t)txBuffer & 0x01) || ((uint32_t)rxBuffer & 0x01)) /* txBuffer or rxBufer is not half-word alignment */
-            {
-                ret = ERROR;
-                return ret;
-            }
-
-            length = length >> 0x01u;
-            /* transmit and receive data */
-            for (i = 0; i < length; i++)
-            {
-                while (!SPI_IsTxEF(SPIx) &&(_time_us < timeout))
-								{
-										udelay(1);
-										_time_us++;
-								}
-                SPI_WriteDataReg(SPIx, ((uint16_t *)txBuffer)[i]);
-                while (!SPI_IsRxFF(SPIx) &&(_time_us < timeout))
-								{
-										udelay(1);
-										_time_us++;
-								}
-                ((uint16_t *)rxBuffer)[i] = SPI_ReadDataReg(SPIx);
-            }
-        }
-        else  /* FrameSize is 4 bits ~ 8 bits */
-        {
-            /* transmit and receive data */
-            for (i = 0; i < length; i++)
-            {
-                while (!SPI_IsTxEF(SPIx) &&(_time_us < timeout))
-								{
-										udelay(1);
-										_time_us++;
-								}
-                SPI_WriteDataReg(SPIx, txBuffer[i]);
-                while (!SPI_IsRxFF(SPIx) &&(_time_us < timeout))
-								{
-										udelay(1);
-										_time_us++;
-								}
-                rxBuffer[i] = (uint8_t)SPI_ReadDataReg(SPIx);
-            }
-        }
-        while ((SPI_IsBusy(SPIx)));
-        SPI_CSRelease(SPIx);
-    }
-
-    /* Check and Clear Tx under flow/ Rx over flow flag */
-    if (MY_SPI_ClearTxUFRxOF(SPIx) == ERROR)
-    {
-        ret = ERROR;
-    }
-    else
-    {
-			if((_time_us >= timeout)){
-				ret = ERROR;
-			}
-        /* Do nothing */
-    }
-
-    return ret;
-}
 
 //CRC对照表
 const uint8_t SPI_TableCRC[256] = 
 {
-	//The ?°crc?± of the position [1] (result from operation [crc ^*(message+Byteidx)])
-	//is 0x00 -> 0x00 XOR 0x11D = 0x00 (1 byte).
 	0x00,
-	//The ?°crc?± of the position [2] is 0x1D -> 0x01 XOR 0x11D = 0x1D (1 byte).
 	0x1D,
-	//The ?°crc?± of the position [3] is 0x3A -> 0x02 XOR 0x11D = 0x3A (1 byte).
 	0x3A,
 	//For all the rest of the cases.
 	0x27, 0x74, 0x69, 0x4E, 0x53, 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB, 0xCD,
@@ -400,26 +333,17 @@ const uint8_t SPI_TableCRC[256] =
 //配置
 uint8_t SPI_CRC8(uint8_t *message, uint8_t Bytelength )
 {
-	//?°crc?± defined as the 8-bits that will be generated through the message till the
-	//final crc is generated. In the example above this are the blue lines out of the
-	//XOR operation.
+
 	uint8_t crc;
-	//?°Byteidx?± is a counter to compare the bytes used for the CRC calculation and
-	//?°Bytelength?±.
 	uint8_t Byteidx;
 	//Initially the CRC remainder has to be set with the original seed (0xFF for the TLE5012B).
 	crc = 0xFF;
 	//For all the bytes of the message.
 	for(Byteidx=0; Byteidx<Bytelength; Byteidx++)
 	{
-		//?°crc?± is the value in the look-up table TableCRC[x] at the position ?°x?±.
-		//The position ?°x?± is determined as the XOR operation between the previous ?°crc?± and
-		//the next byte of the ?°message?±.
-		//?°^?± is the XOR operator.
 		crc = SPI_TableCRC[crc ^ *(message+Byteidx)];
 	}
-	//Return the inverted ?°crc?± remainder(?°~?± is the invertion operator). An alternative
-	//to the ?°~?± operator would be a XOR operation between ?°crc?± and a 0xFF polynomial.
+
 	return(~crc);
 }
 

Device/AngleSensor.h

@@ -48,6 +48,7 @@ extern "C" {
 
 void AngleSensor_Init(void);
 void AngleSensor_Setoffset(float offset);
+void AngleSensor_Read(void);
 float AngleSensor_GetAngle(void);
 float AngleSensor_GetAngleRaw(void);
 void AngleSensor_PrintInfo(void);

Device/IB_Reader.c

@@ -4,7 +4,7 @@
 #define  IB_READ_ROM    (0x33)
 #define  IB_MATCH_ROM   (0x55)
 #define  IB_SEARCH_ROM  (0xF0)
-#define  IB_SKIP_ROM    (0x33)
+#define  IB_SKIP_ROM    (0xCC)
 
 #define  IB_WRITE_SCRATCHPAD  (0x0F)
 #define  IB_READ_SCRATCHPAD   (0xAA)
@@ -12,7 +12,7 @@
 #define  IB_READ_MEMORY       (0xF0)
 
 
-static uint8_t rom_id[8]={0};
+static uint8_t rom_id[16]={0};
 
 /**
    *   @brief   初始化
@@ -138,30 +138,35 @@ return 0;
 
 uint8_t IBread_ROMID(void)
 {
-	uint8_t Ackbit = 0;
+	uint8_t Ackbit = 1;
 	uint8_t i=0;
 	Ackbit = OneWire_Init();		//总线初始化时序
-	OneWire_SenByte(IB_READ_ROM);		//发送读ROMID指令
 	
-	for(i=0; i<8;i++){
-		rom_id[i]=OneWire_ReceiveByte();		//第一个字节为低位
+	if(0 == Ackbit){
+		OneWire_SenByte(IB_READ_ROM);		//发送读ROMID指令
+	
+		for(i=0; i<8;i++){
+			rom_id[i]=OneWire_ReceiveByte();		//第一个字节为低位
+		}
 	}
 
+
 return 0;
 	
 }
 
 uint8_t IBread_Memory(void)
 {
-	uint8_t Ackbit = 0;
+	uint8_t Ackbit = 1;
 	uint8_t i=0;
 	Ackbit = OneWire_Init();		//总线初始化时序
+	
 	OneWire_SenByte(IB_SKIP_ROM);	
 	OneWire_SenByte(IB_READ_MEMORY);	
 	OneWire_SenByte(0x00);	
 	OneWire_SenByte(0x00);	
 	
-	for(i=0; i<8;i++){
+	for(i=0; i<16;i++){
 		rom_id[i]=OneWire_ReceiveByte();		//第一个字节为低位
 	}
 
@@ -172,7 +177,7 @@ return 0;
 
 void IB_Print(void)
 {
-	IBread_Memory();
+	IBread_ROMID();
 	
 	printf("memory:[%x][%x][%x][%x][%x][%x][%x][%x] \r\n", rom_id[0],rom_id[1],rom_id[2],rom_id[3],rom_id[4],rom_id[5],rom_id[6],rom_id[7]);
 

Device/Motor.c

@@ -147,6 +147,8 @@ void Motor_Init(void)
 
     PWM_Init(PWM1, &pwmConfig);	
 		NVIC_SetPriority(PWM1_IRQn, 0); //设置PWM模块中断的优先级
+		
+		Motor_Brake();
 }
 
 
@@ -154,13 +156,21 @@ void Motor_Positive(uint8_t speed)
 {
 	uint16_t chValue=0;
 	if(speed > 100) speed = 100;
-	//speed = 100 -speed;
-		 
+	speed = 100 -speed;
+	
 	chValue = (uint16_t)speed*(MOD_PWM-1)/100;
+	//printf("Motor_Positive: speed = %d \r\n", speed);
 	
-	printf("Motor_Positive, speed:%d \r\n", speed);
-	PWM_SetChannelCountValue(PWM1, PWM_CH_3, 0);
-	PWM_SetChannelCountValue(PWM1, PWM_CH_2, chValue);
+	PWM_SetChannelTrigger(PWM1, PWM_CH_2, DISABLE);
+	PWM_SetChannelTrigger(PWM1, PWM_CH_3, ENABLE);
+	
+
+	PWM_SetChannelCountValue(PWM1, PWM_CH_3, chValue);
+	PWM_SetChannelCountValue(PWM1, PWM_CH_2, MOD_PWM);
+
+	
+//	PWM_SetChannelCountValue(PWM1, PWM_CH_3, 0);
+//	PWM_SetChannelCountValue(PWM1, PWM_CH_2, chValue);
 	
 }
 
@@ -169,14 +179,32 @@ void Motor_Negative(uint8_t speed)
 {
 	uint16_t chValue=0;
 	if(speed > 100) speed = 100;
-	//speed = 100 -speed;
+	speed = 100 -speed;
 		 
-	chValue = (uint16_t)speed*MOD_PWM/100;
-	printf("Motor_Negative, speed:%d \r\n", speed);
+	chValue = (uint16_t)speed*(MOD_PWM-1)/100;
 	
-	PWM_SetChannelCountValue(PWM1, PWM_CH_2, 0);
-	PWM_SetChannelCountValue(PWM1, PWM_CH_3, chValue);
+	//printf("Motor_Negative: speed = %d \r\n", speed);
+	
+	PWM_SetChannelTrigger(PWM1, PWM_CH_3, DISABLE);
+	PWM_SetChannelTrigger(PWM1, PWM_CH_2, ENABLE);
+	
+
+	PWM_SetChannelCountValue(PWM1, PWM_CH_2, chValue);
+	PWM_SetChannelCountValue(PWM1, PWM_CH_3, MOD_PWM);
+	
+//	PWM_SetChannelCountValue(PWM1, PWM_CH_2, 0);
+//	PWM_SetChannelCountValue(PWM1, PWM_CH_3, chValue);
+	
+}
+
+//电机刹车 
+void Motor_Brake(void)
+{
+	PWM_SetChannelTrigger(PWM1, PWM_CH_3, DISABLE);
+	PWM_SetChannelTrigger(PWM1, PWM_CH_2, DISABLE);
 	
+	PWM_SetChannelCountValue(PWM1, PWM_CH_2, MOD_PWM);
+	PWM_SetChannelCountValue(PWM1, PWM_CH_3, MOD_PWM);
 }
 
 //电机停止转动 

Device/Motor.h

@@ -47,16 +47,25 @@ extern "C" {
 
 //#define PRESCALER   				        (PWDT_CLK_PRESCALER_8)
 #define APB_CLK                             (APB_BUS_FREQ)
-#define FREQ                                (2000) ///freq = 20KHz
+#define FREQ                                (20000) ///freq = 20KHz
 #define PWM_PRES                            (0)
 #define MOD_PWM                             (APB_CLK / FREQ / (PWM_PRES + 1))
 
 
+#define MOTOR_TUNR_NULL  (0)
+#define MOTOR_TUNR_P     (1)
+#define MOTOR_TUNR_N     (2)
+
+
 void Motor_Init(void);
 //设置电机正转速度 , speed 取值 0--100；
 void Motor_Positive(uint8_t speed);
 //设置电机反转速度 , speed 取值 0--100；
 void Motor_Negative(uint8_t speed);
+
+//电机刹车 
+void Motor_Brake(void);
+
 //电机停止转动 
 void Motor_Stop(void);
 

Device/adc.c

@@ -109,7 +109,7 @@ void ADC_DMAInit(void)
 
     DMA_Init(DMA0_CHANNEL2, &tmpDMAConfig);                     		 ///<ADC 使用DMA1通道，每个模块对应的DMA通道，可参考 AC781X芯片手册 表20-1 DMA请求列表
     
-		NVIC_SetPriority(DMA0_CHANNEL2_IRQn, 3);
+		NVIC_SetPriority(DMA0_CHANNEL2_IRQn, 2);
 		NVIC_ClearPendingIRQ(DMA0_CHANNEL2_IRQn);
 		NVIC_EnableIRQ(DMA0_CHANNEL2_IRQn);                              ///<使能DMA1中断请求
 }
@@ -131,8 +131,8 @@ void CTU_Config(void)
     ctuConfig.uart0RxCaptureEn = DISABLE;  //去能UART0_RX捕获
     ctuConfig.uartTxModulateEn = DISABLE;  //去能UART0_TX调制
     ctuConfig.clkPsc = CTU_CLK_PRESCALER_1;  //分频
-    ctuConfig.adcRegularTriggerSource = CTU_TRIGGER_ADC_TIMER_CH0_OVERFLOW; //Timer0触发ADC规则组采样。
-    ctuConfig.delay0Time = 0;  //触发延迟
+    ctuConfig.adcRegularTriggerSource = CTU_TRIGGER_ADC_PWM1_MATCH; //Timer0触发ADC规则组采样。
+    ctuConfig.delay0Time = 3;  //触发延迟
 //    ctuConfig.adcInjectTriggerSource = CTU_TRIGGER_ADC_PWM0_INIT;  // 
 //    ctuConfig.delay1Time = 0;
 //    ctuConfig.pwdt0In3Source = CTU_PWDT_IN3_SOURCE_UART0_RX;
@@ -226,10 +226,16 @@ float getMotorCurrent(void)
 	uint8_t i;
 	for(i=0; i<ADC_FILTER_NUM; i++){
 		sum += g_ADCValueBuffer[2*i+1];
+		g_ADCValueBuffer[2*i+1] = 0;
 	}
 	
 	sum = sum/ADC_FILTER_NUM;
-	return ((3.3*sum)/4096);
+	return (4*(3.3*sum)/4096);
+}
+
+void printMotorCurrent(void)
+{
+	printf("motorCurrent:%f \r\n", getMotorCurrent());
 }
 
 /**

Device/adc.h

@@ -51,7 +51,7 @@ extern "C" {
 
 
 #define ADC_SAMPLE_CHANNEL               (2)
-#define ADC_FILTER_NUM               		 (8)
+#define ADC_FILTER_NUM               		 (20)
 
 //void  ADC_DMACallback(void *device, uint32_t wpara, uint32_t lpara);
 //extern uint32_t g_ADCValueBuffer[ADC_SAMPLE_CHANNEL];
@@ -60,6 +60,7 @@ float getBatteryVoltage(void);
 float getMotorCurrent(void);
 
 void printADCValue(void);
+void printMotorCurrent(void);
 
 
 #ifdef __cplusplus

Device/gpio.c

@@ -118,7 +118,10 @@ void Gpio_Init(void)
 		GPIO_SetFunc(GPIOB, GPIO_PIN5, GPIO_FUN3);//SCK
 		GPIO_SetFunc(GPIOB, GPIO_PIN4, GPIO_FUN3);//MISO
 		GPIO_SetFunc(GPIOA, GPIO_PIN7, GPIO_FUN3);//MOSI
-		GPIO_SetFunc(GPIOA, GPIO_PIN8, GPIO_FUN3);//CS
+		//GPIO_SetFunc(GPIOA, GPIO_PIN8, GPIO_FUN3);//CS
+		
+		GPIO_SetFunc(GPIOA, GPIO_PIN8, GPIO_FUN0); //ƬѡÓÃGPIO ¿ØÖÆ 
+		GPIO_SetDir(GPIOA, GPIO_PIN8, GPIO_OUT);
 		
 		/*³õʼ»¯RTC  ds1307*/
 //		GPIO_SetFunc(GPIOB, GPIO_PIN13, GPIO_FUN3);//I2C1_SCL
@@ -130,6 +133,7 @@ void Gpio_Init(void)
 		//Ibutton one wire 
 		GPIO_SetFunc(IBTX_PORT, IBTX_PIN, GPIO_FUN0);
 		GPIO_SetDir(IBTX_PORT, IBTX_PIN, GPIO_OUT);
+		
 		GPIO_SetFunc(IBRX_PORT, IBRX_PIN, GPIO_FUN0);
 		GPIO_SetDir(IBRX_PORT, IBRX_PIN, GPIO_IN);
 }

Device/gpio.h

@@ -103,6 +103,9 @@ extern "C" {
 #define IBTX_WIRE_LOW				GPIO_SetPinLevel(IBTX_PORT, IBTX_PIN, GPIO_LEVEL_HIGH)
 #define IBRX_WIRE_BIT     GPIO_GetPinLevel(IBRX_PORT,IBRX_PIN)
 
+#define ENC_CS_L                   do{GPIO_SetPinLevel(GPIOA, GPIO_PIN8, GPIO_LEVEL_LOW);}while(0)
+#define ENC_CS_H                   do{GPIO_SetPinLevel(GPIOA, GPIO_PIN8, GPIO_LEVEL_HIGH);}while(0)
+
 /*************<enum>*******************/
 
 

Device/timer.c

@@ -38,8 +38,8 @@
 /*************<include>****************/
 #include "string.h"
 //#include "gpio.h"
-#include "process.h"
-#include "uart.h"
+//#include "process.h"
+//#include "uart.h"
 #include "timer.h"
 #include "W25Q64.h"
 
@@ -61,6 +61,10 @@ extern uint32_t g_timer0Cnt;
 /*************<prototype>**************/
 void Timer_Callback(void *device, uint32_t wpara, uint32_t lpara);
 
+extern void Process_Timer0_CB(void);
+extern void Process_Timer1_CB(void);
+extern void Rs485_Timer1_CB(void);
+
 /**
 * @prototype Timer0_Init(void)
 *
@@ -79,7 +83,7 @@ void Timer0_Init(void)
 	/*ÅäÖö¨Ê±Æ÷.*/
 	tmrConfig.linkModeEn	= DISABLE;
 	tmrConfig.interruptEn	= ENABLE;	
-	tmrConfig.periodValue	= TIM_PRD_10ms;
+	tmrConfig.periodValue	= TIM_PRD_5ms;
 	tmrConfig.timerEn		= ENABLE;
 	tmrConfig.callBack 		= Timer_Callback;
 	TIMER_Init(TIMER_CHANNEL0, &tmrConfig);
@@ -114,7 +118,7 @@ void Timer1_Init(void)
 	tmrConfig.callBack 		= Timer_Callback;
 	TIMER_Init(TIMER_CHANNEL1, &tmrConfig);
 	
-	NVIC_SetPriority(TIMER_CHANNEL1_IRQn, 2);
+	NVIC_SetPriority(TIMER_CHANNEL1_IRQn, 1);
   NVIC_ClearPendingIRQ(TIMER_CHANNEL1_IRQn);
   NVIC_EnableIRQ(TIMER_CHANNEL1_IRQn);
 }
@@ -131,9 +135,13 @@ void Timer_Callback(void *device, uint32_t wpara, uint32_t lpara)
 {
 		if (TIMER_CHANNEL0 == device){
 				g_timer0Cnt++;
-				Process_Timer10msCB();
+				Process_Timer0_CB();
 		}else if(TIMER_CHANNEL1 == device){
 			
+				Process_Timer1_CB();
+				Rs485_Timer1_CB();
+			
+			/*
 			if (g_blinkLedTime < 0xFFFF)
 			{
 					g_blinkLedTime++;
@@ -161,6 +169,7 @@ void Timer_Callback(void *device, uint32_t wpara, uint32_t lpara)
 				g_period1000ms++;
 			}
 	
+			*/
 	
 		}
 

Device/uart.c

@@ -52,6 +52,13 @@ uint32_t dmaTxBuf[UART_TRANSMIT_DATA_POOL_COUNT>>2];
 int TransmitData(uint8_t *pdata, uint16_t length);
 
 
+void Rs485_Timer1_CB(void)
+{
+		if(rs485_info.dmasend_count < 0xFF){
+					rs485_info.dmasend_count++;
+		}
+}
+
 /**
 * UARTTX_DMA_EventCallback
 *

Project/AirControlValve.uvoptx

@@ -380,6 +380,30 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>1</GroupNumber>
+      <FileNumber>16</FileNumber>
+      <FileType>5</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\motor_control.h</PathWithFileName>
+      <FilenameWithoutPath>motor_control.h</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>1</GroupNumber>
+      <FileNumber>17</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\motor_control.c</PathWithFileName>
+      <FilenameWithoutPath>motor_control.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -390,7 +414,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>16</FileNumber>
+      <FileNumber>18</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -402,7 +426,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>17</FileNumber>
+      <FileNumber>19</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -414,7 +438,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>18</FileNumber>
+      <FileNumber>20</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -426,7 +450,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>19</FileNumber>
+      <FileNumber>21</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -438,7 +462,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>20</FileNumber>
+      <FileNumber>22</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -450,7 +474,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>21</FileNumber>
+      <FileNumber>23</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -462,7 +486,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>22</FileNumber>
+      <FileNumber>24</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -474,7 +498,7 @@
     </File>
     <File>
       <GroupNumber>2</GroupNumber>
-      <FileNumber>23</FileNumber>
+      <FileNumber>25</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -494,7 +518,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>24</FileNumber>
+      <FileNumber>26</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -506,7 +530,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>25</FileNumber>
+      <FileNumber>27</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -518,7 +542,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>26</FileNumber>
+      <FileNumber>28</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -530,7 +554,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>27</FileNumber>
+      <FileNumber>29</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -542,7 +566,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>28</FileNumber>
+      <FileNumber>30</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -554,7 +578,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>29</FileNumber>
+      <FileNumber>31</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -566,7 +590,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>30</FileNumber>
+      <FileNumber>32</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -578,7 +602,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>31</FileNumber>
+      <FileNumber>33</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -590,7 +614,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>32</FileNumber>
+      <FileNumber>34</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -602,7 +626,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>33</FileNumber>
+      <FileNumber>35</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -614,7 +638,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>34</FileNumber>
+      <FileNumber>36</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -626,7 +650,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>35</FileNumber>
+      <FileNumber>37</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -638,7 +662,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>36</FileNumber>
+      <FileNumber>38</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -650,7 +674,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>37</FileNumber>
+      <FileNumber>39</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -662,7 +686,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>38</FileNumber>
+      <FileNumber>40</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -674,7 +698,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>39</FileNumber>
+      <FileNumber>41</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -686,7 +710,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>40</FileNumber>
+      <FileNumber>42</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -698,7 +722,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>41</FileNumber>
+      <FileNumber>43</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -710,7 +734,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>42</FileNumber>
+      <FileNumber>44</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -722,7 +746,7 @@
     </File>
     <File>
       <GroupNumber>3</GroupNumber>
-      <FileNumber>43</FileNumber>
+      <FileNumber>45</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

Project/AirControlValve.uvprojx

@@ -460,6 +460,16 @@
               <FileType>1</FileType>
               <FilePath>..\User\pid.c</FilePath>
             </File>
+            <File>
+              <FileName>motor_control.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\User\motor_control.h</FilePath>
+            </File>
+            <File>
+              <FileName>motor_control.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\motor_control.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>

User/main.c

@@ -50,7 +50,7 @@ int main(void)
 		//Storage_Init();
 
 		Process_Init();
-		Timer0_Init(); 
+		//Timer0_Init(); 
 		Timer1_Init();
     Task_Init();
 		printf("111111 \r\n ");

User/motor_control.c

@@ -0,0 +1,114 @@
+#include "pid.h"
+#include "motor_control.h"
+#include "math.h"
+
+PID pid_location;
+PID pid_speed;
+PID pid_current;
+
+float motor_speed = 0.0; //电机速度 
+float prev_angle = 0;
+float dt = 0;
+float control_val = 0;
+
+
+float location_out = 0;
+float speed_out = 0;
+float current_out = 0;
+float target_pos = 0;
+float current_pos = 0;
+float current_curr = 0;
+float current_speed = 0;
+
+uint8_t pwm_output = 0;
+
+
+/* 定义位置环PID参数相关宏 */
+#define  L_KP      0.3f             /* P参数 */
+#define  L_KI      0.00f             /* I参数 */
+#define  L_KD      0.00f             /* D参数 */
+/* 定义速度环PID参数相关宏 */
+#define  S_KP      0.5f             /* P参数 */
+#define  S_KI      0.02f             /* I参数 */
+#define  S_KD      0.0f             /* D参数 */
+/* 定义电流环PID参数相关宏 */
+#define  C_KP      8.00f             /* P参数 */
+#define  C_KI      4.0f             /* I参数 */
+#define  C_KD      1.00f             /* D参数 */
+
+#define  SMAPLSE_PID_SPEED   5     /* 采样周期 单位5ms */
+#define  TARGET_CURRENT_MAX  0.9 
+#define  TARGET_SPEED_MAX    0.8
+
+void MC_Init(float angle)
+{
+	
+	PID_Init(&pid_location, L_KP, L_KI, L_KD, 200, 100, -100);
+	PID_Init(&pid_speed, 		S_KP, S_KI, S_KD, 200, 200, -200);
+	PID_Init(&pid_current, 	C_KP, C_KI, C_KD, 200, 100, 0);
+	
+	motor_speed = 0;
+	prev_angle = angle;
+	dt = 1.0*SMAPLSE_PID_SPEED/1000;
+	pwm_output = 50;
+	
+	 location_out = 0;
+	 speed_out = 0;
+	 current_out = 0;
+	 target_pos = 0;
+	 current_pos = 0;
+	 current_curr = 0;
+	 current_speed = 0;
+	
+}
+
+
+uint8_t MC_Calculate(float setvalue, float angle, float current)
+{
+		target_pos = setvalue;
+		current_pos = angle;
+		current_curr = current;
+		// 位置环
+		control_val = PID_Increment(&pid_location, setvalue, angle);
+		location_out = control_val;
+
+		/* 目标速度上限处理 */
+		if (control_val > TARGET_SPEED_MAX)
+		{
+				control_val = TARGET_SPEED_MAX;
+		}
+		else if (control_val < -TARGET_SPEED_MAX)
+		{
+				control_val = -TARGET_SPEED_MAX;
+		}
+	
+		//速度环
+		motor_speed = (angle - prev_angle);
+		prev_angle = angle;
+		
+		current_speed = motor_speed;
+		
+		control_val = PID_Increment(&pid_speed, control_val, motor_speed);
+		speed_out = control_val;
+
+		//电流预处理
+		if(control_val < 0 ){
+				control_val = -control_val;
+		}
+		
+		control_val = (control_val > TARGET_CURRENT_MAX) ? TARGET_CURRENT_MAX : control_val;    // 电流上限处理
+	
+		//电流环
+		control_val = PID_Increment(&pid_current, control_val, current);
+		current_out = control_val;
+
+		if(control_val > 100.0){
+			control_val = 100;
+		}else if(control_val < 10 ){
+			control_val = 10;
+		}
+		pwm_output = control_val;
+		
+		return pwm_output;
+
+}

User/motor_control.h

@@ -0,0 +1,60 @@
+
+/* Copyright Statement:
+ *
+ * This software/firmware and related documentation ("AutoChips Software") are
+ * protected under relevant copyright laws. The information contained herein is
+ * confidential and proprietary to AutoChips Inc. and/or its licensors. Without
+ * the prior written permission of AutoChips inc. and/or its licensors, any
+ * reproduction, modification, use or disclosure of AutoChips Software, and
+ * information contained herein, in whole or in part, shall be strictly
+ * prohibited.
+ *
+ * AutoChips Inc. (C) 2018. All rights reserved.
+ *
+ * BY OPENING THIS FILE, RECEIVER HEREBY UNEQUIVOCALLY ACKNOWLEDGES AND AGREES
+ * THAT THE SOFTWARE/FIRMWARE AND ITS DOCUMENTATIONS ("AUTOCHIPS SOFTWARE")
+ * RECEIVED FROM AUTOCHIPS AND/OR ITS REPRESENTATIVES ARE PROVIDED TO RECEIVER
+ * ON AN "AS-IS" BASIS ONLY. AUTOCHIPS EXPRESSLY DISCLAIMS ANY AND ALL
+ * WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE OR
+ * NONINFRINGEMENT. NEITHER DOES AUTOCHIPS PROVIDE ANY WARRANTY WHATSOEVER WITH
+ * RESPECT TO THE SOFTWARE OF ANY THIRD PARTY WHICH MAY BE USED BY,
+ * INCORPORATED IN, OR SUPPLIED WITH THE AUTOCHIPS SOFTWARE, AND RECEIVER AGREES
+ * TO LOOK ONLY TO SUCH THIRD PARTY FOR ANY WARRANTY CLAIM RELATING THERETO.
+ * RECEIVER EXPRESSLY ACKNOWLEDGES THAT IT IS RECEIVER'S SOLE RESPONSIBILITY TO
+ * OBTAIN FROM ANY THIRD PARTY ALL PROPER LICENSES CONTAINED IN AUTOCHIPS
+ * SOFTWARE. AUTOCHIPS SHALL ALSO NOT BE RESPONSIBLE FOR ANY AUTOCHIPS SOFTWARE
+ * RELEASES MADE TO RECEIVER'S SPECIFICATION OR TO CONFORM TO A PARTICULAR
+ * STANDARD OR OPEN FORUM. RECEIVER'S SOLE AND EXCLUSIVE REMEDY AND AUTOCHIPS'S
+ * ENTIRE AND CUMULATIVE LIABILITY WITH RESPECT TO THE AUTOCHIPS SOFTWARE
+ * RELEASED HEREUNDER WILL BE, AT AUTOCHIPS'S OPTION, TO REVISE OR REPLACE THE
+ * AUTOCHIPS SOFTWARE AT ISSUE, OR REFUND ANY SOFTWARE LICENSE FEES OR SERVICE
+ * CHARGE PAID BY RECEIVER TO AUTOCHIPS FOR SUCH AUTOCHIPS SOFTWARE AT ISSUE.
+ */
+
+/*************<start>******************/
+
+#ifndef MOTOR_CONTROLL_H__
+#define MOTOR_CONTROLL_H__
+
+#if 1
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#include "ac780x.h"
+
+
+void MC_Init(float angle);
+uint8_t MC_Calculate(float setvalue, float angle, float current);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif 
+
+#endif //MOTOR_CONTROLL_H__

User/pid.c

@@ -1,34 +1,42 @@
 #include "pid.h"
+
  
 //用于初始化pid参数的函数
-void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut)
+void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut, float minOut)
 {
     pid->kp = p;
     pid->ki = i;
     pid->kd = d;
     pid->maxIntegral = maxI;
     pid->maxOutput = maxOut;
+		pid->minOutput = minOut;
 	
 		pid->error = 0;
-		pid->lastError = 0;
+		pid->error1 = 0;
+		pid->error2 = 0;
 		pid->integral = 0;
 		pid->output = 0;
 }
  
-//进行一次pid计算
-//参数为(pid结构体,目标值,反馈值)，计算结果放在pid结构体的output成员中
-void PID_Calc(PID *pid, float reference, float feedback)
+
+//位置式PID
+//pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
+//setvalue : 设置值（期望值）
+//actualvalue: 实际值
+//由于全量输出，每次输出均与过去状态有关，计算时要对ek累加，计算量大
+
+float PID_Location(PID *pid, float setvalue, float feedback, float dt)
 {
  	//更新数据
-    pid->lastError = pid->error; //将旧error存起来
-    pid->error = reference - feedback; //计算新error
+    pid->error1 = pid->error; //将旧error存起来
+    pid->error = setvalue - feedback; //计算新error
 	
     //计算微分
-    float dout = (pid->error - pid->lastError) * pid->kd;
+    float dout = (pid->error - pid->error1)/dt * pid->kd;
     //计算比例
     float pout = pid->error * pid->kp;
     //计算积分
-    pid->integral += pid->error * pid->ki;
+    pid->integral += pid->error*dt * pid->ki;
 	
     //积分限幅
     if(pid->integral > pid->maxIntegral) 
@@ -41,10 +49,43 @@ void PID_Calc(PID *pid, float reference, float feedback)
 	
     //输出限幅
     if(pid->output > pid->maxOutput) 
-			pid->output =   pid->maxOutput;
-    else if(pid->output < -pid->maxOutput) 
-			pid->output = -pid->maxOutput;
+			pid->output = pid->maxOutput;
+    if(pid->output < pid->minOutput) 
+			pid->output = pid->minOutput;
 		
+		return pid->output;
 		
 }
 
+//增量式PID
+//pidout+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
+//setvalue : 设置值（期望值）
+//feedback: 实际值
+float PID_Increment(PID *pid, float setvalue, float feedback)
+{
+	 	//更新数据
+    pid->error2 = pid->error1; //将旧error1存起来
+		pid->error1 = pid->error; //将旧error存起来
+	
+    pid->error = setvalue - feedback; //计算新error
+	
+		//计算微分
+    float dout = (pid->error - 2*pid->error1 + pid->error2) * pid->kd;
+    //计算比例
+    float pout = (pid->error - pid->error1 )* pid->kp;
+    //计算积分
+    pid->integral = pid->error * pid->ki;
+
+		//计算输出
+    pid->output += (pout+dout + pid->integral);
+	
+    //输出限幅
+//    if(pid->output > pid->maxOutput) 
+//			pid->output = pid->maxOutput;
+//    if(pid->output < pid->minOutput) 
+//			pid->output = pid->minOutput;
+
+		return pid->output;
+
+}
+

User/pid.h

@@ -37,7 +37,6 @@
 #ifndef PID_H__
 #define PID_H__
 
-#if 1
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -49,14 +48,15 @@ extern "C" {
 typedef struct
 {
    	float kp, ki, kd; //三个系数
-    float error, lastError; //误差、上次误差
+    float error, error1, error2; //误差、上次误差,上上次误差 
     float integral, maxIntegral; //积分、积分限幅
-    float output, maxOutput; //输出、输出限幅
+    float output, maxOutput, minOutput; //输出、输出限幅
 }PID;
 
 
-void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut);
-void PID_Calc(PID *pid, float reference, float feedback);
+void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut, float minOut);
+float PID_Location(PID *pid, float setvalue, float feedback, float dt);
+float PID_Increment(PID *pid, float setvalue, float feedback);
 
 
 #ifdef __cplusplus
@@ -64,6 +64,5 @@ void PID_Calc(PID *pid, float reference, float feedback);
 #endif
 
 
-#endif 
 
 #endif //PID_H__

User/process.c

@@ -6,18 +6,21 @@
 #include "Motor.h"
 #include "W25Q64.h"
 #include "AngleSensor.h"
-#include "pid.h"
+#include "motor_control.h"
 #include "cfg.h"
 #include "Rtcx.h"
 #include "storage.h"
 #include "IB_Reader.h"
 #include "math.h"
 
+
 uint8_t		g_devicebusy = 0;
 uint16_t	g_blinkLedTime = 0;									/*LED闪烁频率控制时间*/
 uint16_t	g_blinkLedTgtTime = BLINK_LED_DFTT;	/*LED目标闪烁频率*/
 uint16_t	g_period1000ms = 0;
 uint8_t		g_detectTime = 0;										/*状态检测时间*/
+uint16_t   g_angleRead_Interval = 5;          //读取编码器角度间隔 
+uint16_t   g_angleRead_Count = 0;             //读取编码器角度计数
 
 uint32_t	g_poweroff_count = 0;				/*外部电源停止后、每秒计数加1*/
 
@@ -33,8 +36,9 @@ uint8_t g_coverstatus = STATUS_COVERCLOSE;
 uint8_t g_motorstate = MOTOR_INIT;
 uint8_t  g_runReady = 0;
 uint16_t g_runTime = 0;
-static float g_sample_position = 0;  // 触发取样时，PID目标位置，取两阀值的均值 
-//uint8_t  g_sampleing_direction = 0;  //触发取样时，电机运行方向， 0 正转， 1反转 
+
+static float g_target_position = 0;  // 动作时，PID目标位置
+static uint8_t g_motor_dir = MOTOR_TUNR_NULL;  		 // 电机动作时的转动方向 ，0 不动 1 正向， 2 反向
 
 //状态持续计数，用来起到滤波作用 
 //uint8_t tmp_status;
@@ -43,9 +47,10 @@ uint8_t g_covercount[STATUS_COVERALL]={0};
 
 static float s_angle = 0.0;
 //static float tmp_angle = 0.0;
-static uint8_t get_lockstatus();
-static PID g_pid = {0}; 
-static uint8_t tmp_speed;
+static uint8_t get_lockstatus(void);
+//static PID g_pid = {0}; 
+//static uint8_t tmp_speed;
+static uint8_t tmp_pwmout;
 //static uint8_t tmp_time = 0;
 
 static void update_runstate()
@@ -83,7 +88,9 @@ void Process_Init(void)
 	
 		g_detectTime = 0;
 		g_runstate = STATE_LOCK;
-
+		g_angleRead_Interval = 500;
+		g_angleRead_Count = 0;
+	
 		s_angle = AngleSensor_GetAngle();
 		g_lockstatus=get_lockstatus();
 
@@ -157,13 +164,15 @@ void Process_RunPrd(void)
 		
 		//g_poweroff_count++;
 		
-		if(Gpio_IsDC24()){
-			printf(" DC24 Supply \r\n");
-		
-		}else{
-			printf(" Battery Supply \r\n");
+//		if(Gpio_IsDC24()){
+//			printf(" DC24 Supply \r\n");
+//		
+//		}else{
+//			printf(" Battery Supply \r\n");
+//		
+//		}
 		
-		}
+		printMotorCurrent();
 		
 		//printf(" Battery Voltage:%f V \r\n", getBatteryVoltage());
 		
@@ -216,13 +225,165 @@ void Process_RunPrd(void)
 	
 }
 
-void Process_Timer10msCB(void)
+static uint8_t get_motor_dir(float target){
+	
+	if(target > s_angle ){
+		return MOTOR_TUNR_P; // 正转 
+	}else if(target < s_angle){
+		return MOTOR_TUNR_N; // 反转 
+	}
+	
+	return MOTOR_TUNR_NULL; 
+}
+
+void Process_Timer1_CB(void)
+{
+		if (g_blinkLedTime < 0xFFFF)
+		{
+				g_blinkLedTime++;
+		}
+		
+		if(g_detectTime < 0xFF){
+				g_detectTime++;
+		}
+		
+		
+		if(g_runReady){
+			g_runTime++;
+		}
+		
+		if(g_period1000ms < 0xFFFF)
+		{
+			g_period1000ms++;
+		}
+		
+		g_angleRead_Count++;
+		if(g_angleRead_Count >= g_angleRead_Interval){
+			
+				g_angleRead_Count=0;
+				AngleSensor_Read();
+		
+		}
+}
+
+void Process_Timer0_CB(void)
+{
+	AngleSensor_Read();
+	
+
+//s_angle = AngleSensor_GetAngle();
+	
+//	if(s_angle < 0){ //处理异常
+//		if((MOTOR_READY != g_motorstate) && (MOTOR_STOPED != g_motorstate)){
+//				Motor_Brake();
+//				g_motorstate = MOTOR_STOPED;
+//				g_runReady =1;
+//				g_runTime=0;
+//		}
+//	}
+//	
+//	g_lockstatus=get_lockstatus();
+//	g_coverstatus=Gpio_IsOpenCover()>0?STATUS_COVEROPEN:STATUS_COVERCLOSE;
+//	
+
+//	//处理开关锁  
+//	switch(g_motorstate){
+//		case MOTOR_READY:
+//			break;
+//		case MOTOR_LOCKING:
+//			if(/*(STATUS_LOCK == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
+//				Motor_Brake();
+//				g_motorstate = MOTOR_STOPED;
+//				g_runReady =1;
+//				g_runTime=0;
+//			}else{
+//				PID_Calc(&g_pid, config->lock_threshold+3, s_angle);
+//				
+//				if(g_pid.error > 0){
+//						tmp_speed =(uint8_t)(g_pid.output);
+//						tmp_speed = tmp_speed<30?30:tmp_speed;
+//						Motor_Positive(tmp_speed);
+//				
+//				}else{
+//					Motor_Brake();
+//					g_motorstate = MOTOR_STOPED;
+//					g_runReady =1;
+//					g_runTime=0;
+//				}
+//				
+//			}
+//			
+//			break;
+//		case MOTOR_UNLOCKING:
+//			if(/*(STATUS_UNLOCK == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
+//				Motor_Brake();
+//				g_motorstate = MOTOR_STOPED;
+//				g_runReady =1;
+//				g_runTime=0;
+//			}else{
+//				PID_Calc(&g_pid, config->unlock_threshold-3, s_angle);
+//				
+//				if(g_pid.error < 0){
+//						tmp_speed =(uint8_t)(-g_pid.output);
+//						tmp_speed = tmp_speed<30?30:tmp_speed;
+//						Motor_Negative(tmp_speed);
+//				
+//				}else{
+//					Motor_Brake();
+//					g_motorstate = MOTOR_STOPED;
+//					g_runReady =1;
+//					g_runTime=0;
+//				}
+//				
+//			}
+//			break;
+//			
+//		case MOTOR_SAMPLEING:
+//			if(/*(STATUS_SAMPLE == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
+//				Motor_Brake();
+//				g_motorstate = MOTOR_STOPED;
+//				g_runReady =1;
+//				g_runTime=0;
+//			}else{
+//				PID_Calc(&g_pid, g_sample_position, s_angle);
+//				
+//				if(g_pid.error < 0){
+//						tmp_speed =(uint8_t)(-g_pid.output);
+//						tmp_speed = tmp_speed<30?30:tmp_speed;
+//						Motor_Negative(tmp_speed);
+//				
+//				}else{
+//					Motor_Brake();
+//					g_motorstate = MOTOR_STOPED;
+//					g_runReady =1;
+//					g_runTime=0;
+//				}
+//				
+//			}
+//			break;
+//		case MOTOR_STOPED:
+//			if(g_runTime >= 5000){ //5S
+//				g_motorstate = MOTOR_READY;
+//			}
+//			
+//			break;
+//		default:
+//			break;
+
+//	};
+//	
+
+
+}
+
+void Process_MotorControl(float angle)
 {
-	s_angle = AngleSensor_GetAngle();
+	//printf("P_M angle: %f \r\n", angle);
+	s_angle = angle;
 	
 	if(s_angle < 0){ //处理异常
 		if((MOTOR_READY != g_motorstate) && (MOTOR_STOPED != g_motorstate)){
-				Motor_Stop();
+				Motor_Brake();
 				g_motorstate = MOTOR_STOPED;
 				g_runReady =1;
 				g_runTime=0;
@@ -238,21 +399,26 @@ void Process_Timer10msCB(void)
 		case MOTOR_READY:
 			break;
 		case MOTOR_LOCKING:
-			if((STATUS_LOCK == g_lockstatus)|| (STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
-				Motor_Stop();
+			if(/*(STATUS_LOCK == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
+				Motor_Brake();
 				g_motorstate = MOTOR_STOPED;
 				g_runReady =1;
 				g_runTime=0;
 			}else{
-				PID_Calc(&g_pid, config->lock_threshold+3, s_angle);
-				
-				if(g_pid.error > 0){
-						tmp_speed =(uint8_t)(g_pid.output);
-						tmp_speed = tmp_speed<80?80:tmp_speed;
-						Motor_Positive(tmp_speed);
+
+				if(angle < g_target_position){
+
+						tmp_pwmout = MC_Calculate(g_target_position, angle, getMotorCurrent());
+						if(MOTOR_TUNR_P == g_motor_dir){
+							Motor_Positive(tmp_pwmout);
+						}else if(MOTOR_TUNR_N == g_motor_dir){
+							Motor_Negative(tmp_pwmout);
+						}
+						
+						
 				
 				}else{
-					Motor_Stop();
+					Motor_Brake();
 					g_motorstate = MOTOR_STOPED;
 					g_runReady =1;
 					g_runTime=0;
@@ -262,22 +428,24 @@ void Process_Timer10msCB(void)
 			
 			break;
 		case MOTOR_UNLOCKING:
-			if((STATUS_UNLOCK == g_lockstatus)|| (STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
-				Motor_Stop();
+			if(/*(STATUS_UNLOCK == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
+				Motor_Brake();
 				g_motorstate = MOTOR_STOPED;
 				g_runReady =1;
 				g_runTime=0;
 			}else{
-				PID_Calc(&g_pid, config->unlock_threshold-3, s_angle);
-				
-				if(g_pid.error < 0){
-						tmp_speed =(uint8_t)(-g_pid.output);
-						tmp_speed = tmp_speed<80?80:tmp_speed;
-						tmp_speed = 100;
-						Motor_Negative(tmp_speed);
+
+				if(angle > g_target_position){
+
+						tmp_pwmout = MC_Calculate(g_target_position, angle, getMotorCurrent());
+						if(MOTOR_TUNR_P == g_motor_dir){
+							Motor_Positive(tmp_pwmout);
+						}else if(MOTOR_TUNR_N == g_motor_dir){
+							Motor_Negative(tmp_pwmout);
+						}
 				
 				}else{
-					Motor_Stop();
+					Motor_Brake();
 					g_motorstate = MOTOR_STOPED;
 					g_runReady =1;
 					g_runTime=0;
@@ -287,21 +455,23 @@ void Process_Timer10msCB(void)
 			break;
 			
 		case MOTOR_SAMPLEING:
-			if((STATUS_SAMPLE == g_lockstatus)|| (STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
-				Motor_Stop();
+			if(/*(STATUS_SAMPLE == g_lockstatus)|| */(STATUS_UNKOWN == g_lockstatus) ||(g_runTime >= 2000)){
+				Motor_Brake();
 				g_motorstate = MOTOR_STOPED;
 				g_runReady =1;
 				g_runTime=0;
 			}else{
-				PID_Calc(&g_pid, g_sample_position, s_angle);
-				
-				if(g_pid.error < 0){
-						tmp_speed =(uint8_t)(-g_pid.output);
-						tmp_speed = tmp_speed<80?80:tmp_speed;
-						Motor_Negative(tmp_speed);
+
+				if(angle > g_target_position){
+						tmp_pwmout = MC_Calculate(g_target_position, angle, getMotorCurrent());
+						if(MOTOR_TUNR_P == g_motor_dir){
+							Motor_Positive(tmp_pwmout);
+						}else if(MOTOR_TUNR_N == g_motor_dir){
+							Motor_Negative(tmp_pwmout);
+						}
 				
 				}else{
-					Motor_Stop();
+					Motor_Brake();
 					g_motorstate = MOTOR_STOPED;
 					g_runReady =1;
 					g_runTime=0;
@@ -312,22 +482,21 @@ void Process_Timer10msCB(void)
 		case MOTOR_STOPED:
 			if(g_runTime >= 5000){ //5S
 				g_motorstate = MOTOR_READY;
+				g_angleRead_Interval = 500;
 			}
 			
+			g_angleRead_Interval = 200;
+			
 			break;
 		default:
 			break;
 
 	};
-	
-
-	//读取时间
-
 
 }
 
 
-uint8_t get_lockstatus()
+uint8_t get_lockstatus(void)
 {
 		static float _tmpf_min=0;
 		static float _tmpf_max = 0;
@@ -392,14 +561,21 @@ uint8_t Process_GetCoverStatus(void)
 
 uint8_t Process_OpLock(uint8_t speed)
 {
-	
+
 	if((MOTOR_READY == g_motorstate) && (STATUS_LOCK != g_lockstatus)){
-		//Motor_Positive(speed);
+
 		g_motorstate = MOTOR_LOCKING;
 		g_runReady =1;
 		g_runTime=0;
+		g_angleRead_Interval = 5;
+		
+		g_target_position = config->lock_threshold+3;
+		MC_Init(s_angle);
+		g_motor_dir = get_motor_dir(g_target_position);
 		
-		PID_Init(&g_pid, 12, 1, 0, 30, 100);
+		printf("Process_OpLock\r\n");
+		
+
 	}
 	
 	return 0;
@@ -407,14 +583,21 @@ uint8_t Process_OpLock(uint8_t speed)
 
 uint8_t Process_OpUnlock(uint8_t speed)
 {
+//	uint8_t id = 0;
 	
 	if((MOTOR_READY == g_motorstate) && (STATUS_UNLOCK != g_lockstatus)){
-		//Motor_Negative(speed);
+
 		g_motorstate = MOTOR_UNLOCKING;
 		g_runReady =1;
 		g_runTime=0;
-	
-		PID_Init(&g_pid,12, 1, 0, 50, 100);
+		g_angleRead_Interval = 5;
+		
+		g_target_position = config->unlock_threshold-3;
+		MC_Init(s_angle);
+		g_motor_dir = get_motor_dir(g_target_position);
+		
+		printf("Process_OpUnlock\r\n");
+		
 	}
 
 	return 0;
@@ -422,21 +605,26 @@ uint8_t Process_OpUnlock(uint8_t speed)
 
 uint8_t Process_OpSample(uint8_t speed)
 {
+//	uint8_t id = 0;
 	// 参数不符合要求
 	if(fabsf(config->sample_threshold1 - config->sample_threshold2) <=  10.0 ){
 		return 0;
 	}
 	
 	if((MOTOR_READY == g_motorstate) && (STATUS_LOCK == g_lockstatus)){
-		//Motor_Negative(speed);
+
 		g_motorstate = MOTOR_SAMPLEING;
 		g_runReady =1;
 		g_runTime=0;
+		g_angleRead_Interval = 5;
 		
-	  g_sample_position = (config->sample_threshold1 + config->sample_threshold2)/2;
+	  g_target_position = (config->sample_threshold1 + config->sample_threshold2)/2;
+		MC_Init(s_angle);
+		g_motor_dir = get_motor_dir(g_target_position);
 		
-		PID_Init(&g_pid,10, 1, 0, 30, 100);
+		printf("Process_OpSample \r\n");
 		
+
 	}
 
 	return 0;
@@ -539,4 +727,22 @@ void Process_Poweroff(void)
 
 }
 
+void Process_MotorP(uint8_t speed)
+{
+		if(speed > 0){
+			Motor_Positive(speed);
+		}else{
+			Motor_Brake();
+		}
+		
+}
+void Process_MotorN(uint8_t speed)
+{
+		if(speed > 0){
+			Motor_Negative(speed);
+		}else{
+			Motor_Brake();
+		}
+}
+
 

User/process.h

@@ -96,7 +96,7 @@ enum _coverstatus {
 void Process_Init(void);
 void Process_RunPrd(void); 
 
-void Process_Timer10msCB(void);
+//void Process_Timer0_CB(void);
 
 
 void Process_Poweroff(void); 
@@ -109,6 +109,10 @@ uint8_t Process_OpUnlock(uint8_t speed);
 uint8_t Process_OpSample(uint8_t speed); 
 uint8_t Process_CalibrationThreshold(uint8_t param); 
 
+//¿ØÖƵç»úÕý·´×ª 
+void Process_MotorP(uint8_t speed); 
+void Process_MotorN(uint8_t speed); 
+
 float Process_GetAngle(void); 
 uint8_t Process_AngleCalibration(void); 
 

User/protocol.c

@@ -12,7 +12,7 @@ static DataItem tmp_record;
 static uint8_t tmp_i=0;
 	
 #ifdef IS_BOOTLOADER
-uint32_t Firmware_Version[4] = {1, 0, 2, 20240927};
+uint32_t Firmware_Version[4] = {1, 0, 2, 20241009};
 #else
 uint32_t Firmware_Version[4] = {1, 1, 8, 20240929};
 #endif
@@ -339,9 +339,11 @@ uint8_t Write_LockOp(uint8_t *pdata, uint8_t len)
 		}else if(0x03 == pdata[0]){ //È¡Ñù 
 			Process_OpSample(pdata[1]);
 		}else if(0x04 == pdata[0]){ //Õýת²âÊÔ 
-			Motor_Positive(pdata[1]);
+			//Motor_Positive(pdata[1]);
+			Process_MotorP(pdata[1]);
 		}else if(0x05 == pdata[0]){ //·´×ª²âÊÔ 
-			Motor_Negative(pdata[1]);
+			//Motor_Negative(pdata[1]);
+			Process_MotorN(pdata[1]);
 		}
 	
 		return RET_OK;