// RoboJoint with A4882 and 16U2, Servo-Pulse on PC7
// 2020-02-02 st-js
// V 0.02
// 2020-02-07 st-js V1.0

#include <FlexyStepper.h>

// Festlegung der Pin-Zuordnung
const byte ServoInputPin = 8;     // Port C7 (INT4)
const byte SmDirPin = 14;         // Port D1
const byte SmStepPin = 13;        // Port D0
const byte HomeSwitchPin = 9;     // Port C6
const byte SmMS1Pin = 11;         // Port C4
const byte SmMS2Pin = 10;         // Port C5
const byte SmResetPin = 5;        // Port B5 Achtung Active LOW
const byte SmEnablePin = 6;       // Port B6 Achtung Active LOW
const byte SmSleepPin = 7;        // Port B7 Achtung Active LOW


// Deklaration von Konstanten und Variablen
const byte PulseTimeMeanFactor = 8;           // Anzahl Werte für den Mittelwert der Servo-Pulsweitenmessung

// Zeitenheit = Tu (entsprechend dem CPU-Takt und Timer-Vorteiler!)
// Werte für 16MHz und Vorteiler = 1
const int MinPulseTime = 15600;               // Minimum Pulsweite in Tu entspricht einer halben -Drehung
const int MaxPulseTime = 31600;               // Maximum Pulsweite in Tu entspricht einer halben +Drehung

const int MotorStepsPerRevolution = 200;      // Anzahl Vollschritte für eine Umdrehung des Motors
const byte MicroSteps = 4;                    // Anzahl Schritte pro Vollschritt der Steuerung
const byte GearRatio = 15;                    // Untersetzung des Getriebes
const int StepsPerHalfTurn = MotorStepsPerRevolution * MicroSteps * GearRatio / 2;    // Anzahl Schrittimpulse für eine halbe Drehung

// Geschwindigkeiten und Beschleunigung sowie Start-Position
const int StepperSpeedInStepsPerSecond = 4000;                  // Maximale Schrittgeschwindigkeit des Motors in Schritten pro Sekunde
const int StepperAccelerationInStepsPerSecondPerSecond = 2000;  // Beschleunigung in Schritten pro Sekunde^2
const int HomeingSpeedInStepsPerSecond = 100;                   // Geschwindigkeit zum Anfahren des Endschalters
const int HomeingDirection = -1;                                // Richtung der Endschalter-Anfahrt (1 oder -1)
const long HomeingMaxSteps = 3 * StepsPerHalfTurn;              // Maximale Schrittzahl zum Anfahren des Endschalters
const long InitialPosition = StepsPerHalfTurn * -1;             // Position nach des Endschalters in Schritten

float StepsPerTu = 0;                         // Anzahl Schritte pro Tu
float MaxPulseWindowTu = 0;                   // Maximales Zeitfenster in Tu


int PulseTimeMeanCounter = 0;                 // Zähler der Einzelmessungen
int InterruptNumber;                          // Interrupt-Nummer (zur einfachen Kontrolle in einer Variablen gespeichert!)

long PulseTimeMeanSample[PulseTimeMeanFactor];   //Array für die Bildung des gleitenden Mittelwerts
long PulseTimeMeanSum = 0;                    // Summe der Einzelmessungen für die Mittelwert-Bildung
long PulseTimeMean = 0;                       // Mittelwert gebildet über PulseTimeMeanFactor Messungen

long StepperSetPoint = 0;                     // Sollwert in Schritten des Motors
long StepperAktPos = 0;                       // Aktuelle Position des Motors
long StepperToGo = 0;                         // Differenz zwischen Soll- und Ist-Wert

bool CalcNewPosition = false;                 // Flag zum Aufteilen der Positionsberechnungen um die Ablaufgeschwindigkeit zu erhöhen

// Deklaration der Variablen der Interrupt-Routine
volatile long StartTime;                      // Start-Zeit des Servo-Pulses in der Interrupt-Routine
volatile long StopTime;                       // Stop-Zeit des Pulses
volatile long PulseTime;                      // Puls-Zeit der Einzelmessung
volatile bool NewPulseTime = false;        // Flag zur Anzeige einer neuen Messung


FlexyStepper stepper;

