508 lines
13 KiB
C++
508 lines
13 KiB
C++
#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");
|
|
}
|
|
|