// Input-Ports
#define TRIMMER1           A0
#define JOYSTICK1          A1
#define TRIMMER2           A2
#define JOYSTICK2          A3
#define TRIMMER3           A4
#define JOYSTICK3          A5
#define AKKUSTATUS         A7 


// Generierung PPM-Signal
#define PPM_PORT            13     // Output-Pin
#define FRAME_LENGTH        20000  // 23ms  Paketlänge
#define PULSE_LENGTH        350    // 420µs Pulslänge (200µs..500µs) (gemessen an FM6014)
#define PRESCALER           CS11   // Prescaler 8, 0.5µs-Takt des Timers
#define MIKRO_SECONDS       2      // Korrektur des Output-Compare-Registers: interner Takt mit Prescaler 8 ist 0,5µs, daher Faktor 2 um auf 1µs Taktzähler umzurechnen
#define PPM_CENTER_VALUE    1500   // 1,5ms Mittelstellung 
#define PPM_LEVEL           500    // 0,5ms Signalhub plus/minus
#define PPM_CHANNELS        8      // Anzahl der PPM-Kanäle in einem Paket

// Ausgangssignale an den Sender
// Container für PPM-Signale für jeden einzelnen Servo
int  ppm[PPM_CHANNELS]   = {PPM_CENTER_VALUE};
unsigned long t0         = millis();

void setup()
{
  Serial.begin(9600);

  // PPM-Generierung und Timer für Interrupt-Erzeugung
  for (int i=0; i < PPM_CHANNELS; i++)  ppm[i] = PPM_CENTER_VALUE;
  
  pinMode(PPM_PORT, OUTPUT);
  digitalWrite(PPM_PORT, LOW);
  cli();
  TCCR1A  = 0;                // TCCR1 Register zurücksetzen
  TCCR1B  = 0;                // TCCR1 Register zurücksetzen
  OCR1A   = 100;              // Compare-Register zum Auslösen des Interrupts
  TCCR1B |= (1 << WGM12);     // CTC Mode (Clear Timer on Compare Match)
  TCCR1B |= (1 << PRESCALER); // Prescaler des Systemtakts für Inkrement von Timer1
  TIMSK1 |= (1 << OCIE1A);    // Aktivierung ISR bei Timer-Compare mir OCR1A-Register
  sei();
}



void loop()
{
  unsigned long dt = millis() - t0;
  float arg        = 2.0f*3.14f*dt/(1000.0f*5.0f);

  int   akkuStatus      = analogRead(AKKUSTATUS);
  float akkuSpannung    = 4.6f*akkuStatus/1023.0f + 4.5f; // Umrechnung 5V Referenz + 4.7V Abfall an Zener-Diode

  // Joystick-Positionen:  
  int  joystick1 = 500*sin(arg) + PPM_CENTER_VALUE;
  int  joystick2 = 500*sin(arg) + PPM_CENTER_VALUE;
  int  joystick3 = 500*sin(arg) + PPM_CENTER_VALUE; // analogRead(JOYSTICK3);
  int  joystick4 = 500*sin(arg) + PPM_CENTER_VALUE; // Wird nicht eingelesen, da Anschluss durch Akkuspannung belegt ist, daher Sinus-Funktion

 
  // Trimmer-Positionen:  
  int  trimmer1  = 500*sin(arg) + PPM_CENTER_VALUE; // analogRead(TRIMMER1);
  int  trimmer2  = 500*sin(arg) + PPM_CENTER_VALUE; // analogRead(TRIMMER2);
  int  trimmer3  = 500*sin(arg) + PPM_CENTER_VALUE; // analogRead(TRIMMER3);
  int  trimmer4  = 500*sin(arg) + PPM_CENTER_VALUE; // Wird nicht eingelesen, da Anschluss durch Akkuspannung belegt ist, daher Sinus-Funktion


  // ----------------------------------------------------------------------------------------
  ppm[0] = constrain(joystick1, PPM_CENTER_VALUE-PPM_LEVEL, PPM_CENTER_VALUE+PPM_LEVEL);
  ppm[1] = constrain(joystick2, PPM_CENTER_VALUE-PPM_LEVEL, PPM_CENTER_VALUE+PPM_LEVEL);
  ppm[2] = constrain(joystick3, PPM_CENTER_VALUE-PPM_LEVEL, PPM_CENTER_VALUE+PPM_LEVEL);
  ppm[3] = constrain(joystick4, PPM_CENTER_VALUE-PPM_LEVEL, PPM_CENTER_VALUE+PPM_LEVEL);
  ppm[4] = constrain( trimmer1, PPM_CENTER_VALUE-PPM_LEVEL, PPM_CENTER_VALUE+PPM_LEVEL);
  ppm[5] = constrain( trimmer2, PPM_CENTER_VALUE-PPM_LEVEL, PPM_CENTER_VALUE+PPM_LEVEL);
  ppm[6] = constrain( trimmer3, PPM_CENTER_VALUE-PPM_LEVEL, PPM_CENTER_VALUE+PPM_LEVEL);
  ppm[7] = constrain( trimmer4, PPM_CENTER_VALUE-PPM_LEVEL, PPM_CENTER_VALUE+PPM_LEVEL);
  
  char out[160];
  Serial.print("Akku: ");
  Serial.print(akkuSpannung);
  sprintf(out,"J1: %4d | J2: %4d | J3: %4d | J4: %4d (not connected)  ||   T1: %4d | T2: %4d | T3: %4d | T4: %4d (not connected)\n",
    joystick1, joystick2, joystick3, joystick4, trimmer1, trimmer2, trimmer3, trimmer4);
  Serial.print(out);

  delay(20);
}


