/*
Takt-/Richtungsgenerator für 2 x Schrittmotore, ATmega48...328

Per Timer1 werden zwei Schrittfrequenzen generiert, die im Bereich 
FREQUENZ_MIN (10 Hz) und FREQUENZ_MAX (ca. 30 kHz) liegen können 
und per RS232 vorgegeben werden.

Die Befehle werden durch das Steuerzeichen ESC oder einen '.' eingeleitet,
welche per ser. Schnittstelle (mit DEF_BAUDRATE) empfangen werden:

.1000A  stellt die Frequenz von Kanal A auf 1 kHz ein
.2000B  stellt die Frequenz von Kanal B auf 2 kHz ein
.-2000B wiederum 2 kHz an Ausgang B allerdings mit anderer Drehrichtung
.0A     Motor A wird angehalten

Die Signale werden als Impulse >= 5 µs an den Ausgängen OCR1A (PortB1)
und OCR1B (PortB2) ausgegeben. Die entsprechenden Richtungssignale liegen
an PortB0 (Kanal A) und PortB3 (Kanal B).


Die eingestellten Werte können auch ausgelesen werden, indem der Befehl ohne Paramter 
angegeben wird:

.A    liefert als Antwort die Frequenz zu Kanal A in der Form:  A1000<CR><LF>
.B    liefert als Antwort die Frequenz zu Kanal B in der Form:  B-2000<CR><LF>
.V    gibt die Version aus

http://www.mino-elektronik.de

Alle Angaben ohne Gewaehr !
2016-03-09

*/

#define   VERSION "mino STEPPER_2x_seriell V1.0 2016-03-09"

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <ctype.h>
#include <math.h>

#define BIT(x)  (1<<x)
#define F_CPU           16000000  // 16MHz ggf. an eff.Frequenz anpassen
#define DEF_BAUDRATE    19200

#define DEF_PERIODE     20000     // 50Hz
#define FREQUENZ_MIN    10
#define FREQUENZ_MAX    30000

#define ESC_BIT         1
#define MINUS_BIT       2
#define ESC             27
#define MINUS           '-'       // Vorzeichen
#define KANAL_A         'A'       // Frequenz in Hz
#define KANAL_B         'B'       // Frequenz in Hz
#define SENDE_VERSION   'V'


uint8_t  ser_status;

#define RXD_PUFFER_LEN  10
#define TXD_PUFFER_LEN  10
#define LF              10
#define CR              13


#define STEP_A        1           // an PB.1
#define DIR_A         0           // an PB.0  
#define STEP_B        2           // an PB.2  
#define DIR_B         3           // an PB.3  

#define FILTER        10          // Für langsame Beschleunigungen nach Bedarf erhöhen


volatile uint8_t  neue_werte_fertig_a, richtung_a, sperre_a,
                  neue_werte_fertig_b, richtung_b, sperre_b;
volatile int32_t  sollfrequenz_a, sollfrequenz_b;       // Vorgabe per RS232
volatile uint32_t neue_periode_a, neue_periode_b;       // errechneter Wert für f

static uint8_t    txd_puffer[TXD_PUFFER_LEN+1], rxd_puffer[RXD_PUFFER_LEN+1];
volatile uint8_t  rxd_puffer_lese_zeiger, rxd_puffer_schreib_zeiger,
                  txd_puffer_lese_zeiger, txd_puffer_schreib_zeiger;
volatile uint8_t  rxd_puffer_anzahl, txd_puffer_anzahl;

volatile uint8_t  ein_aus_flag;               
int32_t zahl=0;

// nicht ins RAM kopieren
const uint32_t tab[] PROGMEM = {1000000000, 100000000, 10000000, 1000000,
                    100000, 10000, 1000, 100, 10, 1, 0};


ISR (USART_RX_vect)
{
uint8_t temp;
  UCSR0B &= ~BIT(RXCIE0);                           // rx zunaechst sperren 
  sei();                                            // Ints zulassen
  temp = UDR0;                                      // zeichen immer lesen
  if(rxd_puffer_anzahl < RXD_PUFFER_LEN) {
    rxd_puffer[rxd_puffer_schreib_zeiger++] = temp;
    if(rxd_puffer_schreib_zeiger >= RXD_PUFFER_LEN) rxd_puffer_schreib_zeiger = 0;
    rxd_puffer_anzahl++;
  }
  UCSR0B |= BIT(RXCIE0);                            // rx wieder freigeben
}

