SWH - snack2go code

// ConstantSpeed.pde
// -*- mode: C++ -*-
//
// Shows how to run AccelStepper in the simplest,
// fixed speed mode with no accelerations
// MultiStepper.pde
// -*- mode: C++ -*-
// Use MultiStepper class to manage multiple steppers and make them all move to
// the same position at the same time for linear 2d (or 3d) motion.

#include <AccelStepper.h>
#include <MultiStepper.h>
#include <NewPing.h> //Sensor einbinden

#define X_STEP_PIN (byte)26
#define X_DIR_PIN (byte)28
#define X_ENABLE_PIN (byte)24

#define Y_STEP_PIN (byte)38
#define Y_DIR_PIN (byte)40
#define Y_ENABLE_PIN (byte)36

#define Z_STEP_PIN (byte)44
#define Z_DIR_PIN (byte)46
#define Z_ENABLE_PIN (byte)42

// defines pins numbers
const int trigPin = 23;
const int echoPin = 53;


// defines variables
long duration;

// EG X-Y position bed driven by 2 steppers
// Alas its not possible to build an array of these with different pins for each :-(
AccelStepper stepper1(AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN);
AccelStepper stepper2(AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN);

// Up to 10 steppers can be handled as a group by MultiStepper
MultiStepper steppers;

//-----------------------------
void setup() {

  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input

  Serial.begin(9600); // Starts the serial communication


  // Configure each stepper
  pinMode(X_ENABLE_PIN    , OUTPUT);
  pinMode(Z_ENABLE_PIN    , OUTPUT);

  digitalWrite(X_ENABLE_PIN    , HIGH);
  digitalWrite(Z_ENABLE_PIN    , HIGH);


  stepper1.setMaxSpeed(400);
  stepper2.setMaxSpeed(100);
  stepper1.setAcceleration(10);
  stepper2.setAcceleration(10);

  // Then give them to MultiStepper to manage
  steppers.addStepper(stepper1);
  steppers.addStepper(stepper2);
}
//--------------------------------------
void moveMotors(long x, long y) {

  long positions[2]; // Array of desired stepper positions

  positions[0] = x + y;
  positions[1] = x - y;

  // Enable!
  digitalWrite(X_ENABLE_PIN    , LOW);
  digitalWrite(Z_ENABLE_PIN    , LOW);

  steppers.moveTo(positions);
  steppers.runSpeedToPosition(); // Blocks until all are in position

  // Disable!
  digitalWrite(X_ENABLE_PIN    , HIGH);
  digitalWrite(Z_ENABLE_PIN    , HIGH);

}
//---------------------------------
void loop() {

  int d;
  d = getDist();

  // Prints the distance on the Serial Monitor
  Serial.print("Distance: ");
  Serial.println(d);

  if (d < 20) {
    moveMotors(300, -400); // digitalWrite(ledPin1, HIGH);
    Serial.println("Motor backwards");  // Then the Motor turns on
    delay(1000);

  } else {

    moveMotors(-100, 100); //digitalWrite(ledPin1, LOW);
    Serial.println("Motor forwards");  // Prints Motor Off
    delay(1000);
  }
}
//-----------------------------------
int getDist() {

  int distance;

  // Clears the trigPin
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);

  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);

  // Calculating the distance
  distance = duration * 0.034 / 2;
  return distance;
}