#include <AccelStepper.h>
#include <MultiStepper.h>
const int dirPin=5;
const int stepPin=2;
const int x1_ls=11;
const int y1_ls=10;
int x1_state;
int x1_lr=0;
int x1_lastState;
int y1_state;
int y1_lr=0;
int y1_lastState;
int ny1=0;
int nx1=0;
int sx1=0;
int sy1=0;
int val1;
int val2;
int G_x1=350;
int G_y1=250;
int e_x1=0;
int e_y1=0;
int e_lx1=0;
int vx1;
int non1=0;
int non2=0;
float last_coordinate=0;
const double Kp=0.001;
const double Ki=0.1;
const double Kd=0.0;
double integral=0;
double previousError=0;
double error=0;
AccelStepper stp_1(AccelStepper:: DRIVER, stepPin, dirPin);
AccelStepper stp_2(AccelStepper:: DRIVER, 3, 6);
void zeroing1()
{
while(1)
{
stp_1.setMaxSpeed(10000);
stp_1.setSpeed(100);
if(non1==0)
stp_1.runSpeed();
stp_2.setMaxSpeed(10000);
stp_2.setSpeed(100);
if(non2==0)
stp_2.runSpeed();
x1_lr=digitalRead(x1_ls);
y1_lr=digitalRead(y1_ls);
if(x1_lr!=x1_lastState)
{
if(x1_lr==LOW)
{
stp_1.stop();
stp_1.setCurrentPosition(0);
non1++;
}
}
x1_lastState=x1_lr;
if(y1_lr!=y1_lastState)
{
if(y1_lr==LOW)
{
stp_2.stop();
stp_2.setCurrentPosition(0);
non2++;
}
}
y1_lastState=y1_lr;
if(stp_1.currentPosition()==0 && stp_2.currentPosition()==0)
break;
}
}
void setup() {
delay(10);
pinMode(x1_ls,INPUT_PULLUP);
pinMode(y1_ls,INPUT_PULLUP);
Serial.begin(9600);
//zeroing1();
stp_1.setMaxSpeed(10000);
stp_1.setAcceleration(10000);
stp_2.setMaxSpeed(1000000);
stp_2.setSpeed(1000000);
stp_2.setAcceleration(1000000);
stp_1.setCurrentPosition(0);
stp_1.moveTo(-100*16);
while(stp_1.currentPosition()!=-(100*16))
stp_1.run();
delay(50);