ISR (USART_UDRE_vect)
{
  UCSR0B &= ~BIT(UDRIE0);                           // sender zunaechst sperren
  sei();                                            // Ints zulassen
  if(txd_puffer_anzahl) {                           // noch zeichen im puffer
    UDR0 = txd_puffer[txd_puffer_lese_zeiger];      // Zeichen senden: ins register
    if(++txd_puffer_lese_zeiger >= TXD_PUFFER_LEN) txd_puffer_lese_zeiger = 0;
    txd_puffer_anzahl--;
    UCSR0B |= BIT(UDRIE0);                          // sender wieder freigeben
  }
}

void set_baudrate1(uint32_t baudrate)
{
  UBRR0L=((F_CPU/16/(baudrate/10)+5)/10) - 1;       // runden !!! 
  UBRR0H=0;
}

void clr_txd_puffer(void)
{
  UCSR0B &= ~BIT(UDRIE0);                           // int sofort sperren 
  txd_puffer_lese_zeiger = 0;
  txd_puffer_schreib_zeiger = 0;
  txd_puffer_anzahl = 0;
}

void clr_rxd_puffer(void)
{
  UCSR0B &= ~BIT(RXCIE0);                           // nicht unterbrechen 
  rxd_puffer_anzahl = 0;
  rxd_puffer_lese_zeiger = 0;
  rxd_puffer_schreib_zeiger = 0;
  UCSR0B |= BIT(RXCIE0);                            // rx immer aktiv 
}

void init_serio1(uint32_t baudrate)
{
  UCSR0B = 0x0;                                     // TX und RX sperren 
  clr_rxd_puffer();
  clr_txd_puffer();
  set_baudrate1(baudrate);
  UCSR0B |= BIT(RXEN0);
  UCSR0B |= BIT(TXEN0);
  UDR0;                                             // evtl. Zeichen l”schen 
  UCSR0B |= BIT(RXCIE0);                            // rx immer aktiv 
  sei();
}

void ser_ausgabe(uint8_t c)
{
  while(txd_puffer_anzahl >= TXD_PUFFER_LEN);       // warten, wenn puffer voll ist 
  txd_puffer[txd_puffer_schreib_zeiger++] = c;      // zeichen in puffer geben 
  if(txd_puffer_schreib_zeiger >= TXD_PUFFER_LEN) txd_puffer_schreib_zeiger = 0;
  UCSR0B &= ~BIT(UDRIE0);                           // nicht unterbrechen 
  txd_puffer_anzahl++;
  UCSR0B |= BIT(UDRIE0);                            // ausgabe starten, wenn zul„ssig 
}

uint8_t lese_rxd_puffer(void)
{
uint8_t c;
  if(rxd_puffer_anzahl == 0) return(0);
  c = rxd_puffer[rxd_puffer_lese_zeiger++];
  if(rxd_puffer_lese_zeiger >= RXD_PUFFER_LEN) rxd_puffer_lese_zeiger = 0;
  UCSR0B &= ~BIT(RXCIE0);                           // nicht unterbrechen 
  rxd_puffer_anzahl--;
  UCSR0B |= BIT(RXCIE0);
  return(c);
}

uint8_t lese_rxd_puffer_anzahl(void)
{return(rxd_puffer_anzahl);}

uint8_t lese_txd_puffer_anzahl(void)
{return(txd_puffer_anzahl);}

void sende_cr_lf(void)
{
  ser_ausgabe(10);
  ser_ausgabe(13);
}

void sende_str(char * s)
{
  while(*s) ser_ausgabe(*s++);
  sende_cr_lf();
}

void sende_version(void)
{
  sende_str(VERSION);
}

// wandelt int32_t nach String; mit0 = 1 zeigt fuehrende '0'en an.
void long2a(int32_t l, char *s, uint8_t mit0)
{
uint32_t temp;
uint8_t i=0,j;
  if(l || mit0) {
    if(l < 0) {
      l = -l;
      *s++ = '-';
    }
    if(mit0) {
      i=2;
      while((uint32_t) pgm_read_dword(&tab[i]) > l) {
          i++;
          *s++ = '0';
      }
    } else
      while((uint32_t) pgm_read_dword(&tab[i]) > l) i++;
      do {
        temp = (uint32_t) pgm_read_dword(&tab[i]);
        j = '0';
        if(temp) {
            for(;j <= '9'; j++) {
              l-=temp;
              if(l < 0) {
                l += temp;
                break;
              }
            }
            *s++ = j;
            i++;
        }
      } while(temp);
    } else *s++ = '0';
  *s = 0;
}

