0% found this document useful (0 votes)
32 views

Arduino Code - New

The document defines constants and variables for controlling motors and sensors on a robotic system. It includes declarations for servo motors that control arm movement and a conveyor belt. It also defines variables for a color sensor, ultrasonic sensor, and load cell. The setup function initializes these devices and motors. The main loop controls the arm and conveyor belt servos based on sensor readings to sort objects by color and weight.

Uploaded by

Ruclaudino
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
0% found this document useful (0 votes)
32 views

Arduino Code - New

The document defines constants and variables for controlling motors and sensors on a robotic system. It includes declarations for servo motors that control arm movement and a conveyor belt. It also defines variables for a color sensor, ultrasonic sensor, and load cell. The setup function initializes these devices and motors. The main loop controls the arm and conveyor belt servos based on sensor readings to sort objects by color and weight.

Uploaded by

Ruclaudino
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 13

#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

You might also like