JrDesignArduinoCode

#include <Stepper.h>
#include <Key.h>
#include <Keypad.h>
#include <Servo.h>

//*******************************************************************************************
#define dirPin   9
#define stepPin  10
#define MS2      8
#define stepsPerRevolution 3500
Stepper myStepper(stepsPerRevolution, dirPin, stepPin);

//*******************************************************************************************
#define DIRpin  5
#define STEPpin 6
#define MS3     4
#define stepsPerRevolution2 3900
Stepper myStepper2(stepsPerRevolution2, DIRpin, STEPpin);

//*******************************************************************************************
Servo myservo;

//*******************************************************************************************

#define USONIC_DIV 58.0
#define MEASURE_SAMPLE_DELAY 5
#define MEASURE_SAMPLES 5
#define MEASURE_DELAY 250

#define TRIGGER_PIN 33
#define ECHO_PIN 32

#define TRIGGER_PIN1 31
#define ECHO_PIN1 30

#define TRIGGER_PIN2 29
#define ECHO_PIN2 28

#define TRIGGER_PIN3 26
#define ECHO_PIN3 27

#define TRIGGER_PIN4 25
#define ECHO_PIN4 24

#define TRIGGER_PIN5 23
#define ECHO_PIN5 22

//*******************************************************************************************
const byte ROWS = 4;
const byte COLS = 4;

char hexaKeys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

byte rowPins[ROWS] = {35, 37, 39, 41};
byte colPins[COLS] = {34, 36, 38, 40};

Keypad customKeypad = Keypad(makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS);


//******************************************************************************************

void setup() {

    Serial.begin(9600);
    // Initializing Trigger Output and Echo Input
    pinMode(TRIGGER_PIN,  OUTPUT);
    pinMode(ECHO_PIN,      INPUT);

    pinMode(TRIGGER_PIN1, OUTPUT);
    pinMode(ECHO_PIN1,     INPUT);

    pinMode(TRIGGER_PIN2, OUTPUT);
    pinMode(ECHO_PIN2,     INPUT);

    pinMode(TRIGGER_PIN3, OUTPUT);
    pinMode(ECHO_PIN3,     INPUT);

    pinMode(TRIGGER_PIN4, OUTPUT);
    pinMode(ECHO_PIN4,     INPUT);

    pinMode(TRIGGER_PIN5, OUTPUT);
    pinMode(ECHO_PIN5,     INPUT);



    // Reset the trigger pin and wait a half a second
    digitalWrite(TRIGGER_PIN,  LOW);
    digitalWrite(TRIGGER_PIN1, LOW);
    digitalWrite(TRIGGER_PIN2, LOW);
    digitalWrite(TRIGGER_PIN3, LOW);
    digitalWrite(TRIGGER_PIN4, LOW);
    digitalWrite(TRIGGER_PIN5, LOW);

    delayMicroseconds(100);
    double peg_reading[6];

    peg_reading[0] = measure();
    delay(500);
    peg_reading[1] = measure1();
    delay(500);
    peg_reading[2] = measure2();
    delay(500);
    peg_reading[3] = measure3();
    delay(500);
    peg_reading[4] = measure4();
    delay(500);
    peg_reading[5] = measure5();


    for(int ind = 0; ind< 6; ind++){

      Serial.println(peg_reading[ind]);
    }



    const int INPUT_SIZE = 6;
    String pegEnd_str    = "000000";

    Serial.print("Enter the order of the peg: ");

    int count = 0;
    while(true){

      char customKey = customKeypad.getKey();

      if (customKey){

        Serial.print(customKey);
        pegEnd_str[count] = customKey;
        count++;

      }

      if(count >=  INPUT_SIZE){

        break;
      }

    }
    Serial.println();

    const int SIZE       = 6;


    String Peg_order  = "000000";

    Serial.println();

    BubbleSortPegs(peg_reading, SIZE);

    ConvertToChar(peg_reading, SIZE, Peg_order);

    Serial.print("This is the current order: ");
    Serial.print(Peg_order);
    Serial.println();
    Serial.println();

    //THIS IS THE PIN TO ATTACH THE SERVO
    myservo.attach(42);
    myservo.write(120);

    //DECLARE PINS AS OUTPUT FOR THE FIRST STEPPER:
    pinMode(stepPin, OUTPUT);
    pinMode(dirPin,  OUTPUT);
    pinMode(MS2,     OUTPUT);
    myStepper.setSpeed(120);

    //DECLARE PINS AS OUTPUT FOR THE SECOND STEPPER:
    pinMode(STEPpin, OUTPUT);
    pinMode(DIRpin,  OUTPUT);
    myStepper2.setSpeed(120);

    SpaceBySpace(Peg_order, pegEnd_str);

}

