vel_control.c

/*
**************************************************************

 Current regulation - Pontus Giselsson, Per-Ola Larsson 18/02/09
           for LTH - reglerteknik

***************************************************************
*/
#include <util/twi.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <inttypes.h>

#include "vel_control.h"
#include "serialio.h"

/*
 * Serial I/O assignments
 *
 *   AO 0 -- Axis 1  motor voltage
 *   A0 1 -- Axis 1  velocity reference !
 *
 *   EI 0 -- Axis 1  position !
 *   EI 1 -- Axis 1  filtered velocity !
 *   EI 5 -- Axis 1  position with predicted fraction
 *   EI 6 -- Axis 1  position unquantized
 *
 *   AI 2 -- Axis 1  current
 *   AI 3 -- Pendulum angle !
 *
 *   AI 4 -- Axis 1  motor voltage (actual)
 *
 *   DI 0 -- Axis 1  endpoint sensor
 */

#define POLL_AXIS1_POSITION      0x01
#define POLL_AXIS1_VELOCITY      0x02
#define POLL_PEND_ANGLE          0x04
#define POLL_CURRENT_REFERENCE   0x08
//#define POLL_AXIS1_RESET         0x0008
#define POLL_CONFIG              0x80

static volatile uint8_t pccom_poll=0;


// reference variables
struct {
  volatile uint16_t samples; // nbr of samples since last ctrl-ref update
  struct {
    volatile unsigned char pending;
    volatile int32_t next;
    volatile int32_t value;
  } vel;
  struct {
    volatile int32_t value;
  } acc;
} ref = {
  .samples = 0,
  .vel.pending = 0,
  .vel.next = 0,
  .vel.value = 0,
  .acc.value = 0,
};
volatile int32_t refCtrl = 0;    // ref used in ctrl-loop (=ref sent from simulink)

// velocity control variables
volatile int32_t u = 0;          // 11 frac bits
volatile int8_t brake = 0;       // brake variable if pos-sample missed
volatile int32_t I = 0;          // 11 frac bits