// Ausgabe eines int32_t Wertes linksbuendig ohne fuehrende '0'
void sende_integer(int32_t l)
{
char s[15];
  long2a(l,s,0); 
  sende_str(s);
}

// integer Zahlen einlesen und Befehl auswerten
void bewerte_ser_input(void)
{
static uint8_t ziffern;
uint8_t temp;
  if(!lese_rxd_puffer_anzahl()) return;         // kein zeichen
  temp = lese_rxd_puffer();                     // sonst lesen
  if(temp == ESC || temp == '.') {              // Datensatz beginnt mit ESC 
    ser_status = ESC_BIT;
    ziffern = 0;
    zahl = 0;
    return;
  }
  if((ser_status & ESC_BIT) == 0) {             // kein ESC_bit
    ser_status = 0;                             // dann alles auf Anfang
    ziffern = 0;
    zahl=0;                                     // vermeintlich doppelte Zuweisungen
    return;                                     // werden vom Compiler optimiert
  }
  if(temp==MINUS) {
    ser_status = ESC_BIT+MINUS_BIT;
    zahl = 0;
    ziffern = 1;
    return;
  } 
  if(temp>= '0' && temp<= '9') {                // Zahlenwert einlesen
    ziffern++;
    zahl = zahl * 10L + (temp & 0xf);           // x 10 + neue Ziffer 
    return;
  }

  // ab hier nur noch befehle 

  temp = toupper(temp);
  switch(temp) {
    case KANAL_A:
      if(ziffern) {                             // neue Frequenz schreiben
        if(zahl > FREQUENZ_MAX) zahl = FREQUENZ_MAX;
        if(ser_status&MINUS_BIT) zahl = -zahl;
        sollfrequenz_a = zahl;
      } else {                                  // andernfalls aktuellen Wert ausgeben
        ser_ausgabe(temp);
        sende_integer(sollfrequenz_a);
      }
    break;
  
    case KANAL_B:
      if(ziffern) {                             // neue Frequenz schreiben
        if(zahl > FREQUENZ_MAX) zahl = FREQUENZ_MAX;
        if(ser_status&MINUS_BIT) zahl = -zahl;
        sollfrequenz_b = zahl;
      } else {                                  // andernfalls aktuellen Wert ausgeben
        ser_ausgabe(temp);
        sende_integer(sollfrequenz_b);
      }
    break;
    case SENDE_VERSION:                         // Versionsanzeige
      sende_version();
    break;
  } //switch
  ser_status = 0;                               // auf neuen Befehl warten
  ziffern = 0;
  zahl=0;
}


int32_t filter_a(int32_t neuer_wert)            // zu schnelle Aenderungen vermeiden
{
static int32_t summe;     
   summe -= summe/FILTER;
   summe += neuer_wert;
   return summe/FILTER;
}

int32_t filter_b(int32_t neuer_wert)            // zu schnelle Aenderungen vermeiden
{
static int32_t summe;     
   summe -= summe/FILTER;
   summe += neuer_wert;
   return summe/FILTER;
}


void t1_sync(void)                              // wartet auf Überlauf von Timer1
{
  while(!(TIFR1 & BIT(TOV1)));
  TIFR1 = BIT(TOV1);
}

ISR(TIMER1_COMPA_vect)
{
static uint8_t akt_richtung_a;
static uint32_t nachlade_temp;
static uint32_t periode =  DEF_PERIODE;         // default Startwert
uint32_t temp = 0xffff;

  if(nachlade_temp <= temp) temp = nachlade_temp;         
  else if(nachlade_temp < 0x1ffff) temp =nachlade_temp/2; 
  nachlade_temp -= temp;                        // laengere Intervalle stueckweise abarbeiten
  OCR1A += temp;
  TCCR1A &= ~BIT(COM1A0);                       // OC1A loeschen falls gesetzt
  TCCR1C = BIT(FOC1A);                          // per Force Output Compare erzwingen

  if(nachlade_temp) return;                     // Intervall noch nicht abgelaufen
  if(akt_richtung_a) PORTB |= BIT(DIR_A);
  else PORTB &= ~BIT(DIR_A);
  if(!sperre_a) {
    TCCR1A |= BIT(COM1A0);                      // beim nächsten Interrupt OC1B synchron setzen
  }
  nachlade_temp = periode;
  if(neue_werte_fertig_a) {                     // update synchronisieren
    periode = neue_periode_a;
    akt_richtung_a = richtung_a;
    neue_werte_fertig_a = 0;
  }
}