void loop() {

}

void forward_stepper1call(){

     // SET THE SPININING DIRECTION CLOCKWISE.
     digitalWrite(MS2, HIGH);
     myStepper.step(-stepsPerRevolution);
     delay(500);

}
void backward_stepper1call(){

    // Set the spinning direction counterclockwise:
    digitalWrite(MS2, HIGH);

    myStepper.step(stepsPerRevolution);
    delay(500);

}

void open_callMyServo(){

   int pos  = 120;
   for (pos = 120; pos <= 146; pos += 1) {

      myservo.write(pos);
      delay(50);

    }

}

void close_callMyServo(){


     int pos  = 146;
     for (pos = 146; pos >= 120; pos -= 1) {

        myservo.write(pos);
        delay(50);
     }


}


void MoveSide2SideStepper2(int &oldPos,  int newPos, int destination, bool &clawStatus){

     //NOTE: Big assumption here is that, whenever the status of the claw is 0(closed), i assume
     //      my claw currently has a peg.

     digitalWrite(MS3, HIGH);
     myStepper2.step((oldPos - newPos) * stepsPerRevolution2);
     delay(250);

     //open  = 0.
     //close = 1.

     if(clawStatus == 1){
         forward_stepper1call();
         delay(100);
         myservo.attach(42);
         close_callMyServo();
         myservo.detach();
         delay(100);

         backward_stepper1call();
         clawStatus = 0;

     }else{
         forward_stepper1call();
         delay(100);
         myservo.attach(42);
         open_callMyServo();
         myservo.detach();
         delay(100);
         backward_stepper1call();
         clawStatus = 1;
      }

      myStepper2.step((newPos - destination) * stepsPerRevolution2);
      delay(250);

     if(clawStatus == 1){
         forward_stepper1call();
         delay(100);
         myservo.attach(42);
         close_callMyServo();
         myservo.detach();
         delay(100);
         backward_stepper1call();
         clawStatus = 0;

     }else{
         forward_stepper1call();
         delay(100);
         myservo.attach(42);
         open_callMyServo();
         myservo.detach();
         delay(100);
         backward_stepper1call();
         clawStatus = 1;
     }

     oldPos = destination;
}

//*****************************************************************

