PID-control.rar_ STM32 PID_PID CONTROL_PID STM32_pid_stm32 pid

#include "stm32f4xx.h" #include "math.h" #include "LCD2x16.c" #define pi 3.141592653589 #define B00 0x0001 #define B01 0x0002 #define B02 0x0004 #define B03 0x0008 #define B04 0x0010 #define B05 0x0020 int Ref[64], x[64], Ptr; // declare circular buffers float Kp = 1.0, Ki = 0.0, Kd = 0.0; // declare & init params int Error[64]; // declare error vector int Reg[64]; // declare past output vector float Prop, Dif, Int = 0; // declare (& init) vars float Ts = 0.0001; // defined by constant 8400 in TIM2->arr; void ADC_setup(void) { RCC->APB2ENR |= 0x00000100; // clock for ADC1 RCC->APB2ENR |= 0x00000200; // clock for ADC2 ADC->CCR = 0x00000006; // Regular simultaneous mode only ADC1->CR2 = 0x00000001; // ADC1 ON ADC1->SQR3 = 0x00000002; // use PA02 as input ADC2->CR2 = 0x00000001; // ADC1 ON ADC2->SQR3 = 0x00000003; // use PA03 as input GPIOA->MODER |= 0x000000f0; // PA02, PA03 are analog inputs ADC1->CR2 |= 0x06000000; // use TIM2, TRG0 as SC source ADC1->CR2 |= 0x10000000; // Enable external SC, rising edge ADC1->CR1 |= 0x00000020; // Enable ADC Interrupt for EOC } void DAC_setup(void) { RCC->APB1ENR |= 0x20000000; // Enable clock for DAC DAC->CR |= 0x00010001; // DAC control reg, both channels ON GPIOA->MODER |= 0x00000f00; // PA04, PA05 are analog outputs } void GPIO_setup(void) { RCC->AHB1ENR |= 0x00000001; // Enable clock for GPIOA RCC->AHB1ENR |= 0x00000010; // Enable clock for GPIOE GPIOE->MODER |= 0x00010000; // output pin PE08: time mark GPIOE->MODER |= 0x00040000; // output pin PE09: toggle GPIOA->MODER |= 0x00001000; // output pin PA06: LED D390 } void Timer2_setup(void) { RCC->APB1ENR |= 0x0001; // Enable clock for Timer 2 TIM2->ARR = 8400; // Auto Reload value: 8400 == 100us TIM2->CR2 |= 0x0020; // select TRGO to be update event (UE) TIM2->CR1 |= 0x0001; // Enable Counting } int main () { GPIO_setup(); // GPIO set-up DAC_setup(); // DAC set-up ADC_setup(); // ADC set-up Timer2_setup(); // Timer 2 set-up NVIC_EnableIRQ(ADC_IRQn); // Enable IRQ for ADC in NVIC LCD_init(); LCD_string("Kp:", 0x00); LCD_string("Kd:", 0x09); LCD_string("Ki:", 0x49); // set gains & waste time - indefinite loop while (1) { if ((GPIOE->IDR & 0x003f) == (B00 + B05)) Kp++; // manually set Kp if ((GPIOE->IDR & 0x003f) == (B00 + B04)) Kp--; if (Kp<0) Kp = 0; if (Kp > 1000) Kp = 1000; if ((GPIOE->IDR & 0x003f) == (B01 + B05)) Kd += 0.001; // manually set Kd if ((GPIOE->IDR & 0x003f) == (B01 + B04)) Kd -= 0.001; if (Kd < 0) Kd = 0; if (Kd > 1) Kd = 1; if ((GPIOE->IDR & 0x003f) == (B02 + B05)) Ki += 0.0001; // manually set Ki if ((GPIOE->IDR & 0x003f) == (B02 + B04)) Ki -= 0.0001; if (Ki < 0) Ki = 0; if (Ki > 1) Ki = 1; LCD_sInt3DG((int)Kp,0x03,1); // write Kp LCD_sInt3DG((int)(Kd*1000),0x0c,1); // write Kd LCD_sInt3DG((int)(Ki*10000),0x4c,1); // write Ki for (int i = 0; i < 2000000; i++) {}; // waste time }; } // IRQ function void ADC_IRQHandler(void) // this takes approx 6us of CPU time! { GPIOE->ODR |= 0x0100; // PE08 up Ref[Ptr] = ADC1->DR; // pass ADC -> circular buffer x1 x[Ptr] = ADC2->DR; // pass ADC -> circular buffer x2 // PID calculation start Error[Ptr] = Ref[Ptr] - x[Ptr]; // calculate error Prop = Kp * (float)Error[Ptr]; // proportional part Dif = Kd * (float)(Error[Ptr] - Error[(Ptr-1) & 63]) / Ts; // differential part Int += Ki * (float)Error[Ptr]; // integral part Reg[Ptr] = (int)(Prop + Dif + Int); // summ all three // PID calculation stop if (Reg[Ptr] > 4095) DAC->DHR12R1 = 4095; // limit output due to the DAC else if (Reg[Ptr] < 0) DAC->DHR12R1 = 0; else DAC->DHR12R1 = (int)Reg[Ptr]; // regulator output -> DAC DAC->DHR12R2 = Error[Ptr] + 2048; // Error -> DAC Ptr = (Ptr + 1) & 63; // increment pointer to circular buffer GPIOE->ODR &= ~0x0100; // PE08 down }