150 lines
3.4 KiB
C++
150 lines
3.4 KiB
C++
#include "CoreXY.hpp"
|
|
|
|
#include "FreeRTOS.h"
|
|
#include "task.h"
|
|
|
|
void homeXY(TMC2209_step_dual * driver, HOME_AXIS_DIRECTION direction, unsigned int speed, unsigned int backupSpace, MotorPosition * last_pos) {
|
|
|
|
uint8_t x_dir = direction & 0b01;
|
|
uint8_t y_dir = (direction & 0b10) >> 1;
|
|
|
|
uint8_t x_dir_inv = x_dir ^ 1;
|
|
uint8_t y_dir_inv = y_dir ^ 1;
|
|
|
|
uint8_t x_inv_limit = x_dir_inv == X_DIRECTION_BOTTOM ? LIMIT_X0 : LIMIT_X1;
|
|
uint8_t y_inv_limit = y_dir_inv == Y_DIRECTION_LEFT ? LIMIT_Y0 : LIMIT_Y1;
|
|
bool backup_x = !gpio_get(x_inv_limit) && (backupSpace > 0);
|
|
bool backup_y = !gpio_get(y_inv_limit) && (backupSpace > 0);
|
|
|
|
// Start Backup if necessary
|
|
if(backup_x) {
|
|
if(MOTOR_X) {
|
|
driver->set_conf1(speed, x_dir_inv);
|
|
driver->pulse1(backupSpace);
|
|
} else {
|
|
driver->set_conf0(speed, y_dir_inv);
|
|
driver->pulse0(backupSpace);
|
|
}
|
|
}
|
|
|
|
if(backup_y) {
|
|
if(MOTOR_Y) {
|
|
driver->set_conf1(speed, y_dir_inv);
|
|
driver->pulse1(backupSpace);
|
|
} else {
|
|
driver->set_conf0(speed, y_dir_inv);
|
|
driver->pulse0(backupSpace);
|
|
}
|
|
}
|
|
|
|
|
|
// Wait for end of Backup
|
|
if(backup_x) {
|
|
if(MOTOR_X) {
|
|
driver->wait1(0);
|
|
} else {
|
|
driver->wait0(0);
|
|
}
|
|
}
|
|
|
|
if(backup_y) {
|
|
if(MOTOR_Y) {
|
|
driver->wait1(0);
|
|
} else {
|
|
driver->wait0(0);
|
|
}
|
|
}
|
|
|
|
vTaskDelay(100 / portTICK_PERIOD_MS);
|
|
|
|
// Start Motors
|
|
const unsigned int total_steps = 25000;
|
|
// Set Configuration
|
|
if(MOTOR_X) {
|
|
driver->set_conf1(speed, x_dir);
|
|
} else {
|
|
driver->set_conf0(speed, x_dir);
|
|
}
|
|
|
|
if(MOTOR_Y) {
|
|
driver->set_conf1(speed, y_dir);
|
|
} else {
|
|
driver->set_conf0(speed, y_dir);
|
|
}
|
|
|
|
// Start Motors
|
|
if(MOTOR_X) {
|
|
driver->pulse1(total_steps);
|
|
} else {
|
|
driver->pulse0(total_steps);
|
|
}
|
|
|
|
if(MOTOR_Y) {
|
|
driver->pulse1(total_steps);
|
|
} else {
|
|
driver->pulse0(total_steps);
|
|
}
|
|
|
|
bool x_done = false;
|
|
bool y_done = false;
|
|
|
|
volatile int last_x = -1;
|
|
volatile int last_y = -1;
|
|
|
|
for(int i=0; i<5000; i++) {
|
|
if(gpio_get(LIMIT_X0) || gpio_get(LIMIT_X1)) {
|
|
x_done = true;
|
|
if(MOTOR_X) {
|
|
driver->pulse1(0);
|
|
} else {
|
|
driver->pulse0(0);
|
|
}
|
|
}
|
|
|
|
if(gpio_get(LIMIT_Y0) || gpio_get(LIMIT_Y1)) {
|
|
y_done = true;
|
|
if(MOTOR_Y) {
|
|
driver->pulse1(0);
|
|
} else {
|
|
driver->pulse0(0);
|
|
}
|
|
|
|
}
|
|
|
|
if(x_done && y_done) {
|
|
break;
|
|
}
|
|
|
|
vTaskDelay(1 / portTICK_PERIOD_MS);
|
|
}
|
|
|
|
// Emergency Stop
|
|
if(!(x_done && y_done)) {
|
|
driver->pulse0(0);
|
|
driver->pulse1(0);
|
|
}
|
|
|
|
driver->wait0(0);
|
|
driver->wait1(0);
|
|
|
|
if(MOTOR_X) {
|
|
if(last_pos != NULL)
|
|
last_pos->x = driver->get_remaining_steps1();
|
|
} else {
|
|
if(last_pos != NULL)
|
|
last_pos->x = driver->get_remaining_steps0();
|
|
}
|
|
|
|
if(MOTOR_Y) {
|
|
last_pos->y = driver->get_remaining_steps1();
|
|
} else {
|
|
last_pos->y = driver->get_remaining_steps0();
|
|
}
|
|
|
|
if(last_pos != NULL)
|
|
last_pos->x = total_steps - last_pos->x;
|
|
|
|
if(last_pos != NULL)
|
|
last_pos->y = total_steps - last_pos->y;
|
|
}
|