#pragma region Libraries

// Libraries
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <math.h>
#include <NewPing.h>
#include <PENO_Library.h>
#include <HX711.h>
#pragma endregion

#pragma region Variables

#pragma region Declarations global variables
// Declarations global variables
unsigned long t = millis(), dataPrev = millis(), prev_arm = millis(), dataPrev2 = millis();
long delData = 0, delData2 = 0;
int r = NULL, g = NULL, b = NULL;
int distance = 0;
bool posLock = false, startSequence = true, distance_on = false;
String commando, parameters;
bool dataConfirmed = false;
bool finished = false;
//Only used when the color sensor is not used
String box = "placeholder";
//Constants
const bool withWifi = true;
const bool withColor = false, test = false;
const int SERVO_FREQ = 50, SERVOMIN = 84, SERVOMAX = 375;
const float full = 5, zeroHeight = 12.5, armLength = 24.5, initialDistance = zeroHeight - full,
fullInterval = full / 3;
const String startsequence = "CMDS/";
const String stopsequence = "/CMDEND/";
const String startsequenceSetup = "SETUPB/";
const String stopsequenceSetup = "/SETUPE/";
const float breakOff = 0.9;
#pragma endregion Declarations global variables
#pragma region Declarations motor variables
// DC variables
uint8_t DC1 = 12;
uint8_t DC2 = 10;
short DCSpeed = -50;
unsigned long prevDC = millis();
long delDC = 0;

//Conveyor belt servo variables

const uint8_t belt1 = 12, belt2 = 13;
bool beltOn = false, beltWay = false;

//Left-Right servo variables

const uint8_t ServoLR = 9;
//Initial value
int currentAngleLR = 90, goToAngleLR;

//Up-Down servo variables

const short fixedAngle = -18;
long delArm = 0;
const uint8_t UD_Servo = 8;
//Initial value
int currentAngleUD = 55, goToAngleUD, passedGoToAngleUD;
#pragma endregion
#pragma region Declarations bean and sensor variables
//Color sensor variables
const uint8_t CS0 = 6, CS1 = 7, CS2 = 8, CS3 = 9, CSOUT = 13;
bool colorOn = true, colorStart = true;

//Sonar variables
const byte TRIGGER = 46, ECHO = 47;
const int MAX_DISTANCE = 400;

//Scale variables
const int LOADCELL_DOUT_PIN = 28;
const int LOADCELL_SCK_PIN = 44;
const float calFac = -401050 / 200;

//Bean variables
const int angleBox1 = 25, angleBox2 = 85, angleBox3 = 140;
const int angles[3] = {angleBox1, angleBox2, angleBox3};
int angleWhite = NULL, angleBrown = NULL, angleBlack = NULL;
int angleBox;
String box1 = "", box2 = "", box3 = "";
int colorBox1[3] = { 0, 0, 0 }, colorBox2[3] = { 0, 0, 0 }, colorBox3[3] = { 0, 0, 0 };
const int WHITE[3] = {48, 82, 88};
const int BROWN[3] = {55, 105, 110};
const int BLACK[3] = {65, 117, 123};
float weightWhite = 0, weightBrown = 0, weightBlack = 0, weight = 0;
String chosenColor = "";
double currentWeight = 0;
#pragma endregion
#pragma endregion Variables

//Initialise ultrasonic sensor values

NewPing sonar(TRIGGER, ECHO, MAX_DISTANCE);

// Global variable for the use of PWM functions

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

//Variable for weight sensor

HX711 scale;

void setup()
{
#pragma region Main setup
// NIET AANPASSEN ------------------------
pwm.begin();
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ);

pinMode(25, OUTPUT);
digitalWrite(25, HIGH);

Serial.begin(9600);
delay(100);
Serial2.begin(115200);
// EINDE "NIET AANPASSEN"

// Setup
pinMode(DC1, OUTPUT);
pinMode(DC2, OUTPUT);
//colorSetup(CS0, CS1, CS2, CS3, CSOUT);
scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
scale.set_scale(calFac);
scale.tare();

//Initial value setup

pwm.setPWM(15 - ServoLR, 0, map(currentAngleLR, 0, 180, SERVOMIN, SERVOMAX));
pwm.setPWM(15 - UD_Servo, 0, map(currentAngleUD, 0, 180, SERVOMIN, SERVOMAX));
#pragma endregion Main setup

#pragma region Data setup

