/*     Arduino Project - Pat's M & M Colour Sorting Machine
 * Modified by Pat McMahon 20/6/2020
 * A030
 *  by Dejan Nedelkovski, www.HowToMechatronics.com
 * and No 26 Adeept Stepper Motor
 */
#include <Servo.h>
#define S0 2
#define S1 3
#define S2 4
#define S3 5
#define sensorOut 6
int Pin0 = 8;//definition digital 8 pins as pin to control the Stepper Motor IN1 (ULN24L01)
int Pin1 = 9;//definition digital 9 pins as pin to control the Stepper Motor IN2 (ULN24L01)
int Pin2 = 10;//definition digital 10 pins as pin to control the Stepper Motor IN3 (ULN24L01)
int Pin3 = 11;//definition digital 11 pins as pin to control the Stepper Motor IN4 (ULN24L01)
int Servo=7;

int _step = 512; 
int _speed = 1; 
Servo bottomServo;
int frequency = 0;
int color=0;
void setup() {
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(sensorOut, INPUT); 
 
  // Setting frequency-scaling to 20%
  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);
  

 pinMode(Pin0, OUTPUT);//Set digital 8 port mode, the OUTPUT for the output of the Stepper Motor
 pinMode(Pin1, OUTPUT);//Set digital 9 port mode, the OUTPUT for the output of the Stepper Motor
 pinMode(Pin2, OUTPUT);//Set digital 10 port mode, the OUTPUT for the output of the Stepper Motor
 pinMode(Pin3, OUTPUT);//Set digital 11 port mode, the OUTPUT for the output of the Stepper Motor
 
 pinMode(bottomServo, OUTPUT);//Set digital 5 port mode, the OUTPUT for the output of the Servo Motor
}


bottomServo.attach(7);
Serial.begin(9600);

void loop() {
  Speed(15);//Stepper motor speed = 15 fast (note:speed from 1 to 15)
  Step(64);//Stepper motor forward 512/8=64 steps ---- 45 angle    
  delay(2000);// delay 2S
   Speed(1); //Stepper motor speed = 1 slow  (note:speed from 1 to 15)
   
  delay(2000);//delay 2S
}
void Speed(int stepperspeed)//set Stepper speed 
{
    _speed = 15 - stepperspeed;
    if( _speed<1){
     _speed = 1;
    }
    if( _speed>15){
     _speed = 15;
    }
}

void setStep(int a, int b, int c, int d)  
{  
    digitalWrite(Pin0, a);     
    digitalWrite(Pin1, b);     
    digitalWrite(Pin2, c);     
    digitalWrite(Pin3, d);     
}

void Step(int _step)//Stepper motor rotation
{
  if(_step>=0){  // Stepper motor forward
    for(int i=0;i<_step;i++){ 
      delay(_speed);  
      setStep(1, 0, 0, 1);
      delay(_speed); 
      setStep(1, 0, 0, 0);
      delay(_speed);
      setStep(1, 1, 0, 0);
      delay(_speed);
      setStep(0, 1, 0, 0);
      delay(_speed);
      setStep(0, 1, 1, 0);
      delay(_speed);
      setStep(0, 0, 1, 0);
      delay(_speed);
      setStep(0, 0, 1, 1);
      delay(_speed); 
      setStep(0, 0, 0, 1);
      delay(_speed);
  }
  
  //}  
  delay(500);
  //int readColor();

  delay(10);  
  switch (color) {
    case 1:
    bottomServo.write(40);
    break;
    case 2:
    bottomServo.write(55);
    break;
    case 3:
    bottomServo.write(75);
    break;
    case 4:
    bottomServo.write(100);
    break;
    case 5:
    bottomServo.write(120);
    break;
    case 6:
    bottomServo.write(140);
    break;
    
    case 0:
    break;
  }
  delay(300);
  
  int readColor();
  color=0;
}

// Custom Function - readColor()
  int readColor(); 
  
  // Setting red filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, LOW);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int R = frequency;
  // Printing the value on the serial monitor
  Serial.print("R= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  delay(50);
  // Setting Green filtered photodiodes to be read
  digitalWrite(S2, HIGH);
  digitalWrite(S3, HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int G = frequency;
  // Printing the value on the serial monitor
  Serial.print("G= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.print("  ");
  delay(50);
  // Setting Blue filtered photodiodes to be read
  digitalWrite(S2, LOW);
  digitalWrite(S3, HIGH);
  // Reading the output frequency
  frequency = pulseIn(sensorOut, LOW);
  int B = frequency;
  // Printing the value on the serial monitor
  Serial.print("B= ");//printing name
  Serial.print(frequency);//printing RED color frequency
  Serial.println("  ");
  delay(50);
  if(R<74 & R>64 & B<81 & B>71){
    color = 1; // Red
  }
  if(G<88 & G>78 & B<77 & >67;{
    color = 2 // Orange
  }
  if(R<78 & R>68 & B<80 & B>70){
    color = 3; //Green
  }
  if(R<56 & R>46 & G<68 & G>58){
    color = 4; // Yellow
  }
  if(R<84 & R>74 & G<100 & G>90){
    color = 5; // Brown
  }
  if (G<91 & G>81 & B<66 &B>56){
    color = 6; // Blue 
  }
  return color;  
}