3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #551 from XPila/MK3-new_xyz_cal

Browse Source 
New XYZ calibration with image processing
This commit is contained in:
PavelSindler 2018-03-13 16:17:50 +01:00 committed by GitHub
commit 0c97f46f25
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 1018 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Firmware/Configuration_prusa.h
View File

@ -95,6 +95,12 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
// Automatic recovery after crash is detected
#define AUTOMATIC_RECOVERY_AFTER_CRASH
// New XYZ calibration
#define NEW_XYZCAL
// Watchdog support
#define WATCHDOG
// Disable some commands
#define _DISABLE_M42_M226

17
Firmware/Marlin_main.cpp
View File

@ -2286,6 +2286,7 @@ bool gcode_M45(bool onlyZ)
current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[Z_AXIS] / 40, active_extruder);
st_synchronize();
#ifndef NEW_XYZCAL
if (result >= 0)
{
point_too_far_mask = 0;
@ -2304,6 +2305,8 @@ bool gcode_M45(bool onlyZ)
st_synchronize();
// if (result >= 0) babystep_apply();
}
#endif //NEW_XYZCAL
lcd_bed_calibration_show_result(result, point_too_far_mask);
if (result >= 0)
{
@ -3276,17 +3279,17 @@ void process_commands()
enquecommand_front_P((PSTR("G28 W0")));
break;
}
lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CAL_WARNING);
bool result = lcd_show_fullscreen_message_yes_no_and_wait_P(MSG_STEEL_SHEET_CHECK, false, false);
if (result)
{
lcd_show_fullscreen_message_and_wait_P(MSG_TEMP_CAL_WARNING);
bool result = lcd_show_fullscreen_message_yes_no_and_wait_P(MSG_STEEL_SHEET_CHECK, false, false);
if (result)
{
current_position[Z_AXIS] = 50;
current_position[Y_AXIS] = 190;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 3000 / 60, active_extruder);
st_synchronize();
lcd_show_fullscreen_message_and_wait_P(MSG_REMOVE_STEEL_SHEET);
}
lcd_update_enable(true);
lcd_show_fullscreen_message_and_wait_P(MSG_REMOVE_STEEL_SHEET);
}
lcd_update_enable(true);
KEEPALIVE_STATE(NOT_BUSY); //no need to print busy messages as we print current temperatures periodicaly
SERIAL_ECHOLNPGM("PINDA probe calibration start");

3
Firmware/config.h
View File

@ -8,5 +8,8 @@
#define ADC_OVRSAMPL 16 //oversampling multiplier
#define ADC_CALLBACK adc_ready //callback function ()
//SM4 configuration
#define SM4_DEFDELAY 500 //default step delay [us]
#endif //_CONFIG_H

25
Firmware/mesh_bed_calibration.cpp
View File

@ -898,6 +898,9 @@ error:
return false;
}
#ifdef NEW_XYZCAL
extern bool xyzcal_find_bed_induction_sensor_point_xy();
#endif //NEW_XYZCAL
// Search around the current_position[X,Y],
// look for the induction sensor response.
// Adjust the current_position[X,Y,Z] to the center of the target dot and its response Z coordinate.
@ -905,8 +908,11 @@ error:
#define FIND_BED_INDUCTION_SENSOR_POINT_Y_RADIUS (6.f)
#define FIND_BED_INDUCTION_SENSOR_POINT_XY_STEP (1.f)
#define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f)
inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
/*inline */bool find_bed_induction_sensor_point_xy(int verbosity_level)
{
#ifdef NEW_XYZCAL
return xyzcal_find_bed_induction_sensor_point_xy();
#else //NEW_XYZCAL
#ifdef SUPPORT_VERBOSITY
if(verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
#endif // SUPPORT_VERBOSITY
@ -1099,8 +1105,10 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
enable_z_endstop(false);
return found;
#endif //NEW_XYZCAL
}
#ifndef NEW_XYZCAL
// Search around the current_position[X,Y,Z].
// It is expected, that the induction sensor is switched on at the current position.
// Look around this center point by painting a star around the point.
@ -1190,7 +1198,9 @@ inline bool improve_bed_induction_sensor_point()
enable_z_endstop(endstop_z_enabled);
return found;
}
#endif //NEW_XYZCAL
#ifndef NEW_XYZCAL
static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
{
SERIAL_ECHOPGM("Measured ");
@ -1203,7 +1213,9 @@ static inline void debug_output_point(const char *type, const float &x, const fl
MYSERIAL.print(z, 5);
SERIAL_ECHOLNPGM("");
}
#endif //NEW_XYZCAL
#ifndef NEW_XYZCAL
// Search around the current_position[X,Y,Z].
// It is expected, that the induction sensor is switched on at the current position.
// Look around this center point by painting a star around the point.
@ -1363,7 +1375,9 @@ canceled:
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
return false;
}
#endif //NEW_XYZCAL
#ifndef NEW_XYZCAL
// Searching the front points, where one cannot move the sensor head in front of the sensor point.
// Searching in a zig-zag movement in a plane for the maximum width of the response.
// This function may set the current_position[Y_AXIS] below Y_MIN_POS, if the function succeeded.
@ -1684,7 +1698,9 @@ canceled:
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
return false;
}
#endif //NEW_XYZCAL
#ifndef NEW_XYZCAL
// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
// write the trigger coordinates to the serial line.
// Useful for visualizing the behavior of the bed induction detector.
@ -1729,6 +1745,7 @@ inline void scan_bed_induction_sensor_point()
current_position[Y_AXIS] = center_old_y;
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
}
#endif //NEW_XYZCAL
#define MESH_BED_CALIBRATION_SHOW_LCD
@ -1855,7 +1872,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
#endif // SUPPORT_VERBOSITY
if (!find_bed_induction_sensor_point_xy(verbosity_level))
return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND;
#if 1
#ifndef NEW_XYZCAL
if (k == 0 || k == 1) {
// Improve the position of the 1st row sensor points by a zig-zag movement.
@ -2017,6 +2034,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level
return result;
}
#ifndef NEW_XYZCAL
BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask)
{
// Don't let the manage_inactivity() function remove power from the motors.
@ -2322,6 +2340,7 @@ canceled:
enable_z_endstop(endstop_z_enabled);
return result;
}
#endif //NEW_XYZCAL
void go_home_with_z_lift()
{
@ -2462,6 +2481,7 @@ bool sample_mesh_and_store_reference()
return true;
}
#ifndef NEW_XYZCAL
bool scan_bed_induction_points(int8_t verbosity_level)
{
// Don't let the manage_inactivity() function remove power from the motors.
@ -2523,6 +2543,7 @@ bool scan_bed_induction_points(int8_t verbosity_level)
enable_z_endstop(endstop_z_enabled);
return true;
}
#endif //NEW_XYZCAL
// Shift a Z axis by a given delta.
// To replace loading of the babystep correction.