//Loop repeats as long as the data is not confirmed
while(!dataConfirmed && !test)
{
//Declarations
String command = checkwifi();

//Processing
if (command.length()>0)
{
commando = command.substring(0,command.indexOf('/'));
parameters = command.substring(command.indexOf('/')+1);
if(commando == "beans")
chosenColor = parameters;
else if(commando.startsWith("gewicht"))
{
weight = parameters.substring(7, parameters.length()).toFloat();
if(chosenColor == "black")
weightBlack = weight;
else if(chosenColor == "brown")
weightBrown = weight;
else if(chosenColor == "white")
weightWhite = weight;
}
else if(commando == "verify")
dataConfirmed = true;
}
}

//This will only be called if the color sensor is not used

if(!withColor)
{
while(!(box == "1" || box == "2" || box == "3"))
{
if(Serial.available())
{
box = Serial.readString();
box.trim();
}
}

if(box == "1")
{
goToAngleLR = angleBox1;
angleBox = angleBox1;
}
 else if(box == "2")
{
goToAngleLR = angleBox2;
angleBox = angleBox2;
}
else if(box == "3")
{
goToAngleLR = angleBox3;
angleBox = angleBox3;
}
}
#pragma endregion Data setup

if(test)
weight = 150;

delay(2000);
}

void loop()
{
//Declarations
distance = realDist(sonar.ping_cm(), currentAngleUD);
currentWeight = measureWeight();
t = millis();

#pragma region color setup

//Processing
if(startSequence && withColor)
{
//Move to position
currentAngleLR = angleServoControl(ServoLR, goToAngleLR, currentAngleLR);

//Scanning colors
delay(200);
r = redPW(CS2, CS3, CSOUT);
delay(200);
g = greenPW(CS2, CS3, CSOUT);
delay(200);
b = bluePW(CS2, CS3, CSOUT);

//Assigning color values to different boxes

colorAssignment(currentAngleLR);
colorAngleAssignment(currentAngleLR);

if(box1 != "" && box2 != "" && box3 != "")

{
if(chosenColor == box1)
{
goToAngleLR = angleBox1;
angleBox = angleBox1;
}
else if(chosenColor == box2)
{
goToAngleLR = angleBox2;
angleBox = angleBox2;
}
 else if(chosenColor == box3)
{
goToAngleLR = angleBox3;
angleBox = angleBox3;
}
startSequence = false;
angleServoControl(ServoLR, goToAngleLR, currentAngleLR);
}

//Changing angles
if(box1 != "" && box2 == "" && box3 == "")
{
goToAngleLR = angleBox2;
}
else if(box1 != "" && box2 != "" && box3 == "")
{
goToAngleLR = angleBox3;
}
}
#pragma endregion color setup
#pragma region fullfilling order
else if(!finished)
{
//Declarations
currentWeight = measureWeight();
goToAngleUD = !(currentWeight >= breakOff * weight) ? angleVert(goToAngleUD) : goToAngleUD;
goToAngleLR = angleHor(currentAngleLR);
delData = t - dataPrev;
delArm = t - prev_arm;
delData2 = t - dataPrev2;

//Processing
currentWeight = measureWeight();
//Print the current weight
Serial.println(currentWeight);

//Sending the data if the data delay is exceeded

if(delData >= 500)
{
sendWeight(String(currentWeight));
dataPrev = millis();
}

if(delData2 >= 15000)

{
goToAngleUD = 20;
passedGoToAngleUD = passedAngle(goToAngleUD);
currentAngleUD = angleServoControl(UD_Servo, passedGoToAngleUD, currentAngleUD);
calculateQuantity();
delay(1000);
dataPrev2 = millis();
}

passedGoToAngleUD = passedAngle(goToAngleUD);

if(currentWeight >= breakOff * weight)

{
 if(delArm >= 10000)
{
prev_arm = millis();
goToAngleUD = 20;
passedGoToAngleUD = passedAngle(goToAngleUD);
beltOn = false;
}
else if(delArm >= 5000)
{
goToAngleUD = angleVert(goToAngleUD);
passedGoToAngleUD = passedAngle(goToAngleUD);
beltWay = false;
beltOn = true;
}

if(currentWeight >= weight && !finished)

{
beltWay = true;
beltOn = true;
goToAngleLR = 90;
DCSpeed = -50;
goToAngleUD = 25;
passedGoToAngleUD = passedAngle(goToAngleUD);
continuousServoControl(belt1, !beltWay, beltOn);
continuousServoControl(belt2, beltWay, false);
currentAngleLR = angleServoControl(ServoLR, goToAngleLR, currentAngleLR);
currentAngleUD = angleServoControl(UD_Servo, passedGoToAngleUD, currentAngleUD);
DCControl(DCSpeed);
delay(5000);
beltOn = false;
finished = true;
}
}
else
{
goToAngleUD = angleVert(goToAngleUD);
passedGoToAngleUD = passedAngle(goToAngleUD);
beltOn = true;
}

//Adjusting servo angles

currentAngleLR = angleServoControl(ServoLR, goToAngleLR, currentAngleLR);
currentAngleUD = angleServoControl(UD_Servo, passedGoToAngleUD, currentAngleUD);

//Adjusting conveyor belt and wheel speeds

continuousServoControl(belt1, beltWay, beltOn);
continuousServoControl(belt2, beltWay, beltOn);
delDC = t - prevDC;
if(delDC >= 5000)
{
DCControl(-100);
delay(200);
prevDC = millis();
}
DCControl(DCSpeed);
}
else
 Serial.println("done");
#pragma endregion fullfilling order
}