void setup() {

  Serial.begin(38400);

  // Pins initialisieren
  pinMode(ServoInputPin, INPUT_PULLUP);
  pinMode(HomeSwitchPin, INPUT_PULLUP);
  // pinMode(SmDirPin, OUTPUT);         // wird in Schrittmotor UPs initialisiert
  // pinMode(SmStepPin, OUTPUT);        // wird in Schrittmotor UPs initialisiert
  pinMode(SmMS1Pin, OUTPUT);
  pinMode(SmMS2Pin, OUTPUT);
  // pinMode(SmResetPin, OUTPUT);       // nicht benötigt, ist mit externem PullUp versehen
  pinMode(SmEnablePin, OUTPUT);
  // pinMode(SmSleepPin, OUTPUT);       // nicht benötigt, ist mit externem PullUp versehen

  // Micro-Schritte mit SmMS1 und SmMS2 einstellen
  if (MicroSteps == 1) {
    // Vollschritt-Mode
    digitalWrite(SmMS1Pin, LOW);
    digitalWrite(SmMS2Pin, LOW);
  }
  else if (MicroSteps == 2) {
    // Halbschritt-Mode
    digitalWrite(SmMS1Pin, HIGH);
    digitalWrite(SmMS2Pin, LOW);
  }
  else if (MicroSteps == 4) {
    // Viertelschritt-Mode
    digitalWrite(SmMS1Pin, LOW);
    digitalWrite(SmMS2Pin, HIGH);
  }
  else if (MicroSteps == 16) {
    // Sechzehntelschritt-Mode
    digitalWrite(SmMS1Pin, HIGH);
    digitalWrite(SmMS2Pin, HIGH);
  }
  else {
    // undefiniert -> Vollschritt-Mode einstellen!
    // Achtung die Schrittzahl-Berechnung stimmt nicht!
    digitalWrite(SmMS1Pin, LOW);
    digitalWrite(SmMS2Pin, LOW);
  }

  // Timer1 konfigurieren
  TCCR1A = 0;               // setzten TCCR1A Register auf 0
  TCCR1B = 0;               // auch TCCR1B
  // TCCR1B |= (1 << CS12);    //  prescaler 256 / 1024
  // TCCR1B |= (1 << CS11);    //  prescaler 8 / 64
  TCCR1B |= (1 << CS10);    //  prescaler 1 / 64 / 1024

  InterruptNumber = digitalPinToInterrupt(ServoInputPin);    // 4 would be nice for digital pin 8 on 16U2
  attachInterrupt(InterruptNumber, GetPulseTime, CHANGE);

  // Variable zur Sollwertberechnung berechnen
  MaxPulseWindowTu  = (MaxPulseTime - MinPulseTime);        // Bereich in Tu der Pulsweite
  StepsPerTu = 2 * StepsPerHalfTurn / MaxPulseWindowTu;     // Anzahl Schritte pro Zeiteinheit

  // Schritt-Motor-UPs initialisieren
  stepper.connectToPins(SmStepPin, SmDirPin);
  stepper.setSpeedInStepsPerSecond(StepperSpeedInStepsPerSecond);
  stepper.setAccelerationInStepsPerSecondPerSecond(StepperAccelerationInStepsPerSecondPerSecond);

  digitalWrite(SmEnablePin, LOW);                           // Strittmotor Treiber freigeben (Aktiv LOW!)

  // Motor auf "End-Position" (Home) bringen
  stepper.moveToHomeInSteps(HomeingDirection, HomeingSpeedInStepsPerSecond, HomeingMaxSteps, HomeSwitchPin);

  // Dies wird die Stellung, die der minimalen Servo-Pulsweite entspricht

  stepper.setCurrentPositionInSteps(StepsPerHalfTurn * -1);

}

void loop() {

  if (CalcNewPosition) {                      // Positionsberechnung nach der Durchschnittsberechnung der Servo-Pulsbreite
    // dies dient der Beschleunigung des Ablaufs
    CalcNewPosition = false;
    // Pulszeit limitieren
    if (PulseTimeMean > MaxPulseTime) {
      PulseTimeMean = MaxPulseTime;
    }
    else if (PulseTime < MinPulseTime) {
      PulseTimeMean = MinPulseTime;
    }
    StepperSetPoint = (StepsPerTu * (PulseTimeMean - MinPulseTime)) - StepsPerHalfTurn;
  }

  if (NewPulseTime) {
    // Flag löschen!
    NewPulseTime = false;
    // zuest den gleitenden Mittelwert der Pulsweite über die in PulseTimeMeanFactor definierte Anzahl bilden

    PulseTimeMeanSample[PulseTimeMeanCounter] = PulseTime;
    PulseTimeMeanCounter = PulseTimeMeanCounter + 1;
    if (PulseTimeMeanCounter >= (PulseTimeMeanFactor)) {
      PulseTimeMeanCounter = 0;
    }

    PulseTimeMeanSum = 0;
    for (int i = 0; i < PulseTimeMeanFactor; i++) {
      PulseTimeMeanSum += PulseTimeMeanSample[i];
    }

    PulseTimeMean = PulseTimeMeanSum / PulseTimeMeanFactor;
    CalcNewPosition = true;
  }

  // ist eine neue Sollposition gefordert?
  StepperAktPos = stepper.getCurrentPositionInSteps();
  StepperToGo = abs(abs(StepperAktPos) - abs(StepperSetPoint));

  if ( StepperToGo > 30) {    // nur Ausführen, wenn 30 Schritte zu fahren sind...
    stepper.setTargetPositionInSteps(StepperSetPoint);
  }

  // Schritterzeugung, wenn nötig (-> Fahren auf Soll-Position mit Beschleinigen und Abbremsen!)
  stepper.processMovement();


}

void GetPulseTime() {
  // Messen der Pulsweite mit Timer1 !
  cli();    // Sperren Interrups
  if (digitalRead(ServoInputPin) == HIGH) {
    StartTime = TCNT1;
  }
  else {
    StopTime = TCNT1;
    PulseTime = StopTime - StartTime;
    if (PulseTime <  0) {                 // wenn negativ -> es fand ein Überlauf des Registers statt!
      PulseTime = PulseTime + 65535;      // 65535 addieren und die Messung stimmt!
    }
    NewPulseTime = true;
  }
  // freigeben der Interrupts
  sei();
}