byte directionPinDrill1 = 12;
byte stepPinDrill1 = 13;
byte directionPinDrill2 = 10;
byte stepPinDrill2 = 11;
byte directionPinX = 8;
byte stepPinX = 9;
int sensorDrill1 = A2;
int sensorDrill2 = A1;
int sensorX = A0;
int detailSensor = A3;
int drillButton1 = A4;
int drillButton2 = A5;
int dowelSwitch = 2;
int detectDetailSwitch = 3;
//-------------------------------------------------------------------------
float stepsPerMillimeter = 50;
float millimetersPerSecondDrill1 = 30;
float millimetersPerSecondDrill2 = 30;
float millimetersPerSecondX = 30;
float workingArea = stepsPerMillimeter * 2800;
float minDistanceCouplerFromBase = stepsPerMillimeter * 32;
float standardDistanceCouplerFromBase = stepsPerMillimeter * 64;
float maxDistanceCouplerFromBase = stepsPerMillimeter * 96;
float maxDistanceBetweenCouplers = stepsPerMillimeter * 480;
float distanceDowelFromCoupler = stepsPerMillimeter * 32;
float distanceSensorFromDrills = stepsPerMillimeter * 50;
float distanceFromHomeSensors = stepsPerMillimeter * 10;
float couplerHoleDept = stepsPerMillimeter * 50;
float dowelHoleDept = stepsPerMillimeter * 30;
float intervalBetweenActions = 1000;
float intervalBetweenStepsDrill1 = 1000000 / (millimetersPerSecondDrill1 * stepsPerMillimeter) / 2;
float intervalBetweenStepsDrill2 = 1000000 / (millimetersPerSecondDrill2 * stepsPerMillimeter) / 2;
float intervalBetweenStepsX = 1000000 / (millimetersPerSecondX * stepsPerMillimeter) / 2;
//-------------------------------------------------------------------------
int selectedMotorStepPin;
int selectedMotorDirectionPin;
int selectedMotorInterval;
float position;
float startPosition;
float endPosition;
float detailLength;
int segmentsCount;
float distanceCouplerFromBase;
float distanceBetweenCouplers;
int couplersCount;
int currentDrill;
//-------------------------------------------------------------------------
int selectMotorPropertiesFunction (int motor) {
switch (motor) {
case 1:
selectedMotorStepPin = stepPinDrill1;
selectedMotorDirectionPin = directionPinDrill1;
selectedMotorInterval = intervalBetweenStepsDrill1;
break;
case 2:
selectedMotorStepPin = stepPinDrill2;
selectedMotorDirectionPin = directionPinDrill2;
selectedMotorInterval = intervalBetweenStepsDrill2;
break;
case 3:
selectedMotorStepPin = stepPinX;
selectedMotorDirectionPin = directionPinX;
selectedMotorInterval = intervalBetweenStepsX;
break;
}
}
//-------------------------------------------------------------------------
int moveFunction (int direction, int Motor) {
selectMotorPropertiesFunction(Motor);
digitalWrite(selectedMotorDirectionPin, direction);
digitalWrite(selectedMotorStepPin, HIGH);
delayMicroseconds(selectedMotorInterval);
digitalWrite(selectedMotorStepPin, LOW);
delayMicroseconds(selectedMotorInterval);
if (direction == HIGH and Motor == 3) { position++; }
else if (direction == LOW and Motor == 3) { position--; }
}
//-------------------------------------------------------------------------
int moveByDistanceFunction (int direction, float distance) {
for (int x = 0; x < distance; x++) {
moveFunction(direction, 3);
}
}
//-------------------------------------------------------------------------
int goToPositionFunction (float newPosition) {
float distance = newPosition - position;
int direction;
if (distance > 0) { direction = HIGH; }
else if (distance < 0) { direction = LOW; }
moveByDistanceFunction (direction, distance);
}
//-------------------------------------------------------------------------
int drillingFunction (int drill, float holeDept) {
for (int x = 0; x < holeDept; x++) {
moveFunction(HIGH, drill);
}
for (int x = 0; x < holeDept; x++) {
moveFunction(LOW, drill);
}
Serial.println(position / stepsPerMillimeter);
}
//-------------------------------------------------------------------------
int goToHomeFunction() {
while (digitalRead(sensorDrill1) == LOW) { moveFunction(LOW, 1); }
while (digitalRead(sensorDrill2) == LOW) { moveFunction(LOW, 2); }
while (digitalRead(sensorX) == LOW) { moveFunction(LOW, 3); }
while (digitalRead(sensorDrill1) == HIGH) { moveFunction(HIGH, 1); }
while (digitalRead(sensorDrill2) == HIGH) { moveFunction(HIGH, 2); }
while (digitalRead(sensorX) == HIGH) { moveFunction(HIGH, 3); }
moveByDistanceFunction(HIGH, distanceFromHomeSensors + distanceSensorFromDrills);
for (int x = 0; x < distanceFromHomeSensors; x++) {
moveFunction(HIGH, 1);
moveFunction(HIGH, 2);
}
position = 0;
currentDrill = 0;
}
//-------------------------------------------------------------------------
int detectDetailFunction() {
goToPositionFunction(0);
while (digitalRead(detailSensor) == LOW and position < workingArea) {
moveFunction(HIGH, 3);
}
startPosition = position;
delay(intervalBetweenActions);
while (digitalRead(detailSensor) == HIGH and position < workingArea) {
moveFunction(HIGH, 3);
}
endPosition = position;
detailLength = endPosition - startPosition;
Serial.println(" ");
Serial.println("------------------------detect detail-------------------------");
Serial.println(" ");
Serial.print("Detail length "); Serial.println(detailLength / stepsPerMillimeter);
Serial.print("Start position "); Serial.print(startPosition / stepsPerMillimeter);
Serial.print("End position "); Serial.println(endPosition / stepsPerMillimeter);
}
//-------------------------------------------------------------------------
int createCouplersDistanceFunction () {
startPosition = 0;
endPosition = 675 * stepsPerMillimeter;
position = endPosition;
detailLength = 675 * stepsPerMillimeter;
if (detailLength - minDistanceCouplerFromBase * 2 <= maxDistanceBetweenCouplers) {
distanceCouplerFromBase = minDistanceCouplerFromBase;
segmentsCount = 1;
} else if (detailLength - maxDistanceCouplerFromBase * 2 <= maxDistanceBetweenCouplers) {
distanceCouplerFromBase = maxDistanceCouplerFromBase;
segmentsCount = 1;
} else if (detailLength - standardDistanceCouplerFromBase * 2 > maxDistanceBetweenCouplers) {
distanceCouplerFromBase = standardDistanceCouplerFromBase;
segmentsCount = ceil((detailLength - distanceCouplerFromBase * 2) / maxDistanceBetweenCouplers);
}
distanceBetweenCouplers = (detailLength - distanceCouplerFromBase * 2) / segmentsCount;
couplersCount = segmentsCount + 1;
Serial.println(" ");
Serial.println("------------------------Coupler Distance-------------------------");
Serial.println(" ");
Serial.print("Detail length "); Serial.println(detailLength / stepsPerMillimeter);
Serial.print("segments Count "); Serial.println(segmentsCount);
Serial.print("Couplers Count "); Serial.println(ceil(couplersCount));
Serial.print("Coupler From Base "); Serial.println(distanceCouplerFromBase / stepsPerMillimeter);
Serial.print("Between Couplers "); Serial.println(distanceBetweenCouplers / stepsPerMillimeter);
}
//-------------------------------------------------------------------------
int selectDrillFunction(int drill) {
switch (drill) {
case 0:
if (currentDrill = 1) {
moveByDistanceFunction (HIGH, distanceSensorFromDrills);
position = position - distanceSensorFromDrills;
} else if (currentDrill = 2) {
moveByDistanceFunction (LOW, distanceSensorFromDrills);
position = position + distanceSensorFromDrills;
}
currentDrill = 0;
break;
case 1:
if (currentDrill = 0) {
moveByDistanceFunction (LOW, distanceSensorFromDrills);
position = position + distanceSensorFromDrills;
} else if (currentDrill = 2) {
moveByDistanceFunction (LOW, distanceSensorFromDrills * 2);
position = position + distanceSensorFromDrills * 2;
}
currentDrill = 1;
break;
case 2:
if (currentDrill = 0) {
moveByDistanceFunction (HIGH, distanceSensorFromDrills);
position = position - distanceSensorFromDrills;
} else if (currentDrill = 1) {
moveByDistanceFunction (HIGH, distanceSensorFromDrills * 2);
position = position - distanceSensorFromDrills * 2;
}
currentDrill = 2;
break;
}
}
//-------------------------------------------------------------------------
int dowelsDrillingFunction(int direction) {
selectDrillFunction(2);
moveByDistanceFunction (direction, distanceCouplerFromBase - distanceDowelFromCoupler);
for (int x = 1; x <= segmentsCount; x++) {
moveByDistanceFunction (direction, distanceDowelFromCoupler * 2);
drillingFunction (2, dowelHoleDept);
moveByDistanceFunction (direction, distanceBetweenCouplers - distanceDowelFromCoupler * 2);
drillingFunction (2, dowelHoleDept);
}
}
//-------------------------------------------------------------------------
int couplersDrillingFunction(int direction, int drill) {
selectDrillFunction(drill);
moveByDistanceFunction (direction, distanceCouplerFromBase);
drillingFunction (drill, couplerHoleDept);
for (int x = 0; x < segmentsCount; x++) {
moveByDistanceFunction (direction, distanceBetweenCouplers);
drillingFunction (drill, couplerHoleDept);
}
moveByDistanceFunction (direction, distanceCouplerFromBase);
}
//-------------------------------------------------------------------------
int mainControllerFunction(int drill, int dowelSwitch, int detectDetailSwitch) {
if (detectDetailSwitch == HIGH or detailLength == 0) { detectDetailFunction(); }
createCouplersDistanceFunction ();
int couplersDirection;
int dowelsDirection;
if (position == startPosition) { couplersDirection = HIGH; dowelsDirection = LOW; }
else if (position == endPosition) { couplersDirection = LOW; dowelsDirection = HIGH; }
if (detailLength >= minDistanceCouplerFromBase * 3) {
couplersDrillingFunction(couplersDirection, drill);
}
if (dowelSwitch == HIGH and detailLength >= distanceDowelFromCoupler + (minDistanceCouplerFromBase + distanceDowelFromCoupler ) * 2) {
dowelsDrillingFunction(dowelsDirection);
}
goToPositionFunction(startPosition);
}
//-------------------------------------------------------------------------
void setup() {
Serial.begin(9600);
pinMode(stepPinDrill1, OUTPUT);
pinMode(directionPinDrill1, OUTPUT);
pinMode(stepPinDrill2, OUTPUT);
pinMode(directionPinDrill2, OUTPUT);
pinMode(stepPinX, OUTPUT);
pinMode(directionPinX, OUTPUT);
pinMode(sensorX, INPUT);
pinMode(sensorDrill1, INPUT);
pinMode(sensorDrill2, INPUT);
pinMode(detailSensor, INPUT);
pinMode(drillButton1, INPUT);
pinMode(drillButton2, INPUT);
pinMode(dowelSwitch, INPUT);
pinMode(detectDetailSwitch, INPUT);
goToHomeFunction();
}
//-------------------------------------------------------------------------
void loop() {
if (digitalRead(drillButton1) == HIGH) {
mainControllerFunction(1, digitalRead(dowelSwitch), digitalRead(detectDetailSwitch));
} else if (digitalRead(drillButton2) == HIGH) {
mainControllerFunction(2, digitalRead(dowelSwitch), digitalRead(detectDetailSwitch));
}
}