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 <compat/deprecated.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
volatile int32_t vel_ref = 0;    // 11 frac bits (variable in mm/s, max 2^12)
volatile int32_t refCtrl = 0;    // ref used in ctrl-loop (=ref sent from simulink)
volatile int32_t acc_ref = 0;    // 14 frac bits (variable in mm/s, max 2^15)
volatile uint16_t nbrSamples = 0; // nbr of samples between ctrl-ref updates

// velocity control variables
volatile int32_t u = 0;          // 11 frac bits
volatile int32_t uSend = 0;      // ctrl sent to simulink
volatile int32_t v = 0;          // 11 frac bits
volatile int32_t vSat = 0;
volatile int8_t brake = 0;       // brake variable if pos-sample missed
volatile int32_t I = 0;          // 11 frac bits
volatile int32_t e = 0;          // 11 frac bits
volatile int8_t intCond = 0;


volatile int32_t K = 807;       // 6 frac bits, prop constant
volatile int32_t Ke = 13;        // 6 frac bits, integral constant
volatile int8_t fr_comp = (10<<3);
#define V_MAX (120<<4)
#define V_MIN (-120<<4)


// encoder variables
#define ENCODERY  (PIND&0x04)        //Positional encoder pins
#define ENCODERX  ((PINB&0x02)<<1)   //Positional encoder pins (shift one step for faster comparison with Y)

// position variables
volatile int16_t pos = 0;        // position in tics
volatile int16_t posCtrl = 0;    // position used in ctrl-loop
volatile int16_t oldPos = 0;     // previous pos used for velocity estimation
volatile int8_t newX;            // encoder signal
volatile int8_t newY;            // encoder signal
volatile int8_t oldX;            // previous encoder signal
volatile int8_t oldY;            // previous encoder signal

// velocity estimation parameters
volatile int32_t velEst = 0;     // vel-estimate, 5 frac bits
volatile int32_t velEstSend = 0; // vel-estimate sent to simulink
int16_t a = 116;                 // 7 frac bits (parameter in velocity estimate)
int16_t b = 152;                 // 5 frac bits (parameter in velocity estimate)

// adc measurement variables
volatile int16_t low;            // temporary variable for ad-reading
volatile int16_t high;           // temporary variable for ad-reading
volatile int16_t angleOffset = 0;

/* return position (in tics) */
int32_t getPosition() {
  int16_t result = 0;
  cli();
  result = pos;
  sei();
  return (result);
}


/* return velocity (in mm/s) */
int32_t getVelocity() {
  return velEstSend;
}


/* return last angle measurement */
int16_t getAngle() {
  low = inp(ADCL);
  high = inp(ADCH);
  int16_t result = ADC - 512 -angleOffset;
  return result;
}


/* return current-reference */
int32_t getCurrentRef() {
  return uSend;
}


/* Set new acceleration reference value */
void setAccRef(int32_t newAccRef) {
  acc_ref = newAccRef;
  nbrSamples = 0;
}


/* Set new velocity reference value */
void setVelRef(int32_t newVelRef) {
  vel_ref = newVelRef;
}

/* Routine used to initialize the positional encoders */
void initPos()
{
  oldX = ENCODERX;
  oldY = ENCODERY;
}

/* Timer 2, Encoder counter, 73 kHz updates position variable */
SIGNAL(TIMER2_COMP_vect) {
  // Update position from encoder
  newX = ENCODERX;
  newY = ENCODERY;
  if((newX != oldX) || (newY != oldY)) { //Check if any value changed
      /*
      sum = (oldX<<2)+oldY+newX+(newY>>2);  
      if (sum == 2 || sum == 4 || sum == 11 || sum == 13) {
	pos = pos+1;
      } else if (sum == 1 || sum == 7 || sum ==  8 || sum == 14) {
	pos = pos-1;
      } else {
	brake = 1; // emergency brake
      }
      */
    // Works like if-clauses above, but faster!
      if ((oldX == newY) && (oldY != newX)) {
	pos = pos+1;
      } else if ((oldX != newY) && (oldY == newX)) {
	pos = pos-1;
      } else {
	brake = 1;
      }
      oldX = newX;
      oldY = newY;
  }
}

SIGNAL(USART_RXC_vect) {
  char ch = UDR;
  switch (serialio_RXC(ch)) {
    case serialio_clearbit: {
      switch (serialio_channel) {
      }
    } break;
    case serialio_setbit: {
      switch (serialio_channel) {
      }
    } break;
    case serialio_pollbit: {
      switch (serialio_channel) {
        //case 0: { addPoll(POLL_AXIS1_RESET); } break;
      }
    } break;
    case serialio_pollchannel: {
      switch (serialio_channel) {
        case 0:  pccom_poll |= POLL_AXIS1_POSITION;    break;
        case 1:  pccom_poll |= POLL_AXIS1_VELOCITY;    break;
        case 2:  pccom_poll |= POLL_PEND_ANGLE;        break;
        case 3:  pccom_poll |= POLL_CURRENT_REFERENCE; break;
        case 31: pccom_poll |= POLL_CONFIG;            break;
      }
    } break;
    case serialio_setchannel: {
      switch (serialio_channel) {
        case 0: { 
          setVelRef(serialio_value - (1L<<12));
        } break;
        case 1: {
          setAccRef(serialio_value - (1L<<15));
        } break;
      }
    } break;
    case serialio_more: {
    } break;
    case serialio_error: {
    } break;
  }
}