// -----------------------------------------------------------------------------------------
// Interrupt-Service-Routine für PPM-Erzeugung
ISR(TIMER1_COMPA_vect)
{  
  static byte         channel   = 0;    // PPM-Kanal (Servo-Signale werden als Sequenz nacheinander ausgegeben)
  static unsigned int totalTime = 0;    // Gesamtdauer der PPM-Sequenz in Mikrosekunden
  static boolean      pulse     = true; 

  // Überflüssig, da CTC-Mode?
  // TCNT1 = 0;
  
  // Puls oder Pause?
  if (pulse) 
  { 
    digitalWrite(PPM_PORT, HIGH);         // Puls wird gesetzt
    OCR1A = PULSE_LENGTH * MIKRO_SECONDS; // Pulsbreite -> Nachfolgende Interruptauslösung zum Start der Pause des PPM-Signals
    pulse = false;                        // Pause folgt auf Puls im nächsten Aufruf der ISR
  } 
  
  // Pause folgt auf den Puls: Gesamtdauer Puls+Pausenzeit codiert das Stellsignal für den aktuellen PPM-Kanal channel
  else
  { 
    digitalWrite(PPM_PORT, LOW);    // Puls wird zurückgesetzt, Pause im PPM-Signal (Servoauslenkung codiert durch die Länge der Pause)
    pulse = true;                   // Puls folgt auf Pause (im nächsten Aufruf der ISR)

    if (channel >= PPM_CHANNELS)
    {
      channel    = 0;               // Zurücksetzen auf Kanal 0 (Sequenz startet von vorn), vorhergehender Puls somit Ende des PPM-Frames
      totalTime += PULSE_LENGTH;    // Gesamtdauer der PPM-Sequenz plus Dauer des letzen Pulses, der das PPM-Signal abschließt
      OCR1A      = (FRAME_LENGTH - totalTime) * MIKRO_SECONDS; // Pausenzeit bis zum Start der neuen PPM-Sequenz, festes Raster durch FRAME_LENGTH gegeben.
      totalTime  = 0;               // Zurücksetzen der Gesamtdauer für die neue PPM-Sequenz
    }
    else
    {
      OCR1A      = (ppm[channel] - PULSE_LENGTH) * MIKRO_SECONDS; // Festlegen der Pulspause für den aktuellen PPM-Kanal, Pulslänge zählt dabei in der Codierung mit
      totalTime += ppm[channel];    // Sukzessive Aufsummierung der Zeitlänge der gesamten PPM-Sequenz mit jedem Kanal, Puls zu Beginn wird dabei mitgezählt
      channel++;                    // Hochzählen des Kanals für nächsten Servo in der PPM-Sequenz -> Vorbereitung für den nächsten Aufruf der ISR
    }     
  }

}
// -----------------------------------------------------------------------------------------