// Raspberry Pi Pico + Stepper Motor Example

#define DIR_A1_PIN 2
#define DIR_A2_PIN 4
#define STEP_A_PIN 3
#define DIR_B1_PIN 6
#define DIR_B2_PIN 7
#define STEP_B_PIN 5
#define SW_ENABLE_A 8
#define SW_ROT_A 9
#define SW_ENABLE_B 10
#define SW_ROT_B 11
#define poti 26

int pd = analogRead(26);
map(pd,0,4095,5,3000);

int Enable_A_State = LOW;
int Enable_B_State = LOW;
int Rot_A_State = LOW;
int Rot_B_State = LOW;

void setup() {
  pinMode(STEP_A_PIN, OUTPUT);
  pinMode(STEP_B_PIN, OUTPUT);
  pinMode(DIR_A1_PIN, OUTPUT);
  pinMode(DIR_A2_PIN, OUTPUT);
  pinMode(DIR_B1_PIN, OUTPUT);
  pinMode(DIR_B2_PIN, OUTPUT);

  pinMode(SW_ENABLE_A, INPUT_PULLUP);
  pinMode(SW_ENABLE_B, INPUT_PULLUP);
  pinMode(SW_ROT_A, INPUT_PULLUP);
  pinMode(SW_ROT_B, INPUT_PULLUP);


  digitalWrite(STEP_A_PIN, LOW);
  digitalWrite(STEP_B_PIN, LOW);


  Serial.begin(115200);

}



void motor() {
  // Move 200 steps (one rotation) CW over one second
  digitalWrite(DIR_A1_PIN, Rot_A_State);
  digitalWrite(DIR_A2_PIN, !Rot_A_State);
  digitalWrite(DIR_B1_PIN, Rot_B_State);
  digitalWrite(DIR_B2_PIN, !Rot_B_State);


  digitalWrite(STEP_A_PIN, Enable_A_State);
  digitalWrite(STEP_B_PIN, Enable_B_State);
  delayMicroseconds(2000);
  digitalWrite(STEP_A_PIN, LOW);
  digitalWrite(STEP_B_PIN, LOW);
  delayMicroseconds(2000); // 5 ms * 200 = 1 second



}

void motor_ori() {
  // Move 200 steps (one rotation) CW over one second
  digitalWrite(DIR_A1_PIN, HIGH);
  digitalWrite(DIR_A2_PIN, LOW);

  for (int i = 0; i < 400; i++) {
    digitalWrite(STEP_A_PIN, HIGH);
    delayMicroseconds(2500);
    digitalWrite(STEP_A_PIN, LOW);
    delayMicroseconds(2500); // 5 ms * 200 = 1 second
  }

  delay(500); // Wait half a second

  // Move 200 steps (one rotation) CCW over 400 millis
  digitalWrite(DIR_B1_PIN, LOW);
  digitalWrite(DIR_B2_PIN, HIGH);

  for (int i = 0; i < 400; i++) {
    digitalWrite(STEP_B_PIN, HIGH);
    delay(1);
    digitalWrite(STEP_B_PIN, LOW);
    delay(1); // 2 ms * 200 = 0.4 seconds
  }

  delay(1000); // Wait another second


}

void sw() {

  Enable_A_State = !digitalRead(SW_ENABLE_A);
  Enable_B_State = !digitalRead(SW_ENABLE_B);
  Rot_A_State = !digitalRead(SW_ROT_A);
  Rot_B_State = !digitalRead(SW_ROT_B);

  Serial.print(Enable_A_State);
  Serial.print(",");
  Serial.print(Enable_B_State);
  Serial.print(",");
  Serial.print(Rot_A_State);
  Serial.print(",");
  Serial.println(Rot_B_State);



}


void switch_chk() {
  attachInterrupt(digitalPinToInterrupt(SW_ENABLE_A), sw, CHANGE);
  attachInterrupt(digitalPinToInterrupt(SW_ENABLE_B), sw, CHANGE);
  attachInterrupt(digitalPinToInterrupt(SW_ROT_A), sw, CHANGE);
  attachInterrupt(digitalPinToInterrupt(SW_ROT_B), sw, CHANGE);
}





void loop() {
  switch_chk();
  motor();
  Serial.println(poti);
  delay(500);


}