hw/lx-rocon_firmware/firmware.c

   1 /* Firmware file for lx-rocon tumbl coprocessor */
   2
   3 #include <stdint.h>
   4
   5 typedef struct
   6 {
   7         int16_t p;
   8         int16_t i;
   9         int32_t irc_period;
  10         int16_t req_current;
  11         int16_t acc_dev;
  12         int16_t max_acc;
  13 } axis_settings;
  14
  15 typedef struct
  16 {
  17         /* Input */
  18         int16_t current;
  19         int32_t irc;
  20         int32_t irc_last;
  21         int32_t irc_norm;
  22
  23         /* Output */
  24         uint16_t pwm[3];
  25 } axis_io;
  26
  27 axis_settings a_settings;
  28
  29 axis_io a_io;
  30 int32_t irc_a_reg;
  31 int16_t current_a_reg;
  32 uint32_t count;
  33 int16_t phase_table[1][1];
  34
  35 void init_defvals()
  36 {
  37         a_settings.p = 20;
  38         a_settings.i = 2;
  39         a_settings.irc_period = 7000;
  40         a_settings.req_current = 500;
  41         a_settings.acc_dev = 0;
  42 }
  43
  44 void read_input(axis_io* io)
  45 {
  46         io->irc_last = io->irc;
  47         io->irc = *((int32_t*)0x8000);
  48 }
  49
  50 void update_axis(axis_settings *settings, axis_io* io)
  51 {
  52         int i;
  53         int16_t dev;
  54         int32_t mag, irc_diff;
  55
  56         dev = settings->req_current - io->current;
  57         settings->acc_dev += dev;
  58
  59         if (settings->acc_dev > settings->max_acc)
  60                 settings->acc_dev = settings->max_acc;
  61         else if (-(settings->acc_dev) <= -(settings->max_acc))
  62                 settings->acc_dev = -(settings->max_acc);
  63
  64         mag = settings->acc_dev * settings->i + dev * settings->p;
  65
  66         irc_diff = io->irc - io->irc_last;
  67         io->irc_last = io->irc;
  68         io->irc_norm += irc_diff;
  69
  70         if (io->irc_norm > settings->irc_period)
  71                 io->irc_norm -= settings->irc_period;
  72         else if (io->irc_norm < 0)
  73                 io->irc_norm += settings->irc_period;
  74
  75         for (i = 0; i < 3; i++)
  76                 io->pwm[i] = (uint16_t)((mag * phase_table[/*i*/ 0][/*irc_norm*/ 0]) >> 16);
  77 }
  78
  79 void main()
  80 {
  81         while (1)
  82         {
  83                 read_input(&a_io);
  84                 update_axis(&a_settings, &a_io);
  85                 a_io.irc = irc_a_reg;
  86                 a_io.current = current_a_reg;
  87                 count++;
  88         }
  89 }