/***********************************************************/
/* Sterownik serwa z silnikiem DC                          */
/* i dwufazowym enkoderem inkrementacyjnym                 */ 
/* o rozdzielczości 2000imp/obr                            */
/* (c) by Grzegorz Kurczyk 2007                            */
/***********************************************************/

#include <avr/io.h>
#include <avr/interrupt.h>
#include "inc/binary.h"

/***************************************************************/
/*                   DEFINICJE MAKROPOLECEŃ                    */
/***************************************************************/

/***************************************************************/
/*                DEFINICJE BITÓW STERUJĄCYCH LED              */
/***************************************************************/
#define LEDR_on     sbi(PORTC, 2) // czerwona
#define LEDR_off    cbi(PORTC, 2) // czerwona
#define LEDY_on     sbi(PORTC, 3) // żółta
#define LEDY_off    cbi(PORTC, 3) // żółta

/***************************************************************/
/*                   DEFINICJE                                 */
/***************************************************************/
// uruchomienie generatora PWM
#define StartPWM TCCR2 = (1<<CS21); // CK/8 = 1MHz; PWM = 3,9kHz

// zatrzymanie generatora PWM
#define StopPWM TCCR2 = 0          


// maski bitów mostka H
//                    uvwWVU
#define MotorStop  b00101000
#define MotorLewo  b00001001
#define MotorPrawo b00100100

/***************************************************************/
/*                     ZMIENNE GLOBALNE                        */
/***************************************************************/
register char SREG_INT0     asm("r5");  // bufor rejestru znaczników dla przerwania INT0
register int  Enkoder       asm("r16"); // bezwzględna pozycja enkodera
// zmienne rejestrowe komutacji silnika
register uint PhasePWM      asm("r12"); // kombinacja bitów faz dla PWM
volatile int NowaPozycja; // pozycja zadana
char PWM;

struct {
  char MaxPWM;
  char Tlumienie;
} Param;  

/*****************************************************/
/*         PROCEDURA OBSŁUGI ENKODERA                */
/* przy maksymalnych obrotach silnika okres impulsów */
/* z enkodera jest > 18us (fmax < 55kHz)             */
/*****************************************************/
/* iloś taktów zegara <= 18                          */
/* czas obsługi przerwaia <= 2,25us (przy 8MHz)      */
/*****************************************************/

void SIG_INTERRUPT0(void) __attribute__ ((naked));
SIGNAL(SIG_INTERRUPT0) {
  SREG_INT0 = SREG; // zapamietaj rejestr znaczników
  if(bit_is_set(PINC, 4)) // sprawdź stan fazy B enkodera
    Enkoder++;
  else
    Enkoder--;
  SREG = SREG_INT0; // odtwórz rejestr znaczników
  asm("reti");
}

/*****************************************************/
/*            ODPOWIEDŹ UKŁADU                       */
/*****************************************************/

uint pwmcnt;

void SetPWM(uint delta) {
  int d = delta / 4;  // obliczenie drogi rozpędzania i hamowania
// odpowieź układu dla dużego sygnału błedu
  if(d > Param.MaxPWM) {
    PWM = Param.MaxPWM;
    OCR2 = PWM;
    return;
  }
// "dociąganie" do zadanej pozycji
// i tłumienie oscylacji końcowych
  if(pwmcnt)
    pwmcnt--;
  else {
    PWM++;
    OCR2 = PWM;
    pwmcnt = Param.Tlumienie * 16;
  }    
}

/*****************************************************************/
/*                         PROGRAM GŁÓWNY                        */
/*                         PROGRAM GŁÓWNY                        */
/*                         PROGRAM GŁÓWNY                        */
/*****************************************************************/

int main(void) {
// PORTB - sterowanie inwertera MOSFET
  
  PORTB = b00000000;
  DDRB  = b00111111;

  PORTC = b00010000;
  DDRC  = b00001100;

  DDRD  = b00000000;
  PORTD = b00000100;
  
  Enkoder = 0;
  NowaPozycja = 0;

  Param.MaxPWM = 250;
  Param.Tlumienie = 10;
  PWM = 0;

// kalibracja wewnętrznego oscylatora PPM
  OSCCAL = eeprom_read_byte(E2END); // załaduj stałą kalibracji

// konfiguracja TIMER2 (generator PWM inwertera MOSFET)
  StartPWM;
  OCR2 = 128;
  
// przerwania z timerów
  TIMSK = (1<<OCIE2)|(1<<TOIE2);

// reakcja na zbocze INT0
  MCUCR = (1<<ISC01)|(0<<ISC00);
  GICR  = (1<<INT0);

// włączenie obsługi przerwań
  sei();

  while(1) {
    int delta = NowaPozycja - Enkoder;
    if(delta == 0) {
      LEDR_off;
      LEDY_off;
      PWM = 0;
      StopPWM;
      OCR2 = PWM;
      LoadPhasePWM(MotorStop); // ustaw motek H na hamowanie elktrodynamiczne
    }  
    if(delta < 0) {
      LEDR_on;
      LEDY_off;
      SetPWM(-delta);
      LoadPhasePWM(MotorLewo); // ustaw motek H na wirowanie w lewo
      StartPWM;
    }  
    if(delta > 0) {
      LEDY_on;
      LEDR_off;
      SetPWM(delta);
      LoadPhasePWM(MotorPrawo); // ustaw motek H na wirowanie w rawo
      StartPWM;
    }  
  }
}