#define EN 23
#define dirPin 25
#define Step 27
#define Tur_pin_Top 18 // RPM Sensor Top Roll Tur-Switch
#define Tur_pin_Bottom 19 // RPM Sensor Bottom Roll Tur-Switch
#define Right_pin 21 // Proximity Switch for Right and Park/Load position
#define Left_pin 3 // Proximity Switch for Left Return position
#define Load_pin 2 // PushButton to move the carriage to Park/Load position
#define PotTurPin A1
//#define PotDelayPin A3
#define Distance 1.75 //Filament Diameter
#define interruptors 2// number of interruptors on opto wheel for Speed calculation
#define Pitch 4 // pitch of leadscrew
#define Resolution 200 // step motor resolution for 1.8deg step angle
#define Microsteps 8 //settings on Step driver
#define spoolWidth 60
long int AutonomousSteps = Microsteps / Pitch * Resolution * spoolWidth;// steps in the loop for movement
unsigned long startTime;
unsigned long endTime;
boolean right = LOW, hall = LOW, left = HIGH, load = LOW;
unsigned long duration = 0;
int Speed = 0;
int dirChangeDelay = 0;
//float delayAdjust = 1;
float speedAdjust = 1;
int c = 0;
float setupNumber = Distance / (Pitch) * Resolution;
//-----SETUP----------------------
void setup()
{
delay(2000);
Serial.begin(9600);
delay(200);
Serial.println("System Initialized....");
initPins();
}
//-----LOOP----------------------
void loop()
{
//delayAdjust = analogRead (PotDelayPin) / 1023.0 + 0.5;
speedAdjust = analogRead (PotTurPin) / 1023.0 + 0.5;
Serial.println (speedAdjust);
if (load)
{
loadProcedure();
// Serial.println("load procedure initiated....");
}
else
{
move();
}
}
//------INITIALIZE PINS---------------------
void initPins()
{
pinMode(EN, OUTPUT); // ENABLE AS OUTPUT
pinMode(Step, OUTPUT); // STEP AS OUTPUT
pinMode(dirPin, OUTPUT); // DIRECTION AS OUTPUT
digitalWrite(EN, HIGH); // SET ENABLE TO HIGH FOR EXTERNAL DRIVER, LOW FOR STEPSTICKS
pinMode(Tur_pin_Top , INPUT_PULLUP);
pinMode(Tur_pin_Bottom , INPUT_PULLUP);
pinMode(Right_pin, INPUT_PULLUP);
pinMode(Left_pin, INPUT_PULLUP);
pinMode(Load_pin, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(Tur_pin_Top ), firstMovementAfterEndstopHit, FALLING);
attachInterrupt(digitalPinToInterrupt(Tur_pin_Bottom ), firstMovementAfterEndstopHit, FALLING);
attachInterrupt(digitalPinToInterrupt(Right_pin), rightBut, FALLING);
attachInterrupt(digitalPinToInterrupt(Left_pin), leftBut, FALLING);
attachInterrupt(digitalPinToInterrupt(Load_pin), loadBut, RISING);
delay(1000);
if (digitalRead(Right_pin) == LOW)
{ digitalWrite(dirPin, HIGH);
// Serial.println("HIGH 84");
firstMovementAfterEndstopHit();
Serial.println("System is on RIGHT side initally ");
Serial.println (" waiting for signal...");
}
else //digitalRead(Left_pin) == LOW)
{
Serial.println("System is on LEFT side initally ");
Serial.println ("going right for start...");
loadProcedure();
}
}
void firstMovementAfterEndstopHit()
{ Serial.println ("firstMovementAfterEndstopHit()");
digitalWrite (EN, HIGH);
digitalWrite (dirPin, HIGH);
Serial.println("HIGH 102");
move();
attachInterrupt(digitalPinToInterrupt(Tur_pin_Top), hallButTop, FALLING);
attachInterrupt(digitalPinToInterrupt(Tur_pin_Bottom), hallButBottom, FALLING);
}
//------DETECT ROTATION/SPEED SIGNAL -------------------
void hallButTop()// ISR
{ Serial.println ("hallButTop()");
hall = HIGH;
c++;
if (c == 1)
{
startTime = millis();
}
else if (c == 2)
{
endTime = millis();
duration = endTime - startTime;
speedAdjust = analogRead (PotTurPin) / 1023.0 + 0.5;
Serial.println (speedAdjust);
Speed = (1000000.0 / (Microsteps * (1000.0 / (duration * interruptors)) * setupNumber)) * speedAdjust; //FORMULA //FORMULA
c = 0;
}
}
void hallButBottom()// ISR
{// Serial.println ("hallButBottom()");
hall = HIGH;
c++;
if (c == 1)
{
startTime = millis();
}
else if (c == 2)
{
endTime = millis();
duration = endTime - startTime;
speedAdjust = analogRead (PotTurPin) / 1023.0 + 0.5;
Serial.println (speedAdjust);
Speed = (1000000.0 / (Microsteps * (1000.0 / (duration * interruptors)) * setupNumber))/2 * speedAdjust; //FORMULA //FORMULA
//dirChangeDelay = duration * interruptors * 1000.0 / 4.0 * delayAdjust;
// Serial.print("Duration : "); Serial.println(duration);
// dirChangeDelay = Resolution / Pitch * Microsteps * Distance * Speed * 2 / 1000.0;
// Serial.print("dCD: "); Serial.println(dirChangeDelay);
// float RPS = 1000.0 / (duration * interruptors);
// Serial.print("Speed "); Serial.println(Speed);
// Serial.print("Speed RPS: "); Serial.println(RPS);
c = 0;
}
}
//---------------------------
void rightBut()// ISR
{ Serial.println("Right EndStop Detected...");
left = HIGH;
right = LOW;
hall = LOW;
// delay(dirChangeDelay);
}
//---------------------------
void leftBut()// ISR
{ Serial.println("Left EndStop Detected...");
left = LOW;
right = HIGH;
hall = LOW;
// delay(dirChangeDelay);
}
//--------When Park-Load Buttun triggers-------------------
void loadBut()// ISR
{
Serial.println("Load Button Pressed..");
Serial.println("Move to Right-EndStop and Wait");
left = LOW;
right = HIGH;
hall = LOW;
load = HIGH;
// delay(100);
}
//------PROCEDURE when PARK-Load button is pressed
void loadProcedure()//no ISR
{
detachInterrupt(digitalPinToInterrupt(Tur_pin_Top));
detachInterrupt(digitalPinToInterrupt(Tur_pin_Bottom));
if (!left)
{
digitalWrite(dirPin, LOW);
Serial.println("LOW 169");
movLoad();
}
else if (left)
{ movLoad();
Serial.println("System should Wait unless Tur-Switch is detected ...");
digitalWrite(dirPin, HIGH);
Serial.println("HIGH 175");
}
attachInterrupt(digitalPinToInterrupt(Tur_pin_Top), hallButTop, FALLING);
attachInterrupt(digitalPinToInterrupt(Tur_pin_Bottom), hallButTop, FALLING);
}
//---------PARKING MOVE----- Go Right and wait
void movLoad()//no ISR
{ //digitalWrite(dirPin, LOW);
// Serial.println("Load pressed");
if (Right_pin)
{ digitalWrite(dirPin, LOW);
Speed = (1000000.0 / (Microsteps * (1000.0 / (180)) * setupNumber)) / 2;
attachInterrupt(digitalPinToInterrupt(Right_pin), hitRightEndstop, FALLING);
delay (500);
Serial.println("attached right endstop");
while (digitalRead, Right_pin)
for (int i = 0; i < 20; i++)//ACT
{
digitalWrite(Step, HIGH);
delayMicroseconds(Speed);
digitalWrite(Step, LOW);
delayMicroseconds(Speed);
}
}
else
{ Serial.println("movLoad Else");
digitalWrite(dirPin, HIGH);
Serial.println("HIGH 204");
move();
}
}
//------When it hits right Enstop during Park/load-----
void hitRightEndstop() //ISR
{
//digitalWrite (EN, LOW);
Serial.println("Right Endstop hit, disabling motor!!");
Serial.println("219");
//-------
digitalWrite(dirPin, HIGH);
for (int i = 0; i < (15); i++) //ACT
{
digitalWrite(Step, HIGH);
delayMicroseconds(Speed);
digitalWrite(Step, LOW);
delayMicroseconds(Speed);
}
//-------
attachInterrupt(digitalPinToInterrupt(Tur_pin_Top), firstMovementAfterEndstopHit, FALLING);
attachInterrupt(digitalPinToInterrupt(Tur_pin_Bottom), firstMovementAfterEndstopHit, FALLING);
Serial.println("222");
digitalWrite(dirPin, HIGH);
}
//-----Main Movement procedure BOTH DIR---------------------
void move()
{
Serial.println("233");
for (int i = 0; i < (10); i++)//ACT
{
digitalWrite(Step, HIGH);
delayMicroseconds(Speed * 4);
digitalWrite(Step, LOW);
delayMicroseconds(Speed * 4);
}
for (int i = 0; i < (20); i++)//ACT
{
digitalWrite(Step, HIGH);
delayMicroseconds(Speed * 3);
digitalWrite(Step, LOW);
delayMicroseconds(Speed * 3);
}
for (int i = 0; i < (40); i++)//ACT
{
digitalWrite(Step, HIGH);
delayMicroseconds(Speed * 2);
digitalWrite(Step, LOW);
delayMicroseconds(Speed * 2);
}
for (int i = 0; i < (AutonomousSteps - 70); i++) //ACT
{
digitalWrite(Step, HIGH);
delayMicroseconds(Speed);
digitalWrite(Step, LOW);
delayMicroseconds(Speed);
}
digitalWrite (dirPin, !digitalRead(dirPin));// toggle direction
}