Camera Slider Code

#include "BasicStepperDriver.h"
#include "MultiDriver.h"
#include "SyncDriver.h"

// This code accompanies the youtube video: https://youtu.be/NB-O0Zpb0e0
// This code functions such that two potentiometers control position and rotation of two NEMA stepper motors,
// with the 'in' and 'out' buttons setting the start and finish positions. A latching 'start' button activates the
// slider motion, after moving each axis to the starting position, if not already there.

bool debug = true; // Enable or disable serial messages
bool primed = false;
bool reset = false;

// TMC2209 defaults to 8 microsteps in the CNC shield
const int microSteps = 8;
const int steps = 200; // 1.8 degree stepper (200 x 1.8 degrees = 360 degrees)

int pos_steps = 20;
int rot_steps = 20;

int curr_pos = 0;
int curr_rot = 0;

int in_pos = 0;
int in_rot = 0;
int out_pos = 0;
int out_rot = 0;

int pot_pos_before = 200;
int pot_rot_before = 200;
int pot_pos_after;
int pot_rot_after;

int initSpeedPos = 70;
int initSpeedRot = 10;
int moveSpeed = 25;

float interp_rot_steps = 0.0;

// DIO
int in_but = 9; // In button - Limit X pin
int out_but = 10; // Out button - Limit Y pin
int start_but = 11; // Start button - Limit Z pin
int batt_LED = 12; // Batt LED - Spindle Ena pin

// AIO
int pos_pot = 14; // Pos Pot - Abort pin
int rot_pot = 15; // Rot Pot - Hold pin
int batt_mon = 16; // Batt Mon - Resume pin
//int pos_pot = 17; // Pos Pot - Coolant En pin
//int rot_pot = 18; // Rot Pot - SDA pin
//int batt_mon = 19; // Batt Mon - SCL pin

BasicStepperDriver PosStepper(steps, 5, 2); // X driver; steps, dir, step
BasicStepperDriver RotStepper(steps, 7, 4); // Z driver; steps, dir, step

void setup() {

  pinMode(in_but, INPUT_PULLUP);
  pinMode(out_but, INPUT_PULLUP);
  pinMode(start_but, INPUT_PULLUP);
  pinMode(pos_pot, INPUT);
  pinMode(rot_pot, INPUT);
  pinMode(batt_mon, INPUT);
  pinMode(batt_LED, OUTPUT);

  PosStepper.setEnableActiveState(LOW);
  RotStepper.setEnableActiveState(LOW);

  //SyncDriver controller(PosStepper, RotStepper);

  Serial.begin(115200);
}

