//Code is Written using Free RTOS and Arduino Language by TEAM PICO.
//MicroController used is ESP32.
//
//In practical Scenario where Steps of motor can be missed a Mechanical EndStop switch can be attached at the ends of the
//the lead screw or in conveyor like inside of a printer is to indicate the starting or Ending Position of motors.
//
//You can change the light intensity to the LDR to see the motor in action to adjust the bottle neck placement or
//solenoid Placement.
//
//Photo Diodes of Photo Transistors can be used for faster operation and more stable operation we didnt used them
//because of the limitation of this software.
//
//When the Centre LDR is Blocked by the Bottle while the left and right are having the laser projected on them
//which tells that the bottle is aligned and same for the Solenoid then only cap is placed and the conveyor is
//continued or returned while the Solenoid is returned to its orignal position.

#define stepPin 23                         // Step and Direction Pin
#define dirPin  22                         // Decleration for
#define stepPin1  21                       // both motor Drivers.
#define dirPin1  19                        // 
#define solenoid_ON  4                     // Used two different
#define solenoid_OFF 2                     // pins to show when the Solenoid is ON or OFF.
#define LDRL 25                            //
#define LDRM 26                            // used to define LDR Left,Middle and Right for Bottle Neck.
#define LDRR 27                            //
#define LDRL_CAP 14                        //
#define LDRM_CAP 12                        // Used to define LDR Left,Middle and Right for Solenoid Motor.
#define LDRR_CAP 13                        //
#define solA 2000                          //solA is the time Solenoid is Activated that is led is RED (more for visualization)
#define captime 2000                       //captime is when solenoid is deactiated and a delay after to keep the conveyor stopped.
#define speed_Delay 300                    //delay b/w each step in microseconds.

TaskHandle_t Task1;                        // Task Handler for Task 1 and Task 2
TaskHandle_t Task2;                        //

long mot_conPOS = 0;                        // To Store Motor Conveyor Position and Motor Solenoid Position.
long mot_solPOS = 0;                        //

const bool conveyor_DIR = 0;  // To set the conveyor direction whether to send the bottle further or receive it back 0->Back 1->Forward

bool LDRL_stat = 0;           //
bool LDRM_stat = 0;           //
bool LDRR_stat = 0;           // To store the Status of LDR Receiving Light or Not.
bool LDRLC_stat = 0;          // 0 -> Light is Received  1-> No Light
bool LDRMC_stat = 0;          //
bool LDRRC_stat = 0;          //
bool conveyor_ALIGN = 0;      // If the Bottle on the Conveyor is aligned with the Lasers and
bool caplead_ALIGN = 0;       // Solenoid is also aligned with its lasers then Result = 1

int conveyor_DIST = 75;       // Indicates the Distance where the bottle should be stopped for capping in CM.
float one_rev = 0.8;          // Indicates the Distance in one Revolution of the motor in CM.
int steps_inrev = 200;        // Indicates the total steps in one Revoltion of the motor.
int tot_CONSTEPS = (conveyor_DIST / one_rev) * steps_inrev; //Formula to find total steps conveyor motor needs to cover = 18750.
int solmotor_DIST = 25;       // Indicates the Distance Solenoid Motor Needs to Cover.
int tot_SOLSTEPS = (solmotor_DIST / one_rev) * steps_inrev; //Formula to find total steps solenoid motor needs to cover = 6250.

void setup() {
  // Serial.begin(115200);     // While using Serial with RTOS on ESP32 in WOKWI we are getting 1/3rd
  pinMode(stepPin, OUTPUT);   // of the performance, in practical Scenario there is no effect on esp32. Thats why we are not using Serial Debugging
  pinMode(dirPin, OUTPUT);    //
  pinMode(stepPin1, OUTPUT);  //
  pinMode(dirPin1, OUTPUT);   //
  pinMode(solenoid_ON, OUTPUT); //  Setting all the required Pins as I/O.
  pinMode(solenoid_OFF, OUTPUT); //
  pinMode(LDRL, INPUT);       //
  pinMode(LDRM, INPUT);       //
  pinMode(LDRR, INPUT);       //
  digitalWrite(solenoid_ON, LOW); // Solenoid is initalized as Deactivated.
  digitalWrite(solenoid_OFF, HIGH); //


  xTaskCreatePinnedToCore(
    Task1code, "Task1", 10000, NULL, 1, &Task1, 0);
  delay(200);
  //Decleration of the Tasks ran on 2 cores of esp32 i.e task1 and task2 to move both motor and perform alignment simultaneously.
  xTaskCreatePinnedToCore(
    Task2code, "Task2", 10000, NULL, 1, &Task2, 1);
  delay(200);

}

