/*
https://hackaday.io/project/168301-arduino-controlled-photogrammetry-3d-scanner
*/
//#define debug_with_lcd
#include <LiquidCrystal.h>
//#include <Stepper.h>
#include <AccelStepper.h>
#include <Servo.h>
LiquidCrystal lcd(11, 2, 4, 5, 6, 7); // (rs, enable, d4, d5, d6, d7)
const int SW_pin = 8; // digital pin connected to switch output
const int X_pin = A0; // analog pin connected to X output
const int Y_pin = A1; // analog pin connected to Y output
#define stepPin 9
#define dirPin 10
// ---------------------- USER SETTINGS ----------------------
#define maxSpeed 3000
#define minSpeed 10
#define stepsPerRevolution 200 // change this to fit the number of steps per revolution
bool reverseXaxis = true;
bool reverseYaxis = false;
#define photoOnTime 300 // On Time for taking a photo
#define photoDelayTime 1000 // delay time after taking a photo
#define servoTriggerPos 30 // Serso position to push photo button
#define servoHomePos 90 // servo initial position
int MenuNr = 0; // Menu number
int PhotoNr = 2; // The amount of photos that have to be taken
bool Flag1 = 0; // This flag is only active during 1 program cycle (prevents constantly adding/subtracting 1 to the menu number when the joystick is pushed to the side)
bool Flag2 = 0; // This flag is only active during 1 program cycle (prevents constantly adding/subtracting 2 to the photo number when the joystick is pushed up or down)
bool Flag3 = 0; // This flag is only active during 1 program cycle (prevents constantly adding/subtracting 1 to the RPM when the joystick is pushed up or down)
bool Flag4 = 0; // This flag is only active during 1 program cycle (prevents constantly adding/subtracting 1 to the turn number when the joystick is pushed to the side)
bool Flag5 = 0; // This flag is only active during 1 program cycle (prevents constantly adding/subtracting 1 to the RPM when the joystick is pushed up or down)
bool Flag6 = 0; // This flag is only active during 1 program cycle to clear the lcd
int SwMenu = 0; // Switch menu (Sub menu's in the main menu's)
bool BtnFlag = 0; // This flag is only active during 1 program cycle (prevents constantly adding of 1 to SwMenu when button is pressed)
int FullRev = stepsPerRevolution;//14336; // 1 full revolution of the big gear -> Small-Big gear ratio is 7:1
int rotationSpeed = 1000; // Adjustable range of 28BYJ-48 stepper is 0~17 rpm
int prevRotationSpeed = 0;
int PhotoTaken = 0; // Amount of photo's that have been taken
long nextStepPerPhoto = 0;
long StepPerPhoto; // Amount of steps per photo (calculated -> see MenuNr 0/SwMenu 2)
int TurnNr = 1; // Amount of turns
int CurrentTurn = 0; // Stores current turn number
int Steps = 0; // Amount of individual steps the stepper motor has to turn
long position = 0;
int remainder = 0;
long prevCount = 0;
long prevFastCount = 0;
bool fastCount = false;
int XValue = 0; // Read the analog value from The X-axis from the joystick
int YValue = 0; // Read the analog value from The Y-axis from the joystick
int SwValue = 0; // Read the digital value from The Button from the joystick
int xZero = 0;
int yZero = 0;
int xZeroed = 0;
int yZeroed = 0;
int prevX = 0;
int prevY = 0;
/*
bool joyUp = false;
bool joyDn = false;
bool joyR = false;
bool joyL = false;
*/
typedef struct {
bool Up = false;
bool Dn = false;
bool R = false;
bool L = false;
bool SW = false;
bool Center = false;
} joyButtons;
joyButtons joy;
int deadzone = 100;
bool exitLoop = false;
//Stepper stepper(stepsPerRevolution, step_pin, dir_pin);//(stepsPerRevolution, 9, 11, 10, 12); // Use these pins for the stepper motor
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);// works for a4988 (Bipolar, constant current, step/direction driver)
Servo Servo1; // Define Servo1 as a servo
// or
#define relayPin 12
//========================================================================================================
//========================================================================================================
//========================================================================================================
void setup() {
lcd.begin(16, 2); //Lcd setup
pinMode(SW_pin, INPUT); //Set pushbutton as input
digitalWrite(SW_pin, HIGH); //Set SW_pin High
//stepper.setSpeed(rotationSpeed); //Set RPM of steppermotor
stepper.setMaxSpeed(1000.0);
stepper.setAcceleration(50.0);
//stepper.moveTo(10000);
Servo1.attach(3); //Attach servo to pin 3
Servo1.write(servoHomePos); //Move servo to mid possition
pinMode(relayPin, OUTPUT);
lcd.setCursor(0, 0); //Startup screen start
lcd.print("STARTING"); // """"" //
delay(100); // """"" //
lcd.print("."); delay(100);
lcd.print("."); delay(100);
lcd.print("."); delay(100);
lcd.print("."); delay(100);
lcd.print("."); delay(100);
lcd.print("."); delay(100);
lcd.print("."); delay(100);
lcd.print("."); delay(300);
lcd.clear();
lcd.setCursor(0, 0);
lcd.print(" Calibrating");
lcd.setCursor(0, 1);
lcd.print(" Don't Touch");
delay(1000);
joystick();
delay(100);
lcd.setCursor(0, 0);
lcd.clear();
lcd.print(" ZEROING");
xZero = XValue;
yZero = YValue;
delay(800);
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("X="); if (!reverseXaxis) lcd.print(" "); lcd.print(XValue); lcd.print(" -> "); if (!reverseXaxis) lcd.print(" "); lcd.print(xZeroed);
lcd.setCursor(0, 1);
lcd.print("Y="); if (!reverseYaxis) lcd.print(" "); lcd.print(YValue); lcd.print(" -> "); if (!reverseYaxis) lcd.print(" "); lcd.print(yZeroed);
delay(1000);
lcd.clear();
//xZero = XValue;
//yZero = YValue;
}
//========================================================================================================
//========================================================================================================
//========================================================================================================
void loop() {
joystick();
if (MenuNr < 0) { //This sets the min number of menu's
MenuNr = 2;
}
else if ( MenuNr > 2) { //This sets the max numbers of menu's
MenuNr = 0;
}
if (joy.R && Flag1 == 0 && SwMenu == 0) { //if the joystick is pushed to the right side and flag1 is 0 then 1 will be added to the menu number (purpose of the flag -> see comment Flags above)
MenuNr = MenuNr + 1;
Flag1 = 1;
lcd.clear();
}
if (joy.L && Flag1 == 0 && SwMenu == 0) { //if the joystick is pushed to the left side and flag1 is 0 then 1 will be subtracted from the menu number (purpose of the flag -> see comment Flags above)
MenuNr = MenuNr - 1;
Flag1 = 1;
lcd.clear();
}
if (!joy.R && !joy.L && !joy.Up && !joy.Dn && Flag1 == 1) { //if joystick is at neutral possition, flag1 is set to 0 (purpose of the flag -> see comment Flags above)
Flag1 = 0;
}
if (joy.SW && BtnFlag == 0) { //if the button is pressed and the flag is 0 -> add 1 to menu
SwMenu = SwMenu + 1;
BtnFlag = 1;
delay(50);
lcd.clear();
}
if (!joy.SW && BtnFlag == 1) { //if the button is not pressed and the flag is 0 -> Reset the flag (purpose of the flag -> see comment Flags above)
BtnFlag = 0;
}
//*********************************************** Photo per degree revolution ***********************************************//
if (MenuNr == 0) { //Menu0 program
if (SwMenu == 0) { //Menu 0 selected
lcd.setCursor(0, 0);
lcd.print("Photogrametry");
}
//---------------------------------------------------------------------- Photo count
if (SwMenu == 1) { //entered menu 0
lcd.setCursor(0, 0);
lcd.print("Qty Photos:");
remainder = FullRev % PhotoNr;
lcd.print(PhotoNr);
if (remainder > 0)lcd.print("*");
int degreesPerPhoto = 360 / PhotoNr; //Calculate amount of steps per photo
lcd.setCursor(0, 1);
lcd.print("degree/photo:");
lcd.print(degreesPerPhoto);
if (joy.Up && Flag2 == 0) { //joystick up -> Add 2 to number of photo's
PhotoNr = PhotoNr + 2;
Flag2 = 1;
lcd.clear();
}
if (joy.Dn && Flag2 == 0) { //joystick down -> Subtract 2 from number of photo's
PhotoNr = PhotoNr - 2;
Flag2 = 1;
lcd.clear();
}
if (joy.Center && Flag2 == 1) { //if the Y-axis is back at it's neutral possition -> Flag3 = 0 -> Prevents constant adding or subtracting of 2
Flag2 = 0;
}
if (PhotoNr < 2) { //Min allowable Nr of photo's
PhotoNr = 2;
}
if (PhotoNr > 200) { //Max allowable Nr of photo's
PhotoNr = 200;
}
}
//---------------------------------------------------------------------- RUN
StepPerPhoto = FullRev / PhotoNr;
if (SwMenu == 2) {
stepper.setCurrentPosition(0);
delay(10);
SwMenu = 3;
}
//---------------------------------------------------------------------- RUN
if (SwMenu == 3) { //Program started
lcd.setCursor(0, 0);
lcd.print("Running");
//lcd.print(stepper.currentPosition());
lcd.setCursor(0, 1);
lcd.print("Photo Nr: ");
lcd.print(PhotoTaken);
for (PhotoTaken = 0; PhotoTaken <= PhotoNr; PhotoTaken++) {
if (PhotoTaken == PhotoNr) break;
if (!exitLoop) {
nextStepPerPhoto = StepPerPhoto * (PhotoTaken + 1);
stepper.setSpeed(rotationSpeed); //Set motor speed
stepper.runToNewPosition(nextStepPerPhoto);
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Pos:");
lcd.print(stepper.currentPosition());
delay(10);
lcd.setCursor(0, 1);
lcd.print("Photo: "); //Taking photo's
lcd.print(PhotoTaken + 1);
lcd.print("/");
lcd.print(PhotoNr);
Servo1.write(servoTriggerPos);
digitalWrite(relayPin, HIGH);
delay(photoOnTime);
Servo1.write(servoHomePos);
digitalWrite(relayPin, LOW);
delay(photoDelayTime);
}
//---------------------------------------------------------------------- exit run
joystick();
if (joy.SW) { //if the button is pressed and the flag is 0 -> add 1 to menu
exitLoop = true;
lcd.clear(); // clear LCD
lcd.setCursor(0, 0);
lcd.print(" EXITED");
delay(2000);
stepper.runToNewPosition(position);
break;
}
}
//---------------------------------------------------------------------- Completed
if (PhotoTaken == PhotoNr) { //If the amount of photos taken is equal to the amount of photos that have to be taken -> Program finished
lcd.clear(); // clear LCD
lcd.setCursor(0, 0);
lcd.print("Program finished");
delay(2000);
lcd.clear(); // clear LCD
PhotoTaken = 0;
PhotoNr = 2;
SwMenu = 0;
Steps = 0;
exitLoop = false;
}
}
}
//*********************************************** Cinematic ***********************************************//
// rotation only, set speed and rotation count
if (MenuNr == 1) { //Menu1 program
if (SwMenu == 0) { //Menu1 selected
lcd.setCursor(0, 0);
lcd.print("Cinematic");
}
//---------------------------------------------------------------------- SPEED
if (SwMenu == 1) { //Entered menu1 - sub menu1
lcd.setCursor(0, 0);
lcd.print("motor speed");
lcd.setCursor(12, 0);
lcd.print(rotationSpeed);
if (joy.Up) { // joystick up -> Add 10 RPM
if (Flag3 == 0) {
rotationSpeed = rotationSpeed + 10;
Flag3 = 1;
lcd.clear();
} else {
if (millis() > prevFastCount + 2000) {
if (millis() > prevCount + 50) {
rotationSpeed = rotationSpeed + 10;
lcd.clear();
prevCount = millis();
}
}
}
}
if (joy.Dn) { // joystick down -> Subtract 10 RPM
if (Flag3 == 0) {
rotationSpeed = rotationSpeed - 10;
Flag3 = 1;
lcd.clear();
} else {
if (millis() > prevFastCount + 2000) {
if (millis() > prevCount + 50) {
rotationSpeed = rotationSpeed - 10;
lcd.clear();
prevCount = millis();
}
}
}
}
if (joy.Center && Flag3 == 1) { //if the Y-axis is back at it's neutral possition -> Flag3 = 0 -> Prevents constant adding or subtracting of 1
Flag3 = 0;
}
if (rotationSpeed < minSpeed) { //Min allowable RPM
rotationSpeed = minSpeed;
}
if (rotationSpeed > maxSpeed) { //Max allowable RPM
rotationSpeed = maxSpeed;
}
}
//---------------------------------------------------------------------- turn count
if (SwMenu == 2) { //Entered menu1 - sub menu2
lcd.setCursor(0, 0);
lcd.print("Nr of turns");
lcd.setCursor(7, 1);
lcd.print(TurnNr);
if (joy.Up && Flag4 == 0) { // joystick up -> Add 1 turn
TurnNr = TurnNr + 1;
Flag4 = 1;
lcd.clear();
}
if (joy.Dn && Flag4 == 0) { // joystick down -> Subtract 1 turn
TurnNr = TurnNr - 1;
Flag4 = 1;
lcd.clear();
}
if (joy.Center && Flag4 == 1) { //if the Y-axis is back at it's neutral possition -> Flag3 = 0 -> Prevents constant adding or subtracting of 1
Flag4 = 0;
}
if (TurnNr < 1) { //Min allowable amount of turns
TurnNr = 1;
}
if (TurnNr > 200) { //Max allowable amount of turns
TurnNr = 200;
}
}
//---------------------------------------------------------------------- RUN
if (SwMenu == 3) {
stepper.setCurrentPosition(0);
delay(10);
SwMenu = 4;
}
//---------------------------------------------------------------------- RUN
if (SwMenu == 4) { //Program started
lcd.setCursor(0, 0);
lcd.print("Program started");
lcd.setCursor(0, 1);
lcd.print("Turns done: ");
lcd.print(CurrentTurn);
if (CurrentTurn < TurnNr) {
stepper.setSpeed(rotationSpeed);
//stepper.step(FullRev);
//stepper.moveTo(FullRev);
stepper.moveTo(FullRev * TurnNr);
stepper.runSpeedToPosition();
//stepper.runToNewPosition(FullRev * TurnNr);
//CurrentTurn = CurrentTurn + 1;
CurrentTurn = stepper.currentPosition()/FullRev;
lcd.setCursor(0, 1);
lcd.print("Turns done: ");
//lcd.print(stepper.currentPosition());
lcd.print(CurrentTurn);
}else{
delay(1000);
}
//---------------------------------------------------------------------- Completed
if (CurrentTurn == TurnNr) { //If the current turn is equal to the amount of thurns that need to be turned -> program finished
lcd.clear(); // clear LCD
lcd.setCursor(0, 0);
lcd.print("Program finished");
delay(5000);
lcd.clear(); //Reset
CurrentTurn = 0;
PhotoNr = 1;
rotationSpeed = 15;
SwMenu = 0;
Steps = 0;
}
}
}
//*********************************************** Manual ***********************************************//
if (MenuNr == 2) { //Menu2 selected
if (SwMenu == 0) {
lcd.setCursor(0, 0);
lcd.print("Manual control");
}
if (SwMenu == 1) { //Entered menu2
if (joy.Up && Flag5 == 0) { // joystick up -> Add RPM
if (rotationSpeed > 50)
rotationSpeed = rotationSpeed + 10;
else
rotationSpeed = rotationSpeed + 1;
Flag5 = 1;
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("speed: ");
lcd.print(rotationSpeed);
}
if (joy.Dn && Flag5 == 0) { // joystick down -> Subtract RPM
if (rotationSpeed > 50)
rotationSpeed = rotationSpeed - 10;
else
rotationSpeed = rotationSpeed - 1;
Flag5 = 1;
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("speed: ");
lcd.print(rotationSpeed);
}
//if (YValue > 399 && YValue < 599 && Flag5 == 1) { //if the Y-axis is back at it's neutral possition -> Flag3 = 0 -> Prevents constant adding or subtracting of 1
// Flag5 = 0;
//}
if (rotationSpeed < 1) { //Min allowable RPM
rotationSpeed = 1;
}
if (rotationSpeed > maxSpeed) { //Max allowable RPM
rotationSpeed = maxSpeed;
}
if (xZeroed > 10) { //if the joystick is pushed to the right side and the neutral flag is 0 then 1 will be added to the menu number (purpose of the flag -> see comment Flag1 above)
//position = position + 10;
stepper.setSpeed(rotationSpeed);
//stepper.setSpeed(rotationSpeed);
//stepper.step(position);
//stepper.moveTo(position);
//stepper.runToNewPosition(position);
stepper.runSpeed();
lcd.setCursor(4, 1);
lcd.print("Pos:");
lcd.print(stepper.currentPosition());
lcd.setCursor(14, 1);
lcd.print("->");
Flag6 = 1;
}
if (xZeroed < -10) { //if the joystick is pushed to the left side and the neutral flag is 0 then 1 will be subtracted from the menu number (purpose of the flag -> see comment Flag1 above)
//position = position - 10;
stepper.setSpeed(rotationSpeed);
//stepper.setSpeed(rotationSpeed);
//stepper.step(-position);
//stepper.moveTo(-position);
//stepper.runToNewPosition(position);
stepper.runSpeed();
lcd.setCursor(4, 1);
lcd.print("Pos:");
lcd.print(stepper.currentPosition());
lcd.setCursor(0, 1);
lcd.print("<-");
Flag6 = 1;
}
if (joy.Center) { //if the Y-axis is back at it's neutral possition -> Flag3 = 0 -> Prevents constant adding or subtracting of 1
//position = 0;
if (Flag6 == 1) {
lcd.setCursor(0, 1);
//lcd.print(" ");
lcd.setCursor(14, 1);
//lcd.print(" ");
Flag6 = 0; //This flag is only active during 1 program cycle to clear the lcd
}
}
}
/*
if (SwMenu == 2) { //Leave menu 2 when button is pressed
lcd.clear(); // clear LCD
lcd.setCursor(0, 0);
lcd.print("Exit Manual Mode");
CurrentTurn = 0;
PhotoNr = 0;
rotationSpeed = 1500;
SwMenu = 0;
position = 0;
stepper.runToNewPosition(position);
delay(1000);
}*/
}
//*********************************************** End of Menus ***********************************************//
/*
// Read new position
int position = map(xZeroed,-512,512,-stepsPerRevolution,stepsPerRevolution);
stepper.moveTo(position);
stepper.setSpeed(100);
stepper.runSpeedToPosition();
*/
if (joy.Center) {
joy.R = false;
joy.L = false;
joy.Up = false;
joy.Dn = false;
joy.Center = false;
prevFastCount = millis();
}
}
//========================================================================================================
//========================================================================================================
//========================================================================================================
void joystick() {
if (reverseXaxis) {
XValue = analogRead(X_pin); // Read the reverse analog value from The X-axis from the joystick
xZeroed = XValue - xZero;
xZeroed = xZeroed * (-1);
} else {
XValue = analogRead(X_pin); // Read the analog value from The X-axis from the joystick
xZeroed = XValue - xZero;
}
if (reverseYaxis) {
YValue = analogRead(Y_pin); // Read the reverse analog value from The Y-axis from the joystick
yZeroed = YValue - yZero;
yZeroed = yZeroed * (-1);
} else {
YValue = analogRead(Y_pin); // Read the analog value from The Y-axis from the joystick
yZeroed = YValue - yZero;
}
joy.SW = !digitalRead(SW_pin); // Read the digital value from The Button from the joystick
if (xZeroed != prevX || yZeroed != prevY) {
lcd.clear();
lcd.setCursor(0, 0);
if (xZeroed > deadzone) {
joy.R = true;
}
if (xZeroed < -deadzone) {
joy.L = true;
}
if (yZeroed > deadzone) {
joy.Up = true;
}
if (yZeroed < -deadzone) {
joy.Dn = true;
}
if (xZeroed > -deadzone && xZeroed < deadzone && yZeroed > -deadzone && yZeroed < deadzone) {
joy.Center = true;
}
prevX = xZeroed;
prevY = yZeroed;
}
}