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

 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>


// control variables
volatile int16_t ref=0;        // reference value (from velocity controller)
volatile int16_t y;            // measurement, 9 frac bits
volatile uint16_t low, high;   // when reading AD-conversion
volatile int16_t e = 0;        // control error, 9 frac bits
volatile int16_t v = 0;        // temporary ctrl signal, 9 frac bits
volatile int16_t vSat = 0;     // saturated temporary ctrl signal, 9 frac bits
volatile int16_t I = 0;        // integral part of ctrl, 13 frac bits
volatile int16_t u = 0;        // ctrl signal = pwm high time (8 bits)
volatile int16_t K = 111;      // 7 frac bits
volatile int16_t Ke = 38;      // 7 frac bits, K*h/Ti
volatile int8_t intCond = 0;   // flag for conditional integration

#define V_MAX 508              //max/min for saturation
#define V_MIN -508

// twi variable
volatile int16_t status;

// logging variables
/*
#define log_len 100
volatile int16_t ctrl_log[log_len];
volatile int16_t error_log[log_len];
volatile int32_t I_log[log_len];
volatile int16_t skipSamples = 1000;
volatile int16_t countSamples = 0;
volatile int16_t jj=0;
volatile int8_t stop = 0;
*/

/* Routine used to transmit bytes on the serial port */
/*
static void putchar(unsigned char ch)
{
	while ((inp(UCSRA) & 0x20) == 0) {};
	outp(ch, UDR);
	while ((inp(UCSRA) & 0x20) == 0) {};
}
*/

/* Send logged data over Serial connection */
/*
static inline void sendData() {
  int16_t ii = 0;
  while (ii < log_len) {

    putchar((unsigned char) ((ctrl_log[ii]&0xff00)>>8));
    putchar((unsigned char) (ctrl_log[ii]&0x00ff));

    putchar((unsigned char) ((error_log[ii]&0xff00)>>8));
    putchar((unsigned char) (error_log[ii]&0x00ff));

    putchar((unsigned char) ((I_log[ii]&0xff000000)>>24));
    putchar((unsigned char) ((I_log[ii]&0x00ff0000)>>16));

    putchar((unsigned char) ((I_log[ii]&0x0000ff00)>>8));
    putchar((unsigned char) (I_log[ii]&0x000000ff));

    ii++;
  }
}
*/



/* Timer 2 compare match interupt, 28.8 kHz, syncronized with pwm-period */
SIGNAL(TIMER2_COMP_vect) {

  // Start AD conversion
  ADCSRA |= _BV(ADSC);
  
  // Read previous AD-conversion result
  low = inp(ADCL);
  high = inp(ADCH);
  y = ((int16_t)((high<<8) | low)) - 512; // y 9 frac bits

  // control error
  e = ref-y; // e 9 frac bits
  
  // temporary ctrl-signal
  v = (((K*e+64)>>7)+((I+8)>>4));

  // variable that decides if I-part should be updated
  intCond = 1;

  // saturation and update integral part of ctrl with antiwindup
  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<<2))>>3);

  // ctrl signal, 7 bits + direction
  u = ((vSat+2)>>2); //7 frac bits to pwm 

  // set pwm switching time
  if (u < 0) {
    PORTC |= 0x80;  // set direction of motion
    OCR1BL = ((unsigned char) (-u)); // set length of pwm-high
  } else {
    PORTC &= 0x7f; // set direction of motion
    OCR1BL = ((unsigned char) (u)); // set length of pwm-high
  }

  
  // TWI-communication, recieve reference from velocity controller
  if ((_BV(TWINT)&inp(TWCR))) {
    status = (inp(TWSR)&0xf8);
    // status 0x80 means data recieved
    if (status == 0x80) {
      ref = (int16_t)((int8_t)inp(TWDR)); // read 8 bit reference
      ref = (ref<<2);  // shift up 2 steps for 10 bits reference in loop
    } 
    else {
    }
    outp(_BV(TWINT)|_BV(TWEN)|_BV(TWEA),TWCR);
  }

  // For logging purposes
  /*
  countSamples++;
  if (countSamples == skipSamples) {
    ctrl_log[jj] = u;
    I_log[jj] = I;
    error_log[jj] = e;
    jj++;
    countSamples = 0;
    
    // Stop after a while
    if ((jj == (log_len-1)) & !stop) {
	outp(0x7f,OCR1BL);
	stop = 1;
	sendData();
    }
  }
  */
  
}


int main()
{
  // clear interrupts (might not be needed)
  cli();
  
  //Port directions
  outp(0x08,PORTC); // pull up on overtemperature signals
  outp(0xa0,DDRC);  // output on direction and brake
  outp(0x10,DDRD);  // output on pwm-signal
  outp(0x40,DDRB);  // temp pwm output

  /* Timer section */
  // Timer 1, fast PWM no prescaling (non-inverting mode (start high, switch to low))
  outp(_BV(COM1B1)|_BV(WGM11)|_BV(WGM10),TCCR1A);
  outp(_BV(CS10)|_BV(WGM13)|_BV(WGM12),TCCR1B);
  
  // Reset Timer1 and set TOP-value to 128 (means 7-bit pwm-signal-> 115.2 kHz)
  outp(0x00,OCR1AH);
  outp(0x7f,OCR1AL);
  outp(0x00,TCNT1H);
  outp(0x00,TCNT1L);
  outp(0x00,OCR1BH);
  outp(0x7f,OCR1BL); // to not start motor-rotation before control


  
  /* Timer 2, 4 times pwm-period, for control sampling, prescaler 8, 28.8 kHz */
  outp(_BV(WGM21)|_BV(CS21),TCCR2);
  outp(0x3f,OCR2);
  /* Reset timer 2 */
  outp(0,TCNT2);

  // Enable timer2 compare match interrupts
  outp(_BV(OCIE2),TIMSK);

  //Serial communication
  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
  
  /* AREF (AREF is 5V) pin external capacitor, MUX0 for current measurement */
  outp(_BV(REFS0)|_BV(MUX0),ADMUX); 	
  
  // Enable ADC, start first conversion, prescaler 32, not free running mode
  outp(_BV(ADEN)|_BV(ADSC)|_BV(ADPS2)|_BV(ADPS0),ADCSRA);


  // Initialize TWI
  outp(0x02,TWBR);  // set SCL-frequency CPU-freq/(16+2*2)
  outp(0x02,TWAR);  // slave address
  outp(_BV(TWEA)|_BV(TWEN),TWCR); // enable TWI, enable ACK

  //Enable interrupts
  sei();

  // loop
  while (1) {}
}