// Creality 42-34-ZZ
// Motor: BJ42D22-23V01. 1,8 degrees/step is 200steps/rev, 0,4Nm, 1-1000rpm, 1A adjustement A4988 = 0,8V , DRV8825 = 0,5V
//A4988 current limit= Vref/(8*Rcs) if Rcs = 0,1Ohm then Vref @ 1A=0,6V, DRV8825 current limit=Vref*2 ifRcs = 0,1Ohm then Vref @1A=0,5V
// 4,83V, 1,5A
// Spindle pich: 8mm

#include <Streaming.h>
#include <AccelStepper.h>

// Voor de Arduino Uno + CNC shield V3
#define MOTOR_Z_ENABLE_PIN 8
#define MOTOR_Z_STEP_PIN 4
#define MOTOR_Z_DIR_PIN 7
#define motorInterfaceType 1

AccelStepper motorZ(motorInterfaceType, MOTOR_Z_STEP_PIN, MOTOR_Z_DIR_PIN);

//variables for serial read routine
String strResult = "";   //gebruikt bij read serial input string
int intResult = 0;       // input string als integer reultaat
float fltResult = 0.00;  // input string als float reultaat

int hys = 10; //hysteresis, laat de motor minder trillen en vibreren doordat waardes fluctueren.
int oudepotmeter;
int potwaarde = 0;
int serieelwaarde = 0;
int servopositie = 0;
int run = 1;
int next = 0;

void setup() {

  Serial.begin(115200);

  //pinMode(MOTOR_Z_ENABLE_PIN, OUTPUT);

  motorZ.setEnablePin(MOTOR_Z_ENABLE_PIN);
  motorZ.setPinsInverted(false, false, true);
  motorZ.setAcceleration(15000); //was 15000
  motorZ.setMaxSpeed(10000);  //was 10000
  motorZ.setSpeed(10000);  //was 10000
  motorZ.setCurrentPosition(0);
  motorZ.enableOutputs();

}

void loop() {

  //int potmeter = analogRead(A8);
  //  potwaarde = map(potmeter, 0, 1023, 0, 10000); //200 * 1.8graad = 360 graden rond.
  //if (potmeter < oudepotmeter - hys || potmeter > oudepotmeter + hys){
  //    motorZ.moveTo(potwaarde);
  //    oudepotmeter = potmeter;
  //  }
  /*
    if (next==0){
    motorZ.moveTo(9999);
    motorZ.run();
    }
    if  (next==1){
    motorZ.moveTo(133);
    motorZ.run();
    }
    servopositie=motorZ.currentPosition();
    run=motorZ.isRunning();
    if (run==0){
    Serial<<"Potmeter waarde: "<<potwaarde<<", Seriele waarde: "<<serieelwaarde<<", Current Position; "<<servopositie<<endl;
    next=!next;
    }
  */
 
  if (run == 0) {
    Serial << "Potmeter waarde: " << potwaarde << ", Seriele waarde: " << serieelwaarde << ", Current Position; " << servopositie << endl;
    serialRead();
    potwaarde = intResult;
  }
  servopositie=potwaarde;
  motorZ.moveTo(servopositie);
  motorZ.run();
  run = motorZ.isRunning();
}

void serialRead() {
  while (Serial.available() == 0) {  //wait for input
    ;
  }
  strResult = Serial.readString();  //read input data
  strResult.trim();                 //Remove white spaces
  intResult = strResult.toInt();    //Return result as integer
  fltResult = strResult.toFloat();  //Return result as float
}