#include <AccelStepper.h>

#define Disable PORTB|=0b00000010  //Schaltet Schrittmotortreiber ein
#define Enable  PORTB&=0b11111101  //Schaltet Schrittmotortreiber aus

#define DRV1_stepPin 6
#define DRV1_dirPin 11
#define DRV2_stepPin 13
#define DRV2_dirPin 12
#define DRV1_faultPin 4
#define DRV2_faultPin  5
#define EnablePin 15

const bool DRV1 = 0;
const bool DRV2 = 1;

byte DRV1_speed;
byte DRV1_dir;
byte DRV2_speed;
byte DRV2_dir;

AccelStepper DRV1stepper = AccelStepper(AccelStepper::DRIVER, DRV1_stepPin, DRV1_dirPin);
//AccelStepper DRV2stepper = AccelStepper(AccelStepper::DRIVER, DRV2_stepPin, DRV2_dirPin);

//Funktion zum einstellen des Microsteppings
void Microstep(bool DRV, byte value)
{
  byte true_value;
  DDRC = DDRC | 0b00111111;
  if (!DRV)
  { switch (value) {
      case 2: {
          PORTC &= 0b11111001;
          PORTC |= 0b00000001;
          true_value = 2;
          break;
        }
      case 4: {
          PORTC &= 0b11111010;
          PORTC |= 0b00000010;
          true_value = 4;
          break;
        }
      case 8: {
          PORTC &= 0b11111011;
          PORTC |= 0b00000011;
          true_value = 8;
          break;
        }
      case 16: {
          PORTC &= 0b11111100;
          PORTC |= 0b00000100;
          true_value = 16;
          break;
        }
      case 32: {
          PORTC &= 0b11111101;
          PORTC |= 0b00000101;
          true_value = 32;
          break;
        }
      default: {
          PORTC &= 0b11111000;
          true_value = 1;
          break;
        }
    }
  }
  else
  { switch (value)
    { case 2: {
          PORTC &= 0b11001111;
          PORTC |= 0b00001000;
          true_value = 2;
          break;
        }
      case 4: {
          PORTC &= 0b11010111;
          PORTC |= 0b00010000;
          true_value = 4;
          break;
        }
      case 8: {
          PORTC &= 0b11011111;
          PORTC |= 0b00011000;
          true_value = 8;
          break;
        }
      case 16: {
          PORTC &= 0b11100111;
          PORTC |= 0b00100000;
          true_value = 16;
          break;
        }
      case 32: {
          PORTC &= 0b11101111;
          PORTC |= 0b00101000;
          true_value = 32;
          break;
        }
      default: {
          PORTC &= 0b11000111;
          true_value = 1;
          break;
        }
    }
  }
  return;
}



void setup() {

  DRV1stepper.setEnablePin(EnablePin);
  //DRV2stepper.setEnablePin(EnablePin);
  Serial.begin(9600);
  
  // 1/4 Microstepping
  Microstep(DRV1, 4);
  Microstep(DRV2, 4);

  pinMode(EnablePin, OUTPUT);
  pinMode(DRV1_dirPin, OUTPUT);
  pinMode(DRV1_stepPin, OUTPUT);
  pinMode(DRV2_dirPin, OUTPUT);
  pinMode(DRV2_stepPin, OUTPUT);
  pinMode(DRV1_faultPin, INPUT);
  pinMode(DRV2_faultPin, INPUT);

  DRV1stepper.setMaxSpeed(1000);
  //DRV2stepper.setMaxSpeed(1000);
  DRV1stepper.setSpeed(500);
  //DRV2stepper.setSpeed(500);
  
  //DRV1stepper.enableOutputs();  //Schrittmotortreiber einschalten
  //DRV2stepper.enableOutputs();
  Enable; //Schrittmotortreiber einschalten
}

void loop() {

  DRV1stepper.runSpeed();
  //DRV2stepper.runSpeed();


}