Updated PID demo

1 files changed, 44 insertions, 25 deletions

diff --git a/demos/STM32/RT-STM32F303-DISCOVERY-PID/main.c b/demos/STM32/RT-STM32F303-DISCOVERY-PID/main.c
index 788c19e..6f09f0c 100644
--- a/demos/STM32/RT-STM32F303-DISCOVERY-PID/main.c
+++ b/demos/STM32/RT-STM32F303-DISCOVERY-PID/main.c
@@ -17,13 +17,14 @@
 #include "ch.h"

 #include "hal.h"

 #include "pid.h"

+#include <stdlib.h>

 

-pid_t pid;

-float input = 0, output = 0, target = 3500;

-

+#define STM32_UUID ((uint32_t *)0x1FFFF7AC)

 #define ADC_GRP1_NUM_CHANNELS   1

 #define ADC_GRP1_BUF_DEPTH      8

 

+static pid_t pid;

+static float input = 0, output = 0, target = 0;

 static adcsample_t samples[ADC_GRP1_NUM_CHANNELS * ADC_GRP1_BUF_DEPTH];

 

 /*

@@ -33,8 +34,22 @@ static void adccallback(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
 

   (void)adcp;

   (void)n;

+  uint32_t i, tmp = 0;

   if (samples == buffer) {

-    input = buffer[4];

+

+    for (i = 0; i < n; i++) {

+        tmp += buffer[i];

+    }

+    input = tmp / n;

+    

+    if (input <= target) {

+        palClearLine(LINE_LED7_GREEN);

+        palSetLine(LINE_LED6_GREEN);

+    }

+    else {

+        palClearLine(LINE_LED6_GREEN);

+        palSetLine(LINE_LED7_GREEN);

+    }

   }

 }

 

@@ -51,11 +66,11 @@ static const ADCConversionGroup adcgrpcfg1 = {
   ADC_CFGR_CONT,            /* CFGR    */

   ADC_TR(0, 4095),          /* TR1     */

   {                         /* SMPR[2] */

-    ADC_SMPR1_SMP_AN0(ADC_SMPR_SMP_19P5),

+    ADC_SMPR1_SMP_AN1(ADC_SMPR_SMP_181P5),

     0

   },

   {                         /* SQR[4]  */

-    ADC_SQR1_SQ1_N(ADC_CHANNEL_IN0),

+    ADC_SQR1_SQ1_N(ADC_CHANNEL_IN1),

     0,

     0,

     0

@@ -72,32 +87,35 @@ static void gptcb(GPTDriver *drv) {
     

     (void)drv;

     pid_compute(&pid);

-    dacPutChannelX(&DACD1, 0, output);

+    dacPutChannelX(&DACD1, 0, (dacsample_t)output);

 }

 

 /*

  * GPT6 configuration.

  */

 static const GPTConfig gpt6cfg1 = {

-  .frequency    = 100U,

+  .frequency    = 10000U, // 10 KHz

   .callback     = gptcb,

   .cr2          = TIM_CR2_MMS_1,    /* MMS = 010 = TRGO on Update Event.    */

   .dier         = 0U

 };

 

+

 /*

  * Red LEDs blinker thread, times are in milliseconds.

  */

 static THD_WORKING_AREA(waThread1, 128);

 static THD_FUNCTION(Thread1, arg) {

+    

+  srand(osalOsGetSystemTimeX() + STM32_UUID[0]);

 

   (void)arg;

   chRegSetThreadName("blinker");

   while (true) {

-    palSetPad(GPIOE, GPIOE_LED10_RED);

-    chThdSleepMilliseconds(500);

-    palClearPad(GPIOE, GPIOE_LED10_RED);

+    palToggleLine(LINE_LED10_RED);

     chThdSleepMilliseconds(500);

+    

+    target = 1000 + (rand() % 2500); // Change the PID target every 500ms.

   }

 }

 

@@ -110,10 +128,10 @@ int main(void) {
   chSysInit();

 

   /*

-  * Set PA0 PA3 PA4 to Analog (ADC1_CH1, DAC1_CH1)

+  * Set PA0 PA3 PA4 to Analog (ADC1_CH2, DAC1_CH1)

   * You will have to connect these with a jumper wire

   */

-  palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG);

+  palSetPadMode(GPIOA, 1, PAL_MODE_INPUT_ANALOG);

   palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG);

 

   /*

@@ -122,29 +140,30 @@ int main(void) {
   adcStart(&ADCD1, NULL);

   dacStart(&DACD1, &dac1cfg1);

   gptStart(&GPTD6, &gpt6cfg1);

-

+  

   /*

-   * Starting a continuous conversion.

+   * Start PID

    */

-  gptStartContinuous(&GPTD6, 2U);

-  

-  pid_create(&pid, &input, &output, &target, 2.0, 5.0, 1.0, PID_ON_M, PID_DIRECT);

+  pid_create(&pid, &input, &output, &target, 1.0, 1.0, 1.0, PID_ON_M, PID_DIRECT);

+  pid_setOutputLimits(&pid, 0.0, 4095.0); // Max DAC range

   pid_setSampleTime(&pid, 10);

   pid_setMode(&pid, PID_AUTOMATIC);

-  

-    /*

-   * Creates the blinker thread.

+

+  /*

+   * Starting a continuous conversion.

    */

-  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

-  

+  gptStartContinuous(&GPTD6, 101U); // 10000 / 101

   adcStartConversion(&ADCD1, &adcgrpcfg1, samples, ADC_GRP1_BUF_DEPTH);

-

+  

+  /*

+  * Creates the blinker thread.

+  */

+  chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);

 

   /*

    * Normal main() thread activity.

    */

   while (true) {

-

     chThdSleepMilliseconds(250);

     palToggleLine(LINE_LED3_RED);

   }