void StepperController::enableSteppers(bool x, bool y, bool z)
{
if (x)
PORTP(steppers[0].enPort) |= steppers[0].enPin;
if (y)
PORTP(steppers[1].enPort) |= steppers[1].enPin;
if (z)
PORTP(steppers[2].enPort) |= steppers[2].enPin;
}
lref_t linput_portp(lref_t obj)
{
if (PORTP(obj) && PORT_INPUTP(obj))
return obj;
return boolcons(false);
}
lref_t loutput_portp(lref_t obj)
{
if (PORTP(obj) && PORT_OUTPUTP(obj))
return obj;
return boolcons(false);
}
lref_t lport_openp(lref_t obj)
{
if (!PORTP(obj))
vmerror_wrong_type_n(1, obj);
return boolcons(!PORT_CLOSEDP(obj));
}
lref_t lidebug_printer(lref_t obj, lref_t port, lref_t machine_readable_p)
{
if (!PORTP(port))
vmerror_wrong_type_n(2, port);
return debug_print_object(obj, port, TRUEP(machine_readable_p));
}
INLINE lref_t PORT_UNDERLYING(lref_t text_port)
{
lref_t underlying = PORT_USER_OBJECT(text_port);
assert(PORTP(underlying));
return underlying;
}
size_t port_length(lref_t port)
{
assert(PORTP(port));
if (PORT_CLASS(port)->length)
return PORT_CLASS(port)->length(port);
return 0;
}
lref_t lport_name(lref_t port)
{
if (NULLP(port))
port = CURRENT_INPUT_PORT();
if (!PORTP(port))
vmerror_wrong_type_n(1, port);
return PORT_PINFO(port)->port_name;
}
lref_t lport_class_name(lref_t port)
{
if (NULLP(port))
port = CURRENT_INPUT_PORT();
if (!PORTP(port))
vmerror_wrong_type_n(1, port);
return strconsbuf(PORT_CLASS(port)->name);
}
void StepperController::moveTo(int32_t x,int32_t y,int32_t z,int32_t feedRate)
{
uint32_t moveTime;
uint8_t i;
for (i=0;i<3;i++)
{
steppers[i].targetStep = x;
int32_t deltaSteps = x - steppers[i].currentStep;
steppers[i].bDirectionPositive = deltaSteps >= 0;
steppers[i].deltaSteps = ABS(deltaSteps);
if (steppers[i].deltaSteps > 0)
{
PORTP(steppers[i].enPort) |= steppers[i].enPin;
if (steppers[i].bDirectionPositive)
PORTP(steppers[i].dirPort) |= steppers[i].dirPin;
else
PORTP(steppers[i].dirPort) &= ~steppers[i].dirPin;
}
moveTime = MAX(moveTime,steppers[i].deltaSteps / feedRate);
}
for (i=0;i<3;i++)
{
if (steppers[i].deltaSteps > 0)
{
steppers[i].stepDelay = STEPPER_HZ / (steppers[i].deltaSteps / moveTime);
steppers[i].currentStepTime = steppers[i].stepDelay;
}
}
mbMoving = isMoving();
if (mbMoving)
timer.enable();
}
void StepperController::update()
{
if (mbMoving)
{
#ifdef STEPPERX_DISABLE_INACTIVE
if (!steppers[0].deltaSteps)
PORTP(steppers[0].enPort) &= ~steppers[0].enPin;
#endif
#ifdef STEPPERY_DISABLE_INACTIVE
if (!steppers[1].deltaSteps)
PORTP(steppers[1].enPort) &= ~steppers[1].enPin;
#endif
#ifdef STEPPERZ_DISABLE_INACTIVE
if (!steppers[2].deltaSteps)
PORTP(steppers[2].enPort) &= ~steppers[2].enPin;
#endif
mbMoving = isMoving();
if (!mbMoving)
timer.disable();
}
}
lref_t lopen_text_output_port(lref_t underlying)
{
if (!PORTP(underlying))
vmerror_wrong_type_n(1, underlying);
if (!BINARY_PORTP(underlying))
vmerror_unsupported(_T("cannot open text output on binary port"));
return portcons(&text_port_class,
lport_name(underlying),
PORT_OUTPUT | PORT_TEXT,
underlying,
NULL);
}
lref_t lclose_port(lref_t port)
{
if (!PORTP(port))
vmerror_wrong_type_n(1, port);
if (PORT_OUTPUTP(port))
lflush_port(port);
if (PORT_CLASS(port)->close)
PORT_CLASS(port)->close(port);
PORT_PINFO(port)->mode = PORT_CLOSED;
return port;
}
void port_gc_free(lref_t port)
{
assert(PORTP(port));
assert(PORT_CLASS(port));
if (PORT_CLASS(port)->close)
PORT_CLASS(port)->close(port);
if (PORT_TEXT_INFO(port))
gc_free(PORT_TEXT_INFO(port));
if (PORT_CLASS(port)->gc_free)
PORT_CLASS(port)->gc_free(port);
gc_free(PORT_PINFO(port));
}
lref_t lflush_port(lref_t port)
{
if (!PORTP(port))
vmerror_wrong_type_n(1, port);
if (TEXT_PORTP(port)
&& PORT_TEXT_INFO(port)->translate
&& PORT_TEXT_INFO(port)->needs_lf)
{
write_char(port, _T('\n'));
}
if (PORT_CLASS(port)->flush)
PORT_CLASS(port)->flush(port);
return port;
}
lref_t lrich_write(lref_t obj, lref_t machine_readable, lref_t port)
{
if (NULLP(port))
port = CURRENT_OUTPUT_PORT();
if (!PORTP(port))
vmerror_wrong_type_n(3, port);
if (PORT_INPUTP(port))
vmerror_unsupported(_T("cannot rich-write to input ports"));
if (PORT_CLASS(port)->rich_write == NULL)
return boolcons(false);
if (PORT_CLASS(port)->rich_write(port, obj, TRUEP(machine_readable)))
return port;
return boolcons(false);
}
size_t debug_port_write_chars(lref_t port, const _TCHAR *buf, size_t count)
{
UNREFERENCED(port);
assert(PORTP(port));
_TCHAR block[DEBUG_PORT_BLOCK_SIZE + 1];
size_t buf_loc = 0;
size_t block_loc = 0;
size_t len = count;
/* Filter nulls out of the input string, and ensure that the
* buffer we pass to OutputDebugString has as terminating
* null. */
while (len > 0)
{
for (block_loc = 0;
(block_loc < DEBUG_PORT_BLOCK_SIZE) && (len > 0); block_loc++, buf_loc++, len--)
{
if (buf[buf_loc] == '\0')
block[block_loc] = '~';
else
block[block_loc] = buf[buf_loc];
}
block[block_loc] = '\0';
if (DEBUG_FLAG(DF_DEBUGGER_TO_ODS))
sys_output_debug_string(block);
else
fputs(block, stderr);
}
return len;
}
lref_t lportp(lref_t obj)
{
return boolcons(PORTP(obj));
}
/* A C function to do Lisp-style Formatted I/O ******************
*
* ~s - write the lisp object
* ~a - display the lisp object
* REVISIT: remove scvwritef ~u in favor of some kind of print_unreadable_object call
* ~u - display the lisp object in unprintable fashion (ie. <type@addr...>
*
* ~cs - display the C string
* ~cS - display the C string/arglist with a recursive call to scvwritef
* ~cd - display the C integer
* ~cf - display the C flonum
* ~c& - display the C pointer
* ~cc - display the C character
* ~cC - display the C integer as an octal character constant
* ~cB - display the C integer as a byte
*
* Prefixing a format code with a #\! (ie. ~!L) causes the corresponding
* value to be returned from the function as a Lisp object.
*/
lref_t scvwritef(const _TCHAR * format_str, lref_t port, va_list arglist)
{
char ch;
if (NULLP(port))
port = CURRENT_OUTPUT_PORT();
assert(PORTP(port));
_TCHAR buf[STACK_STRBUF_LEN];
lref_t lisp_arg_value = NULL;
_TCHAR *str_arg_value = NULL;
_TCHAR char_arg_value = _T('\0');
long int long_arg_value = 0;
unsigned long int ulong_arg_value = 0;
flonum_t flonum_arg_value = 0.0;
lref_t unprintable_object = NIL;
lref_t return_value = NIL;
for (;;)
{
ch = *format_str;
if (ch == '\0')
break;
bool return_next_value = false;
format_str++;
if (ch != '~')
{
write_char(port, ch);
continue;
}
ch = *format_str;
format_str++;
if (ch == '!')
{
ch = *format_str;
format_str++;
return_next_value = true;
}
switch (ch)
{
case 's':
lisp_arg_value = va_arg(arglist, lref_t);
if (return_next_value)
return_value = lisp_arg_value;
debug_print_object(lisp_arg_value, port, true);
break;
case 'a':
lisp_arg_value = va_arg(arglist, lref_t);
if (return_next_value)
return_value = lisp_arg_value;
debug_print_object(lisp_arg_value, port, false);
break;
case 'u':
unprintable_object = va_arg(arglist, lref_t);
if (return_next_value)
return_value = unprintable_object;
if (DEBUG_FLAG(DF_PRINT_FOR_DIFF))
scwritef("#<~cs@(no-addr)", port, typecode_name(TYPE(unprintable_object)));
else
scwritef("#<~cs@~c&", port,
typecode_name(TYPE(unprintable_object)), unprintable_object);
break;
case '~':
write_char(port, '~');
break;
case 'c': /* C object prefix */
ch = *format_str; /* read the next format character */
format_str++;
switch (ch)
{
case 's':
str_arg_value = va_arg(arglist, _TCHAR *);
if (return_next_value)
return_value = strconsbuf(str_arg_value);
if (str_arg_value)
write_text(port, str_arg_value, _tcslen(str_arg_value));
else
WRITE_TEXT_CONSTANT(port, _T("<null>"));
break;
case 'S':
str_arg_value = va_arg(arglist, _TCHAR *);
if (return_next_value)
return_value = scvwritef(str_arg_value, port, arglist);
else
scvwritef(str_arg_value, port, arglist);
break;
case 'd':
long_arg_value = va_arg(arglist, long int);
if (return_next_value)
return_value = fixcons(long_arg_value);
_sntprintf(buf, STACK_STRBUF_LEN, _T("%d"), (int) long_arg_value);
write_text(port, buf, _tcslen(buf));
break;
case 'x':
long_arg_value = va_arg(arglist, long int);
if (return_next_value)
return_value = fixcons(long_arg_value);
_sntprintf(buf, STACK_STRBUF_LEN, _T("%08lx"), long_arg_value);
write_text(port, buf, _tcslen(buf));
break;
case 'f':
flonum_arg_value = va_arg(arglist, flonum_t);
if (return_next_value)
return_value = flocons(flonum_arg_value);
_sntprintf(buf, STACK_STRBUF_LEN, _T("%f"), flonum_arg_value);
write_text(port, buf, _tcslen(buf));
break;
case '&':
_sntprintf(buf, STACK_STRBUF_LEN, _T("%p"), (void *) va_arg(arglist, void *));
if (return_next_value)
return_value = strconsbuf(buf);
write_text(port, buf, _tcslen(buf));
break;
case 'c':
ulong_arg_value = va_arg(arglist, unsigned long int);
if (return_next_value)
return_value = fixcons(ulong_arg_value);
char_arg_value = (_TCHAR) ulong_arg_value;
write_text(port, &char_arg_value, 1);
break;
case 'C':
ulong_arg_value = va_arg(arglist, unsigned long int);
if (return_next_value)
return_value = fixcons(ulong_arg_value);
_sntprintf(buf, STACK_STRBUF_LEN, _T("%03o"), (uint32_t) ulong_arg_value);
write_text(port, buf, _tcslen(buf));
break;
case 'B':
ulong_arg_value = va_arg(arglist, unsigned long int);
if (return_next_value)
return_value = fixcons(ulong_arg_value);
_sntprintf(buf, STACK_STRBUF_LEN, _T("0x%02x"), (uint32_t) ulong_arg_value);
write_text(port, buf, _tcslen(buf));
break;
default:
panic(_T("Invalid C object format character in scwritef"));
break;
};
break;
default:
panic(_T("Invalid format character in scwritef"));
break;
}
return_next_value = false;
}
va_end(arglist);
if (!NULLP(unprintable_object))
scwritef(">", port);
return return_value;
}
void StepperController::doISR()
{
//HACK: move z axis first if it needs to be moved to the head does not try to go through the object
//essentially the host software should split the move into two commands, move z first and move xy after
if (steppers[3].deltaSteps > 0)
{
steppers[3].currentStepTime--;
if (!steppers[3].currentStepTime)
{
PORTP(steppers[3].stepPort) |= steppers[3].stepPin;
PORTP(steppers[3].stepPort) &= ~steppers[3].stepPin;
steppers[3].currentStepTime = steppers[3].stepDelay;
steppers[3].deltaSteps--;
steppers[3].currentStep += steppers[3].bDirectionPositive ? 1 : -1;
}
}
else
{
for (int i=0;i<2;i++)
{
steppers[i].currentStepTime--;
if (!steppers[i].currentStepTime)
{
PORTP(steppers[i].stepPort) |= steppers[i].stepPin;
PORTP(steppers[i].stepPort) &= ~steppers[i].stepPin;
steppers[i].currentStepTime = steppers[i].stepDelay;
steppers[i].deltaSteps--;
steppers[i].currentStep += steppers[i].bDirectionPositive ? 1 : -1;
}
}
}
/*
if (steppers[3]->needStepping())
{
currentStepTime[3]--;
if (currentStepTime[3] == 0)
{
steppers[3]->doStep();
currentStepTime[3] = stepDelay[3];
}
}
else
{
for (int i=0;i<2;i++)
{
if (steppers[i]->needStepping())
{
currentStepTime[i]--;
if (currentStepTime[i] == 0)
{
steppers[i]->doStep();
currentStepTime[i] = stepDelay[i];
}
}
}
}
*/
#if 0
for (uint8_t i=0;i<3;i++)
{
if (steppers[i].needStepping())
{
currentStepTime[i]--;
if (currentStepTime[i] == 0)
{
steppers[i].doStep();
currentStepTime[i] = stepDelay[i];
}
}
}
#endif
}
