diff --git a/sketch/Stepper.cpp b/sketch/Stepper.cpp index 898d693..d635f4f 100644 --- a/sketch/Stepper.cpp +++ b/sketch/Stepper.cpp @@ -95,7 +95,10 @@ class Stepper { targetPos = currentPos = position; n = 0; stepInterval = 0; - speed = 0.0; + } + + void setCurrentPos(long position){ + targetPos = currentPos = position; } void stop(){ @@ -112,6 +115,13 @@ class Stepper { { targetPos = absolute; stepInterval = fabs(1000000.0 / speed); + Serial.print("StepInterval "); + Serial.println(stepInterval); + } + if(currentPos < absolute){ + direction = DIRECTION_CW; + }else { + direction = DIRECTION_CCW; } } @@ -158,7 +168,7 @@ class Stepper { stepInterval = fabs(1000000.0 / speed); unsigned long time = micros(); - if (time - lastStepTime >= stepInterval) + if (time - lastStepTime >= 100) { if (direction == DIRECTION_CW) { diff --git a/sketch/old/sketch.ino b/sketch/old/sketch.ino new file mode 100644 index 0000000..ede1b80 --- /dev/null +++ b/sketch/old/sketch.ino @@ -0,0 +1,508 @@ +#include +#include +#include +#include +#include +#include "Stepper.cpp" + +#define MotorInterfaceType 4 + + +#define X_STEP_PIN 2 +#define X_DIR_PIN 5 + +#define Y_STEP_PIN 3 +#define Y_DIR_PIN 6 + + +#define Z_STEP_PIN 23 +#define Z_DIR_PIN 25 +#define Z_STEP_PIN1 27 +#define Z_STEP_PIN2 29 +// IN1 IN3 IN2 IN4 +// 23 27 25 29 +//AccelStepper stepperZ(MotorInterfaceType, Z_STEP_PIN, Z_STEP_PIN1, Z_DIR_PIN, Z_STEP_PIN2); + + + +#define X_MIN_PIN 9 +#define Y_MIN_PIN 10 +#define Z_MIN_PIN 11 +#define SW_PIN 14 +#define X_PIN A15 +#define Y_PIN A14 + + +createBufferedOutput(bufferedOut, 80, DROP_UNTIL_EMPTY); + +const int CALIBRATION_STEP_SPEED = 100; +const int MIN_SPEED = 0; +const int MAX_SPEED = 4000; +int joyX = 0, joyY = 0; +bool joyPressed = false; + + + + +// Speed control +const int SLOW_SPEED = 500; +const int MEDIUM_SPEED = 1000; +const int FAST_SPEED = 4000; +int currentSpeedMode = MEDIUM_SPEED; +int currentMoveDistance = 100; +const int deadZone = 6; + + +millisDelay joyStickDelay; +millisDelay stepperMotorDelay; + +Stepper stepperX(X_STEP_PIN, X_DIR_PIN); +Stepper stepperY(Y_STEP_PIN, Y_DIR_PIN); +AccelStepper stepperZ(MotorInterfaceType, Z_STEP_PIN, Z_STEP_PIN1, Z_DIR_PIN, Z_STEP_PIN2); + + +void setup() { + Serial.begin(115200); + Serial.println("AutoScope System Initializing"); + // bufferedOut.connect(Serial); + + pinMode(X_MIN_PIN, INPUT_PULLUP); + pinMode(Y_MIN_PIN, INPUT_PULLUP); + pinMode(Z_MIN_PIN, INPUT_PULLUP); + pinMode(SW_PIN, INPUT_PULLUP); + + stepperX.init(); + stepperX.setMaxSpeed(MAX_SPEED); + stepperX.setSpeed(MEDIUM_SPEED); + stepperY.init(); + stepperY.setMaxSpeed(MAX_SPEED); + stepperY.setSpeed(MEDIUM_SPEED); + + // // stepperMotorDelay.start(25); + joyStickDelay.start(50);//update joystick pos every 50 ms + // Serial.println("Autoscope System Ready!"); + + + stepperZ.setMaxSpeed(MAX_SPEED); + stepperZ.setAcceleration(200.0); + stepperZ.setSpeed(MEDIUM_SPEED); +} + +void loop() { + bufferedOut.nextByteOut(); + getCurrentJoystickPos(); + runStepper(); + + // // char c = readChar(); + // // processChar(c); + // if (!stepperX.isRunning()) + // stepperX.disableOutputs(); + // else + // stepperX.enableOutputs(); + // if (!stepperY.isRunning()) + // stepperY.disableOutputs(); + // else + // stepperY.enableOutputs(); + // if (!stepperZ.isRunning()) + // stepperZ.disableOutputs(); + // else + // stepperZ.enableOutputs(); +} + + +void runStepper(){ + loopTimer.check(bufferedOut); + stepperX.run(); + stepperZ.run(); + stepperY.run(); +} + + +void getCurrentPositionCM(float &x, float &y, float &z) { + x = stepperX.currentPosition(); + y = stepperY.currentPosition(); + z = stepperZ.currentPosition(); +} + + +void printCurrentPosition() { + float x, y, z; + getCurrentPositionCM(x, y, z); + Serial.print("Current Position - X: "); Serial.print(x); + Serial.print(", Y: "); Serial.print(y); + Serial.print(", Z: "); Serial.print(z); Serial.println(""); +} + + + +void getCurrentJoystickPos() { + // if(joyStickDelay.justFinished()){ + // joyStickDelay.repeat(); + bool locJoyPressed = !digitalRead(SW_PIN); + int locJoyX = mapJoystickValue(analogRead(X_PIN)); + int locJoyY = mapJoystickValue(analogRead(Y_PIN)); + bool zPressedChanged = false, xChanged = false, yChanged = false; + if(locJoyPressed != joyPressed){ + zPressedChanged = true; + } + if(locJoyX != joyX && abs(locJoyX) > deadZone){ + xChanged = true; + } + if(locJoyY != joyY && abs(locJoyY) > deadZone){ + yChanged = true; + } + joyX = locJoyX; + joyY = locJoyY; + joyPressed = locJoyPressed; + + if(zPressedChanged || xChanged || yChanged){ + joySteppersUpdate(xChanged, yChanged, zPressedChanged); + }else { + joySteppersContinue(); + } + // } +} + + +void joySteppersUpdate(bool xChanged, bool yChanged, bool zBtnChanged) { + if(zBtnChanged && joyPressed){ + stepperX.setSpeed(0); + stepperY.setSpeed(0); + stopX(); + stopY(); + }else if(zBtnChanged){ + stepperZ.setSpeed(0); + stopZ(); + } + + Serial.print("Pressed: "); Serial.print(joyPressed); + Serial.print(" joyY: "); Serial.print(joyY); + Serial.print(" joyX: "); Serial.println(joyX); + + if(joyPressed){ + // Control Z axis with joystick Y + if(abs(joyY) > deadZone){ + float zSpeed = mapJoystickToSpeed((float)joyY); + stepperZ.setSpeed(zSpeed); // Positive joyY = positive speed, negative joyY = negative speed + stepperZ.runSpeed(); + } else { + stopZ(); + } + } else { + // Control X/Y axes + if(abs(joyX) > deadZone){ + stepperX.setSpeed(mapJoystickToSpeed((float)joyX)); + if(joyX > 0) + stepperX.setDirection(1); + else + stepperX.setDirection(-1); + stepperX.runSpeed(); + } else { + stopX(); + } + + if(abs(joyY) > deadZone){ + stepperY.setSpeed(mapJoystickToSpeed((float)joyY)); + if(joyY > 0) + stepperY.setDirection(1); + else + stepperY.setDirection(-1); + stepperY.runSpeed(); + } else { + stopY(); + } + } +} + +void joySteppersContinue() { + if(joyPressed){ + stepperX.setSpeed(0); + stepperY.setSpeed(0); + stopX(); + stopY(); + stepperZ.setSpeed(MAX_SPEED / 4); + }else{ + // stepperX.setSpeed(MAX_SPEED / 4); + // stepperY.setSpeed(MAX_SPEED / 4); + stepperZ.setSpeed(0); + stopZ(); + } + + Serial.print("joySteppersContinue Pressed: "); Serial.print(joyPressed);Serial.print(" joyY: "); Serial.print(joyY);Serial.print("joyX: "); Serial.println(joyX); + if(joyPressed){ + // if(yChanged){ + // stepperZ.setSpeed(-1 * ((joyY / 100.0) * MAX_SPEED)); + // } + // stepperZ.setSpeed(-1 * ((joyY / 100.0) * MAX_SPEED)); + if(abs(joyY) > deadZone){ + float zSpeed = mapJoystickToSpeed((float)joyY); + stepperZ.setSpeed(zSpeed); + stepperZ.runSpeed(); + }else { + stopZ(); + } + }else { + if(abs(joyY) > deadZone){ + if(joyY > 0) + stepperY.setDirection(1); + else + stepperY.setDirection(-1); + stepperY.runSpeed(); + }else { + stopY(); + // Serial.println("StopY"); + } + if(abs(joyX) > deadZone){ + if(joyX > 0) + stepperX.setDirection(1); + else + stepperX.setDirection(-1); + stepperX.runSpeed(); + }else { + stopX(); + // Serial.println("StopX"); + } + } +} + +void stopZ(){ + stepperZ.stop(); +} +void stopX(){ + stepperX.stop(); +} +void stopY(){ + stepperY.stop(); +} + +int mapJoystickValue(int potValue) { + return map(potValue, 0, 1024, -100, 100); +} +float mapJoystickToSpeed(float joystickValue) { + // Apply dead zone + if (abs(joystickValue) <= 6) { + return 0.0; + } + + // Determine direction and work with positive values + bool isNegative = joystickValue < 0; + float absValue = abs(joystickValue); + + // Normalize to 0.0 to 1.0 (excluding dead zone) + float normalized = (float)(absValue - 6) / (100 - 6); + + // Apply power curve (try values between 1.5 and 3.0) + float exponent = 2.0; // Start with this, adjust to taste + float curved = pow(normalized, exponent); + // Apply direction and max speed + if (isNegative) { + return -curved * MAX_SPEED; + } else { + return curved * MAX_SPEED; + } +} + + +char readChar() { + char c = 0; + if (Serial.available()) { + c = Serial.read(); + while (Serial.available()) { + Serial.read(); + } + } + return c; +} + +void processChar(char c) { + if (c == 0) { // usual + return; + } + + // MOVEMENT COMMANDS + if (c == 'N') { // Move North (Y+)7 + moveDirection(0, 1, 0); + } else if (c == 'S') { // Move South (Y-) + moveDirection(0, -1, 0); + } else if (c == 'E') { // Move East (X+) + moveDirection(1, 0, 0); + } else if (c == 'W') { // Move West (X-) + moveDirection(-1, 0, 0); + } else if (c == 'U') { // Move Up (Z+) + moveDirection(0, 0, 1); + } else if (c == 'D') { // Move Down (Z-) + moveDirection(0, 0, -1); + } else if (c == 'X') { // Stop all movement + stopAllSteppers(); + + // HOMING COMMANDS + } else if (c == 'H') { // Home all axes + homeAllAxes(); + } else if (c == 'h') { // Home current axis only + homeCurrentAxis(); + + // POSITION COMMANDS + } else if (c == 'P') { // Print current position + printCurrentPosition(); + } else if (c == 'Z') { // Zero/reset position counters + zeroPosition(); + + // SPEED CONTROL + } else if (c == 'F') { // Fast speed mode + setSpeedMode(FAST_SPEED); + } else if (c == 'M') { // Medium speed mode + setSpeedMode(MEDIUM_SPEED); + } else if (c == 'L') { // Low speed mode + setSpeedMode(SLOW_SPEED); + + // SCANNING COMMANDS + } else if (c == 'G') { // Go to origin (0,0,0) + goToOrigin(); + } else if (c == 'R') { // Ready status check + reportStatus(); + + // CALIBRATION/SETUP + } else if (c == 'C') { // Calibrate/find limits + calibrateAxes(); + } else if (c == 'I') { // Initialize system + initializeSystem(); + + // EMERGENCY + } else if (c == '!') { // Emergency stop + emergencyStop(); + + } else if ((c == '\n') || (c == '\r')) { + // skip end of line chars + } else if (c != 0) { + Serial.print("Invalid cmd:"); Serial.println(c); + } +} + +// Movement function implementations +void moveDirection(int xDir, int yDir, int zDir) { + int moveDistance = getCurrentMoveDistance(); // Based on current speed mode + + if (xDir != 0) { + stepperX.moveTo(stepperX.currentPosition() + (xDir * moveDistance)); + } + if (yDir != 0) { + stepperY.moveTo(stepperY.currentPosition() + (yDir * moveDistance)); + } + if (zDir != 0) { + stepperZ.moveTo(stepperZ.currentPosition() + (zDir * moveDistance)); + } +} + +void homeCurrentAxis() { + +} + + +void stopAllSteppers() { + stepperX.stop(); + stepperY.stop(); + stepperZ.stop(); + Serial.println("STOPPED"); +} + +void homeAllAxes() { + Serial.println("HOMING_START"); + // Home X axis + homeAxis(stepperX, X_MIN_PIN, -1); + // Home Y axis + homeAxis(stepperY, Y_MIN_PIN, -1); + // Home Z axis + homeAxisZ(stepperZ, Z_MIN_PIN, -1); + Serial.println("HOMING_COMPLETE"); +} + +void homeAxis(Stepper &stepper, int limitPin, int direction) { + stepper.setSpeed(CALIBRATION_STEP_SPEED * direction); + while (digitalRead(limitPin) == HIGH) { + stepper.runSpeed(); + } + stepper.setCurrentPosition(0); + stepper.stop(); +} + +void homeAxisZ(AccelStepper &stepper, int limitPin, int direction) { + stepper.setSpeed(CALIBRATION_STEP_SPEED * direction); + while (digitalRead(limitPin) == HIGH) { + stepper.runSpeed(); + } + stepper.setCurrentPosition(0); + stepper.stop(); +} + +void goToOrigin() { + Serial.println("GOTO_ORIGIN"); + stepperX.moveTo(0); + stepperY.moveTo(0); + stepperZ.moveTo(0); +} + +void zeroPosition() { + stepperX.setCurrentPosition(0); + stepperY.setCurrentPosition(0); + stepperZ.setCurrentPosition(0); + Serial.println("POSITION_ZEROED"); +} + +void setSpeedMode(int speed) { + currentSpeedMode = speed; + stepperX.setMaxSpeed(speed); + stepperY.setMaxSpeed(speed); + stepperZ.setMaxSpeed(speed); + + if (speed == SLOW_SPEED) { + currentMoveDistance = 50; + Serial.println("SPEED_SLOW"); + } else if (speed == MEDIUM_SPEED) { + currentMoveDistance = 100; + Serial.println("SPEED_MEDIUM"); + } else if (speed == FAST_SPEED) { + currentMoveDistance = 200; + Serial.println("SPEED_FAST"); + } +} + +int getCurrentMoveDistance() { + return currentMoveDistance; +} + + +void reportStatus() { + Serial.print("STATUS:"); + Serial.print("X:"); Serial.print(stepperX.currentPosition()); + Serial.print(",Y:"); Serial.print(stepperY.currentPosition()); + Serial.print(",Z:"); Serial.print(stepperZ.currentPosition()); + Serial.print(",RUNNING:"); + Serial.print(stepperX.isRunning() || stepperY.isRunning() || stepperZ.isRunning()); + Serial.println(); +} + +void calibrateAxes() { + Serial.println("CALIBRATE_START"); + homeAllAxes(); + // Could add max limit finding here too + Serial.println("CALIBRATE_COMPLETE"); +} + +void initializeSystem() { + Serial.println("INIT_START"); + zeroPosition(); + setSpeedMode(MEDIUM_SPEED); + Serial.println("INIT_COMPLETE"); +} + +void emergencyStop() { + // Immediately stop all motors + stepperX.stop(); + stepperY.stop(); + stepperZ.stop(); + stepperX.disableOutputs(); + stepperY.disableOutputs(); + stepperZ.disableOutputs(); + Serial.println("EMERGENCY_STOP"); +} + diff --git a/sketch/oldsketch.ino b/sketch/oldsketch.ino deleted file mode 100644 index 6b116cd..0000000 --- a/sketch/oldsketch.ino +++ /dev/null @@ -1,509 +0,0 @@ -// #include -// #include -// #include -// #include -// #include "Stepper.cpp" - -// #define MotorInterfaceType 4 - - - -// #define Z_STEP_PIN 2 -// #define Z_DIR_PIN 5 - -// #define Z2_STEP_PIN 3 -// #define Z2_DIR_PIN 6 - -// #define Y_STEP_PIN 4 -// #define Y_DIR_PIN 7 - -// #define Y2_STEP_PIN 12 -// #define Y2_DIR_PIN 13 - - - -// #define X_STEP_PIN 52 -// #define X_DIR_PIN 53 - - -// //end names dont match function -// #define END_STEP_PIN 24 -// #define END_DIR_PIN 26 - - -// #define X_MIN_PIN 48 -// #define X_MAX_PIN 46 -// #define Y_MIN_PIN 44 -// #define Y_MAX_PIN 42 -// #define Z_MIN_PIN 40 -// #define Z_MAX_PIN 38 -// #define E_HOME_PIN 36 -// #define ELECTROMAGNET_RELAY_PIN 10 -// #define LATCH_PIN 11 - -// //Mem addresses -// #define EEPROM_X_MIN 0 -// #define EEPROM_X_MAX 4 -// #define EEPROM_Y_MIN 8 -// #define EEPROM_Y_MAX 12 -// #define EEPROM_Z_MIN 16 -// #define EEPROM_Z_MAX 20 -// #define EEPROM_E_MAX 24 -// #define EEPROM_CALIBRATED 28 -// #define EEPROM_X_CUR 32 -// #define EEPROM_Y_CUR 36 -// #define EEPROM_Z_CUR 40 - - -// createBufferedOutput(bufferedOut, 80, DROP_UNTIL_EMPTY); - -// // Global variables for axis limits and steps per CM -// long xMinSteps, xMaxSteps, yMinSteps, yMaxSteps, zMinSteps, zMaxSteps, eMaxSteps; -// float xStepsPerCM, yStepsPerCM, zStepsPerCM, eStepsPerIndex; -// short EFFECTOR_INDEX = 0; - -// // consts -// const float END_EFFECTOR_WIDTH_CM = 5.0; -// const float HORIZONTAL_CARRAGE_WIDTH_CM = 5.6; -// const float VERTICAL_CARRAGE_WIDTH_CM = 2.5; -// const float STOPPER_WIDTH_CM = 1.0; -// const float X_TRAVEL_CM = 25.0 - END_EFFECTOR_WIDTH_CM; -// const float Y_TRAVEL_CM = 23.5 - VERTICAL_CARRAGE_WIDTH_CM - STOPPER_WIDTH_CM; -// const float Z_TRAVEL_CM = 20.0 - HORIZONTAL_CARRAGE_WIDTH_CM - STOPPER_WIDTH_CM; -// const int CALIBRATION_STEP_SPEED = 100; -// const int MIN_DIR_SIGN = 1; -// const int MAX_DIR_SIGN = -1; -// const int EFFECTOR_COUNT = 6; -// const int UNLATCH_ANGLE = 180; -// const int LATCH_ANGLE = 110; - - - - -// Stepper stepperX(X_STEP_PIN, X_DIR_PIN, 23); -// Stepper stepperY1(Y_STEP_PIN, Y_DIR_PIN); -// Stepper stepperY2(Y2_STEP_PIN, Y2_DIR_PIN); -// Stepper stepperZ1(Z_STEP_PIN, Z_DIR_PIN); -// Stepper stepperZ2(Z2_STEP_PIN, Z2_DIR_PIN); - -// Stepper stepperEnd(END_STEP_PIN, END_DIR_PIN, 25); - -// Servo servo; -// void setup() { -// Serial.begin(115200); -// Serial.println("Gantry System Initializing"); -// bufferedOut.connect(Serial); -// servo.attach(LATCH_PIN); -// servo.write(UNLATCH_ANGLE); - -// pinMode(X_MIN_PIN, INPUT_PULLUP); -// pinMode(X_MAX_PIN, INPUT_PULLUP); -// pinMode(Y_MIN_PIN, INPUT_PULLUP); -// pinMode(Y_MAX_PIN, INPUT_PULLUP); -// pinMode(Z_MIN_PIN, INPUT_PULLUP); -// pinMode(Z_MAX_PIN, INPUT_PULLUP); -// pinMode(E_HOME_PIN, INPUT_PULLUP); - -// // Set electromagnet relay pin as output and ensure it's off initially -// pinMode(ELECTROMAGNET_RELAY_PIN, OUTPUT); -// digitalWrite(ELECTROMAGNET_RELAY_PIN, LOW); - -// stepperX.init(); -// stepperX.setMaxSpeed(1000); -// stepperX.setSpeed(1000); -// stepperY1.init(); -// stepperY1.setMaxSpeed(1000); -// stepperY1.setSpeed(1000); -// stepperY2.init(); -// stepperY2.setMaxSpeed(1000); -// stepperY2.setSpeed(1000); -// stepperZ1.init(); -// stepperZ1.setMaxSpeed(1000); -// stepperZ1.setSpeed(1000); -// stepperZ2.init(); -// stepperZ2.setMaxSpeed(1000); -// stepperZ2.setSpeed(1000); - -// stepperEnd.init(); -// stepperEnd.setSpeed(1000); -// stepperEnd.setMaxSpeed(1000); - -// //calibrate each time -// // if (EEPROM.read(EEPROM_CALIBRATED) != 0xAA) { -// Serial.println("Calibration needed. Starting"); -// // calibrateAndHome(); -// // } else { -// // Serial.println("Loading stored calibration data"); -// // loadCalibrationData(); -// // calculateStepsPerCM(); -// // calculateStepsPerIndex(); -// // Serial.println("Calibration data loaded successfully."); -// // moveToCM(X_TRAVEL_CM / 2, 0, 0); -// // } - -// Serial.println("Gantry System Ready!"); -// } - -// void loop() { -// stepperX.runSpeed(); -// stepperY1.runSpeed(); -// stepperY2.runSpeed(); -// stepperZ1.runSpeed(); -// stepperZ2.runSpeed(); -// stepperEnd.runSpeed(); - -// // connectElectromagnet(); -// // delay(2000); -// // disconnectElectromagnet(); -// // delay(2000); - -// // attachment/detachment sequences -// // attachAtPosition(15.0, 10.0, 5.0); // Move to position and connect -// // delay(3000); -// // detachAndMove(5.0, 5.0, 12.0); // Disconnect and move away -// // delay(3000); - -// // moveToCM(5.0, 3.0, 7.5); -// // delay(2000); -// // moveToHome(); -// // delay(2000); -// // if (!stepperX.isRunning()) -// // stepperX.disableOutputs(); -// // else -// // stepperX.enableOutputs(); -// // if (!stepperY1.isRunning()) -// // stepperY1.disableOutputs(); -// // else -// // stepperY1.enableOutputs(); -// // if (!stepperY2.isRunning()) -// // stepperY2.disableOutputs(); -// // else -// // stepperY2.enableOutputs(); -// // if (!stepperZ1.isRunning()) -// // stepperZ1.disableOutputs(); -// // else -// // stepperZ1.enableOutputs(); -// // if (!stepperZ2.isRunning()) -// // stepperZ2.disableOutputs(); -// // else -// // stepperZ2.enableOutputs(); -// // if (!stepperEnd.isRunning()) -// // stepperEnd.disableOutputs(); -// // else -// // stepperEnd.enableOutputs(); - -// } - -// void calibrateAndHome() { -// Serial.println("Starting calibration"); - - -// calibrateXaxis(); -// calibrateZaxis(); -// calibrateYaxis(); -// calibrateEndEffectorCarousal(); -// saveCalibrationData(); - -// calculateStepsPerCM(); -// calculateStepsPerIndex(); - -// moveToHome(); - -// Serial.println("Calibration complete"); -// } - -// void calibrateEndEffectorCarousal(){ -// Serial.println("Calibrating End Effector Carousal"); -// stepperEnd.setCurrentPosition(0); -// while (digitalRead(E_HOME_PIN) == HIGH) { -// stepperEnd.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperEnd.run(); -// } -// stepperEnd.setCurrentPosition(0); - -// while (digitalRead(E_HOME_PIN) == LOW) { -// stepperEnd.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperEnd.run(); -// } - -// while (digitalRead(E_HOME_PIN) == HIGH) { -// stepperEnd.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperEnd.run(); -// } -// eMaxSteps = stepperEnd.currentPosition(); -// Serial.print("End Max: "); Serial.println(eMaxSteps); -// } -// void calibrateXaxis(){ -// Serial.println("Calibrating X axis"); -// stepperX.setCurrentPosition(0); -// while (digitalRead(X_MIN_PIN) == HIGH) { -// stepperX.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperX.run(); -// } -// stepperX.setCurrentPosition(0); -// xMinSteps = stepperX.currentPosition(); -// Serial.print("X Min: "); Serial.println(xMinSteps); - - -// while (digitalRead(X_MAX_PIN) == HIGH) { -// stepperX.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperX.run(); -// } -// xMaxSteps = stepperX.currentPosition(); -// Serial.print("X Max: "); Serial.println(xMaxSteps); -// } -// void calibrateYaxis(){ -// Serial.println("Calibrating Y axis"); - -// while (digitalRead(Y_MIN_PIN) == HIGH) { -// stepperY1.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperY2.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperY1.run(); -// stepperY2.run(); -// } -// stepperY1.setCurrentPosition(0); -// stepperY2.setCurrentPosition(0); -// yMinSteps = stepperY1.currentPosition(); -// Serial.print("Y Min: "); Serial.println(yMinSteps); - -// while (digitalRead(Y_MAX_PIN) == HIGH) { -// stepperY1.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperY2.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperY1.run(); -// stepperY2.run(); -// } -// yMaxSteps = stepperY1.currentPosition(); -// Serial.print("Y Max: "); Serial.println(yMaxSteps); -// } - -// void calibrateZaxis(){ -// Serial.println("Calibrating Z axis"); - -// while (digitalRead(Z_MIN_PIN) == HIGH) { -// stepperZ1.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperZ2.move(MIN_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperZ1.run(); -// stepperZ2.run(); -// } -// stepperZ1.setCurrentPosition(0); -// stepperZ2.setCurrentPosition(0); -// zMinSteps = stepperZ1.currentPosition(); -// Serial.print("Z Min: "); Serial.println(zMinSteps); - -// while (digitalRead(Z_MAX_PIN) == HIGH) { -// stepperZ1.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperZ2.move(MAX_DIR_SIGN * CALIBRATION_STEP_SPEED); -// stepperZ1.run(); -// stepperZ2.run(); -// } -// zMaxSteps = stepperZ1.currentPosition(); -// Serial.print("Z Max: "); Serial.println(zMaxSteps); -// } -// void saveCalibrationData() { -// EEPROM.put(EEPROM_X_MIN, xMinSteps); -// EEPROM.put(EEPROM_X_MAX, xMaxSteps); -// EEPROM.put(EEPROM_Y_MIN, yMinSteps); -// EEPROM.put(EEPROM_Y_MAX, yMaxSteps); -// EEPROM.put(EEPROM_Z_MIN, zMinSteps); -// EEPROM.put(EEPROM_Z_MAX, zMaxSteps); -// EEPROM.put(EEPROM_E_MAX, eMaxSteps); -// EEPROM.write(EEPROM_CALIBRATED, 0xAA); -// EEPROM.put(EEPROM_X_CUR, stepperX.currentPosition()); -// EEPROM.put(EEPROM_Y_CUR, stepperY1.currentPosition()); -// EEPROM.put(EEPROM_Z_CUR, stepperZ1.currentPosition()); -// Serial.println("Calibration data saved to EEPROM"); -// } - -// void saveCurrentAxisStepperPositions(){ -// EEPROM.put(EEPROM_X_CUR, stepperX.currentPosition()); -// EEPROM.put(EEPROM_Y_CUR, stepperY1.currentPosition()); -// EEPROM.put(EEPROM_Z_CUR, stepperZ1.currentPosition()); -// Serial.println("Current Stepper Position data saved to EEPROM"); -// } - -// void loadCalibrationData() { -// long xCur, yCur, zCur; -// EEPROM.get(EEPROM_X_MIN, xMinSteps); -// EEPROM.get(EEPROM_X_MAX, xMaxSteps); -// EEPROM.get(EEPROM_Y_MIN, yMinSteps); -// EEPROM.get(EEPROM_Y_MAX, yMaxSteps); -// EEPROM.get(EEPROM_Z_MIN, zMinSteps); -// EEPROM.get(EEPROM_Z_MAX, zMaxSteps); -// EEPROM.get(EEPROM_E_MAX, eMaxSteps); -// EEPROM.get(EEPROM_X_CUR, xCur); -// EEPROM.get(EEPROM_Y_CUR, yCur); -// EEPROM.get(EEPROM_Z_CUR, zCur); -// stepperX.setCurrentPosition(xCur); -// stepperY1.setCurrentPosition(yCur); -// stepperY2.setCurrentPosition(yCur); -// stepperZ1.setCurrentPosition(zCur); -// stepperZ2.setCurrentPosition(zCur); -// } - -// void calculateStepsPerCM() { -// xStepsPerCM = abs(xMaxSteps - xMinSteps) / X_TRAVEL_CM; -// yStepsPerCM = abs(yMaxSteps - yMinSteps) / Y_TRAVEL_CM; -// zStepsPerCM = abs(zMaxSteps - zMinSteps) / Z_TRAVEL_CM; - -// Serial.println("Steps per CM calculated:"); -// Serial.print("X: "); Serial.println(xStepsPerCM); -// Serial.print("Y: "); Serial.println(yStepsPerCM); -// Serial.print("Z: "); Serial.println(zStepsPerCM); -// } - -// void calculateStepsPerIndex() { -// eStepsPerIndex = eMaxSteps / EFFECTOR_COUNT; -// //EFFECTOR_COUNT -// Serial.println("Steps per Index calculated:"); -// Serial.print("E: "); Serial.println(eStepsPerIndex); -// } - -// // Convert CM position to steps for each axis -// long cmToStepsX(float cm) { -// if (cm < 0) cm = 0; -// if (cm > X_TRAVEL_CM) cm = X_TRAVEL_CM; -// return xMinSteps + (long)(cm * xStepsPerCM); -// } - -// long cmToStepsY(float cm) { -// if (cm < 0) cm = 0; -// if (cm > Y_TRAVEL_CM) cm = Y_TRAVEL_CM; -// return yMinSteps + (long)(cm * yStepsPerCM); -// } - -// long cmToStepsZ(float cm) { -// if (cm < 0) cm = 0; -// if (cm > Z_TRAVEL_CM) cm = Z_TRAVEL_CM; -// return zMinSteps + (long)(cm * zStepsPerCM); -// } - -// // Convert steps to CM position for each axis -// float stepsToCmX(long steps) { -// return (float)(steps - xMinSteps) / xStepsPerCM; -// } - -// float stepsToCmY(long steps) { -// return (float)(steps - yMinSteps) / yStepsPerCM; -// } - -// float stepsToCmZ(long steps) { -// return (float)(steps - zMinSteps) / zStepsPerCM; -// } - -// // Move to specific CM coordinates -// void moveToCM(float xCM, float yCM, float zCM) { - -// long xTarget = cmToStepsX(xCM); -// long yTarget = cmToStepsY(yCM); -// long zTarget = cmToStepsZ(zCM); - -// Serial.print("Moving to: X="); Serial.print(xCM); -// Serial.print("cm, Y="); Serial.print(yCM); -// Serial.print("cm, Z="); Serial.print(zCM); Serial.println("cm"); - -// stepperX.moveTo(xTarget); -// stepperY1.moveTo(yTarget); -// stepperY2.moveTo(yTarget); -// stepperZ1.moveTo(zTarget); -// stepperZ2.moveTo(zTarget); - -// while (stepperX.distanceToGo() != 0 || stepperY1.distanceToGo() != 0 || -// stepperY2.distanceToGo() != 0 || stepperZ1.distanceToGo() != 0 || -// stepperZ2.distanceToGo() != 0) { -// stepperX.run(); -// stepperY1.run(); -// stepperY2.run(); -// stepperZ1.run(); -// stepperZ2.run(); -// } -// Serial.println("Move complete"); -// } - -// void moveToHome() { -// moveToCM(0, 0, Z_TRAVEL_CM); -// Serial.println("Moved to home position"); -// } - -// // Get current position in CM -// void getCurrentPositionCM(float &xCM, float &yCM, float &zCM) { -// xCM = stepsToCmX(stepperX.currentPosition()); -// yCM = stepsToCmY(stepperY1.currentPosition()); -// zCM = stepsToCmZ(stepperZ1.currentPosition()); -// } - -// // Print current position -// void printCurrentPosition() { -// float x, y, z; -// getCurrentPositionCM(x, y, z); -// Serial.print("Current Position - X: "); Serial.print(x); -// Serial.print("cm, Y: "); Serial.print(y); -// Serial.print("cm, Z: "); Serial.print(z); Serial.println("cm"); -// } - - -// void connectElectromagnet() { -// digitalWrite(ELECTROMAGNET_RELAY_PIN, HIGH); -// Serial.println("Electromagnet ON - End effector connected"); -// } - -// void disconnectElectromagnet() { -// digitalWrite(ELECTROMAGNET_RELAY_PIN, LOW); -// Serial.println("Electromagnet OFF - End effector disconnected"); -// } - - -// bool isElectromagnetConnected() { -// return digitalRead(ELECTROMAGNET_RELAY_PIN) == HIGH; -// } - - -// void attachAtPosition(float xCM, float yCM, float zCM) { -// Serial.println("Starting attachment sequence"); - - -// disconnectElectromagnet(); -// moveToCM(xCM, yCM, zCM); - -// delay(500); - -// connectElectromagnet(); -// unlatch(); - -// Serial.println("Attachment sequence complete"); -// } - -// void detachAndMove(float xCM, float yCM, float zCM) { -// Serial.println("Starting detachment sequence"); - -// disconnectElectromagnet(); -// latch(); -// delay(200); - -// moveToCM(xCM, yCM, zCM); - -// Serial.println("Detachment sequence complete"); -// } -// void moveAndDetach(float xCM, float yCM, float zCM) { -// Serial.println("Starting detachment sequence"); - -// moveToCM(xCM, yCM, zCM); -// delay(200); -// disconnectElectromagnet(); -// latch(); - -// Serial.println("Detachment sequence complete"); -// } - - - -// void unlatch() -// { -// servo.write(UNLATCH_ANGLE); -// delay(500); -// } - -// void latch() -// { -// servo.write(LATCH_ANGLE); -// delay(500); -// } diff --git a/sketch/sketch.ino b/sketch/sketch.ino index 58b8df8..a987017 100644 --- a/sketch/sketch.ino +++ b/sketch/sketch.ino @@ -1,695 +1,584 @@ +// ============================================================================ +// AutoScope - Automated Microscope Slide Scanner +// ============================================================================ + #include #include -#include -#include #include +#include +#include #include "Stepper.cpp" +// ============================================================================ +// PIN DEFINITIONS +// ============================================================================ + +// Motor interface type for AccelStepper (4-wire stepper) #define MotorInterfaceType 4 +// X-axis stepper (step/dir driver) +#define X_STEP_PIN 2 +#define X_DIR_PIN 5 - -#define Z_STEP_PIN 4 -#define Z_DIR_PIN 7 - -#define Z2_STEP_PIN 2 -#define Z2_DIR_PIN 5 - +// Y-axis stepper (step/dir driver) #define Y_STEP_PIN 3 #define Y_DIR_PIN 6 -#define Y2_STEP_PIN 12 -#define Y2_DIR_PIN 13 +// Z-axis stepper (4-wire stepper motor) +#define Z_STEP_PIN 23 +#define Z_DIR_PIN 25 +#define Z_STEP_PIN1 27 +#define Z_STEP_PIN2 29 +// IN1 IN3 IN2 IN4 +// 23 27 25 29 +// Limit switches +#define Z_MAX_PIN 10 +#define Z_MIN_PIN 11 +// Joystick +#define SW_PIN 14 +#define X_PIN A15 +#define Y_PIN A14 -#define X_STEP_PIN 52 -#define X_DIR_PIN 53 +// ============================================================================ +// CONSTANTS +// ============================================================================ +const int CALIBRATION_STEP_SPEED = 100; +const int MIN_SPEED = 0; +const int MAX_SPEED = 4000; +const int SLOW_SPEED = 20; +const int MEDIUM_SPEED = 50; +const int FAST_SPEED = 80; -#define END_STEP_PIN 27 -#define END_DIR_PIN 29 -// #define END_STEP_PIN1 24 -// #define END_STEP_PIN2 22 +const int deadZone = 6; +// ============================================================================ +// GLOBAL STATE +// ============================================================================ -#define X_MIN_PIN 38 -#define X_MAX_PIN 36 -#define Y_MIN_PIN 44 -#define Y_MAX_PIN 42 -#define Z_MIN_PIN 46 -#define Z_MAX_PIN 40 -#define E_HOME_PIN 48 -#define ELECTROMAGNET_RELAY_PIN 10 -#define LATCH_PIN 11 +// Joystick state +int joyX = 0; +int joyY = 0; +bool joyPressed = false; +bool lastJoyPressed = false; -//Mem addresses -#define EEPROM_X_MIN 0 -#define EEPROM_X_MAX 4 -#define EEPROM_Y_MIN 8 -#define EEPROM_Y_MAX 12 -#define EEPROM_Z_MIN 16 -#define EEPROM_Z_MAX 20 -#define EEPROM_E_MAX 24 -#define EEPROM_CALIBRATED 28 -#define EEPROM_X_CUR 32 -#define EEPROM_Y_CUR 36 -#define EEPROM_Z_CUR 40 +// Control mode +enum ControlMode { MODE_SERIAL, MODE_JOYSTICK }; +ControlMode currentMode = MODE_SERIAL; +// Triple-click detection +const int CLICKS_REQUIRED = 3; +const unsigned long CLICK_WINDOW_MS = 5000; +unsigned long clickTimes[CLICKS_REQUIRED]; +int clickIndex = 0; +int speedArr[7] = {2,5,10, 30, 50, 70, 100}; +// Speed/movement settings +int currentSpeedMode = MEDIUM_SPEED; +int currentMoveDistance = 100; + +// ============================================================================ +// OBJECTS +// ============================================================================ createBufferedOutput(bufferedOut, 80, DROP_UNTIL_EMPTY); -// Global variables for axis limits and steps per CM -long xMinSteps, xMaxSteps, yMinSteps, yMaxSteps, zMinSteps, zMaxSteps, eMaxSteps; -float xStepsPerCM, yStepsPerCM, zStepsPerCM, eStepsPerIndex; -short EFFECTOR_INDEX = 0; +millisDelay joyStickDelay; +millisDelay stepperMotorDelay; -const short STEPS_PER_ROT = 200; +Stepper stepperX(X_STEP_PIN, X_DIR_PIN); +Stepper stepperY(Y_STEP_PIN, Y_DIR_PIN); +AccelStepper stepperZ(MotorInterfaceType, Z_STEP_PIN, Z_STEP_PIN1, Z_DIR_PIN, Z_STEP_PIN2); -// consts -const float END_EFFECTOR_WIDTH_CM = 5.0; -const float HORIZONTAL_CARRAGE_WIDTH_CM = 5.6; -const float VERTICAL_CARRAGE_WIDTH_CM = 2.5; -const float STOPPER_WIDTH_CM = 1.0; -float X_TRAVEL_CM = 0.0; -float Y_TRAVEL_CM = 0.0; -float Z_TRAVEL_CM = 0.0; -const int CALIBRATION_STEP_SPEED = 1000; -const int MIN_DIR_SIGN = 1; -const int MAX_DIR_SIGN = -1; -const int EFFECTOR_COUNT = 6; -const int UNLATCH_ANGLE = 180; -const int LATCH_ANGLE = 110; -const float THREAD_ROTATIONS_PER_CM = 5.5; -//x ratio 20:35 -//yz ration 20:60 -//5.5 rotations per cm +// ============================================================================ +// SETUP +// ============================================================================ - -Stepper stepperX(X_STEP_PIN, X_DIR_PIN, 23); -Stepper stepperY1(Y_STEP_PIN, Y_DIR_PIN); -Stepper stepperY2(Y2_STEP_PIN, Y2_DIR_PIN); -Stepper stepperZ1(Z_STEP_PIN, Z_DIR_PIN); -Stepper stepperZ2(Z2_STEP_PIN, Z2_DIR_PIN); - - -Stepper stepperEnd(END_STEP_PIN, END_DIR_PIN, 25); - -Servo servo; void setup() { Serial.begin(115200); - Serial.println("Gantry System Initializing"); - bufferedOut.connect(Serial); - servo.attach(LATCH_PIN); - servo.write(UNLATCH_ANGLE); + Serial.println("AutoScope System Initializing"); - pinMode(X_MIN_PIN, INPUT_PULLUP); - pinMode(X_MAX_PIN, INPUT_PULLUP); - pinMode(Y_MIN_PIN, INPUT_PULLUP); - pinMode(Y_MAX_PIN, INPUT_PULLUP); + // Configure limit switch pins pinMode(Z_MIN_PIN, INPUT_PULLUP); pinMode(Z_MAX_PIN, INPUT_PULLUP); - pinMode(E_HOME_PIN, INPUT_PULLUP); + pinMode(SW_PIN, INPUT_PULLUP); - // Set electromagnet relay pin as output and ensure it's off initially - pinMode(ELECTROMAGNET_RELAY_PIN, OUTPUT); - digitalWrite(ELECTROMAGNET_RELAY_PIN, LOW); - int initSpeed = 4000; + for (int i = 0; i < CLICKS_REQUIRED; i++) { + clickTimes[i] = 0; + } + + // Initialize X stepper stepperX.init(); - stepperX.setMaxSpeed(initSpeed); - stepperX.setSpeed(initSpeed); - stepperY1.init(); - stepperY1.setMaxSpeed(initSpeed); - stepperY1.setSpeed(initSpeed); - stepperY2.init(); - stepperY2.setMaxSpeed(initSpeed); - stepperY2.setSpeed(initSpeed); - stepperZ1.init(); - stepperZ1.setMaxSpeed(initSpeed); - stepperZ1.setSpeed(initSpeed); - stepperZ2.init(); - stepperZ2.setMaxSpeed(initSpeed); - stepperZ2.setSpeed(initSpeed); + stepperX.setMaxSpeed(MAX_SPEED); + stepperX.setSpeed(MEDIUM_SPEED); - stepperEnd.init(); - stepperEnd.setMaxSpeed(initSpeed); - stepperEnd.setSpeed(initSpeed); - stepperEnd.setDirection(-1); - // stepperEnd.move(-1200); - delay(5000); + // Initialize Y stepper + stepperY.init(); + stepperY.setMaxSpeed(MAX_SPEED); + stepperY.setSpeed(MEDIUM_SPEED); - //calibrate each time - // if (EEPROM.read(EEPROM_CALIBRATED) != 0xAA) { - Serial.println("Calibration needed. Starting"); - calibrateAndHome(); - // } else { - // Serial.println("Loading stored calibration data"); - // loadCalibrationData(); - // calculateStepsPerCM(); - // calculateStepsPerIndex(); - // Serial.println("Calibration data loaded successfully."); - // moveToCM(X_TRAVEL_CM / 2, 0, 0); - // } + // Initialize Z stepper + stepperZ.setMaxSpeed(MAX_SPEED); + // stepperZ.setAcceleration(200.0); + stepperZ.setSpeed(MEDIUM_SPEED); - Serial.println("Gantry System Ready!"); + // Start joystick polling timer + joyStickDelay.start(50); + Serial.println("MODE:SERIAL"); } +// ============================================================================ +// MAIN LOOP +// ============================================================================ + void loop() { -// stepperX.runSpeed(); -// stepperY1.runSpeed(); -// stepperY2.runSpeed(); -// stepperZ1.runSpeed(); -// stepperZ2.runSpeed(); + bufferedOut.nextByteOut(); - // stepperEnd.run(); - // stepperEnd.run(); - // connectElectromagnet(); - // delay(2000); - // disconnectElectromagnet(); - // delay(2000); - - // attachment/detachment sequences - // attachAtPosition(15.0, 10.0, 5.0); // Move to position and connect - // delay(3000); - // detachAndMove(5.0, 5.0, 12.0); // Disconnect and move away - // delay(3000); - - // moveToCM(5.0, 3.0, 7.5); - // delay(2000); - // moveToHome(); - // delay(2000); - // if (!stepperX.isRunning()) - // stepperX.disableOutputs(); - // else - // stepperX.enableOutputs(); - // if (!stepperY1.isRunning()) - // stepperY1.disableOutputs(); - // else - // stepperY1.enableOutputs(); - // if (!stepperY2.isRunning()) - // stepperY2.disableOutputs(); - // else - // stepperY2.enableOutputs(); - // if (!stepperZ1.isRunning()) - // stepperZ1.disableOutputs(); - // else - // stepperZ1.enableOutputs(); - // if (!stepperZ2.isRunning()) - // stepperZ2.disableOutputs(); - // else - // stepperZ2.enableOutputs(); - // if (!stepperEnd.isRunning()) - // stepperEnd.disableOutputs(); - // else - // stepperEnd.enableOutputs(); - -} - -void calibrateAndHome() { - Serial.println("Starting calibration"); - - - // calibrateEndEffectorCarousal(); - // delay(2500); - // moveEffectorIndex(1); - // delay(2500); - // moveEffectorIndex(2); - // delay(2500); - // moveEffectorIndex(3); - // delay(2500); - // moveEffectorIndex(4); - // delay(2500); - // moveEffectorIndex(5); - // delay(2500); - // moveEffectorIndex(2); - // delay(2500); - // moveEffectorIndex(0); - calibrateXaxis(); - calculateStepsPerCM(); - moveToXCM(X_TRAVEL_CM / 2); - - calibrateZaxis(); - calculateStepsPerCM(); - moveToZCM(Z_TRAVEL_CM / 2); - - calibrateYaxis(); - calculateStepsPerCM(); - moveToYCM(Y_TRAVEL_CM / 2); - + checkModeToggle(); - saveCalibrationData(); - float zCM = 0.0; - float yCM = 0.0; - float xCM = 0.0; - // moveToHome(); - // Serial.println("Calibration complete"); - Serial.print("CurrentX "); - Serial.print(xCM); - Serial.print(" CurrentY "); - Serial.print(yCM); - Serial.print(" CurrentZ "); - Serial.print(zCM); - // getCurrentPositionCM(xCM, yCM, zCM); - // moveToCM(xCM / 2, yCM / 2, zCM / 2); + if (currentMode == MODE_JOYSTICK) { + // Joystick control mode + bool modeChanged = updateJoystickState(); + controlSteppers(modeChanged); + } + char c = readChar(); + processChar(c); + runSteppers(); } -void calibrateEndEffectorCarousal(){ - // Serial.println("Calibrating End Effector Carousal"); - // stepperEnd.setPosition(0); - stepperEnd.move(-1200); - while (digitalRead(E_HOME_PIN) == HIGH) { - stepperEnd.run(); - delay(15); +void runSteppers() { + loopTimer.check(bufferedOut); + stepperX.run(); + stepperY.run(); + //Different Z due to AccelStepper lib vs custom stepper for X & Y + if(abs(stepperZ.distanceToGo()) > 10){ + stepperZ.setSpeed((stepperZ.distanceToGo() > 0 ? 1 : -1) * (currentSpeedMode * 10)); + stepperZ.runSpeed(); + // Serial.print("Speed "); + // Serial.print(stepperZ.speed()); + // Serial.print(" CurSpeed "); + // Serial.print(currentSpeedMode); + // Serial.print(" Dist "); + // Serial.println(stepperZ.distanceToGo()); } - delay(500); - while (digitalRead(E_HOME_PIN) == LOW) { - stepperEnd.runSpeed(); - delay(15); - } - delay(2500); - // stepperEnd.move(-20); - // Serial.println(digitalRead(E_HOME_PIN)); - // Serial.println(abs(millis() - start)); - // while (stepperEnd.run()) { - // stepperEnd.run(); - // delay(15); - // } - // delay(2500); - // stepperEnd.move(15); - // while (stepperEnd.run()) { - // delay(15); - // } - // delay(2500); - // Serial.println(digitalRead(E_HOME_PIN)); - long position = stepperEnd.currentPosition(); - while (digitalRead(E_HOME_PIN) == HIGH) { - stepperEnd.move(-1 * CALIBRATION_STEP_SPEED); - stepperEnd.run(); - delay(15); - } - eMaxSteps = stepperEnd.currentPosition()- position; - Serial.print("End Max: "); Serial.println(eMaxSteps); - // stepperEnd.setPosition(0); - calculateStepsPerIndex(); - // stepperEnd.move(-1 * eMaxSteps); } -void calibrateXaxis(){ - Serial.println("Calibrating X axis"); - while (digitalRead(X_MIN_PIN) == HIGH) { + +// ============================================================================ +// JOYSTICK INPUT +// ============================================================================ + +int mapJoystickValue(int potValue) { + return map(potValue, 0, 1024, -100, 100); +} + +float mapJoystickToSpeed(float joystickValue) { + // Apply dead zone + if (abs(joystickValue) <= 6) { + return 0.0; + } + + // Determine direction and work with positive values + bool isNegative = joystickValue < 0; + float absValue = abs(joystickValue); + + // Normalize to 0.0 to 1.0 (excluding dead zone) + float normalized = (float)(absValue - 6) / (100 - 6); + + // Apply power curve (try values between 1.5 and 3.0) + float exponent = 2.0; + float curved = pow(normalized, exponent); + + // Apply direction and max speed + if (isNegative) { + return -curved * MAX_SPEED; + } else { + return curved * MAX_SPEED; + } +} + +bool updateJoystickState() { + bool locJoyPressed = !digitalRead(SW_PIN); + int locJoyX = mapJoystickValue(analogRead(X_PIN)); + int locJoyY = mapJoystickValue(analogRead(Y_PIN)); + + // Detect mode change (button press/release) + bool modeChanged = (locJoyPressed != joyPressed); + + // Update global state + joyX = locJoyX; + joyY = locJoyY; + joyPressed = locJoyPressed; + + return modeChanged; +} + +// ============================================================================ +// JOYSTICK -> STEPPER CONTROL +// ============================================================================ + +void controlSteppers(bool modeChanged) { + // Serial.print("Pressed: "); Serial.print(joyPressed); + // Serial.print(" joyY: "); Serial.print(joyY); + // Serial.print(" joyX: "); Serial.println(joyX); + delay(2.5); + // Handle mode transitions - stop the axes we're switching away from + if (modeChanged) { + if (joyPressed) { + // Switching to Z-mode: stop X/Y + stepperX.setSpeed(0); + stepperY.setSpeed(0); + stopX(); + stopY(); + } else { + // Switching to X/Y-mode: stop Z + stepperZ.setSpeed(0); + stopZ(); + } + } + + // Control the active axes + if (joyPressed) { + controlZAxisJoyStick(); + } else { + controlXAxisJoyStick(); + controlYAxisJoyStick(); + } +} + +void controlZAxisJoyStick() { + if (abs(joyY) > deadZone) { + float zSpeed = mapJoystickToSpeed((float)joyY); + stepperZ.setSpeed(zSpeed); + stepperZ.runSpeed(); + } else { + stopZ(); + } +} + +void controlXAxisJoyStick() { + if (abs(joyX) > deadZone) { + stepperX.setSpeed(mapJoystickToSpeed((float)joyX)); + Serial.println(mapJoystickToSpeed((float)joyX)); + stepperX.setDirection(joyX > 0 ? 1 : -1); stepperX.runSpeed(); - delay(1); - } - xMinSteps = stepperX.currentPosition(); - Serial.print("X Min: "); Serial.println(xMinSteps); - - - while (digitalRead(X_MAX_PIN) == HIGH) { - stepperX.setDirection(-1); - stepperX.runSpeed(); - } - stepperX.setDirection(1); - xMaxSteps = stepperX.currentPosition(); - Serial.print("X Max: "); Serial.println(xMaxSteps); -} -void calibrateYaxis(){ - Serial.println("Calibrating Y axis"); - - while (digitalRead(Y_MAX_PIN) == HIGH) { - stepperY1.runSpeed(); - stepperY2.runSpeed(); - } - yMinSteps = stepperY1.currentPosition(); - Serial.print("Y Min: "); Serial.println(yMinSteps); - - while (digitalRead(Y_MIN_PIN) == HIGH) { - stepperY1.setDirection(-1); - stepperY2.setDirection(-1); - stepperY1.runSpeed(); - stepperY2.runSpeed(); - } - stepperY1.setDirection(1); - stepperY2.setDirection(1); - yMaxSteps = stepperY1.currentPosition(); - Serial.print("Y Max: "); Serial.println(yMaxSteps); -} - -void calibrateZaxis(){ - Serial.println("Calibrating Z axis"); - - while (digitalRead(Z_MIN_PIN) == HIGH) { - stepperZ1.runSpeed(); - stepperZ2.runSpeed(); - } - zMinSteps = stepperZ1.currentPosition(); - Serial.print("Z Min: "); Serial.println(zMinSteps); - - while (digitalRead(Z_MAX_PIN) == HIGH) { - stepperZ1.setDirection(-1); - stepperZ2.setDirection(-1); - stepperZ1.runSpeed(); - stepperZ2.runSpeed(); - } - stepperZ1.setDirection(1); - stepperZ2.setDirection(1); - zMaxSteps = stepperZ1.currentPosition(); - Serial.print("Z Max: "); Serial.println(zMaxSteps); -} -void saveCalibrationData() { - EEPROM.put(EEPROM_X_MIN, xMinSteps); - EEPROM.put(EEPROM_X_MAX, xMaxSteps); - EEPROM.put(EEPROM_Y_MIN, yMinSteps); - EEPROM.put(EEPROM_Y_MAX, yMaxSteps); - EEPROM.put(EEPROM_Z_MIN, zMinSteps); - EEPROM.put(EEPROM_Z_MAX, zMaxSteps); - EEPROM.put(EEPROM_E_MAX, eMaxSteps); - EEPROM.write(EEPROM_CALIBRATED, 0xAA); - EEPROM.put(EEPROM_X_CUR, stepperX.currentPosition()); - EEPROM.put(EEPROM_Y_CUR, stepperY1.currentPosition()); - EEPROM.put(EEPROM_Z_CUR, stepperZ1.currentPosition()); - Serial.println("Calibration data saved to EEPROM"); -} - -void saveCurrentAxisStepperPositions(){ - EEPROM.put(EEPROM_X_CUR, stepperX.currentPosition()); - EEPROM.put(EEPROM_Y_CUR, stepperY1.currentPosition()); - EEPROM.put(EEPROM_Z_CUR, stepperZ1.currentPosition()); - Serial.println("Current Stepper Position data saved to EEPROM"); -} - -void loadCalibrationData() { - long xCur, yCur, zCur; - EEPROM.get(EEPROM_X_MIN, xMinSteps); - EEPROM.get(EEPROM_X_MAX, xMaxSteps); - EEPROM.get(EEPROM_Y_MIN, yMinSteps); - EEPROM.get(EEPROM_Y_MAX, yMaxSteps); - EEPROM.get(EEPROM_Z_MIN, zMinSteps); - EEPROM.get(EEPROM_Z_MAX, zMaxSteps); - EEPROM.get(EEPROM_E_MAX, eMaxSteps); - EEPROM.get(EEPROM_X_CUR, xCur); - EEPROM.get(EEPROM_Y_CUR, yCur); - EEPROM.get(EEPROM_Z_CUR, zCur); - stepperX.setPosition(xCur); - stepperY1.setPosition(yCur); - stepperY2.setPosition(yCur); - stepperZ1.setPosition(zCur); - stepperZ2.setPosition(zCur); -} - -void calculateStepsPerCM() { - // Motor specifications - const int STEPS_PER_ROTATION = 200; - - const float X_DRIVE_GEAR = 20.0; - const float X_DRIVEN_GEAR = 35.0; - const float X_GEAR_RATIO = X_DRIVEN_GEAR / X_DRIVE_GEAR; // 35/20 = 1.75 - - const float YZ_DRIVE_GEAR = 20.0; - const float YZ_DRIVEN_GEAR = 60.0; - const float YZ_GEAR_RATIO = YZ_DRIVEN_GEAR / YZ_DRIVE_GEAR; // 60/20 = 3.0 - - const float CM_PER_DRIVEN_ROTATION = 1.0 / THREAD_ROTATIONS_PER_CM; // 1cm per 5.5 rotations = ~0.1818 cm/rotation - - // Calculate steps per centimeter for each axis - - // X-axis: Motor steps -> drive gear -> driven gear -> linear motion - // Steps per driven gear rotation = motor steps per rotation * gear ratio - float xStepsPerDrivenRotation = STEPS_PER_ROTATION * X_GEAR_RATIO; - xStepsPerCM = xStepsPerDrivenRotation / CM_PER_DRIVEN_ROTATION; - - // Y and Z axis - float yzStepsPerDrivenRotation = STEPS_PER_ROTATION * YZ_GEAR_RATIO; - yStepsPerCM = yzStepsPerDrivenRotation / CM_PER_DRIVEN_ROTATION; - zStepsPerCM = yzStepsPerDrivenRotation / CM_PER_DRIVEN_ROTATION; - - Serial.println("Steps per CM calculated:"); - Serial.print("X-axis: "); - Serial.print(xStepsPerCM); - Serial.println(" steps/cm"); - Serial.print("Y-axis: "); - Serial.print(yStepsPerCM); - Serial.println(" steps/cm"); - Serial.print("Z-axis: "); - Serial.print(zStepsPerCM); - Serial.println(" steps/cm"); - - if (xMaxSteps != 0 || xMinSteps != 0) { - X_TRAVEL_CM = abs(xMaxSteps - xMinSteps) / xStepsPerCM; - Serial.print("X-axis total travel: "); - Serial.print(X_TRAVEL_CM); - Serial.println(" cm"); - } - - if (yMaxSteps != 0 || yMinSteps != 0) { - Y_TRAVEL_CM = abs(yMaxSteps - yMinSteps) / yStepsPerCM; - Serial.print("Y-axis total travel: "); - Serial.print(Y_TRAVEL_CM); - Serial.println(" cm"); - } - - if (zMaxSteps != 0 || zMinSteps != 0) { - Z_TRAVEL_CM = abs(zMaxSteps - zMinSteps) / zStepsPerCM; - Serial.print("Z-axis total travel: "); - Serial.print(Z_TRAVEL_CM); - Serial.println(" cm"); + } else { + stopX(); } } -void calculateStepsPerIndex() { - eStepsPerIndex = eMaxSteps / (EFFECTOR_COUNT); - //EFFECTOR_COUNT - Serial.println("Steps per Index calculated:"); - Serial.print("E: "); Serial.println(eStepsPerIndex); -} - -// Convert CM position to steps for each axis -long cmToStepsX(float cm) { - if (cm < 0) cm = 0; - if (cm > X_TRAVEL_CM) cm = X_TRAVEL_CM; - // Use the minimum step position as the 0 CM reference - return min(xMinSteps, xMaxSteps) + (long)(cm * xStepsPerCM); -} - -long cmToStepsY(float cm) { - if (cm < 0) cm = 0; - if (cm > Y_TRAVEL_CM) cm = Y_TRAVEL_CM; - return min(yMinSteps, yMaxSteps) + (long)(cm * yStepsPerCM); -} - -long cmToStepsZ(float cm) { - if (cm < 0) cm = 0; - if (cm > Z_TRAVEL_CM) cm = Z_TRAVEL_CM; - return min(zMinSteps, zMaxSteps) + (long)(cm * zStepsPerCM); -} - -// Convert steps to CM position for each axis -float stepsToCmX(long steps) { - return (float)abs(steps - min(xMinSteps, xMaxSteps)) / xStepsPerCM; -} - -float stepsToCmY(long steps) { - return (float)abs(steps - min(yMinSteps, yMaxSteps)) / yStepsPerCM; -} - -float stepsToCmZ(long steps) { - return (float)abs(steps - min(zMinSteps, zMaxSteps)) / zStepsPerCM; -} - - -void moveToXCM(float xCM){ - long xTarget = cmToStepsX(xCM); - - Serial.print("Moving to: X="); Serial.print(xCM); Serial.println("cm"); - - stepperX.moveTo(xTarget); - - while (stepperX.run()) { - stepperX.run(); +void controlYAxisJoyStick() { + if (abs(joyY) > deadZone) { + stepperY.setSpeed(mapJoystickToSpeed((float)joyY)); + stepperY.setDirection(joyY > 0 ? 1 : -1); + stepperY.runSpeed(); + } else { + stopY(); } - Serial.println("Move complete"); -} -void moveToYCM(float yCM) { - long yTarget = cmToStepsY(yCM); - - Serial.print("Moving to: Y="); Serial.print(yCM);Serial.println("cm"); - - stepperY1.moveTo(yTarget); - stepperY2.moveTo(yTarget); - - while (stepperY1.run() && - stepperY2.run()) { - stepperY1.run(); - stepperY2.run(); - } - Serial.println("Move complete"); -} -// Move to specific CM coordinates -void moveToZCM(float zCM) { - - long zTarget = cmToStepsZ(zCM); - - Serial.print("Moving to: Z="); Serial.print(zCM); Serial.println("cm"); - - stepperZ1.moveTo(zTarget); - stepperZ2.moveTo(zTarget); - - while (stepperZ1.run() && - stepperZ2.run()) { - stepperZ1.run(); - stepperZ2.run(); - } - Serial.println("Move complete"); } -// Move to specific CM coordinates -void moveToCM(float xCM, float yCM, float zCM) { +// ============================================================================ +// STOP FUNCTIONS +// ============================================================================ - long xTarget = cmToStepsX(xCM); - long yTarget = cmToStepsY(yCM); - long zTarget = cmToStepsZ(zCM); - - Serial.print("Moving to: X="); Serial.print(xCM); - Serial.print("cm, Y="); Serial.print(yCM); - Serial.print("cm, Z="); Serial.print(zCM); Serial.println("cm"); - - stepperX.moveTo(xTarget); - stepperY1.moveTo(yTarget); - stepperY2.moveTo(yTarget); - stepperZ1.moveTo(zTarget); - stepperZ2.moveTo(zTarget); - - while (stepperX.run()|| stepperY1.run() || - stepperY2.run() || stepperZ1.run() || - stepperZ2.run()) { - stepperX.run(); - stepperY1.run(); - stepperY2.run(); - stepperZ1.run(); - stepperZ2.run(); - } - Serial.println("Move complete"); +void stopX() { + stepperX.stop(); } -void moveToHome() { - moveToCM(0, 0, Z_TRAVEL_CM); - Serial.println("Moved to home position"); +void stopY() { + stepperY.stop(); } -// Get current position in CM -void getCurrentPositionCM(float &xCM, float &yCM, float &zCM) { - xCM = stepsToCmX(stepperX.currentPosition()); - yCM = stepsToCmY(stepperY1.currentPosition()); - zCM = stepsToCmZ(stepperZ1.currentPosition()); +void stopZ() { + stepperZ.setCurrentPosition(stepperZ.currentPosition()); + stepperZ.stop(); +} + +void stopAllSteppers() { + stepperX.stop(); + stepperY.stop(); + stepperZ.stop(); + Serial.println("STOPPED"); +} + +// ============================================================================ +// POSITION FUNCTIONS +// ============================================================================ + +void getCurrentPositionCM(float &x, float &y, float &z) { + x = stepperX.currentPosition(); + y = stepperY.currentPosition(); + z = stepperZ.currentPosition(); } -// Print current position void printCurrentPosition() { float x, y, z; getCurrentPositionCM(x, y, z); Serial.print("Current Position - X: "); Serial.print(x); - Serial.print("cm, Y: "); Serial.print(y); - Serial.print("cm, Z: "); Serial.print(z); Serial.println("cm"); + Serial.print(", Y: "); Serial.print(y); + Serial.print(", Z: "); Serial.print(z); + Serial.println(""); +} + +void zeroPosition() { + stepperX.setCurrentPosition(0); + stepperY.setCurrentPosition(0); + stepperZ.setCurrentPosition(0); + Serial.println("POSITION_ZEROED"); +} + +// ============================================================================ +// MOVEMENT FUNCTIONS +// ============================================================================ + +void moveDirection(int xDir, int yDir, int zDir) { + int moveDistance = getCurrentMoveDistance(); + + if (xDir != 0) { + stepperX.moveTo(stepperX.currentPosition() + (xDir * moveDistance)); + } + if (yDir != 0) { + stepperY.moveTo(stepperY.currentPosition() + (yDir * moveDistance)); + } + if (zDir != 0) { + stepperZ.moveTo(stepperZ.currentPosition() + (zDir * moveDistance)); + } } -void connectElectromagnet() { - digitalWrite(ELECTROMAGNET_RELAY_PIN, HIGH); - Serial.println("Electromagnet ON - End effector connected"); +int getCurrentMoveDistance() { + return currentMoveDistance; } -void disconnectElectromagnet() { - digitalWrite(ELECTROMAGNET_RELAY_PIN, LOW); - Serial.println("Electromagnet OFF - End effector disconnected"); +void controlZAxis(bool dir) { + // stepperZ.setSpeed((dir == 1 ? 1 : -1) * 500); + stepperZ.setCurrentPosition(0); + stepperZ.moveTo(((dir == 1 ? 1 : -1) * 100000)); + // stepperZ.runSpeed(); +} + +void controlXAxis(bool dir) { + stepperX.setSpeed(currentSpeedMode); + stepperX.setDirection(dir == 1 ? 1 : -1); + stepperX.runSpeed(); +} + +void controlYAxis(bool dir) { + stepperY.setSpeed(currentSpeedMode); + stepperY.setDirection(dir == 1 ? 1 : -1); + stepperY.runSpeed(); } -bool isElectromagnetConnected() { - return digitalRead(ELECTROMAGNET_RELAY_PIN) == HIGH; +// ============================================================================ +// HOMING FUNCTIONS +// ============================================================================ + +// ============================================================================ +// SPEED CONTROL +// ============================================================================ + +void setSpeedMode(int speed) { + currentSpeedMode = speed; + stepperX.setMaxSpeed(speed); + stepperY.setMaxSpeed(speed); + + if (speed == SLOW_SPEED) { + currentMoveDistance = 50; + Serial.println("SPEED_SLOW"); + } else if (speed == MEDIUM_SPEED) { + currentMoveDistance = 100; + Serial.println("SPEED_MEDIUM"); + } else if (speed == FAST_SPEED) { + currentMoveDistance = 200; + Serial.println("SPEED_FAST"); + } } +// ============================================================================ +// STATUS & DIAGNOSTICS +// ============================================================================ -void attachAtPosition(float xCM, float yCM, float zCM) { - Serial.println("Starting attachment sequence"); - - - disconnectElectromagnet(); - moveToCM(xCM, yCM, zCM); - - delay(500); - - connectElectromagnet(); - unlatch(); - - Serial.println("Attachment sequence complete"); +void reportStatus() { + Serial.print("STATUS:"); + Serial.print("X:"); Serial.print(stepperX.currentPosition()); + Serial.print(",Y:"); Serial.print(stepperY.currentPosition()); + Serial.print(",Z:"); Serial.print(stepperZ.currentPosition()); + Serial.print(",RUNNING:"); + Serial.print(stepperX.isRunning() || stepperY.isRunning() || stepperZ.isRunning()); + Serial.println(); + Serial.print("Z Min: "); + Serial.print(digitalRead(Z_MIN_PIN)); + Serial.print(" Z Max: "); + Serial.print(digitalRead(Z_MAX_PIN)); + Serial.println(); } -void detachAndMove(float xCM, float yCM, float zCM) { - Serial.println("Starting detachment sequence"); - - disconnectElectromagnet(); - latch(); - delay(200); - - moveToCM(xCM, yCM, zCM); - - Serial.println("Detachment sequence complete"); +// ============================================================================ +// CALIBRATION & INITIALIZATION +// ============================================================================ + +void emergencyStop() { + stepperX.stop(); + stepperY.stop(); + stepperZ.stop(); + stepperX.disableOutputs(); + stepperY.disableOutputs(); + stepperZ.disableOutputs(); + Serial.println("EMERGENCY_STOP"); } -void moveAndDetach(float xCM, float yCM, float zCM) { - Serial.println("Starting detachment sequence"); - - moveToCM(xCM, yCM, zCM); - delay(200); - disconnectElectromagnet(); - latch(); - - Serial.println("Detachment sequence complete"); +void stop() { + stepperX.stop(); + stepperY.stop(); + stepperZ.stop(); + Serial.println("STOP"); } +// ============================================================================ +// SERIAL COMMAND INTERFACE +// ============================================================================ - -void unlatch() -{ - servo.write(UNLATCH_ANGLE); - delay(500); +char readChar() { + char c = 0; + if (Serial.available()) { + c = Serial.read(); + while (Serial.available()) { + Serial.read(); + } + } + return c; } -void latch() -{ - servo.write(LATCH_ANGLE); - delay(500); -} -// cur 5 goal 2 -// - - -void moveEffectorIndex(short index) { - Serial.println("moveEffectorIndex"); - if(index < 0 || index >= EFFECTOR_COUNT || index == EFFECTOR_INDEX){ +void processChar(char c) { + if (c == 0) { return; } - // long position = stepperEnd.currentPosition() % eMaxSteps; - int indexSteps = eStepsPerIndex; - int diff = 0; - if(index > EFFECTOR_INDEX){ - diff = abs(index - EFFECTOR_INDEX) * indexSteps; - }else { - // Serial.print("EFFECTOR_INDEX: "); - // Serial.print(EFFECTOR_INDEX); - // Serial.print(", EFFECTOR_COUNT: "); - // Serial.print(EFFECTOR_COUNT); - // Serial.print(", EFFECTOR_INDEX: "); - // Serial.print(EFFECTOR_INDEX); - // Serial.print(", index: "); - // Serial.print(index); - // Serial.print(", res: "); - // Serial.println(((EFFECTOR_COUNT - (EFFECTOR_INDEX)) + index)); - diff = ((EFFECTOR_COUNT - (EFFECTOR_INDEX)) + index) * indexSteps; + + // Movement commands + if (c == 'N') { + controlYAxis(1); // Move North (Y+) + } else if (c == 'S') { + controlYAxis(0); // Move South (Y-) + } else if (c == 'E') { + controlXAxis(1); // Move East (X+) + } else if (c == 'W') { + controlXAxis(0); // Move West (X-) + } else if (c == 'U') { + controlZAxis(1); // Move Up (Z+) + } else if (c == 'D') { + controlZAxis(0); // Move Down (Z-) + } else if (c == 'n') { + stopY(); + } else if (c == 's') { + stopY(); + } else if (c == 'e') { + stopX(); + } else if (c == 'w') { + stopX(); + } else if (c == 'u') { + stopZ(); + } else if (c == 'd') { + stopZ(); + } else if (c == 'X') { + stopAllSteppers(); + + // Position commands + } else if (c == 'P') { + printCurrentPosition(); + } else if (c == 'Z') { + zeroPosition(); + + // Speed control + } else if (c == 'F') { + setSpeedMode(FAST_SPEED); + } else if (c == 'M') { + setSpeedMode(MEDIUM_SPEED); + } else if (c == 'L') { + setSpeedMode(SLOW_SPEED); + + } else if (c == '1') { + setSpeedMode(speedArr[0]); + } else if (c == '2') { + setSpeedMode(speedArr[1]); + } else if (c == '3') { + setSpeedMode(speedArr[2]); + } else if (c == '4') { + setSpeedMode(speedArr[3]); + } else if (c == '5') { + setSpeedMode(speedArr[4]); + } else if (c == '6') { + setSpeedMode(speedArr[5]); + } else if (c == '0') { + setSpeedMode(0); + + // Scanning commands + } else if (c == 'G') { + // goToOrigin(); + } else if (c == 'R') { + reportStatus(); + + // Emergency + } else if (c == '@') { + emergencyStop(); + + } else if (c == '?') { + reportStatus(); + }else if(c == '!') { + stop(); + // Ignore whitespace + } else if ((c == '\n') || (c == '\r')) { + // skip + } else if (c == 'T') { + toggleMode(); + } else { + Serial.print("Invalid cmd:"); + Serial.println(c); + } +} + +// ============================================================================ +// MODE TOGGLE (Triple-click detection) +// ============================================================================ + +void checkModeToggle() { + bool currentPressed = !digitalRead(SW_PIN); + + // Detect rising edge (button just pressed) + if (currentPressed && !lastJoyPressed) { + unsigned long now = millis(); + + // Record this click + clickTimes[clickIndex] = now; + clickIndex = (clickIndex + 1) % CLICKS_REQUIRED; + + // Check if we have 3 clicks within the window + int oldestIndex = clickIndex; // After increment, this points to oldest + unsigned long oldestClick = clickTimes[oldestIndex]; + + // Only check if we've had at least CLICKS_REQUIRED clicks + // and the oldest one is within our window + if (oldestClick > 0 && (now - oldestClick) <= CLICK_WINDOW_MS) { + toggleMode(); + // Reset click tracking + for (int i = 0; i < CLICKS_REQUIRED; i++) { + clickTimes[i] = 0; + } + clickIndex = 0; + } + } + + lastJoyPressed = currentPressed; +} + +void toggleMode() { + if (currentMode == MODE_SERIAL) { + currentMode = MODE_JOYSTICK; + stopAllSteppers(); + Serial.println("MODE:JOYSTICK"); + } else { + currentMode = MODE_SERIAL; + stopAllSteppers(); + Serial.println("MODE:SERIAL"); } - Serial.println(diff); - stepperEnd.move(diff); - EFFECTOR_INDEX = index; - while (stepperEnd.run()) { - delay(15); - } - delay(500); } diff --git a/sketch/sketch/sketch.ino b/sketch/sketch/sketch.ino new file mode 100644 index 0000000..6440c77 --- /dev/null +++ b/sketch/sketch/sketch.ino @@ -0,0 +1,30 @@ +#include + +// Define step constant +#define MotorInterfaceType 4 + +#define Z_STEP_PIN 23 +#define Z_DIR_PIN 25 +#define Z_STEP_PIN1 27 +#define Z_STEP_PIN2 29 +// Creates an instance +// Pins entered in sequence IN1-IN3-IN2-IN4 for proper step sequence +AccelStepper myStepper(MotorInterfaceType, Z_STEP_PIN, Z_STEP_PIN1, Z_DIR_PIN, Z_STEP_PIN2); +const int MAX_SPEED = 4000; +const int SPEED = 1000; +void setup() { + // set the maximum speed, acceleration factor, + // initial speed and the target position + myStepper.setMaxSpeed(MAX_SPEED); + myStepper.setAcceleration(200.0); + myStepper.setSpeed(SPEED); + myStepper.moveTo(2048); +} + +void loop() { + +// if (myStepper.distanceToGo() == 0) +// myStepper.moveTo(-myStepper.currentPosition()); + // Move the motor one step + myStepper.runSpeed(); +}