Better Control Code
This commit is contained in:
parent
d14c800b73
commit
d92c17de61
5 changed files with 1047 additions and 1119 deletions
|
|
@ -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)
|
||||
{
|
||||
|
|
|
|||
508
sketch/old/sketch.ino
Normal file
508
sketch/old/sketch.ino
Normal file
|
|
@ -0,0 +1,508 @@
|
|||
#include <AccelStepper.h>
|
||||
#include <SafeString.h>
|
||||
#include <Servo.h>
|
||||
#include <loopTimer.h>
|
||||
#include <BufferedOutput.h>
|
||||
#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");
|
||||
}
|
||||
|
||||
|
|
@ -1,509 +0,0 @@
|
|||
// #include <SafeString.h>
|
||||
// #include <EEPROM.h>
|
||||
// #include <BufferedOutput.h>
|
||||
// #include <Servo.h>
|
||||
// #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);
|
||||
// }
|
||||
1117
sketch/sketch.ino
1117
sketch/sketch.ino
File diff suppressed because it is too large
Load diff
30
sketch/sketch/sketch.ino
Normal file
30
sketch/sketch/sketch.ino
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
#include <AccelStepper.h>
|
||||
|
||||
// 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();
|
||||
}
|
||||
Loading…
Reference in a new issue