s32 MIXER_ApplyLimits(unsigned channel, struct Limit *limit, volatile s32 *_raw,
volatile s32 *_Channels, enum LimitMask flags)
{
int applied_safety = 0;
s32 value = _raw[NUM_INPUTS + 1 + channel] + get_trim(NUM_INPUTS + 1 + channel);
if (channel >= NUM_OUT_CHANNELS)
return value;
if ((flags & APPLY_SAFETY) && MIXER_SRC(limit->safetysw) && switch_is_on(limit->safetysw, _raw)) {
applied_safety = 1;
value = PCT_TO_RANGE(Model.limits[channel].safetyval);
}
if (flags & APPLY_SCALAR) {
if (value >= 0 || limit->servoscale_neg == 0)
value = (s32)value * limit->servoscale / 100;
else
value = (s32)value * limit->servoscale_neg / 100;
}
if ((flags & APPLY_REVERSE) && (limit->flags & CH_REVERSE)) {
value = -value;
}
if (flags & APPLY_SUBTRIM)
value += PCT_TO_RANGE(limit->subtrim) / 10;
if (! applied_safety) {
//degrees / 100msec
if (_Channels && (flags & APPLY_SPEED) && limit->speed) {
s32 rate = CHAN_MAX_VALUE * limit->speed / 60 * MEDIUM_PRIORITY_MSEC / 100;
if (value - _Channels[channel] > rate)
value = _Channels[channel] + rate;
else if(value - _Channels[channel] < -rate)
value = _Channels[channel] - rate;
}
}
if (flags & APPLY_LIMITS) {
if (value > PCT_TO_RANGE(limit->max))
value = PCT_TO_RANGE(limit->max);
else if( value < PCT_TO_RANGE(-(int)limit->min))
value = PCT_TO_RANGE(-(int)limit->min);
} else {
if (value > INT16_MAX)
value = INT16_MAX;
else if (value < INT16_MIN)
value = INT16_MIN;
}
return value;
}
bool DeltaCalibrationStrategy::calibrate_delta_endstops(Gcode *gcode)
{
float target = 0.03F;
if(gcode->has_letter('I')) target = gcode->get_value('I'); // override default target
if(gcode->has_letter('J')) this->probe_radius = gcode->get_value('J'); // override default probe radius
bool keep = false;
if(gcode->has_letter('K')) keep = true; // keep current settings
gcode->stream->printf("Calibrating Endstops: target %fmm, radius %fmm\n", target, this->probe_radius);
// get probe points
float t1x, t1y, t2x, t2y, t3x, t3y;
std::tie(t1x, t1y, t2x, t2y, t3x, t3y) = getCoordinates(this->probe_radius);
float trimx = 0.0F, trimy = 0.0F, trimz = 0.0F;
if(!keep) {
// zero trim values
if(!set_trim(0, 0, 0, gcode->stream)) return false;
} else {
// get current trim, and continue from that
if (get_trim(trimx, trimy, trimz)) {
gcode->stream->printf("Current Trim X: %f, Y: %f, Z: %f\r\n", trimx, trimy, trimz);
} else {
gcode->stream->printf("Could not get current trim, are endstops enabled?\n");
return false;
}
}
// find the bed, as we potentially have a temporary z probe we don't know how low under the nozzle it is
// so we need to find the initial place that the probe triggers when it hits the bed
float bedht= findBed();
if(isnan(bedht)) return false;
gcode->stream->printf("initial Bed ht is %f mm\n", bedht);
// move to start position
zprobe->home();
zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
// get initial probes
// probe the base of the X tower
int s;
if(!zprobe->doProbeAt(s, t1x, t1y)) return false;
float t1z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T1-0 Z:%1.4f C:%d\n", t1z, s);
// probe the base of the Y tower
if(!zprobe->doProbeAt(s, t2x, t2y)) return false;
float t2z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T2-0 Z:%1.4f C:%d\n", t2z, s);
// probe the base of the Z tower
if(!zprobe->doProbeAt(s, t3x, t3y)) return false;
float t3z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T3-0 Z:%1.4f C:%d\n", t3z, s);
float trimscale = 1.2522F; // empirically determined
auto mm = std::minmax({t1z, t2z, t3z});
if((mm.second - mm.first) <= target) {
gcode->stream->printf("trim already set within required parameters: delta %f\n", mm.second - mm.first);
return true;
}
// set trims to worst case so we always have a negative trim
trimx += (mm.first - t1z) * trimscale;
trimy += (mm.first - t2z) * trimscale;
trimz += (mm.first - t3z) * trimscale;
for (int i = 1; i <= 10; ++i) {
// set trim
if(!set_trim(trimx, trimy, trimz, gcode->stream)) return false;
// home and move probe to start position just above the bed
zprobe->home();
zprobe->coordinated_move(NAN, NAN, -bedht, zprobe->getFastFeedrate(), true); // do a relative move from home to the point above the bed
// probe the base of the X tower
if(!zprobe->doProbeAt(s, t1x, t1y)) return false;
t1z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T1-%d Z:%1.4f C:%d\n", i, t1z, s);
// probe the base of the Y tower
if(!zprobe->doProbeAt(s, t2x, t2y)) return false;
t2z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T2-%d Z:%1.4f C:%d\n", i, t2z, s);
// probe the base of the Z tower
if(!zprobe->doProbeAt(s, t3x, t3y)) return false;
t3z = zprobe->zsteps_to_mm(s);
gcode->stream->printf("T3-%d Z:%1.4f C:%d\n", i, t3z, s);
mm = std::minmax({t1z, t2z, t3z});
if((mm.second - mm.first) <= target) {
gcode->stream->printf("trim set to within required parameters: delta %f\n", mm.second - mm.first);
break;
}
// set new trim values based on min difference
trimx += (mm.first - t1z) * trimscale;
trimy += (mm.first - t2z) * trimscale;
trimz += (mm.first - t3z) * trimscale;
// flush the output
THEKERNEL->call_event(ON_IDLE);
}
if((mm.second - mm.first) > target) {
gcode->stream->printf("WARNING: trim did not resolve to within required parameters: delta %f\n", mm.second - mm.first);
}
return true;
}
void MIXER_ApplyMixer(struct Mixer *mixer, volatile s32 *raw, s32 *orig_value)
{
s32 value;
if (! MIXER_SRC(mixer->src))
return;
if (! switch_is_on(mixer->sw, raw)) {
// Switch is off, so this mixer is not active
return;
}
//1st: Get source value with trim
value = raw[MIXER_SRC(mixer->src)];
//Invert if necessary
if (MIXER_SRC_IS_INV(mixer->src))
value = - value;
//2nd: apply curve
value = CURVE_Evaluate(value, &mixer->curve);
//3rd: apply scalar and offset
value = value * mixer->scalar / 100 + PCT_TO_RANGE(mixer->offset);
//4th: multiplex result
switch(MIXER_MUX(mixer)) {
case MUX_REPLACE:
break;
case MUX_MULTIPLY:
value = raw[mixer->dest + NUM_INPUTS + 1] * value / CHAN_MAX_VALUE;
break;
case MUX_ADD:
value = raw[mixer->dest + NUM_INPUTS + 1] + value;
break;
case MUX_MAX:
value = raw[mixer->dest + NUM_INPUTS + 1] > value
? raw[mixer->dest + NUM_INPUTS + 1]
: value;
break;
case MUX_MIN:
value = raw[mixer->dest + NUM_INPUTS + 1] < value
? raw[mixer->dest + NUM_INPUTS + 1]
: value;
break;
case MUX_DELAY:
{
//value initially represents 20ths of seconds to cover 60-degrees
//convert value to represent #msecs to cover 60-degrees (zero->full)
if (value == 0 || orig_value == NULL) {
value = raw[mixer->dest + NUM_INPUTS + 1];
break;
}
value = abs(RANGE_TO_PCT(value)) * 50;
//rate represents the maximum travel per iteration (once per MEDIUM_PRIORITY_MSEC)
s32 rate = CHAN_MAX_VALUE * MEDIUM_PRIORITY_MSEC / value;
value = raw[mixer->dest + NUM_INPUTS + 1];
if (value - *orig_value > rate)
value = *orig_value + rate;
else if(value - *orig_value < -rate)
value = *orig_value - rate;
}
case MUX_LAST: break;
}
//5th: apply trim
if (MIXER_APPLY_TRIM(mixer))
value = value + (MIXER_SRC_IS_INV(mixer->src) ? -1 : 1) * get_trim(MIXER_SRC(mixer->src));
//Ensure we don't overflow
if (value > INT16_MAX)
value = INT16_MAX;
else if (value < INT16_MIN)
value = INT16_MIN;
raw[mixer->dest + NUM_INPUTS + 1] = value;
}
int cvar_set(const char *name, const char *value)
{
if (name == NULL)
return 0;
struct cvar_assoc *c = cvar_get(name);
if (c == NULL)
return 0;
switch(c->type)
{
case CVAR_INT:
{
t_cvar_int *p = (t_cvar_int *) c->ptr;
if (value != NULL)
{
if (value[0] == '0'
&& (value[1] == 'x' || value[1] == 'X'))
*p = strtoll(value + 2, NULL, 16);
else
*p = strtoll(value, NULL, 10);
}
xprintf("%s = %ld\n", name, *p);
return 1;
}
case CVAR_BOOL:
{
t_cvar_bool *p = (t_cvar_bool *) c->ptr;
if (value != NULL)
{
if (0 == strcasecmp(value, "true"))
*p = 1;
else if (0 == strcasecmp(value, "false"))
*p = 0;
else
*p = strtol(value, NULL, 10) ? 1 : 0;
}
xprintf("%s = %d\n", name, *p);
return 1;
}
case CVAR_STR:
{
t_cvar_str *p = (t_cvar_str *) c->ptr;
if (value != NULL)
{
free(*p);
*p = get_trim(value);
if (!*p[0])
{
free(*p);
*p = NULL;
}
}
xprintf("%s = %s\n", name, *p);
return 1;
}
default:
return 0;
}
}
