// 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
}
mega:SCL
mega:SDA
mega:AREF
mega:GND.1
mega:13
mega:12
mega:11
mega:10
mega:9
mega:8
mega:7
mega:6
mega:5
mega:4
mega:3
mega:2
mega:1
mega:0
mega:14
mega:15
mega:16
mega:17
mega:18
mega:19
mega:20
mega:21
mega:5V.1
mega:5V.2
mega:22
mega:23
mega:24
mega:25
mega:26
mega:27
mega:28
mega:29
mega:30
mega:31
mega:32
mega:33
mega:34
mega:35
mega:36
mega:37
mega:38
mega:39
mega:40
mega:41
mega:42
mega:43
mega:44
mega:45
mega:46
mega:47
mega:48
mega:49
mega:50
mega:51
mega:52
mega:53
mega:GND.4
mega:GND.5
mega:IOREF
mega:RESET
mega:3.3V
mega:5V
mega:GND.2
mega:GND.3
mega:VIN
mega:A0
mega:A1
mega:A2
mega:A3
mega:A4
mega:A5
mega:A6
mega:A7
mega:A8
mega:A9
mega:A10
mega:A11
mega:A12
mega:A13
mega:A14
mega:A15
pot1:GND
pot1:SIG
pot1:VCC
A4988
drv1:ENABLE
drv1:MS1
drv1:MS2
drv1:MS3
drv1:RESET
drv1:SLEEP
drv1:STEP
drv1:DIR
drv1:GND.1
drv1:VDD
drv1:1B
drv1:1A
drv1:2A
drv1:2B
drv1:GND.2
drv1:VMOT
stepper1:A-
stepper1:A+
stepper1:B+
stepper1:B-