commiting codebase
This commit is contained in:
commit
1e8f70f7a9
2 changed files with 876 additions and 0 deletions
181
sketch/Stepper.cpp
Normal file
181
sketch/Stepper.cpp
Normal file
|
|
@ -0,0 +1,181 @@
|
|||
#import <Arduino.h>
|
||||
#define DIRECTION_CW 1
|
||||
#define DIRECTION_CCW 0
|
||||
|
||||
|
||||
class Stepper {
|
||||
private:
|
||||
short enablePin = 8;
|
||||
long currentPos = 0;
|
||||
long targetPos = 0;
|
||||
float speed = 0;
|
||||
float maxSpeed = 1000;
|
||||
float acceleration = 0;
|
||||
short stepPin = 0;
|
||||
short dirPin = 0;
|
||||
long n;
|
||||
float c0;
|
||||
float cn;
|
||||
float cmin;
|
||||
unsigned long stepInterval;
|
||||
unsigned long lastStepTime;
|
||||
unsigned long minPulseWidth = 1;
|
||||
bool direction = DIRECTION_CW; // 1 == CW
|
||||
|
||||
public:
|
||||
Stepper(short sP, short dP) {
|
||||
stepPin = sP;
|
||||
dirPin = dP;
|
||||
}
|
||||
Stepper(short sP, short dP, short en) {
|
||||
stepPin = sP;
|
||||
dirPin = dP;
|
||||
enablePin = en;
|
||||
}
|
||||
void setSpeed(float sp){
|
||||
if(sp > maxSpeed){
|
||||
speed = maxSpeed;
|
||||
return;
|
||||
}
|
||||
speed = sp;
|
||||
return;
|
||||
}
|
||||
void setMaxSpeed(float sp){
|
||||
maxSpeed = sp;
|
||||
return;
|
||||
}
|
||||
|
||||
float getSpeed(){
|
||||
return speed;
|
||||
}
|
||||
float getMaxSpeed(){
|
||||
return maxSpeed;
|
||||
}
|
||||
void init(){
|
||||
pinMode(enablePin, OUTPUT);
|
||||
pinMode(stepPin, OUTPUT);
|
||||
pinMode(dirPin, OUTPUT);
|
||||
enableOutputs();
|
||||
// digitalWrite(enablePin, LOW);
|
||||
}
|
||||
void disableOutputs(){
|
||||
digitalWrite(enablePin, HIGH);
|
||||
}
|
||||
void enableOutputs(){
|
||||
digitalWrite(enablePin, LOW);
|
||||
}
|
||||
long distanceToGo()
|
||||
{
|
||||
return targetPos - currentPos;
|
||||
}
|
||||
|
||||
long targetPosition()
|
||||
{
|
||||
return targetPos;
|
||||
}
|
||||
|
||||
long currentPosition()
|
||||
{
|
||||
return currentPos;
|
||||
}
|
||||
void blockingRunToTarget(){
|
||||
while(abs(distanceToGo()) > 0){
|
||||
run();
|
||||
}
|
||||
}
|
||||
void setCurrentPosition(long position)
|
||||
{
|
||||
targetPos = currentPos = position;
|
||||
n = 0;
|
||||
stepInterval = 0;
|
||||
speed = 0.0;
|
||||
}
|
||||
|
||||
void stop()
|
||||
{
|
||||
targetPos = currentPos;
|
||||
n = 0;
|
||||
speed = 0.0;
|
||||
// move(0);
|
||||
}
|
||||
|
||||
bool isRunning()
|
||||
{
|
||||
return !(speed == 0.0 && targetPos == currentPos);
|
||||
}
|
||||
void moveTo(long absolute)
|
||||
{
|
||||
if (targetPos != absolute)
|
||||
{
|
||||
targetPos = absolute;
|
||||
stepInterval = fabs(1000000.0 / speed);
|
||||
}
|
||||
}
|
||||
|
||||
void move(long relative)
|
||||
{
|
||||
if(0 < relative){
|
||||
direction = DIRECTION_CW;
|
||||
}else {
|
||||
direction = DIRECTION_CCW;
|
||||
}
|
||||
moveTo(currentPos + relative);
|
||||
}
|
||||
|
||||
void setDirection(int dir){
|
||||
if(0 < dir){
|
||||
direction = DIRECTION_CW;
|
||||
}else {
|
||||
direction = DIRECTION_CCW;
|
||||
}
|
||||
}
|
||||
|
||||
bool runSpeed()
|
||||
{
|
||||
// Dont do anything unless we actually have a step interval
|
||||
if (!speed)
|
||||
return false;
|
||||
stepInterval = fabs(1000000.0 / speed);
|
||||
|
||||
unsigned long time = micros();
|
||||
if (time - lastStepTime >= stepInterval)
|
||||
{
|
||||
if (direction == DIRECTION_CW)
|
||||
{
|
||||
// Clockwise
|
||||
currentPos += 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Anticlockwise
|
||||
currentPos -= 1;
|
||||
}
|
||||
step();
|
||||
|
||||
lastStepTime = time; // Caution: does not account for costs in step()
|
||||
|
||||
return true;
|
||||
}else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool run()
|
||||
{
|
||||
runSpeed();
|
||||
return speed != 0.0 || distanceToGo() != 0;
|
||||
}
|
||||
|
||||
void step()
|
||||
{
|
||||
if(direction == DIRECTION_CW)
|
||||
digitalWrite(dirPin, HIGH);
|
||||
else
|
||||
digitalWrite(dirPin, LOW);
|
||||
digitalWrite(stepPin,HIGH);
|
||||
delayMicroseconds(400);
|
||||
digitalWrite(stepPin,LOW);
|
||||
delayMicroseconds(400);
|
||||
}
|
||||
};
|
||||
695
sketch/sketch.ino
Normal file
695
sketch/sketch.ino
Normal file
|
|
@ -0,0 +1,695 @@
|
|||
#include <AccelStepper.h>
|
||||
#include <SafeString.h>
|
||||
#include <EEPROM.h>
|
||||
#include <BufferedOutput.h>
|
||||
#include <Servo.h>
|
||||
#include "Stepper.cpp"
|
||||
|
||||
#define MotorInterfaceType 4
|
||||
|
||||
|
||||
|
||||
#define Z_STEP_PIN 4
|
||||
#define Z_DIR_PIN 7
|
||||
|
||||
#define Z2_STEP_PIN 2
|
||||
#define Z2_DIR_PIN 5
|
||||
|
||||
#define Y_STEP_PIN 3
|
||||
#define Y_DIR_PIN 6
|
||||
|
||||
#define Y2_STEP_PIN 12
|
||||
#define Y2_DIR_PIN 13
|
||||
|
||||
|
||||
|
||||
#define X_STEP_PIN 52
|
||||
#define X_DIR_PIN 53
|
||||
|
||||
|
||||
|
||||
#define END_STEP_PIN 27
|
||||
#define END_DIR_PIN 29
|
||||
// #define END_STEP_PIN1 24
|
||||
// #define END_STEP_PIN2 22
|
||||
|
||||
|
||||
#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
|
||||
|
||||
//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;
|
||||
|
||||
const short STEPS_PER_ROT = 200;
|
||||
|
||||
// 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
|
||||
|
||||
|
||||
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);
|
||||
int initSpeed = 600;
|
||||
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);
|
||||
|
||||
stepperEnd.init();
|
||||
stepperEnd.setMaxSpeed(initSpeed);
|
||||
stepperEnd.setSpeed(initSpeed);
|
||||
stepperEnd.setDirection(-1);
|
||||
// stepperEnd.move(-1200);
|
||||
delay(5000);
|
||||
|
||||
//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.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);
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
void calibrateEndEffectorCarousal(){
|
||||
// Serial.println("Calibrating End Effector Carousal");
|
||||
// stepperEnd.setPosition(0);
|
||||
stepperEnd.move(-1200);
|
||||
while (digitalRead(E_HOME_PIN) == HIGH) {
|
||||
stepperEnd.run();
|
||||
delay(15);
|
||||
}
|
||||
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) {
|
||||
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");
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
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) {
|
||||
|
||||
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 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);
|
||||
}
|
||||
// cur 5 goal 2
|
||||
//
|
||||
|
||||
|
||||
void moveEffectorIndex(short index) {
|
||||
Serial.println("moveEffectorIndex");
|
||||
if(index < 0 || index >= EFFECTOR_COUNT || index == EFFECTOR_INDEX){
|
||||
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;
|
||||
}
|
||||
Serial.println(diff);
|
||||
stepperEnd.move(diff);
|
||||
EFFECTOR_INDEX = index;
|
||||
while (stepperEnd.run()) {
|
||||
delay(15);
|
||||
}
|
||||
delay(500);
|
||||
}
|
||||
Loading…
Reference in a new issue