void SpaceBySpace( String pegOrder, String pegEnd){

    /* pegOrder: This is the current position state of the pegs to be sorted
     *
     * pegEnd  : This is the desired state of the pegs after sorting.
     *
     */

    int  oldPos = 0, newPos = 0;
    int  count1 = 0, count2 = 0;
    int  destination = 0;
    bool clawStatus  = 0;

    for(int i = 0; i < 6; i++){

        if((pegOrder[i] != pegEnd[i])){

            // if the current Element on the pegOrder does not match the current element
            // on the pegEnd position, traverse the pegorder to locate the empty square once
            // and for all.

            while(pegOrder[count2] != '0' && count2 < 6){

                // count2: This will be the index of the empty square.
                count2++;
            }

            //FUNCTION CALL TO SIDE TO SIDE STEPPER.
            newPos      = i;
            destination = count2;

            if(newPos != destination){
            MoveSide2SideStepper2(oldPos, newPos, destination, clawStatus);

            // Move the incorrect peg to the empty square.
            Serial.print("Move Peg ");
            Serial.print(pegOrder[i]);
            Serial.print(" to space ");
            Serial.print(count2);
            Serial.println();
            }

            // The index of the empty square now holds the incorrect peg and the empty square
            // moves to the former index of the incorrect peg.
            pegOrder[count2] = pegOrder[i];
            pegOrder[i] = '0';

            // Now find the correct peg by traversing the pegOrder to find the element
            // that equals the element at pegEnd[i].
            while(pegEnd[i] != pegOrder[count1] && count1 < 6){

                // count1: This will be the index of the correct peg in pegOrder.
                count1++;
            }

            //FUNCTION CALL TO SIDE TO SIDE STEPPER.
            newPos      = count1;
            destination = i;

            if(newPos != destination){
            MoveSide2SideStepper2(oldPos, newPos, destination, clawStatus);

            Serial.print("Move Peg ");
            Serial.print(pegOrder[i]);
            Serial.print(" to space ");
            Serial.print(i);
            Serial.println();
        }
            }

            // Re-Wrote: pegOrder[i] = pegEnd[i] to pegOrder[i] = pegOrder[count1];;
            // i did this because it will help us to know what is contained in pegOrder[count1];
            // this is to ascertain whether it contains a good or bad  data.


            // Insert the correct peg in the pegOrder[i], the former index of the correct peg is
            // set to empty space.
            pegOrder[i]      = pegEnd[i];
            pegOrder[count1] = '0';

            // Making count2 = count1, eliminates the process of looking for the empty square
            // all the time, making the run-tim decrease efficiently by 25%.
            count2 = count1;
            count1 = 0;


    }


    // Print the order of the pegs.

    Serial.println();
    for(int i = 0; i < 6; i++){

        Serial.print(pegOrder[i]);
    }

    Serial.println();
    Serial.println();

    Serial.println("Pegs are in order.");
}

//***********************************************************************

void BubbleSortPegs(double* arr, int n){

    /*
     * arr: This is a pointer to an interger or block of integers
     * cpy: Just a copy of the array.
     * pos: This is a string where the position of the arr is stored
     * n  : number of integers contained in the block;
     */

    double cpy[n];

    for(int ind = 0; ind < 6; ind++){

        cpy[ind] = arr[ind];
    }

    const int EPSILON =  0.0001;

    for(int ind = 0; ind < n - 1; ind++){

        for(int ndx = 0; ndx < n - 1; ndx++){

            if(cpy[ndx] < cpy[ndx + 1]){

                swap_Peg(&cpy[ndx], &cpy[ndx + 1]);
            }
        }
    }


    for(int ind = 0; ind < n; ind++){

        for(int ndx = 0; ndx < n; ndx++){

            if(fabs(cpy[ind] - arr[ndx]) == EPSILON){

                arr[ndx] = ind;
            }
        }

    }

}

//*****************************************************************

void swap_Peg(double* peg1, double* peg2){

    /*
     * Peg1: Pointer to the value of peg1
     * peg2: pointer to the value of peg2
     */

    double  temp = *peg1;
    *peg1        = *peg2;
    *peg2        = temp;
}

//*****************************************************************

void  ConvertToChar(double arr[], int size, String& val){

    for(int ind = 0; ind < size; ind++){

        val[ind] = arr[ind] + '0';
    }

}

//*****************************************************************

long singleMeasurement()
{
  long duration = 0;
  // Measure: Put up Trigger...
  digitalWrite(TRIGGER_PIN, HIGH);
  // ... wait for 11 µs ...
  delayMicroseconds(11);
  // ... put the trigger down ...
  digitalWrite(TRIGGER_PIN, LOW);
  // ... and wait for the echo ...
  duration = pulseIn(ECHO_PIN, HIGH);
  return (long) (((float) duration / USONIC_DIV) * 10.0);
}


