My code is not commented very well yet, I apologize.
#include <Servo.h>
#define NUM_STOPS 4
/*************PINS**************/
const int directionPin = 4;
const int speedPin = 5;
const int headPin = 3;
const int armdir = 7;
const int armspeed = 6;
const int pos0 = A2;
const int pos1 = A3;
const int pos2 = A4;
const int pos3 = A5;
const int estop = 2; //has to be 2 or 3 on arduino nano and uno (interrupt)
// const int targetSelectorPin[NUM_STOPS] = { A0, 3, 4, 5 };
const int needfillPin[NUM_STOPS] = { 8, 9, 10, 11 };
const int refillingPin[NUM_STOPS] = { 12, 13, A0, A1 };
/**********VARIABLES FOR ADJUSTMENT************/
/////////////// MOTORS /////////////////
const unsigned long acceltime = 2700;
const int maxspeed = 255;
const int minspeed = 30;
int currentspeed = 0;
////////////////////////////////////////
/////////////// SERVOS /////////////////
const int headLeft = 1900;
const int headCenter = 1500;
const int headRight = 1100;
const unsigned long servoRefreshRate = 30;
////////////////////////////////////////
/******************** VARIABLES TO NOT MESS *****************/
int STATE = 1;
int LASTSTATE = 2;
int refillState = 0;
bool freshwait = true;
bool targetselected = false;
bool targetdirection;
bool newstate = true;
bool speedreporting;
bool targetreached = false;
bool needfill[NUM_STOPS] = { false, false, false, false };
///////////////TIME////////////////
unsigned long currentTime = 0;
unsigned long lastaccelTime = 0;
unsigned long lastservoTime = 0;
const unsigned long stepTime = acceltime / (maxspeed - minspeed);
unsigned long stoppedTime;
unsigned long movestartTime;
unsigned long waitTime, refillTime;
const unsigned long extendTime = 3800;
const unsigned long travelTime[NUM_STOPS][NUM_STOPS] = {
//rows = from | column = to
// 0 1 2 3
{ 0, 2800, 5500, 8100 },
{ 2800, 0, 2500, 5000 },
{ 5600, 2500, 0, 2500 },
{ 8100, 5100, 2500, 0 }
};
///////////////////////////////////
//////////// SERVO /////////////////
int headTarget = headCenter;
float servosmoothFactor = .96; //The closer to 1 the smoother the acceleration
float servoSmoothed = headCenter;
float servoprevSmoothed = headCenter;
Servo headServo;
////////////////////////////////////
///////////// TARGETING ////////////
int target = 0;
int lasttarget = 0;
// int lastposition = 0;
////////////////////////////////////
const int sensorpin[NUM_STOPS] = { pos0, pos1, pos2, pos3 };
bool stop = true;
/*********************************************************************************/
void setup() {
Serial.begin(9600);
pinMode(estop, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(estop), emergencyStop, LOW);
delay(1000);
headServo.attach(headPin);
/************ I/O **************/
pinMode(directionPin, OUTPUT);
pinMode(speedPin, OUTPUT);
pinMode(headPin, OUTPUT);
pinMode(armdir, OUTPUT);
pinMode(armspeed, OUTPUT);
pinMode(pos0, INPUT);
pinMode(pos1, INPUT);
pinMode(pos2, INPUT);
pinMode(pos3, INPUT);
for (int i = 0; i < NUM_STOPS; i++) {
// pinMode(targetSelectorPin[i], INPUT_PULLUP);
pinMode(needfillPin[i], INPUT);
pinMode(refillingPin[i], OUTPUT);
digitalWrite(refillingPin[i], HIGH);
}
/************HOMING*************/
Serial.println("...HOMING...");
digitalWrite(directionPin, LOW);
analogWrite(speedPin, minspeed);
digitalWrite(armdir, LOW);
analogWrite(armspeed, 255);
while (!targetreached) {
for (int i = 0; i < NUM_STOPS; i++) {
if (digitalRead(sensorpin[i]) == HIGH) {
analogWrite(speedPin, 0);
lasttarget = i;
target = i;
targetreached = true;
break;
}
}
}
targetreached = false;
Serial.print("Homed Position: ");
Serial.println(lasttarget);
Serial.println("Complete");
delay(5000);
}
void loop() {
currentTime = millis();
// lastpositioncheck();
printstate();
checkfilllevel();
switch (STATE) {
case 1: //Targeting
choosetarget();
if (targetselected) {
targetselected = false;
newstate = true;
STATE = 2;
speedreporting = true;
movestartTime = millis();
}
// delay(1000);
break;
case 2: //Moving
aimhead();
move();
break;
case 3: //Refilling
switch (refillState) {
case 0: //Extending
digitalWrite(armdir, HIGH);
// analogWrite(armspeed, 255);
if (currentTime - refillTime >= extendTime) {
Serial.println("Filling");
refillState = 1;
}
break;
case 1: //Filling
// analogWrite(armspeed, 0);
if (needfill[target]) {
digitalWrite(refillingPin[target], LOW);
} else {
refillState = 2;
refillTime = currentTime;
Serial.println("Retracting");
}
break;
case 2: //Retracting
digitalWrite(armdir, LOW);
// analogWrite(armspeed, 255);
if (currentTime - refillTime >= extendTime) {
Serial.println("Refill Complete");
digitalWrite(refillingPin[target], HIGH);
//do i add a wait?
// analogWrite(armspeed, 0);
newstate = true;
STATE = 1;
refillState = 0;
}
break;
}
}
servosmoothing();
// Serial.println(lastposition);
}
void emergencyStop() {
analogWrite(speedPin, 0);
digitalWrite(armdir, LOW);
while (1) {
Serial.print("ERROR - Target was: ");
Serial.println(target);
}
}
// void lastpositioncheck() {
// if (digitalRead(pos0) == LOW) {
// lastposition = 0;
// }
// if (digitalRead(pos1) == HIGH) {
// lastposition = 1;
// }
// if (digitalRead(pos2) == HIGH) {
// lastposition = 2;
// }
// if (digitalRead(pos3) == LOW) {
// lastposition = 3;
// }
// }
void choosetarget() {
lasttarget = target;
if (targetdirection) {
for (int i = 0; i < NUM_STOPS; i++) {
if (needfill[i]) {
target = i;
Serial.print("Moving From: ");
Serial.print(lasttarget);
Serial.print(" Target: ");
Serial.println(target);
targetselected = true;
targetreached = false;
break;
}
}
targetdirection = !targetdirection;
} else {
for (int i = NUM_STOPS - 1; i >= 0; i--) {
if (needfill[i]) {
target = i;
Serial.print("Moving From: ");
Serial.print(lasttarget);
Serial.print(" Target: ");
Serial.println(target);
targetselected = true;
targetreached = false;
break;
}
}
targetdirection = !targetdirection;
}
}
void aimhead() {
if (target < lasttarget) {
digitalWrite(directionPin, LOW);
headTarget = headLeft;
} else if (target > lasttarget) {
digitalWrite(directionPin, HIGH);
headTarget = headRight;
} else {
headTarget = headCenter;
}
stop = false;
}
void move() {
LASTSTATE = STATE;
if (currentTime - movestartTime >= travelTime[lasttarget][target]) { // {
stop = true; //start decceleration
headTarget = headCenter;
}
if (stop == true) {
if (currentspeed == maxspeed) {
Serial.println("Deccelerating");
}
if (currentTime - lastaccelTime >= stepTime) {
if (currentspeed > minspeed) {
currentspeed--;
lastaccelTime = millis();
}
}
}
// if ((target == 0) || (target == NUM_STOPS - 1)) {
// if (digitalRead(sensorpin[target]) == LOW) {
// targetreached = true;
// }
// } else {
if (digitalRead(sensorpin[target]) == HIGH) {
targetreached = true;
}
// }
if (targetreached) {
stop = true;
Serial.println("Arrived");
currentspeed = 0;
newstate = true;
STATE = 3;
refillTime = currentTime;
}
if (stop == false) {
if (currentspeed <= minspeed) {
currentspeed = minspeed;
Serial.println("Accelerating");
}
if (currentTime - lastaccelTime >= stepTime) {
if (currentspeed < maxspeed) {
currentspeed++;
lastaccelTime = millis();
}
}
if ((currentspeed == maxspeed) && (speedreporting)) {
Serial.println("Full Speed Ahead");
speedreporting = false;
}
}
analogWrite(speedPin, currentspeed);
// Serial.print("Target: ");
// Serial.print(target);
// Serial.print(" Stop: ");
// Serial.print(stop);
// Serial.print(" Current Speed: ");
// Serial.println(currentspeed);
}
void checkfilllevel() {
for (int i = 0; i < NUM_STOPS; i++) {
if (digitalRead(needfillPin[i]) == HIGH) {
needfill[i] = true;
} else {
needfill[i] = false;
}
}
}
void servosmoothing() {
if (currentTime - lastservoTime >= servoRefreshRate) {
lastservoTime = millis();
servoSmoothed = headTarget * (1 - servosmoothFactor) + servoprevSmoothed * servosmoothFactor;
servoprevSmoothed = servoSmoothed;
// Serial.print(Target[i]);
// Serial.print(" , ");
// Serial.print(Smoothed[i]);
// Serial.print(" , ");
headServo.writeMicroseconds(servoSmoothed);
}
}
void printstate() {
if (newstate) {
switch (STATE) {
case 0:
Serial.println("...WAITING... ");
break;
case 1:
Serial.println("...TARGETING...");
break;
case 2:
Serial.println("...MOVING...");
break;
case 3:
Serial.println("...REFILLING...");
Serial.println("Extending");
break;
}
}
newstate = false;
}