#if 0
/* Timer 0, serial communication with simulink */
SIGNAL(TIMER0_COMP_vect) {

  TIMSK &= ~(_BV(OCIE0)|_BV(OCIE1A));   // Disable communication and ctrl-interrupts

  sei(); // enable interrupts from encoder-counter
  
  // Poll UART receiver
  uint8_t status = UCSRA;
  if (status & (1<<RXC)) {
    char ch = UDR;
    pccom_receiveByte(ch);
    
    if (status & ((1<<FE)|(1<<DOR)|(1<<PE))) { 
      //main_emergencyStop(); // stop on USART error
    }     
  }
  
  // Poll UART sender
  if (UCSRA & (1<<UDRE)) {
    int16_t toSend = pccom_getNextByteToSend();
    if (toSend >= 0) UDR = (char)toSend;
  }

  TIFR = (1<<OCF0);        // skip pending interrupts from serial comm, (but not from ctrl)

  TIMSK |= (_BV(OCIE0)|_BV(OCIE1A)); // reenable communication and ctrl-interrupts

}
#endif


/* Timer 0, control loop , 1 kHz */
SIGNAL(TIMER1_COMPA_vect) {
  posCtrl = pos; // store pos to use in this loop

  sei(); // to enable interupts from encoder counter and communication

  // velocity estimation in mm/s
  velEst = (((a*velEst+64)>>7)+b*(posCtrl-oldPos));  // 5 fracbits on velEst
  oldPos = posCtrl;
  
  // store velEst and ref to be sent/used here
  cli();
  velEstSend = velEst;
  nbrSamples++;
  refCtrl = vel_ref + ((acc_ref*nbrSamples)>>10);  //shift nbrSamples 10 steps (= nbrSamples*h)
  if (nbrSamples > 500) {
    refCtrl = 0;   // for safety reasons if doesn't send anything in 0.5 s
  }
  // vel_ref = vel_ref*(1-brake); // emergency stop
  sei();

  // control error
  e = refCtrl-((velEst+16)>>5);  // mm/s

  // temporary ctrl-signal
  v = (((K*e+(1<<5))>>6)+((I+(1<<3))>>4));

  // friction compensation
  if (refCtrl > 0) {
    v = v+fr_comp;
  } else if (refCtrl < 0) {
    v = v-fr_comp;
  }

  // variable that decides if I-part should be updated
  intCond = 1;
  
  // saturation of v
  if (v > V_MAX) {
    vSat = V_MAX;
    if (e > 0)
      intCond = 0;
  } else if (v < V_MIN) {
    vSat = V_MIN;
    if (e < 0)
      intCond = 0;
  } else {
    vSat = v;
  }
  
  if (intCond)
    I = I + (((Ke*e)+(1<<1))>>2);

  // scale ctrl-signal to send over twi
  u = (vSat+8)>>4; // u=127 gives current = 6.75 A, vSat makes u saturate at 114

  // u that is sent to simulink
  cli();
  uSend = u;
  sei();


  // TWI-communication to set current reference on the other atmel
  // send start command
  outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTA),TWCR);
  while (!(TWCR&_BV(TWINT))) {}
    
  // Contact slave  
  outp(0x02,TWDR);  // slave is 0x02 (sla+w)
  outp(_BV(TWINT)|_BV(TWEN),TWCR);
  while (!(TWCR&_BV(TWINT))) {}
 
  // Send reference byte to slave
  outp((int8_t)u,TWDR);  // send 8 bits reference
  outp(_BV(TWINT)|_BV(TWEN),TWCR);
  while (!(TWCR&_BV(TWINT))) {}
    
  // stop transmission
  outp(_BV(TWINT)|_BV(TWEN)|_BV(TWSTO),TWCR);
}