#pragma region Functions

#pragma region Motor functions
///
// servoNr is a number corresponding to the port the servo you want to control is connected to
// angle is a number in degrees to which the servo-motor has to move
///
short angleServoControl(byte servoNr, short angle, short currentAngle)
{
//Processing
if(currentAngle < angle)
for(;currentAngle < angle; currentAngle++)
{
pwm.setPWM(15 - servoNr, 0, map(currentAngle, 0, 180, SERVOMIN, SERVOMAX));
delay(20);
}
else if(currentAngle > angle)
for(;currentAngle > angle; currentAngle--)
{
pwm.setPWM(15 - servoNr, 0, map(currentAngle, 0, 180, SERVOMIN, SERVOMAX));
delay(20);
}

//Output
return currentAngle;
}

///
// servoNr is a number corresponding to the port the servo you want to control is connected to
///
void continuousServoControl(uint8_t servoNr, bool way, bool on)
{
//Declarations
short clockOrCounter;

//Processing
clockOrCounter = servoNr == belt2 ? (way ? 200 : 400) : (way ? 400 : 200);
clockOrCounter = !on ? 300 : clockOrCounter;
pwm.setPWM(15 - servoNr, 0, clockOrCounter);
}

///
// Speed is a value between -100 and +100 that roughly corresponds to the actual speed
// The actual speed is 1 of 5 values: CCW fast, CCW slow, stationary, CW slow, CW fast
// -50, 0 and 50 are the switch values for the speeds
///
void DCControl(int8_t speed)
{
//Processing
if (speed < -50)
{
analogWrite(DC1, 255);
analogWrite(DC2, 255);
}
else if (-50 <= speed < 0)
 {
analogWrite(DC1, 255);
analogWrite(DC2, 0);
}
else if (0 < speed <= 50)
{
analogWrite(DC1, 0);
analogWrite(DC2, 0);
}
else if (50 < speed)
{
analogWrite(DC1, 0);
analogWrite(DC2, 255);
}
}
#pragma endregion Motor functions
#pragma region Processing functions
///
// This method is used to assign angles to different colors
///
void colorAngleAssignment(short angle)
{
switch(angle)
{
case angleBox1:
box1 = beanAssignment(r, g, b);
angleWhite = angleAssignment("white", angle, angleWhite);
angleBrown = angleAssignment("brown", angle, angleBrown);
angleBlack = angleAssignment("black", angle, angleBlack);
break;
case angleBox2:
box2 = beanAssignment(r, g, b);
angleWhite = angleAssignment("white", angle, angleWhite);
angleBrown = angleAssignment("brown", angle, angleBrown);
angleBlack = angleAssignment("black", angle, angleBlack);
break;
case angleBox3:
box3 = beanAssignment(r, g, b);
angleWhite = angleAssignment("white", angle, angleWhite);
angleBrown = angleAssignment("brown", angle, angleBrown);
angleBlack = angleAssignment("black", angle, angleBlack);
break;
}
}

///
// To calculate the angle of the Up-Down-servo
///
short angleVert(short currentAngle)
{
//Processing
if(distance > 15)
return currentAngle - 20;
else if(distance == 5)
return currentAngle;
else if(distance > 5)
return currentAngle - 1;
else
 return currentAngle + 1;
}

///
// To calculate the angle of the Left-Right-servo
///
int angleHor(short currentAngle)
{
//Processing
if(angleBox - currentAngle >= 0)
//Output
return angleBox + 8;
else if(angleBox - currentAngle < 0)
//Output
return angleBox - 6;
}