long singleMeasurement1()
{
  long duration = 0;
  // Measure: Put up Trigger...
  digitalWrite(TRIGGER_PIN1, HIGH);
  // ... wait for 11 µs ...
  delayMicroseconds(11);
  // ... put the trigger down ...
  digitalWrite(TRIGGER_PIN1, LOW);
  // ... and wait for the echo ...
  duration = pulseIn(ECHO_PIN1, HIGH);
  return (long) (((float) duration / USONIC_DIV) * 10.0);
}


long singleMeasurement2()
{
  long duration = 0;
  // Measure: Put up Trigger...
  digitalWrite(TRIGGER_PIN2, HIGH);
  // ... wait for 11 µs ...
  delayMicroseconds(11);
  // ... put the trigger down ...
  digitalWrite(TRIGGER_PIN2, LOW);
  // ... and wait for the echo ...
  duration = pulseIn(ECHO_PIN2, HIGH);
  return (long) (((float) duration / USONIC_DIV) * 10.0);
}


long singleMeasurement3()
{
  long duration = 0;
  // Measure: Put up Trigger...
  digitalWrite(TRIGGER_PIN3, HIGH);
  // ... wait for 11 µs ...
  delayMicroseconds(11);
  // ... put the trigger down ...
  digitalWrite(TRIGGER_PIN3, LOW);
  // ... and wait for the echo ...
  duration = pulseIn(ECHO_PIN3, HIGH);
  return (long) (((float) duration / USONIC_DIV) * 10.0);
}

long singleMeasurement4()
{
  long duration = 0;
  // Measure: Put up Trigger...
  digitalWrite(TRIGGER_PIN4, HIGH);
  // ... wait for 11 µs ...
  delayMicroseconds(11);
  // ... put the trigger down ...
  digitalWrite(TRIGGER_PIN4, LOW);
  // ... and wait for the echo ...
  duration = pulseIn(ECHO_PIN4, HIGH);
  return (long) (((float) duration / USONIC_DIV) * 10.0);
}


long singleMeasurement5()
{
  long duration = 0;
  // Measure: Put up Trigger...
  digitalWrite(TRIGGER_PIN5, HIGH);
  // ... wait for 11 µs ...
  delayMicroseconds(11);
  // ... put the trigger down ...
  digitalWrite(TRIGGER_PIN5, LOW);
  // ... and wait for the echo ...
  duration = pulseIn(ECHO_PIN5, HIGH);
  return (long) (((float) duration / USONIC_DIV) * 10.0);
}
long measure()
{
  long measureSum = 0;
  for (int i = 0; i < MEASURE_SAMPLES; i++)
  {
    delay(MEASURE_SAMPLE_DELAY);
    measureSum += singleMeasurement();
  }
  return measureSum / MEASURE_SAMPLES;
}


long measure1()
{
  long measureSum = 0;
  for (int i = 0; i < MEASURE_SAMPLES; i++)
  {
    delay(MEASURE_SAMPLE_DELAY);
    measureSum += singleMeasurement1();
  }
  return measureSum / MEASURE_SAMPLES;
}


long measure2()
{
  long measureSum = 0;
  for (int i = 0; i < MEASURE_SAMPLES; i++)
  {
    delay(MEASURE_SAMPLE_DELAY);
    measureSum += singleMeasurement2();
  }
  return measureSum / MEASURE_SAMPLES;
}


long measure3()
{
  long measureSum = 0;
  for (int i = 0; i < MEASURE_SAMPLES; i++)
  {
    delay(MEASURE_SAMPLE_DELAY);
    measureSum += singleMeasurement3();
  }
  return measureSum / MEASURE_SAMPLES;
}


long measure4()
{
  long measureSum = 0;
  for (int i = 0; i < MEASURE_SAMPLES; i++)
  {
    delay(MEASURE_SAMPLE_DELAY);
    measureSum += singleMeasurement4();
  }
  return measureSum / MEASURE_SAMPLES;
}


long measure5()
{
  long measureSum = 0;
  for (int i = 0; i < MEASURE_SAMPLES; i++)
  {
    delay(MEASURE_SAMPLE_DELAY);
    measureSum += singleMeasurement5();
  }
  return measureSum / MEASURE_SAMPLES;
}

//*****************************************************************