ISR(TIMER1_COMPB_vect)
{
static uint8_t akt_richtung_b;
static uint32_t nachlade_temp;
static uint32_t periode =  DEF_PERIODE;         // default Startwert
uint32_t temp = 0xffff;

  if(nachlade_temp <= temp) temp = nachlade_temp;         
  else if(nachlade_temp < 0x1ffff) temp =nachlade_temp/2; 
  nachlade_temp -= temp;                        // laengere Intervalle stueckweise abarbeiten
  OCR1B += temp;
  TCCR1A &= ~BIT(COM1B0);                       // OC1B loeschen falls gesetzt
  TCCR1C = BIT(FOC1B);                          // per Force Output Compare erzwingen

  if(nachlade_temp) return;                     // Intervall noch nicht abgelaufen
  if(akt_richtung_b) PORTB |= BIT(DIR_B);
  else PORTB &= ~BIT(DIR_B);
  if(!sperre_b) {
    TCCR1A |= BIT(COM1B0);                      // beim nächsten Interrupt OC1B synchron setzen
  }
  nachlade_temp = periode;
  if(neue_werte_fertig_b) {                     // update synchronisieren
    periode = neue_periode_b;
    akt_richtung_b = richtung_b;
    neue_werte_fertig_b = 0;
  }
}


void init_timer1()
{

  DDRB  |= BIT(STEP_A) | BIT(DIR_A) |           // STEP + DIR Ausgang Kanal A
           BIT(STEP_B) | BIT(DIR_B);            // STEP + DIR Ausgang Kanal B

  TCCR1B = 0;                                   // stoppen
  TCNT1  = 0;                             
  TCCR1B = BIT(CS10);                           // freilaufend mit vorteiler /1
  TCCR1A = BIT(COM1A1) | BIT(COM1B1);
  OCR1A  = 0;
  OCR1B  = 0; 
  TIFR1  = BIT(OCF1A) | BIT(OCF1B);             // OCF1x flag loeschen
  TIMSK1 |= BIT(OCIE1A) | BIT(OCIE1B);
  sei();
}



// Hauptprogramm
int main(void)
{
int32_t f_temp;
  CLKPR = 0x80; CLKPR = 0x0;                    // CKDIV auf 0; voller CPU-Takt

  init_timer1();
  init_serio1(DEF_BAUDRATE);
//  sende_version();

  while(1) {
    bewerte_ser_input();
    t1_sync();                                  // Verzoegerung fuer Filter

// Berechnung für Stepper A
    if(!neue_werte_fertig_a) { 
      f_temp = filter_a(sollfrequenz_a);        // gemittelter Wert

      richtung_a = 0;
      if(f_temp >= 0) richtung_a = 1;           // Richtung-Signal abhaengig vom Vorzeichen
      f_temp = abs(f_temp);                     // eff. Ausgangsfrequenz
      if(f_temp < FREQUENZ_MIN) {
        f_temp = FREQUENZ_MIN;                  // nach unten begrenzen
        sperre_a = 1;
      } else sperre_a = 0;
      neue_periode_a = F_CPU/f_temp;            // eff. Periodendauer
      neue_werte_fertig_a = 1;  
    } 

// Berechnung für Stepper B      
    if(!neue_werte_fertig_b) { 
      f_temp = filter_b(sollfrequenz_b);        // gemittelter Wert

      richtung_b = 0;
      if(f_temp >= 0) richtung_b = 1;           // Richtung-Signal abhaengig vom Vorzeichen
      f_temp = abs(f_temp);                     // eff. Ausgangsfrequenz
      if(f_temp < FREQUENZ_MIN) {
        f_temp = FREQUENZ_MIN;                  // nach unten begrenzen
        sperre_b = 1;
      } else sperre_b = 0;
      neue_periode_b = F_CPU/f_temp;            // eff. Periodendauer
      neue_werte_fertig_b = 1;  
    } 
    

  }
}