///
// For assigning RGB values to the boxes
///
void colorAssignment(short angle)
{
//Processing
switch(angle)
{
case angleBox1:
colorBox1[0] = r;
colorBox1[1] = g;
colorBox1[2] = b;
break;
case angleBox2:
colorBox2[0] = r;
colorBox2[1] = g;
colorBox2[2] = b;
break;
case angleBox3:
colorBox3[0] = r;
colorBox3[1] = g;
colorBox3[2] = b;
break;
}
}

///
// Assigning boxes to bean colors
///
String beanAssignment(int R, int G, int B)
{
//Declarations
float deviationWhite = 0, deviationBrown = 0, deviationBlack = 0, minimumDeviation;
int RGB[3] = {R, G, B};
String color;

//Processing
for(byte i = 0; i <= 2; i++)
deviationWhite += pow(RGB[i] - WHITE[i], 2);
for(byte i = 0; i <= 2; i++)
deviationBrown += pow(RGB[i] - BROWN[i], 2);
 for(byte i = 0; i <= 2; i++)
deviationBlack += pow(RGB[i] - BLACK[i], 2);
minimumDeviation = minimum(deviationWhite, deviationBrown, deviationBlack);
if(minimumDeviation == deviationWhite)
color = "white";
else if(minimumDeviation == deviationBrown)
color = "brown";
else
color = "black";

//Output
return color;
}

///
// Assgining angles to the colors corresponding to the box they are in
///
int angleAssignment(String color, short angle, short colorAngle)
{
//Declarations
int retAngle = NULL;

//Processing
if(colorAngle == NULL)
{
if(color == box1)
if(angleBox1 == angle)
retAngle = angleBox1;
else if(color == box2)
if(angleBox2 == angle)
retAngle = angleBox2;
else if(color == box3)
if(angleBox3 == angle)
retAngle = angleBox3;
}
else
retAngle = colorAngle;

//Output
return retAngle;
}
///
// Function to measure the weight
///
float measureWeight()
{
//Output
return scale.get_units();
}

///
// Function to calculate the amount of beans in the box
///
void calculateQuantity()
{
//Declarations
byte amount, box;
float currentHeight;
 //Processing
if(angleBox == angleBox3)
box = 3;
else if(angleBox == angleBox2)
box = 2;
else if(angleBox == angleBox1)
box = 1;

currentHeight = currentLevel();

if(currentHeight > 2 * fullInterval)

amount = 3;
else if(currentHeight > fullInterval)
amount = 2;
else
amount = 1;

//Output
sendQuantity(box, amount);
}
#pragma endregion Processing functions TODO: angleHor, angleVert
#pragma region Wifi functions
///
// Functions for sending data to the application
///
void fromMonitorToApp(String message)
{
sendWifi("MonitorToApp/" + message);
}

void sendWeight(String weight)

{
fromMonitorToApp("Weight: " + weight);
}

void sendQuantity(byte pile, byte quantity)

{
fromMonitorToApp("Quantity: " + String(pile) + "," + String(quantity));
}

///
// Function for decoding signals sent from the application
///
String checkwifi()
{
//Declarations
String command = "";

//Processing
while (Serial2.available())
command = Serial2.readString();
if(command.indexOf(startsequence) > 0)
{
command = command.substring(command.indexOf(startsequence) + startsequence.length());
command = command.substring(0, command.indexOf(stopsequence));
}
//Checking if it is a setup command
 else if(command.indexOf(startsequenceSetup) > 0)
{
displayESP32Setup(command);
command = "";
}
else
command = "";

//Output
return command;
}
#pragma endregion Wifi functions
#pragma region Helper functions
///
// Helper function for calculating the level of hte beans
///
float currentLevel()
{
//Processing & Output
return zeroHeight - (distance - sin(goToAngleUD / 180 * PI) * armLength);
}

///
// Minimum helper function
///
float minimum(float x, float y, float z)
{
//Processing
if(x <= y && x <= z)
//Output
return x;
else if(y <= x && y <= z)
//Output
return y;
else
//Output
return z;
}

///
// Helper function to calculate the real distance
///
float realDist(float dist, short angle)
{
//Output
return dist * cos(angle / 180 * PI);
}

///
// Helper function to convert the servo angle into the actual angle
//
short passedAngle(short angle)
{
//Output
return angle + 39;
}

///
// Helper function for sending data over wifi-signal to the application
///
void sendWifi(String message)
{
//Output
Serial2.println(message);
}

///
// Helper function for wifi setup
///
void displayESP32Setup(String command)
{
//Processing
command = command.substring(command.indexOf(startsequenceSetup) + startsequenceSetup.length());
command = command.substring(0, command.indexOf(stopsequenceSetup));
Serial.println(command);
}
#pragma endregion Helper functions
#pragma endregion Functions