181 lines
4.5 KiB
C
181 lines
4.5 KiB
C
//sm4.c - simple 4-axis stepper control
|
|
|
|
#include "sm4.h"
|
|
#include <avr/io.h>
|
|
|
|
#include "boards.h"
|
|
#define bool int8_t
|
|
#define false 0
|
|
#define true 1
|
|
#include "Configuration_prusa.h"
|
|
|
|
/**/
|
|
|
|
//direction signal pinout - 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 pinout - 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_update_pos_cb sm4_update_pos = 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))
|
|
}
|
|
|
|
int isqrt(int n)
|
|
{
|
|
int a = 1;
|
|
int b = n;
|
|
while (abs(a - b) > 1)
|
|
{
|
|
b = n / a;
|
|
a = (a + b) / 2;
|
|
}
|
|
return a;
|
|
}
|
|
|
|
uint8_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t delay_us, sm4_stop_cb stop_cb)
|
|
{
|
|
uint16_t dd = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz))) + 0.5);
|
|
uint16_t nd = dd;
|
|
uint16_t cx = dd;
|
|
uint16_t cy = dd;
|
|
uint16_t cz = dd;
|
|
uint16_t x = 0;
|
|
uint16_t y = 0;
|
|
uint16_t z = 0;
|
|
uint8_t stop = 0;
|
|
while ((nd--) && stop_cb && !(stop = (*stop_cb)()))
|
|
{
|
|
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++;
|
|
}
|
|
cx -= dx;
|
|
cy -= dy;
|
|
cz -= dz;
|
|
sm4_do_step(sm);
|
|
delayMicroseconds(delay_us);
|
|
}
|
|
if (sm4_update_pos) (*sm4_update_pos)(x, y, z, 0);
|
|
return stop;
|
|
}
|
|
|