#include <AccelStepper.h>
//#include <MultiStepper.h>
#define DT 0.01 //sampling time in seconds
#define MAX_ACCELERATION 100.0 //maximum acceleration in mm/s^2
#define MAX_JERK 500.0 //maximum jerk in mm/s^3

double position = 0.0; //current position in mm
double velocity = 0.0; //current velocity in mm/s
double acceleration = 0.0; //current acceleration in mm/s^2
double desired_position = 100.0; //desired position in mm
double desired_velocity = 0.0; //desired velocity in mm/s
double desired_acceleration = 0.0; //desired acceleration in mm/s^2
double error_position = 0.0; //position error in mm
double error_velocity = 0.0; //velocity error in mm/s
double error_acceleration = 0.0; //acceleration error in mm/s^2
double jerk = 0.0; //jerk in mm/s^3
long int times=0;
void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, ESP32!");
  pinMode(4, OUTPUT);
  pinMode(2, OUTPUT);
  times=millis();
  Serial.println(times);
}

void loop() {

    //calculate position, velocity, and acceleration errors
    error_position = desired_position - position;
    error_velocity = desired_velocity - velocity;
    error_acceleration = desired_acceleration - acceleration;

    //calculate desired jerk
    jerk = error_acceleration / DT + (error_velocity - acceleration * DT) * 2.0 / (DT * DT);
    jerk = jerk > MAX_JERK ? MAX_JERK : jerk;
    jerk = jerk < -MAX_JERK ? -MAX_JERK : jerk;

    //calculate desired acceleration
    desired_acceleration = jerk * DT + acceleration;
    desired_acceleration = desired_acceleration > MAX_ACCELERATION ? MAX_ACCELERATION : desired_acceleration;
    desired_acceleration = desired_acceleration < -MAX_ACCELERATION ? -MAX_ACCELERATION : desired_acceleration;

    //calculate desired velocity
    desired_velocity = desired_acceleration * DT + velocity;

    //update position
    position = desired_velocity * DT + position;

    //update velocity
    velocity = desired_velocity;

    //update acceleration
    acceleration = desired_acceleration;

    //wait for next sample
    delay(DT * 1000);
      digitalWrite(4, HIGH);
      digitalWrite(4, LOW);

    //Serial.println(times-millis());
    //print position for debugging
    if((millis()%1000)>990)
    {
    //Serial.print(millis()%1000);
    Serial.println("ok");
    times=millis();
    }


}

/**

void setup()
{
    //initialize serial communication
    Serial.begin(9600);
}

void loop()
{
}**/
esp:VIN
esp:GND.2
esp:D13
esp:D12
esp:D14
esp:D27
esp:D26
esp:D25
esp:D33
esp:D32
esp:D35
esp:D34
esp:VN
esp:VP
esp:EN
esp:3V3
esp:GND.1
esp:D15
esp:D2
esp:D4
esp:RX2
esp:TX2
esp:D5
esp:D18
esp:D19
esp:D21
esp:RX0
esp:TX0
esp:D22
esp:D23
stepper1:A-
stepper1:A+
stepper1:B+
stepper1:B-
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