Here's my code currently (it verifies good). I'm more concerned with getting the code's "control structure" correct. From there, I can tweak my functions as necessary.
I'm aware that my "void loop" section looks a bit funny. It's easier for me to read this way.
/*
//HARDWARE LIST
- Arduino Uno R3 (x1)
- Servo Motors (MG996R, x2)
- Parallax Continuious Rotation Servo Motor with Feedback Pin (x1)
- Adafruit INA219 Current Sensor (x2)
- Adafruit IR Sensor (x1)
- 4-Pin LED Module (x4)
*/
//---------------
//LIBRARIES
#include <Wire.h> //Used for the current monitors
#include <Adafruit_INA219.h> //Used for the INA219 current sensors
#include <Servo.h> //Used for Servo's 2 and 3
#include "FeedBackServo.h" //Used for Servo 1
//CURRENT MONITORS
Adafruit_INA219 ina219_1; //Instance for Servo1's current monitor
Adafruit_INA219 ina219_3; //Inmstance for Servo3's current monitor
//PARALLAX SERVO 1
const int FEEDBACK_PIN = 2; //Define feedback signal pin (Yellow wire)
const int SERVO_PIN = 3; //Define control pin (White wire)
FeedBackServo Servo_1 = FeedBackServo(FEEDBACK_PIN); //Servo_1 - Set feedback signal pin number
//SERVO 2 & SERVO 3
Servo Servo_2; //Instance for the servo motor 2
Servo Servo_3; //Instance for the servo motor 3
//IR SENSOR
const int IR_Sensor_Pin = 4; //Defines the IR sensor pin to pin 2
//LEDS
const int RED = 3; //PWM pin 3
const int GREEN = 5; //PWM pin 5
const int BLUE = 6; //PWM pin 6
//VARIABLES
const int Reset_Button_Pin = 8;
int Servo_1_Micro_Position_2_Count = 0;
int Servo_1_Position_1_Count = 0;
int Servo_1_Position_1_Dispensing_Count = 0;
int Servo_1_Position_2_Count = 0;
int Servo_1_Position_3_Count = 0;
int Servo_3_Position = 0;
//STATES
int IR_Sensor_State = HIGH; //HIGH = The natural "state" of the variable when the beam is unbroken
int Reset_Button_State = HIGH; //State of the Reset_Button without being pressed
//FLAGS
bool IR_Beam_Broken = false; //false = The natural "state" of the variable
bool Servo_1_Micro_Adjustments_Complete = false;
bool Servo_1_Micro_Position_2_Complete = false;
bool Servo_1_Position_1_Complete = false;
bool Servo_1_Position_2_Complete = false;
bool Servo_1_Position_3_Complete = false;
bool Servo_2_Forward_Complete = false;
bool Servo_2_Backward_Complete = false;
bool Servo_3_Slowly_Forward_Complete = false;
bool Servo_3_Backward_Complete = false;
bool Dispense_Bowls_Complete = false;
bool Maint_Light_Active_Complete = false;
bool Maint_Light_Inactive_Complete = false;
//---------------
void setup()
{
//Initialize Serial Monitor
Serial.begin(9600); //Initialize the serial monitor
//Initialize LED's
pinMode(RED, OUTPUT);
pinMode(GREEN, OUTPUT);
pinMode(BLUE, OUTPUT);
analogWrite(RED, 0);
analogWrite(GREEN, 0);
analogWrite(BLUE, 0);
//Initialize IR Sensor
pinMode(IR_Sensor_Pin, INPUT); //Initialize the IR sensor pin as an input
digitalWrite(IR_Sensor_Pin, HIGH); //Turn on the internal Arduino pullup resistor to set the sensor HIGH
//Initialize Current Sensors
ina219_1.begin(); //Start Servo 1's current module
ina219_3.begin(); //Start Servo 3's current module
uint32_t currentFrequency; //Part of the INA219 library. Not sure what this is for, but if I delete it, the current sensors wont work right
//Initialize Servo 1
Servo_1.setServoControl(SERVO_PIN); //Set Servo1 control pin number
Servo_1.setKp(1.0);
//Initialize Servo's 2 and 3
Servo_2.attach(10); //Attach servo 2 to PWM pin 10
Servo_3.attach(11); //Attach servo 3 to PWM pin 11
//Servo 3 Backward Setup
Servo_3_Backward_Setup();
//Servo 2 Backward Setup
Servo_2_Backward_Setup();
//Servo 2 Forward Setup
Servo_2_Forward_Setup();
//Servo 1 Position 1 Setup
Servo_1_Position_1_Setup();
}
//---------------
void loop()
{
IR_Beam();
if (IR_Beam_Broken = true);
{
Servo_3_Slowly_Forward();
if (Servo_3_Slowly_Forward_Complete = true);
{
Servo_2_Backward();
if (Servo_2_Backward_Complete = true);
{
Servo_1_Micro_Adjustments();
if (Servo_1_Micro_Position_2_Complete = true);
{
Servo_1_Micro_Position_2_Count++;
}
if (Servo_1_Position_1_Complete = true)
{
Servo_1_Position_1_Count++;
}
if ((Servo_1_Micro_Position_2_Count == 3) && (Servo_1_Position_1_Count == 3));
{
Servo_1_Micro_Adjustments_Complete = true;
}
if (Servo_1_Micro_Adjustments_Complete = true)
{
Servo_2_Forward();
if (Servo_2_Forward_Complete = true)
{
Servo_3_Backward();
if (Servo_3_Backward_Complete = true)
{
Dispense_12_Bowls();
if (Servo_1_Position_2_Complete = true);
{
Servo_1_Position_2_Count++;
}
if (Servo_1_Position_1_Complete = true);
{
Servo_1_Position_1_Dispensing_Count++;
}
if ((Servo_1_Position_2_Count == 12) && (Servo_1_Position_1_Count == 12));
{
Dispense_Bowls_Complete = true;
}
if (Dispense_Bowls_Complete = true)
{
Reset_All_Flags(); //End of loop
}
}
}
}
}
}
}
}
//---------------
//SETUP FUNCTION LIST
void Servo_3_Backward_Setup()
{
Servo_3.write(0); //Move Servo 3 backward to 0 degrees
delay(1500);
//Create an if statement here to determine if Servo 3 moved backward sucessfully
Serial.println("Servo 3 Backward Setup completed successfully");
Serial.println();
}
void Servo_2_Backward_Setup()
{
Servo_2.write(0); //Move Servo 2 backward to 0 degrees
delay(1500);
Serial.println("Servo 2 Backward Setup completed successfully");
Serial.println();
}
void Servo_2_Forward_Setup()
{
Servo_2.write(180); //Move Servo 2 forward to 180 degrees
delay(1500);
Serial.println("Servo 2 Forward Setup completed successfully");
Serial.println();
}
void Servo_1_Position_1_Setup()
{
Servo_1.rotate(0, 4); //Rotate to 0 degrees + - 4 degrees
Serial.println("Servo_1_Position_1_Setup completed successfully");
Serial.println();
}
//---------------
//LOOP FUNCTION LIST
void IR_Beam() //Check the IR beam's "state"
{
if (IR_Sensor_State == LOW) //LOW = The IR beam is broken
{
Serial.println("The IR beam is broken");
Serial.println();
IR_Beam_Broken = true;
}
else (IR_Sensor_State == HIGH); //HIGH = The IR beam is unbroken
{
return; //Would this exit the function keep looping until the "IR_Sensor_State" changes from HIGH to LOW?
}
}
void Servo_1_Position_1()
{
Servo_1.rotate(0, 4); //Rotate to 0 degrees + - 4 degrees
//Create an if statement here to determine if servo 1 moved sucessfully to position 1
Serial.println("Servo 1 Position 1 completed successfully");
Serial.println();
Servo_1_Position_1_Complete = true;
}
void Servo_1_Micro_Position_2()
{
Servo_1.rotate(35, 4); //Rotate to 35 degrees + - 4 degrees
//Create an if statement to determine if servo 1 moved sucessfully to micro-position 1
Serial.println("Servo 1 Micro Position 1 has completed successfully");
Serial.println();
Servo_1_Micro_Position_2_Complete = true;
}
void Servo_1_Position_2()
{
Servo_1.rotate(175, 4); //Rotate to 175 degrees + - 4 degrees
//Create an if statement here to determine if servo 1 moved sucessfully to position 2
Serial.println("Servo 1 Position 2 completed successfully");
Serial.println();
Servo_1_Position_2_Complete = true;
}
void Servo_1_Position_3()
{
Servo_1.rotate(220, 4); //Rotate to 220 degrees + - 4 degrees
//Create an if statement here to determine if Servo 1 moved sucessfully to position 3
Serial.println("Servo 1 Position 2 completed successfully");
Serial.println();
Servo_1_Position_2_Complete = true;
}
void Servo_2_Forward()
{
Servo_2.write(180); //Move Servo 2 forward to 180 degrees
delay(1500);
//Create an if statement here to determine if Servo 1 moved sucessfully to position 3
Serial.println("Servo 2 Forward completed successfully");
Serial.println();
Servo_2_Forward_Complete = true;
}
void Servo_2_Partially_Backward()
{
Servo_2.write(150); //Move Servo 2 backward 30 degrees
delay(1500);
//Create an if statement here to determine if Servo 2 moved successfully
Serial.println("Servo 2 Forward completed successfully");
Serial.println();
Servo_2_Forward_Complete = true;
}
void Servo_2_Backward()
{
Servo_2.write(0); //Move Servo 2 backward to 0 degrees
delay(1500);
Serial.println("Servo_2_Backward completed successfully");
Serial.println();
Servo_2_Backward_Complete = true;
}
void Servo_3_Slowly_Forward() //Servo 3 slowly goes from 0 degrees to 180 degrees in increments of 1 degree at a time
{
for (Servo_3_Position = 0; Servo_3_Position <= 180; Servo_3_Position += 1)
{
Servo_3.write(Servo_3_Position); //Move Servo 3 to 'Servo_3_Position'
float current_mA = 0; //This is for the current monitor
current_mA = ina219_3.getCurrent_mA(); //This is for the current monitor
Serial.print("Servo 3 Forward: ");
Serial.print(current_mA); //Serial print "current_mA"
Serial.println(" mA"); //Serial print "mA"
Serial.print("Degree: ");
Serial.println(Servo_3_Position); //Serial print "Servo_3_Position"
Serial.println();
delay(15); //Controls Servo 3's rotational speed from 1 degree to the next. Example: Going from 1 degree to 2 degrees has a 15ms delay in between each degree
if ((current_mA) >= 120.00 && Servo_3_Position > 15) //If the current monitor detects a value greater than or equal to 120.00 and has a degree greater than 15. This eliminates the chance of a false flag due to the initial current inrush when Servo_3 starts.
{
delay(1);
Serial.println("Servo 3 Slowly Forward - FLAG #1 - Failed to move forward successfully");
Serial.println();
float current_mA = 0; //This is for the current monitor
current_mA = ina219_3.getCurrent_mA(); //This is for the current monitor
Serial.print("Servo 3 Forward: ");
Serial.print(current_mA); //Serial print "current_mA"
Serial.println(" mA"); //Serial print "mA"
Serial.print("Degree: ");
Serial.println(Servo_3_Position); //Serial print "Servo_3_Position"
Serial.println();
if ((current_mA) >= 120.00) //If the current monitor detects a value greater than or equal to 120.00 for the 2nd time
{
delay(1);
Serial.println("Servo 3 Slowly Forward - FLAG #2 - Failed to move forward successfully");
Serial.println();
float current_mA = 0; //This is for the current monitor
current_mA = ina219_3.getCurrent_mA(); //This is for the current monitor
Serial.print("Servo 3 Forward: ");
Serial.print(current_mA); //Serial print "current_mA"
Serial.println(" mA"); //Serial print "mA"
Serial.print("Degree: ");
Serial.println(Servo_3_Position); //Serial print "Servo 3 Position"
Serial.println();
if ((current_mA) >= 120.00) //If the current monitor detects a value greater than or equal to 120.00 for the 3rd time
{
Serial.println("Servo 3 Slowly Forward - FLAG #3 - Failed to move forward successfully");
Serial.println();
float current_mA = 0; //This is for the current monitor
current_mA = ina219_3.getCurrent_mA(); //This is for the current monitor
Serial.print("Servo 3 Forward: ");
Serial.print(current_mA); //Serial print "current_mA"
Serial.println(" mA"); //Serial print "mA"
Serial.print("Degree: ");
Serial.println(Servo_3_Position); //Serial print the variable "Servo_3_Position"
Serial.println();
if ((current_mA) >= 120.00) //If the current monitor detects a value greater than or equal to 120.00 for the 4th time
{
Servo_3.detach(); //Disconnect Servo 3 from PWM pin 9
Blue_LEDs_On(); //Turn on the blue LED's
Serial.println("Servo 3 Slowly Forward - Flag #4 - Software Reset");
Serial.println();
delay(1500);
Reset_All_Flags();
Software_Reset();
}
}
}
}
}
Serial.print("////////////////////////////////////"); //Print a dividing line for easy viewing
Serial.println();
Serial.println();
delay(1000);
Serial.println("Servo 3 Slowly Forward has completed successfully");
Serial.println();
Servo_3_Slowly_Forward_Complete = true;
}
void Servo_3_Backward()
{
Servo_3.write(0); //Move Servo 3 backward to 0 degrees
delay(1500);
//Create an if statement to determine if Servo 3 moved backward sucessfully
Serial.println("Servo 3 Backward Setup completed successfully");
Serial.println();
}
//---------------
//MISC. FUNCTIONS
void Servo_1_Micro_Adjustments()
{
Servo_1_Micro_Position_2();
Servo_1_Position_1();
}
void Dispense_12_Bowls()
{
Servo_1_Position_2();
Servo_1_Position_1();
}
void Green_LEDs_On()
{
analogWrite(RED, 0); //Off
analogWrite(GREEN, 255); //On - Max value
analogWrite(BLUE, 0); //Off
}
void Yellow_LEDs_On()
{
analogWrite(RED, 255); //On - Max value
analogWrite(GREEN, 155); //On
analogWrite(BLUE, 0); //Off
}
void Red_LEDs_On()
{
analogWrite(RED, 255); //ON - Max value
analogWrite(GREEN, 0); //Off
analogWrite(BLUE, 0); //Off
}
void Blue_LEDs_On()
{
analogWrite(RED, 0); //Off
analogWrite(GREEN, 0); //Off
analogWrite(BLUE, 255); //On - Max Value
}
void LEDs_Off()
{
analogWrite(RED, 0); //Off
analogWrite(GREEN, 0); //Off
analogWrite(BLUE, 0); //Off
}
void Maint_Light_Active()
{
Blue_LEDs_On();
Servo_2.detach();
Servo_3.detach();
Serial.println("The Maintenance Light has been activated");
Serial.println();
Maint_Light_Active_Complete = true;
//Add code to check if the maintenance bypass key has been used. If so, call Maint_Light_Inactive
}
void Maint_Light_Inactive()
{
//Code to watch if the maint key has been used, if so:
LEDs_Off();
Serial.println("The Maintenance Light has been deactivated");
Serial.println();
Maint_Light_Inactive_Complete = true;
Software_Reset();
}
void Reset_Button()
{
//Add code here to check to see if the reset button has been pressed
}
void Software_Reset()
{
//Add code here to reset the Arduino via software command
}
void Reset_All_Flags()
{
bool IR_Beam_Broken = false; //false = The natural "state" of the variable
bool Servo_1_Micro_Adjustments_Complete = false;
bool Servo_1_Micro_Position_2_Complete = false;
bool Servo_1_Position_1_Complete = false;
bool Servo_1_Position_2_Complete = false;
bool Servo_1_Position_3_Complete = false;
bool Servo_2_Forward_Complete = false;
bool Servo_2_Backward_Complete = false;
bool Servo_3_Slowly_Forward_Complete = false;
bool Servo_3_Backward_Complete = false;
bool Dispense_Bowls_Complete = false;
bool Maint_Light_Active_Complete = false;
bool Maint_Light_Inactive_Complete = false;
}