void Task1code( void * pvParameters ) {                   // Task on Core 1 Conveyor Motor and Alignment.

  digitalWrite(dirPin, HIGH);
  for (mot_conPOS; mot_conPOS < tot_CONSTEPS; mot_conPOS++) {
    digitalWrite(stepPin, HIGH);                // To reach first a distance of roughly 75CM by the bottle on conveyor.
    delayMicroseconds(speed_Delay);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(speed_Delay);
  }

  for (;;) {
    LDRL_stat = digitalRead(LDRL);
    LDRM_stat = digitalRead(LDRM);
    LDRR_stat = digitalRead(LDRR);
    if (LDRL_stat == 1 && LDRR_stat == 0) {
      digitalWrite(dirPin, HIGH);
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(speed_Delay);
      digitalWrite(stepPin, LOW);                      // If the bottle is behind the desired position
      delayMicroseconds(speed_Delay);                 // then it is brought forward step by step until desired position
      mot_conPOS++;
    }
    else if (LDRL_stat == 0 && LDRR_stat == 1) {
      digitalWrite(dirPin, LOW);
      digitalWrite(stepPin, HIGH);
      delayMicroseconds(speed_Delay);               // If the bottle is ahead of the desired position
      digitalWrite(stepPin, LOW);                  // then it is brought back step by step until desired position
      delayMicroseconds(speed_Delay);
      mot_conPOS--;
    }
    else if (LDRL_stat == 0 && LDRM_stat == 1 && LDRR_stat == 0) {
      conveyor_ALIGN = 1;       // If the bottle is at the desired position set conveyor_ALIGN flag HIGH and wait.
    }

    if (conveyor_DIR == 0 && caplead_ALIGN == 1 && conveyor_ALIGN == 1) {
      delay(captime + solA);
      digitalWrite(dirPin, LOW);
      for (mot_conPOS; mot_conPOS != 0; mot_conPOS--) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(speed_Delay);  //IF the direction is set to be backward after capping the bottle will return to orignal pos and code will stop.
        digitalWrite(stepPin, LOW);
        delayMicroseconds(speed_Delay);
      }
      vTaskSuspend(Task1);
    }
    else if (conveyor_DIR == 1 && caplead_ALIGN == 1 && conveyor_ALIGN == 1) {
      delay(captime + solA);
      digitalWrite(dirPin, HIGH);
      for (int mot_conPOSN = mot_conPOS; mot_conPOSN < (mot_conPOS + tot_CONSTEPS) ; mot_conPOSN++) {
        digitalWrite(stepPin, HIGH);
        delayMicroseconds(speed_Delay); //IF the direction is set to be forward after capping the bottle will go forward more 75cm and code will stop.
        digitalWrite(stepPin, LOW);
        delayMicroseconds(speed_Delay);
      }
      vTaskSuspend(Task1);
    }
  }
}

void Task2code( void * pvParameters ) {                             // Task on Core 2 Solenoid Motor and Alignment.

  digitalWrite(dirPin1, HIGH);
  for (mot_solPOS; mot_solPOS < tot_SOLSTEPS; mot_solPOS++) {
    digitalWrite(stepPin1, HIGH);
    delayMicroseconds(speed_Delay);         // To reach first a distance of roughly 25CM by the solenoid carrying cap.
    digitalWrite(stepPin1, LOW);
    delayMicroseconds(speed_Delay);
  }

  for (;;) {

    LDRLC_stat = digitalRead(LDRL_CAP);
    LDRMC_stat = digitalRead(LDRM_CAP);
    LDRRC_stat = digitalRead(LDRR_CAP);
    if (LDRLC_stat == 1 && LDRRC_stat == 0) {
      digitalWrite(dirPin1, HIGH);
      digitalWrite(stepPin1, HIGH);
      delayMicroseconds(speed_Delay);         //Same like bottle solenoid is alligned with LASERS and LDRs.
      digitalWrite(stepPin1, LOW);
      delayMicroseconds(speed_Delay);
      mot_solPOS++;
    }
    else if (LDRLC_stat == 0 && LDRRC_stat == 1) {
      digitalWrite(dirPin1, LOW);
      digitalWrite(stepPin1, HIGH);
      delayMicroseconds(speed_Delay);
      digitalWrite(stepPin1, LOW);
      delayMicroseconds(speed_Delay);
      mot_solPOS--;
    }
    else if (LDRLC_stat == 0 && LDRMC_stat == 1 && LDRRC_stat == 0) {
      caplead_ALIGN = 1;
    }

    if (caplead_ALIGN == 1 && conveyor_ALIGN == 1) {    //if both the bottle and cap is alligned
      digitalWrite(solenoid_ON, HIGH);                //Solenoid is Activated for solA time = 2000ms.
      digitalWrite(solenoid_OFF, LOW);
      delay(solA);                    //delay for the solenoid to attach the cap to bottle.
      digitalWrite(solenoid_ON, LOW);        //Solenoid is Deactivated and the conveyor stays still for captime = 2000ms.
      digitalWrite(solenoid_OFF, HIGH);
      delay(captime);                    //delay to completely detach the cap from Solenoid.


      digitalWrite(dirPin1, LOW);
      for (mot_solPOS; mot_solPOS != 0; mot_solPOS--) {
        digitalWrite(stepPin1, HIGH);
        delayMicroseconds(speed_Delay);         //stepping the Motor to Starting Position
        digitalWrite(stepPin1, LOW);
        delayMicroseconds(speed_Delay);
      }
      vTaskSuspend(Task2);
    }

  }
}


void loop() {     //not used because using Free RTOS.

}