#include <AccelStepper.h>

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

#define DRV1_dirPin 6
#define DRV1_stepPin 11
#define DRV2_dirPin 13
#define DRV2_stepPin 12

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

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

AccelStepper DRV1_stepper = AccelStepper(2, 11, 6);
AccelStepper DRV2_stepper = AccelStepper(2, 12, 13);

//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() {

  // 1/4 Microstepping
  Microstep(DRV1, 4);
  Microstep(DRV2, 4);

  DRV1_stepper.setMaxSpeed(5000);
  DRV2_stepper.setMaxSpeed(5000);
  DRV1_stepper.setSpeed(4000);
  DRV2_stepper.setSpeed(4000);

  Serial.begin(9600);

  Enable; //Schrittmotortreiber einschalten
}

void loop() {

  DRV1_stepper.runSpeed();
  DRV2_stepper.runSpeed();


}