int main()
{
  cli();
  
  //Port directions
  //outp(0xf0,DDRB);   // Led output
  outp(0x10,DDRC);  // timer calculation port
  PORTD = 0x40;  // pull up on reset switch


  /* Timer section */
  // Enable timer0, timer1, timer2 compare match interrupts
  outp(_BV(OCIE0)|_BV(OCIE1A)|_BV(OCIE2),TIMSK);
  outp(_BV(OCIE1A)|_BV(OCIE2),TIMSK);
  
  /* Timer 2, 73 kHz Prescaler 1, encoder counter for position measurement */
  outp(_BV(WGM21)|_BV(CS20),TCCR2);
  outp(200,OCR2);
  /* Reset timer 2 */
  outp(0,TCNT2);
  
  /* Timer 1, 1 kHz , prescaler 1, ctrl-loop */
  outp(_BV(WGM12)|_BV(CS10),TCCR1B);
  outp(0x38,OCR1AH);
  outp(0x3f,OCR1AL);
  outp(0,TCNT1H);
  outp(0,TCNT1L);
  

  /* Timer 0, 40 kHz, Prescaler 8, serial communication */
  outp(_BV(WGM01)|_BV(CS01),TCCR0);
  //outp(184-1,OCR0); // 10 kHz
  outp(45-1,OCR0); // 40 kHz
   /* Reset timer 0 */
  outp(0,TCNT0);


  // syncronization (ctrl interrupt 34 micros before communication interrupt)
  TCNT1 = TCNT0*8+500;
  
  
  //Serial communication initialization
  outp(0x00, UCSRA);	// USART:
  outp(0x18, UCSRB);	// USART: RxEnable|TxEnable
  outp(0x86, UCSRC);	// USART: 8bit, no parity
  outp(0x00, UBRRH);	// USART: 115200 @ 14.7456MHz
  outp(7,UBRRL);	// USART: 115200 @ 14.7456MHz 

  UCSRA = 0x00;     // USART: 
  UCSRB = 0x98;     // USART: RxIntEnable|RxEnable|TxEnable 
  UCSRC = 0x86;     // USART: 8bit, no parity, 9600 
  UBRRH = 0x0;      // USART: 115200 @ 14.7456MHz
  UBRRL = 7;        // USART: 115200 @ 14.7456MHz
  
  
  /* AREF (AREF is 5V) pin external capacitor, ADC3 for pendulum angle */
  outp(_BV(REFS0)|_BV(MUX0)|_BV(MUX1),ADMUX);
  
  // Enable ADC on adc3, start first conversion, prescaler 128, free running mode
  outp(_BV(ADEN)|_BV(ADSC)|_BV(ADATE)|_BV(ADPS2)|_BV(ADPS1)|_BV(ADPS0),ADCSRA);


  // Initialize Master TWI
  outp(0x10,TWBR);  // set SCL-frequency CPU-freq/(16+2*16)
  outp(_BV(TWEN),TWCR); // enable TWI

  // initialize position measurements
  initPos();

  // initialize pc-communication
  serialio_init();
  pccom_poll = 0;
  
  //Enable interrupts
  sei();

  // loop
  while (1) {
    unsigned char to_poll;
    // reset position, velocity estimate and integral part of ctrl if reset button pushed
    if (!(PIND & 0x40)) {
      cli();
      low = inp(ADCL);
      high = inp(ADCH);
      pos = 0; // reset position
      angleOffset =  ((int16_t) ((high<<8) | low) - 512);
      oldPos = 0;
      velEst = 0; // reset velocity estimate
      I = 0; // reset integral part of controller
      u = 0; // reset ctrl signal
      sei();
    }
    cli();
    to_poll = pccom_poll;
    pccom_poll = 0;
    sei();    
    if (to_poll & POLL_AXIS1_POSITION) {
      serialio_putchannel(0, getPosition()+(1L<<15)); 
    }
    if (to_poll & POLL_AXIS1_VELOCITY) {
      serialio_putchannel(1, getVelocity()+(1L<<17)); 
    }
    if (to_poll & POLL_PEND_ANGLE) {
      serialio_putchannel(2, getAngle()+(1L<<10)); 
    }
    if (to_poll & POLL_CURRENT_REFERENCE) {
      serialio_putchannel(3, getCurrentRef()+(1L<<7)); 
    }
    if (to_poll & POLL_CONFIG) {
      CONF_ANALOG_IN(0, CONF_RESOLUTION(16));     // Position (now reference)
      CONF_ANALOG_IN(0, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_IN(0, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_IN(1, CONF_RESOLUTION(18));     // Velocity estimate
      CONF_ANALOG_IN(1, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_IN(1, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_IN(2, CONF_RESOLUTION(11));     // Pend angle measurement
      CONF_ANALOG_IN(2, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_IN(2, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_IN(3, CONF_RESOLUTION(8));     // Current reference
      CONF_ANALOG_IN(3, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_IN(3, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_OUT(0, CONF_RESOLUTION(13));    // Reference to vel-ctrl
      CONF_ANALOG_OUT(0, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_OUT(0, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_ANALOG_OUT(1, CONF_RESOLUTION(16));    // Reference to acc-ctrl
      CONF_ANALOG_OUT(1, CONF_MIN(CONF_NEGATIVE_VOLT(1)));
      CONF_ANALOG_OUT(1, CONF_MAX(CONF_POSITIVE_VOLT(1)));

      CONF_END();
    }
  }
}