2
Firmware/mesh_bed_calibration.h
View File

@ -161,7 +161,9 @@ enum BedSkewOffsetDetectionResultType {
};
extern BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level, uint8_t &too_far_mask);
#ifndef NEW_XYZCAL
extern BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8_t verbosity_level, uint8_t &too_far_mask);
#endif //NEW_XYZCAL
extern bool sample_mesh_and_store_reference();

194
Firmware/sm4.c Normal file
View File

@ -0,0 +1,194 @@
//sm4.c - simple 4-axis stepper control
#include "sm4.h"
#include <avr/io.h>
#include <avr/pgmspace.h>
#include "boards.h"
#define bool int8_t
#define false 0
#define true 1
#include "Configuration_prusa.h"
#ifdef NEW_XYZCAL
// Signal pinouts
// direction signal - MiniRambo
//#define X_DIR_PIN 48 //PL1 (-)
//#define Y_DIR_PIN 49 //PL0 (-)
//#define Z_DIR_PIN 47 //PL2 (-)
//#define E0_DIR_PIN 43 //PL6 (+)
//direction signal - EinsyRambo
//#define X_DIR_PIN 49 //PL0 (+)
//#define Y_DIR_PIN 48 //PL1 (-)
//#define Z_DIR_PIN 47 //PL2 (+)
//#define E0_DIR_PIN 43 //PL6 (-)
//step signal pinout - common for all rambo boards
//#define X_STEP_PIN 37 //PC0 (+)
//#define Y_STEP_PIN 36 //PC1 (+)
//#define Z_STEP_PIN 35 //PC2 (+)
//#define E0_STEP_PIN 34 //PC3 (+)
sm4_stop_cb_t sm4_stop_cb = 0;
sm4_update_pos_cb_t sm4_update_pos_cb = 0;
sm4_calc_delay_cb_t sm4_calc_delay_cb = 0;
uint16_t sm4_cpu_time = 0;
uint8_t sm4_get_dir(uint8_t axis)
{
switch (axis)
{
#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
case 0: return (PORTL & 2)?0:1;
case 1: return (PORTL & 1)?0:1;
case 2: return (PORTL & 4)?0:1;
case 3: return (PORTL & 64)?1:0;
#else if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304))
case 0: return (PORTL & 1)?1:0;
case 1: return (PORTL & 2)?0:1;
case 2: return (PORTL & 4)?1:0;
case 3: return (PORTL & 64)?0:1;
#endif
}
return 0;
}
void sm4_set_dir(uint8_t axis, uint8_t dir)
{
switch (axis)
{
#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
case 0: if (!dir) PORTL |= 2; else PORTL &= ~2; break;
case 1: if (!dir) PORTL |= 1; else PORTL &= ~1; break;
case 2: if (!dir) PORTL |= 4; else PORTL &= ~4; break;
case 3: if (dir) PORTL |= 64; else PORTL &= ~64; break;
#else if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304))
case 0: if (dir) PORTL |= 1; else PORTL &= ~1; break;
case 1: if (!dir) PORTL |= 2; else PORTL &= ~2; break;
case 2: if (dir) PORTL |= 4; else PORTL &= ~4; break;
case 3: if (!dir) PORTL |= 64; else PORTL &= ~64; break;
#endif
}
asm("nop");
}
uint8_t sm4_get_dir_bits(void)
{
uint8_t register dir_bits = 0;
uint8_t register portL = PORTL;
//TODO -optimize in asm
#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
if (portL & 2) dir_bits |= 1;
if (portL & 1) dir_bits |= 2;
if (portL & 4) dir_bits |= 4;
if (portL & 64) dir_bits |= 8;
dir_bits ^= 0x07; //invert XYZ, do not invert E
#else if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304))
if (portL & 1) dir_bits |= 1;
if (portL & 2) dir_bits |= 2;
if (portL & 4) dir_bits |= 4;
if (portL & 64) dir_bits |= 8;
dir_bits ^= 0x0a; //invert YE, do not invert XZ
#endif
return dir_bits;
}
void sm4_set_dir_bits(uint8_t dir_bits)
{
uint8_t register portL = PORTL;
portL &= 0xb8; //set direction bits to zero
//TODO -optimize in asm
#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
dir_bits ^= 0x07; //invert XYZ, do not invert E
if (dir_bits & 1) portL |= 2; //set X direction bit
if (dir_bits & 2) portL |= 1; //set Y direction bit
if (dir_bits & 4) portL |= 4; //set Z direction bit
if (dir_bits & 8) portL |= 64; //set E direction bit
#else if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304))
dir_bits ^= 0x0a; //invert YE, do not invert XZ
if (dir_bits & 1) portL |= 1; //set X direction bit
if (dir_bits & 2) portL |= 2; //set Y direction bit
if (dir_bits & 4) portL |= 4; //set Z direction bit
if (dir_bits & 8) portL |= 64; //set E direction bit
#endif
PORTL = portL;
asm("nop");
}
void sm4_do_step(uint8_t axes_mask)
{
#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203) || (MOTHERBOARD == 303) || (MOTHERBOARD == 304))
uint8_t register portC = PORTC & 0xf0;
PORTC = portC | (axes_mask & 0x0f); //set step signals by mask
asm("nop");
PORTC = portC; //set step signals to zero
asm("nop");
#endif //((MOTHERBOARD == 200) || (MOTHERBOARD == 203) || (MOTHERBOARD == 303) || (MOTHERBOARD == 304))
}
uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
{
uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz) + ((uint32_t)de*de))) + 0.5);
uint16_t nd = dd;
uint16_t cx = dd;
uint16_t cy = dd;
uint16_t cz = dd;
uint16_t ce = dd;
uint16_t x = 0;
uint16_t y = 0;
uint16_t z = 0;
uint16_t e = 0;
while (nd)
{
if (sm4_stop_cb && (*sm4_stop_cb)()) break;
uint8_t sm = 0; //step mask
if (cx <= dx)
{
sm |= 1;
cx += dd;
x++;
}
if (cy <= dy)
{
sm |= 2;
cy += dd;
y++;
}
if (cz <= dz)
{
sm |= 4;
cz += dd;
z++;
}
if (ce <= de)
{
sm |= 4;
ce += dd;
e++;
}
cx -= dx;
cy -= dy;
cz -= dz;
ce -= de;
sm4_do_step(sm);
uint16_t delay = SM4_DEFDELAY;
if (sm4_calc_delay_cb) delay = (*sm4_calc_delay_cb)(nd, dd);
if (delay) delayMicroseconds(delay);
nd--;
}
if (sm4_update_pos_cb) (*sm4_update_pos_cb)(x, y, z, e);
return nd;
}
#endif //NEW_XYZCAL

