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

 Current regulation - Aleks Ponjavic 18/09/07
           for LTH - reglerteknik

   Note that the voltage is actually a duty
   cycle for the PWM. The PWM is running at
  12V with a duty cycle set by a 10bit value

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

//#include <math.h>		//Only include if sin will be used, takes up memory!
#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <inttypes.h>


// reference generation variables
volatile int16_t ref = 200;
volatile int16_t refCount = 0;
volatile int8_t refFlag = 0;



// control variables
volatile unsigned char lbyte,hbyte;  
volatile int16_t result;
volatile int16_t ctrl; 
volatile int16_t ctrl_out;
volatile int16_t temp;
volatile uint8_t alt = 1;

volatile uint8_t low, high;



/* Routine used to set the red LED */
void setLED(uint8_t on)
{
  if (on) PORTB &= 0x7f;	//Turn on
  else PORTB |= 0x80;	//Turn off
}

/* Routine used to set pin PD7 */
void setPA4(uint8_t on)
{
  if (on == 0) PORTA &= 0xef;	//Turn off
  else PORTA |= 0x10;	//Turn on
}

/* 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) {};
}

/* Interrupt service routine for handling incoming bytes on the serial port 
 might be needed to catch incoming bytes */
SIGNAL(SIG_UART_RECV){}


static inline int16_t readInput() {
  uint8_t low, high;
  ADCSRA |= 0x40;
  while (ADCSRA & 0x40);
  low = ADCL;
  high = ADCH;
  return ((high<<8) | low) - 512;
}
	

/* Write 8-bit output using the PWM-generator */
static inline void writeOutput(int16_t val) {
  if (val < 0) {
    PORTC = 0x80+(PORTC & 0x7F);
    OCR1BH = 0;    //(unsigned char) (-val)&0xff00;
    OCR1BL = (unsigned char) (-val)&0x00ff;
  } else {
    PORTC = (PORTC & 0x7F);
    OCR1BH = 0;    //(unsigned char) (val&0xff00);
    OCR1BL = (unsigned char) (val&0x00ff);
  }
}



/* Interrupt when AD-conversion completes */
SIGNAL(SIG_ADC)
{
  setPA4(1);

  // Read input
  low = inp(ADCL);
  high = inp(ADCH);
  result =  ((high<<8) | low) - 512;
  
  // Scale incoming current measurement
  result = result*3; //!!!!!!!!!!!!!!!!!!!!!!!

  // Scale incoming current measurement
  // result += 13;
    
  //control
  ctrl = ref-result;
  ctrl = ((ctrl + (1<< 2)) >> 3);
  
  //saturation/rounding to 8 bit
  if (ctrl > 511)
    ctrl_out = 511;
  else if (ctrl < -511)
    ctrl_out = -511;
  else
    ctrl_out = ctrl;


  // write output
  if (ctrl_out < 0) {
    PORTC = 0x80+(PORTC & 0x7F);
    OCR1BL = (unsigned char) (-ctrl_out);
  } else {
    PORTC = (PORTC & 0x7F);
    OCR1BL = (unsigned char) (ctrl_out);
  }
  setPA4(0);
}


/* reference square- or triangle wave generator with timer 0 */
SIGNAL(SIG_OVERFLOW0) {
  int8_t rectangle = 1;
  refCount++;
  if (rectangle == 1) {
    if (refFlag == 0) {
      if (refCount == 10) {
	refFlag = 1;
	ref = -ref;
	refCount = 0;
      }
    } else {
      if (refCount == 20) {
	ref = -ref;
	refCount = 0;
      }
    }
  } else {
    if (refCount <= 400) {  // ref*2
      ref -= 1;
    } else {
      ref += 1;
    }
    if (refCount == 800) { // ref*4
      refCount = 0;
    }
  }
}


int main()
{
  int i,j;
  
  //Port directions
  outp(0x80,PORTB); // LED off
  outp(0x80,DDRB);  // output on LED
  outp(0x08,PORTC); // pull up on overtemp signals
  outp(0xa0,DDRC);  // output on dir and brake
  outp(0x80,PORTD); // pull up on reset switch
  outp(0x10,DDRD);  // output on pwm for motor 1
  
  outp(0x10,DDRA);  // test pin output

  /* Timer section */
  // Enable timer0 overflow interrupts
  outp(BV(TOIE0),TIMSK);
  
  /* Timer 1, 8 bit fast PWM no prescaling -> h_pwm=17.35 micros */
  outp(BV(COM1A1)|BV(COM1B1)|BV(WGM10),TCCR1A);
  // Fast PWM
  // outp(BV(WGM12)|BV(CS10),TCCR1B);
  // Phase and frequency correct pwm
  outp(BV(CS10),TCCR1B);
  /* Reset Timer1*/
  
  //outp(0x00,TCNT1H);   		// Reset TCNT1 high
  //outp(0x00,TCNT1L);		// Reset TCNT1 low

  
  
  /* Timer 2 (control loop), prescaler 256, clear on compare match (28), -> h = 0.5 ms */
  //  outp(BV(WGM21)|BV(CS22)|BV(CS21),TCCR2);
  //outp(28,OCR2);
  /* Reset timer 2 */
  //outp(0,TCNT2);

  /* Timer 0 for reference generation, prescaler = 1024 periodic h = ? */
  outp(BV(CS02)|BV(CS00),TCCR0);
  
  //Serial communication
  outp(0x00, UCSRA);	// USART:
  outp(0x98, UCSRB);	// USART: RxIntEnable|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, MUX3?? for pendulum angle */
  outp(BV(REFS0)|BV(MUX0),ADMUX); 	
  
  // Enable ADC interrupts, start first conversion (what do adps0-2 do), prescaler 32
  outp(BV(ADEN)|BV(ADATE)|BV(ADSC)|BV(ADIE)|BV(ADPS2)|BV(ADPS0),ADCSRA);

  // outp(BV(ADEN)|BV(ADSC),ADCSRA);
  
  /* Wait a little bit, probably not needed...*/
  int tmp;
  for(i=0;i<2000;i++) 
    for(j=0;j<400;j++)
      tmp = j*j*j;
  

  //Enable interrupts
  sei();

  // loop
  while (1) {}
}