#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-
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