void loop() {

  delay(100);

  PosStepper.begin(initSpeedPos, microSteps);
  RotStepper.begin(initSpeedRot, microSteps);

  // Battery voltage check (10V = 100, 12V = 120, etc)
  int batt_level = map(analogRead(batt_mon), 0, 1024, 0, 220);
  //Serial.println(batt_level); // 4K7+15K or 10K+33K divider - max voltage at pin of 5V equates to ~22V at battery
  if (batt_level <= 100) {
    digitalWrite(batt_LED, HIGH);
    Serial.println(batt_level); // 2K2+6K8 or 4K7+15K or 10K+33K divider - max voltage at pin of 5V equates to ~21V at battery
    //Serial.println("LOW BATTERY");
  } else {
    digitalWrite(batt_LED, LOW); // Set low unless the below is true
  }

  // Read pos & rot
  pot_pos_after = map(analogRead(pos_pot), 0, 1024, 400, 0);
  pot_rot_after = map(analogRead(rot_pot), 0, 1024, 400, 0);

  PosStepper.move(stepAmountFiltered(pot_pos_before, pot_pos_after, pos_steps*100));
  curr_pos += stepAmountFiltered(pot_pos_before, pot_pos_after, pos_steps)*100;
  /*if (pot_pos_after > pot_pos_before) {
    PosStepper.move(pos_steps);
    curr_pos += pos_steps;
  } else if (pot_pos_after < pot_pos_before) {
    PosStepper.move(-pos_steps);
    curr_pos -= pos_steps;
  }*/

  // Move rot as pot is turned
  RotStepper.move(stepAmountFiltered(pot_rot_before, pot_rot_after, rot_steps*3));
  curr_rot += stepAmountFiltered(pot_rot_before, pot_rot_after, rot_steps)*3;
  // Move rot as pot is turned
  /*if (pot_rot_after > pot_rot_before) {
    RotStepper.move(rot_steps);
    curr_rot += rot_steps;
  } else if (pot_rot_after < pot_rot_before) {
    RotStepper.move(-rot_steps);
    curr_rot -= rot_steps;
  }*/

  // Save in pos and rot
  if (digitalRead(in_but) == LOW) {
    delay(200);
    primed = false;
    reset = true;
    Serial.println("In button pushed");

    in_pos = curr_pos;
    in_rot = curr_rot;

    if (debug) {
      Serial.print("Start Pos: ");
      Serial.println(in_pos);
      Serial.print("Start Rot: ");
      Serial.println(in_rot);
    }
  }

  // Save out pos and rot
  if (digitalRead(out_but) == LOW) {
    delay(200);
    primed = false;
    reset = true;
    Serial.println("Out button pushed");

    out_pos = curr_pos;
    out_rot = curr_rot;

    if (debug) {
      Serial.print("End Pos: ");
      Serial.println(out_pos);
      Serial.print("End Rot: ");
      Serial.println(out_rot);
    }
  }

  if (debug) {
    Serial.print("Pos: ");
    Serial.println(curr_pos);
    Serial.print("Rot: ");
    Serial.println(curr_rot);
    //delay(500);
  }

  // Move whilst start button held
  while (digitalRead(start_but) == LOW && reset == true) {
    //delay(500);
    //if (in_pos == out_pos || in_rot == out_rot && primed == false) { // Uncomment if you want an error if in and out pos AND rot are the same
    if (in_pos == out_pos && primed == false) { // Comment out if you don't want an error if in and out pos ONLY are the same
    //if (in_rot == out_rot && primed == false) { // Uncomment if you want an error if in and out rot ONLY are the same
      PosStepper.move(400);
      delay(500):
      PosStepper.move(-400);
      delay(500):
      PosStepper.move(400);
      delay(500):
      PosStepper.move(-400);
      Serial.println("In and out positions identical");
      break;
    }

    if (curr_pos != in_pos && primed == false) {
      PosStepper.move(stepAmount(curr_pos, in_pos, pos_steps*20));
      curr_pos += stepAmount(curr_pos, in_pos, pos_steps)*20;

      if (debug) {
        Serial.print("Moving to start position ");
        Serial.print(in_pos);
        Serial.print(" from ");
        Serial.println(curr_pos);
      }
    }

    if (curr_rot != in_rot && primed == false) {
      RotStepper.move(stepAmount(curr_rot, in_rot, rot_steps*3));
      curr_rot += stepAmount(curr_rot, in_rot, rot_steps)*3;

      if (debug) {
        Serial.print("Moving to start rotation ");
        Serial.print(in_rot);
        Serial.print(" from ");
        Serial.println(curr_rot);
      }
    }

    if (curr_pos == in_pos && curr_rot == in_rot) {
      primed = true;

      // Interpolate rotation distance to be proportionate to pos distance
      interp_rot_steps = in_rot - out_rot;
      interp_rot_steps = interp_rot_steps/(in_pos - out_pos);
      interp_rot_steps = interp_rot_steps*rot_steps;

      // If the interpolated rotation motion is too small, set the steps to 1
      if (interp_rot_steps < 1 && interp_rot_steps > 0.1) {
        interp_rot_steps = 1;
      } else if (interp_rot_steps > -1 && interp_rot_steps < -0.1) {
        interp_rot_steps = -1;
      }  else if (interp_rot_steps >= -0.1 && interp_rot_steps <= 0.1) {
        interp_rot_steps = 0;
      }

      PosStepper.begin(moveSpeed, microSteps);
      RotStepper.begin(moveSpeed, microSteps);
    }

    if (primed == true) {
      //controller.rotate(((in_pos - out_pos)/steps)*360), ((in_rot - out_rot)/steps)*360)));

      PosStepper.move(stepAmount(in_pos, out_pos, pos_steps));
      curr_pos += stepAmount(in_pos, out_pos, pos_steps);

      RotStepper.move(stepAmount(in_rot, out_rot, abs(interp_rot_steps)));
      curr_rot += stepAmount(in_rot, out_rot, abs(interp_rot_steps));

      if (debug) {
        Serial.print("Moving pos ");
        Serial.print(curr_pos);
        Serial.print(" to ");
        Serial.println(out_pos);
        Serial.print("Moving rot ");
        Serial.print(curr_rot);
        Serial.print(" to ");
        Serial.println(out_rot);
      }

      if (curr_pos == out_pos) {
        primed = false;
        reset = false;
        PosStepper.begin(initSpeedPos, microSteps);
        RotStepper.begin(initSpeedRot, microSteps);
        break;
      }
    }
  }

  pot_pos_before = pot_pos_after;
  pot_rot_before = pot_rot_after;
}

int stepAmount(int first, int second, int steps) {
  if (first > second) {
    return -steps;
  } else if (first < second) {
    return steps;
  } else {
    return 0;
  }
}


int stepAmountFiltered(int first, int second, int steps) {
  if (first > (second+5)) {
    return -steps;
  } else if (first < (second-5)) {
    return steps;
  } else {
    return 0;
  }
}