56
Firmware/sm4.h Normal file
View File

@ -0,0 +1,56 @@
//sm4.h - simple 4-axis stepper control
#ifndef _SM4_H
#define _SM4_H
#include <inttypes.h>
#include "config.h"
#if defined(__cplusplus)
extern "C" {
#endif //defined(__cplusplus)
// callback prototype for stop condition (return 0 - continue, return 1 - stop)
typedef uint8_t (*sm4_stop_cb_t)();
// callback prototype for updating position counters
typedef void (*sm4_update_pos_cb_t)(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
// callback prototype for calculating delay
typedef uint16_t (*sm4_calc_delay_cb_t)(uint16_t nd, uint16_t dd);
// callback pointer - stop
extern sm4_stop_cb_t sm4_stop_cb;
// callback pointer - update_pos
extern sm4_update_pos_cb_t sm4_update_pos_cb;
// callback pointer - calc_delay
extern sm4_calc_delay_cb_t sm4_calc_delay_cb;
// returns direction for single axis (0 - positive, 1 - negative)
extern uint8_t sm4_get_dir(uint8_t axis);
// set direction for single axis (0 - positive, 1 - negative)
extern void sm4_set_dir(uint8_t axis, uint8_t dir);
// returns direction of all axes as bitmask (0 - positive, 1 - negative)
extern uint8_t sm4_get_dir_bits(void);
// set direction for all axes as bitmask (0 - positive, 1 - negative)
extern void sm4_set_dir_bits(uint8_t dir_bits);
// step axes by bitmask
extern void sm4_do_step(uint8_t axes_mask);
// xyze linear-interpolated relative move, returns remaining diagonal steps (>0 means stoped)
extern uint16_t sm4_line_xyze_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
#if defined(__cplusplus)
}
#endif //defined(__cplusplus)
#endif //_SM4_H

3
Firmware/temperature.h
View File

@ -27,6 +27,9 @@
#include "stepper.h"
#endif
#define ENABLE_TEMPERATURE_INTERRUPT() TIMSK0 |= (1<<OCIE0B)
#define DISABLE_TEMPERATURE_INTERRUPT() TIMSK0 &= ~(1<<OCIE0B)
// public functions
void tp_init(); //initialize the heating
void manage_heater(); //it is critical that this is called periodically.

682
Firmware/xyzcal.cpp Normal file
View File

@ -0,0 +1,682 @@
//xyzcal.cpp - xyz calibration with image processing
#include "Configuration_prusa.h"
#ifdef NEW_XYZCAL
#include "xyzcal.h"
#include <avr/wdt.h>
#include "stepper.h"
#include "temperature.h"
#include "sm4.h"
#define XYZCAL_PINDA_HYST_MIN 20 //50um
#define XYZCAL_PINDA_HYST_MAX 100 //250um
#define XYZCAL_PINDA_HYST_DIF 5 //12.5um
#define ENABLE_FANCHECK_INTERRUPT() EIMSK |= (1<<7)
#define DISABLE_FANCHECK_INTERRUPT() EIMSK &= ~(1<<7)
#define _PINDA ((READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)?1:0)
//#define DBG(args...) printf_P(args)
#define DBG(args...)
#define _n PSTR
#define _X ((int16_t)count_position[X_AXIS])
#define _Y ((int16_t)count_position[Y_AXIS])
#define _Z ((int16_t)count_position[Z_AXIS])
#define _E ((int16_t)count_position[E_AXIS])
#define _PI 3.14159265F
extern long count_position[NUM_AXIS];
uint8_t check_pinda_0();
uint8_t check_pinda_1();
void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de);
uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd);
void xyzcal_meassure_enter(void)
{
DBG(_n("xyzcal_meassure_enter\n"));
disable_heater();
DISABLE_TEMPERATURE_INTERRUPT();
#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
DISABLE_FANCHECK_INTERRUPT();
#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
DISABLE_STEPPER_DRIVER_INTERRUPT();
#ifdef WATCHDOG
wdt_disable();
#endif //WATCHDOG
sm4_stop_cb = 0;
sm4_update_pos_cb = xyzcal_update_pos;
sm4_calc_delay_cb = xyzcal_calc_delay;
}
void xyzcal_meassure_leave(void)
{
DBG(_n("xyzcal_meassure_leave\n"));
planner_abort_hard();
ENABLE_TEMPERATURE_INTERRUPT();
#if (defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
ENABLE_FANCHECK_INTERRUPT();
#endif //(defined(FANCHECK) && defined(TACH_1) && (TACH_1 >-1))
ENABLE_STEPPER_DRIVER_INTERRUPT();
#ifdef WATCHDOG
wdt_enable(WDTO_4S);
#endif //WATCHDOG
sm4_stop_cb = 0;
sm4_update_pos_cb = 0;
sm4_calc_delay_cb = 0;
}
uint8_t check_pinda_0()
{
return _PINDA?0:1;
}
uint8_t check_pinda_1()
{
return _PINDA?1:0;
}
uint8_t xyzcal_dm = 0;
void xyzcal_update_pos(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de)
{
DBG(_n("xyzcal_update_pos dx=%d dy=%d dz=%d dir=%02x\n"), dx, dy, dz, xyzcal_dm);
if (xyzcal_dm&1) count_position[0] -= dx; else count_position[0] += dx;
if (xyzcal_dm&2) count_position[1] -= dy; else count_position[1] += dy;
if (xyzcal_dm&4) count_position[2] -= dz; else count_position[2] += dz;
DBG(_n(" after xyzcal_update_pos x=%ld y=%ld z=%ld\n"), count_position[0], count_position[1], count_position[2]);
}
uint16_t xyzcal_sm4_delay = 0;
//#define SM4_ACCEL_TEST
#ifdef SM4_ACCEL_TEST
uint16_t xyzcal_sm4_v0 = 2000;
uint16_t xyzcal_sm4_vm = 45000;
uint16_t xyzcal_sm4_v = xyzcal_sm4_v0;
uint16_t xyzcal_sm4_ac = 2000;
uint16_t xyzcal_sm4_ac2 = (uint32_t)xyzcal_sm4_ac * 1024 / 10000;
//float xyzcal_sm4_vm = 10000;
#endif //SM4_ACCEL_TEST
uint16_t xyzcal_calc_delay(uint16_t nd, uint16_t dd)
{
return xyzcal_sm4_delay;
#ifdef SM4_ACCEL_TEST
uint16_t del_us = 0;
if (xyzcal_sm4_v & 0xf000) //>=4096
{
del_us = (uint16_t)62500 / (uint16_t)(xyzcal_sm4_v >> 4);
xyzcal_sm4_v += (xyzcal_sm4_ac2 * del_us + 512) >> 10;
if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
if (del_us > 25) return del_us - 25;
}
else
{
del_us = (uint32_t)1000000 / xyzcal_sm4_v;
xyzcal_sm4_v += ((uint32_t)xyzcal_sm4_ac2 * del_us + 512) >> 10;
if (xyzcal_sm4_v > xyzcal_sm4_vm) xyzcal_sm4_v = xyzcal_sm4_vm;
if (del_us > 50) return del_us - 50;
}
// uint16_t del_us = (uint16_t)(((float)1000000 / xyzcal_sm4_v) + 0.5);
// uint16_t del_us = (uint32_t)1000000 / xyzcal_sm4_v;
// uint16_t del_us = 100;
// uint16_t del_us = (uint16_t)10000 / xyzcal_sm4_v;
// v += (ac * del_us + 500) / 1000;
// xyzcal_sm4_v += (xyzcal_sm4_ac * del_us) / 1000;
// return xyzcal_sm4_delay;
// DBG(_n("xyzcal_calc_delay nd=%d dd=%d v=%d del_us=%d\n"), nd, dd, xyzcal_sm4_v, del_us);
return 0;
#endif //SM4_ACCEL_TEST
}
bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda)
{
DBG(_n("xyzcal_lineXYZ_to x=%d y=%d z=%d check=%d\n"), x, y, z, check_pinda);
x -= (int16_t)count_position[0];
y -= (int16_t)count_position[1];
z -= (int16_t)count_position[2];
xyzcal_dm = ((x<0)?1:0) | ((y<0)?2:0) | ((z<0)?4:0);
sm4_set_dir_bits(xyzcal_dm);
sm4_stop_cb = check_pinda?((check_pinda<0)?check_pinda_0:check_pinda_1):0;
xyzcal_sm4_delay = delay_us;
// uint32_t u = micros();
bool ret = sm4_line_xyze_ui(abs(x), abs(y), abs(z), 0)?true:false;
// u = micros() - u;
return ret;
}
bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, int16_t rotation, uint16_t delay_us, int8_t check_pinda, uint16_t* pad)
{
bool ret = false;
float r = 0; //radius
uint8_t n = 0; //point number
uint16_t ad = 0; //angle [deg]
float ar; //angle [rad]
uint8_t dad = 0; //delta angle [deg]
uint8_t dad_min = 4; //delta angle min [deg]
uint8_t dad_max = 16; //delta angle max [deg]
uint8_t k = 720 / (dad_max - dad_min); //delta calculation constant
ad = 0;
if (pad) ad = *pad % 720;
DBG(_n("xyzcal_spiral2 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
for (; ad < 720; ad++)
{
if (radius > 0)
{
dad = dad_max - (ad / k);
r = (float)(((uint32_t)ad) * radius) / 720;
}
else
{
dad = dad_max - ((719 - ad) / k);
r = (float)(((uint32_t)(719 - ad)) * (-radius)) / 720;
}
ar = (ad + rotation)* (float)_PI / 180;
float _cos = cos(ar);
float _sin = sin(ar);
int x = (int)(cx + (_cos * r));
int y = (int)(cy + (_sin * r));
int z = (int)(z0 - ((float)((int32_t)dz * ad) / 720));
if (xyzcal_lineXYZ_to(x, y, z, delay_us, check_pinda))
{
ad += dad + 1;
ret = true;
break;
}
n++;
ad += dad;
}
if (pad) *pad = ad;
return ret;
}
bool xyzcal_spiral8(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, uint16_t delay_us, int8_t check_pinda, uint16_t* pad)
{
bool ret = false;
uint16_t ad = 0;
if (pad) ad = *pad;
DBG(_n("xyzcal_spiral8 cx=%d cy=%d z0=%d dz=%d radius=%d ad=%d\n"), cx, cy, z0, dz, radius, ad);
if (!ret && (ad < 720))
if (ret = xyzcal_spiral2(cx, cy, z0 - 0*dz, dz, radius, 0, delay_us, check_pinda, &ad))
ad += 0;
if (!ret && (ad < 1440))
if (ret = xyzcal_spiral2(cx, cy, z0 - 1*dz, dz, -radius, 0, delay_us, check_pinda, &ad))
ad += 720;
if (!ret && (ad < 2160))
if (ret = xyzcal_spiral2(cx, cy, z0 - 2*dz, dz, radius, 180, delay_us, check_pinda, &ad))
ad += 1440;
if (!ret && (ad < 2880))
if (ret = xyzcal_spiral2(cx, cy, z0 - 3*dz, dz, -radius, 180, delay_us, check_pinda, &ad))
ad += 2160;
if (pad) *pad = ad;
return ret;
}
#ifdef XYZCAL_MEASSURE_PINDA_HYSTEREZIS
int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples)
{
DBG(_n("xyzcal_meassure_pinda_hysterezis\n"));
int8_t ret = -1; // PINDA signal error
int16_t z = _Z;
int16_t sum_up = 0;
int16_t sum_dn = 0;
int16_t up;
int16_t dn;
uint8_t sample;
xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
if (!_PINDA)
{
for (sample = 0; sample < samples; sample++)
{
dn = _Z;
if (!xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1)) break;
dn = dn - _Z;
up = _Z;
if (!xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1)) break;
up = _Z - up;
DBG(_n("%d. up=%d dn=%d\n"), sample, up, dn);
sum_up += up;
sum_dn += dn;
if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
{
ret = -2; // difference between up-dn to high
break;
}
}
if (sample == samples)
{
up = sum_up / samples;
dn = sum_dn / samples;
uint16_t hyst = (up + dn) / 2;
if (abs(up - dn) > XYZCAL_PINDA_HYST_DIF)
ret = -2; // difference between up-dn to high
else if ((hyst < XYZCAL_PINDA_HYST_MIN) || (hyst > XYZCAL_PINDA_HYST_MAX))
ret = -3; // hysterezis out of range
else
ret = hyst;
}
}
xyzcal_lineXYZ_to(_X, _Y, z, delay_us, 0);
return ret;
}
#endif //XYZCAL_MEASSURE_PINDA_HYSTEREZIS
void xyzcal_scan_pixels_32x32(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels)
{
DBG(_n("xyzcal_scan_pixels_32x32 cx=%d cy=%d min_z=%d max_z=%d\n"), cx, cy, min_z, max_z);
// xyzcal_lineXYZ_to(cx - 1024, cy - 1024, max_z, 2*delay_us, 0);
// xyzcal_lineXYZ_to(cx, cy, max_z, delay_us, 0);
int16_t z = (int16_t)count_position[2];
xyzcal_lineXYZ_to(cx, cy, z, 2*delay_us, 0);
for (uint8_t r = 0; r < 32; r++)
{
int8_t _pinda = _PINDA;
xyzcal_lineXYZ_to((r&1)?(cx+1024):(cx-1024), cy - 1024 + r*64, z, 2*delay_us, 0);
xyzcal_lineXYZ_to(_X, _Y, min_z, delay_us, 1);
xyzcal_lineXYZ_to(_X, _Y, max_z, delay_us, -1);
z = (int16_t)count_position[2];
sm4_set_dir(X_AXIS, (r&1)?1:0);
for (uint8_t c = 0; c < 32; c++)
{
uint16_t sum = 0;
int16_t z_sum = 0;
for (uint8_t i = 0; i < 64; i++)
{
int8_t pinda = _PINDA;
int16_t pix = z - min_z;
pix += (pinda)?23:-24;
if (pix < 0) pix = 0;
if (pix > 255) pix = 255;
sum += pix;
z_sum += z;
// if (_pinda != pinda)
// {
// if (pinda)
// DBG(_n("!1 x=%d z=%d\n"), c*64+i, z+23);
// else
// DBG(_n("!0 x=%d z=%d\n"), c*64+i, z-24);
// }
sm4_set_dir(Z_AXIS, !pinda);
if (!pinda)
{
if (z > min_z)
{
sm4_do_step(Z_AXIS_MASK);
z--;
}
}
else
{
if (z < max_z)
{
sm4_do_step(Z_AXIS_MASK);
z++;
}
}
sm4_do_step(X_AXIS_MASK);
delayMicroseconds(600);
_pinda = pinda;
}
sum >>= 6; //div 64
if (z_sum < 0)
{
z_sum = -z_sum;
z_sum >>= 6; //div 64
z_sum = -z_sum;
}
else
z_sum >>= 6; //div 64
if (pixels) pixels[((uint16_t)r<<5) + ((r&1)?(31-c):c)] = sum;
// DBG(_n("c=%d r=%d l=%d z=%d\n"), c, r, sum, z_sum);
count_position[0] += (r&1)?-64:64;
count_position[2] = z;
}
if (pixels)
for (uint8_t c = 0; c < 32; c++)
DBG(_n("%02x"), pixels[((uint16_t)r<<5) + c]);
DBG(_n("\n"));
}
// xyzcal_lineXYZ_to(cx, cy, z, 2*delay_us, 0);
}
void xyzcal_histo_pixels_32x32(uint8_t* pixels, uint16_t* histo)
{
for (uint8_t l = 0; l < 16; l++)
histo[l] = 0;
for (uint8_t r = 0; r < 32; r++)
for (uint8_t c = 0; c < 32; c++)
{
uint8_t pix = pixels[((uint16_t)r<<5) + c];
histo[pix >> 4]++;
}
for (uint8_t l = 0; l < 16; l++)
DBG(_n(" %2d %d\n"), l, histo[l]);
}
void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo)
{
uint8_t l;
uint16_t max_c = histo[0];
uint8_t max_l = 0;
for (l = 1; l < 16; l++)
{
uint16_t c = histo[l];
if (c > max_c)
{
max_c = c;
max_l = l;
}
}
DBG(_n("max_c=%2d max_l=%d\n"), max_c, max_l);
for (l = 15; l > 8; l--)
if (histo[l] >= 10)
break;
uint8_t pix_min = (max_l + 3) << 4;
uint8_t pix_max = l << 4;
uint8_t pix_dif = pix_max - pix_min;
DBG(_n(" min=%d max=%d dif=%d\n"), pix_min, pix_max, pix_dif);
for (int16_t i = 0; i < 32*32; i++)
{
uint16_t pix = pixels[i];
if (pix > pix_min) pix -= pix_min;
else pix = 0;
pix <<= 8;
pix /= pix_dif;
// if (pix < 0) pix = 0;
if (pix > 255) pix = 255;
pixels[i] = (uint8_t)pix;
}
for (uint8_t r = 0; r < 32; r++)
{
for (uint8_t c = 0; c < 32; c++)
DBG(_n("%02x"), pixels[((uint16_t)r<<5) + c]);
DBG(_n("\n"));
}
}
/*
void xyzcal_draw_pattern_12x12_in_32x32(uint8_t* pattern, uint32_t* pixels, int w, int h, uint8_t x, uint8_t y, uint32_t and, uint32_t or)
{
for (int i = 0; i < 8; i++)
for (int j = 0; j < 8; j++)
{
int idx = (x + j) + w * (y + i);
if (pattern[i] & (1 << j))
{
pixels[idx] &= and;
pixels[idx] |= or;
}
}
}
*/
int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t c, uint8_t r)
{
uint8_t thr = 64;
int16_t match = 0;
for (uint8_t i = 0; i < 12; i++)
for (uint8_t j = 0; j < 12; j++)
{
if (((i == 0) || (i == 11)) && ((j < 2) || (j >= 10))) continue; //skip corners
if (((j == 0) || (j == 11)) && ((i < 2) || (i >= 10))) continue;
uint16_t idx = (c + j) + 32 * (r + i);
uint8_t val = pixels[idx];
if (pattern[i] & (1 << j))
{
if (val > thr) match ++;
else match --;
}
else
{
if (val <= thr) match ++;
else match --;
}
}
return match;
}
int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr)
{
uint8_t max_c = 0;
uint8_t max_r = 0;
int16_t max_match = 0;
for (uint8_t r = 0; r < (32 - 12); r++)
for (uint8_t c = 0; c < (32 - 12); c++)
{
int16_t match = xyzcal_match_pattern_12x12_in_32x32(pattern, pixels, c, r);
if (max_match < match)
{
max_c = c;
max_r = r;
max_match = match;
}
}
DBG(_n("max_c=%d max_r=%d max_match=%d\n"), max_c, max_r, max_match);
if (pc) *pc = max_c;
if (pr) *pr = max_r;
return max_match;
}
#ifdef XYZCAL_FIND_POINT_CENTER
int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns)
{
uint8_t n;
uint16_t ad;
float ar;
float _cos;
float _sin;
int16_t r_min = 0;
int16_t r_max = 0;
int16_t x_min = 0;
int16_t x_max = 0;
int16_t y_min = 0;
int16_t y_max = 0;
int16_t r = 10;
int16_t x = x0;
int16_t y = y0;
int16_t z = z0;
int8_t _pinda = _PINDA;
for (n = 0; n < turns; n++)
{
uint32_t r_sum = 0;
for (ad = 0; ad < 720; ad++)
{
ar = ad * _PI / 360;
_cos = cos(ar);
_sin = sin(ar);
x = x0 + (int)(_cos * r);
y = y0 + (int)(_sin * r);
xyzcal_lineXYZ_to(x, y, z, 1000, 0);
int8_t pinda = _PINDA;
if (pinda)
r += 1;
else
{
r -= 1;
ad--;
r_sum -= r;
}
if (ad == 0)
{
x_min = x0;
x_max = x0;
y_min = y0;
y_max = y0;
r_min = r;
r_max = r;
}
else if (pinda)
{
if (x_min > x) x_min = (2*x + x_min) / 3;
if (x_max < x) x_max = (2*x + x_max) / 3;
if (y_min > y) y_min = (2*y + y_min) / 3;
if (y_max < y) y_max = (2*y + y_max) / 3;
/* if (x_min > x) x_min = x;
if (x_max < x) x_max = x;
if (y_min > y) y_min = y;
if (y_max < y) y_max = y;*/
if (r_min > r) r_min = r;
if (r_max < r) r_max = r;
}
r_sum += r;
/* if (_pinda != pinda)
{
if (pinda)
DBG(_n("!1 x=%d y=%d\n"), x, y);
else
DBG(_n("!0 x=%d y=%d\n"), x, y);
}*/
_pinda = pinda;
// DBG(_n("x=%d y=%d rx=%d ry=%d\n"), x, y, rx, ry);
}
DBG(_n("x_min=%d x_max=%d y_min=%d y_max=%d r_min=%d r_max=%d r_avg=%d\n"), x_min, x_max, y_min, y_max, r_min, r_max, r_sum / 720);
if ((n > 2) && (n & 1))
{
x0 += (x_min + x_max);
y0 += (y_min + y_max);
x0 /= 3;
y0 /= 3;
int rx = (x_max - x_min) / 2;
int ry = (y_max - y_min) / 2;
r = (rx + ry) / 3;//(rx < ry)?rx:ry;
DBG(_n("x0=%d y0=%d r=%d\n"), x0, y0, r);
}
}
xyzcal_lineXYZ_to(x0, y0, z, 200, 0);
}
#endif //XYZCAL_FIND_POINT_CENTER
uint8_t xyzcal_xycoords2point(int16_t x, int16_t y)
{
uint8_t ix = (x > 10000)?1:0;
uint8_t iy = (y > 10000)?1:0;
return iy?(3-ix):ix;
}
//MK3
#if ((MOTHERBOARD == 303) || (MOTHERBOARD == 304))
const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200};
const int16_t PROGMEM xyzcal_point_ycoords[4] = {600, 600, 19800, 19800};
#endif //((MOTHERBOARD == 303) || (MOTHERBOARD == 304))
//MK2.5
#if ((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200};
const int16_t PROGMEM xyzcal_point_ycoords[4] = {700, 700, 19800, 19800};
#endif //((MOTHERBOARD == 200) || (MOTHERBOARD == 203))
const uint16_t PROGMEM xyzcal_point_pattern[12] = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000};
bool xyzcal_searchZ(void)
{
DBG(_n("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
int16_t x0 = _X;
int16_t y0 = _Y;
int16_t z0 = _Z;
// int16_t min_z = -6000;
// int16_t dz = 100;
int16_t z = z0;
while (z > -2300) //-6mm + 0.25mm
{
uint16_t ad = 0;
if (xyzcal_spiral8(x0, y0, z, 100, 900, 320, 1, &ad)) //dz=100 radius=900 delay=400
{
int16_t x_on = _X;
int16_t y_on = _Y;
int16_t z_on = _Z;
DBG(_n(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), x_on, y_on, z_on, ad);
return true;
}
z -= 400;
}
DBG(_n("xyzcal_searchZ no signal\n x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
return false;
}
bool xyzcal_scan_and_process(void)
{
DBG(_n("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE);
// DBG(_n("sizeof(pixels)=%d\n"), 32*32);
// DBG(_n("sizeof(histo)=%d\n"), 2*16);
// DBG(_n("sizeof(pattern)=%d\n"), 2*12);
DBG(_n("sizeof(total)=%d\n"), 32*32+2*16+2*12);
bool ret = false;
int16_t x = _X;
int16_t y = _Y;
int16_t z = _Z;
uint8_t* pixels = (uint8_t*)block_buffer;
xyzcal_scan_pixels_32x32(x, y, z - 128, 2400, 200, pixels);
uint16_t* histo = (uint16_t*)(pixels + 32*32);
xyzcal_histo_pixels_32x32(pixels, histo);
xyzcal_adjust_pixels(pixels, histo);
uint16_t* pattern = (uint16_t*)(histo + 2*16);
for (uint8_t i = 0; i < 12; i++)
{
pattern[i] = pgm_read_word_far((uint16_t*)(xyzcal_point_pattern + i));
// DBG(_n(" pattern[%d]=%d\n"), i, pattern[i]);
}
uint8_t c = 0;
uint8_t r = 0;
if (xyzcal_find_pattern_12x12_in_32x32(pixels, pattern, &c, &r) > 66) //total pixels=144, corner=12 (1/2 = 66)
{
DBG(_n(" pattern found at %d %d\n"), c, r);
c += 6;
r += 6;
x += ((int16_t)c - 16) << 6;
y += ((int16_t)r - 16) << 6;
DBG(_n(" x=%d y=%d z=%d\n"), x, y, z);
xyzcal_lineXYZ_to(x, y, z, 200, 0);
ret = true;
}
for (uint16_t i = 0; i < sizeof(block_t)*BLOCK_BUFFER_SIZE; i++)
pixels[i] = 0;
return ret;
}
bool xyzcal_find_bed_induction_sensor_point_xy(void)
{
DBG(_n("xyzcal_find_bed_induction_sensor_point_xy x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]);
bool ret = false;
st_synchronize();
int16_t x = _X;
int16_t y = _Y;
int16_t z = _Z;
uint8_t point = xyzcal_xycoords2point(x, y);
x = pgm_read_word_far((uint16_t*)(xyzcal_point_xcoords + point));
y = pgm_read_word_far((uint16_t*)(xyzcal_point_ycoords + point));
DBG(_n("point=%d x=%d y=%d z=%d\n"), point, x, y, z);
xyzcal_meassure_enter();
xyzcal_lineXYZ_to(x, y, z, 200, 0);
if (xyzcal_searchZ())
{
int16_t z = _Z;
xyzcal_lineXYZ_to(x, y, z, 200, 0);
if (xyzcal_scan_and_process())
{
ret = true;
}
}
xyzcal_meassure_leave();
return ret;
}
#endif //NEW_XYZCAL

39
Firmware/xyzcal.h Normal file
View File

@ -0,0 +1,39 @@
//xyzcal.h - xyz calibration with image processing
#ifndef _XYZCAL_H
#define _XYZCAL_H
#include <inttypes.h>
extern void xyzcal_meassure_enter(void);
extern void xyzcal_meassure_leave(void);
extern bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda);
extern bool xyzcal_spiral2(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, int16_t rotation, uint16_t delay_us, int8_t check_pinda, uint16_t* pad);
extern bool xyzcal_spiral8(int16_t cx, int16_t cy, int16_t z0, int16_t dz, int16_t radius, uint16_t delay_us, int8_t check_pinda, uint16_t* pad);
//extern int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples);
extern void xyzcal_scan_pixels_32x32(int16_t cx, int16_t cy, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t* pixels);
extern void xyzcal_histo_pixels_32x32(uint8_t* pixels, uint16_t* histo);
extern void xyzcal_adjust_pixels(uint8_t* pixels, uint16_t* histo);
extern int16_t xyzcal_match_pattern_12x12_in_32x32(uint16_t* pattern, uint8_t* pixels, uint8_t x, uint8_t y);
extern int16_t xyzcal_find_pattern_12x12_in_32x32(uint8_t* pixels, uint16_t* pattern, uint8_t* pc, uint8_t* pr);
//extern int8_t xyzcal_find_point_center(int16_t x0, int16_t y0, int16_t z0, int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t turns);
extern bool xyzcal_searchZ(void);
extern bool xyzcal_scan_and_process(void);
extern bool xyzcal_find_bed_induction_sensor_point_xy(void);
#endif //_XYZCAL_H