// AuxFeeder made from RoboJoint with A4882 and 16U2, Servo-Pulse on PC7
// 2021-05-11 st-js V0.1

#include <SpeedyStepper.h>

// Festlegung der Pin-Zuordnung
const byte StepInputPin = 8;      // Port C7 (INT4)
const byte SmStepPin = 13;        // Port D0
const byte SmDirPin = 14;         // Port D1
const byte FrInputPin = 19;       // Port D6
const byte SmMS1Pin = 11;         // Port C4
const byte SmMS2Pin = 10;         // Port C5
const byte DirInputPin = 9;       // Port C6
const byte FfInputPin = 0;        // Port B0
const byte ManInputPin = 4;       // Port B4
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

const long InputStepsXmm = 1600;               // Anzahl Impulse für Xmm am StepInputPin
const long OutputStepsXmm = 114;              // Anzahl Impulse für Xmm am Feeder-Motor
const long OutputStepsManual = 40*OutputStepsXmm;    // Vorschub der FF- und der FR-Taste

const int MotorStepsPerRevolution = 200;      // Anzahl Vollschritte für eine Umdrehung des Motors
const byte MicroSteps = 16;                    // 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 = 6000;                  // Maximale Schrittgeschwindigkeit des Motors in Schritten pro Sekunde
const int StepperAccelerationInStepsPerSecondPerSecond = 12000;  // Beschleunigung in Schritten pro Sekunde^2

int InterruptNumber;                          // Interrupt-Nummer (zur einfachen Kontrolle in einer Variablen gespeichert!)

// Deklaration der Variablen der Interrupt-Routine
volatile long InputSteps;                     // Zähler für die Schritt Impulse
volatile int FeedXmm = 0;                     // Zähler zum Ausführen der Vorschübe 

SpeedyStepper stepper;

void setup() {

  // Pins initialisieren
  pinMode(StepInputPin, INPUT_PULLUP);
  pinMode(DirInputPin, INPUT_PULLUP);
  pinMode(FrInputPin, INPUT_PULLUP);
  pinMode(FfInputPin, INPUT_PULLUP);
  pinMode(ManInputPin, 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);
  }

  InterruptNumber = digitalPinToInterrupt(StepInputPin);    // 4 would be nice for digital pin 8 on 16U2
  attachInterrupt(InterruptNumber, GetInputSteps, RISING);

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

}

void loop() {

  if (digitalRead(ManInputPin) == LOW) {
    // Manueller Betrieb?
    digitalWrite(SmEnablePin, HIGH);                        // Strittmotor Treiber AUS)
    InputSteps = 0;
    FeedXmm = 0;
   
    if (digitalRead(FfInputPin) == LOW) {
      digitalWrite(SmEnablePin, LOW);                       // Strittmotor Treiber freigeben (Aktiv LOW!)
      stepper.moveRelativeInSteps(OutputStepsManual);
      InputSteps = 0;
      FeedXmm = 0;
    }
    else if (digitalRead(FrInputPin) == LOW) {
      digitalWrite(SmEnablePin, LOW);                       // Strittmotor Treiber freigeben (Aktiv LOW!)
      stepper.moveRelativeInSteps(-OutputStepsManual);
      InputSteps = 0;
      FeedXmm = 0;
    }
  }  
  else {

    if (FeedXmm > 0) {
      digitalWrite(SmEnablePin, LOW);                         // Strittmotor Treiber freigeben (Aktiv LOW!)      
      // Flag löschen!
      --FeedXmm;
      stepper.setupRelativeMoveInSteps(OutputStepsXmm);

      while(!stepper.motionComplete())          // Schritte laden
      {
        stepper.processMovement();              // Voeschub ausführen
      }
      // Stromverbrauch kann so gesenkt werden!
      digitalWrite(SmEnablePin, HIGH);                        // Strittmotor Treiber AUS)    
      }
      
  }
}

void GetInputSteps() {
  // Zählen der Schritt-Impulse an StepInputPin
  cli();    // Sperren Interrups
  if (digitalRead(DirInputPin) == HIGH) {
    ++InputSteps;   // inkrement um 1 Schritt
  }
  else {
    --InputSteps;   // dekrement um 1 Schritt
    }
  if (InputSteps >= InputStepsXmm) {
    ++FeedXmm;      // ein zusätzlicher Vorschub
    InputSteps = InputSteps - InputStepsXmm;
  }
  // freigeben der Interrupts
  sei();
}