Ejemplo n.º 1
0
void abort_move(int pos) { // {{{
	aborting = true;
	//debug("abort pos %d", pos);
	//debug("abort; cf %d rf %d first %d computing_move %d fragments, regenerating %d ticks", current_fragment, running_fragment, first_fragment, computing_move, pos);
	//debug("try aborting move");
	current_fragment = running_fragment;
	//debug("current_fragment = running_fragment; %d", current_fragment);
	//debug("current abort -> %x", current_fragment);
	while (pos < 0) {
		if (current_fragment == first_fragment) {
			pos = 0;
		}
		else {
			current_fragment = (current_fragment + FRAGMENTS_PER_BUFFER - 1) % FRAGMENTS_PER_BUFFER;
			//debug("current_fragment = (current_fragment + FRAGMENTS_PER_BUFFER - 1) %% FRAGMENTS_PER_BUFFER; %d", current_fragment);
			pos += SAMPLES_PER_FRAGMENT;
			running_fragment = current_fragment;
		}
	}
	restore_settings();
	//debug("free abort reset");
	current_fragment_pos = 0;
	computing_move = true;
	while (computing_move && current_fragment_pos < unsigned(pos)) {
		//debug("abort reconstruct %d %d", current_fragment_pos, pos);
		apply_tick();
	}
	if (spaces[0].num_axes > 0)
		cpdebug(0, 0, "ending hwpos %f", arch_round_pos(0, 0, spaces[0].motor[0]->settings.current_pos) + avr_pos_offset[0]);
	// Flush queue.
	settings.queue_start = 0;
	settings.queue_end = 0;
	settings.queue_full = false;
	// Copy settings back to previous fragment.
	store_settings();
	computing_move = false;
	current_fragment_pos = 0;
	for (int s = 0; s < NUM_SPACES; ++s) {
		Space &sp = spaces[s];
		sp.settings.dist[0] = 0;
		sp.settings.dist[1] = 0;
		for (int a = 0; a < sp.num_axes; ++a) {
			//debug("setting axis %d source to %f", a, sp.axis[a]->settings.current);
			if (!std::isnan(sp.axis[a]->settings.current))
				sp.axis[a]->settings.source = sp.axis[a]->settings.current;
			sp.axis[a]->settings.dist[0] = NAN;
			sp.axis[a]->settings.dist[1] = NAN;
		}
	}
	mdebug("aborted move");
	aborting = false;
} // }}}
Ejemplo n.º 2
0
static bool do_steps(double &factor, int32_t current_time) { // {{{
	//debug("steps");
	if (factor <= 0) {
		movedebug("end move");
		return false;
	}
	// Check if more steps should be done, to detect end of move.
	bool have_steps = false;
	for (int s = 0; !have_steps && s < NUM_SPACES; ++s) {
		if (!settings.single && s == 2)
			continue;
		Space &sp = spaces[s];
		for (int m = 0; m < sp.num_motors; ++m) {
			Motor &mtr = *sp.motor[m];
			// Check if there are steps to be done; ignore factor.
			double num = (mtr.settings.current_pos / mtr.steps_per_unit + mtr.settings.target_dist);
			//debug("num: %f ; current: %f", num, mtr.settings.current_pos);
			if (arch_round_pos(s, m, mtr.settings.current_pos) != arch_round_pos(s, m, num * mtr.steps_per_unit)) {
				//debug("have steps %f %f %f", mtr.settings.current_pos, mtr.settings.target_dist, factor);
				have_steps = true;
				break;
			}
			//debug("no steps yet %d %d %f %f %f", s, m, mtr.settings.current_pos, mtr.settings.target_dist, factor);
		}
	}
	for (int s = 0; s < NUM_SPACES; ++s) {
		if (!settings.single && s == 2)
			continue;
		Space &sp = spaces[s];
		for (int a = 0; a < sp.num_axes; ++a) {
			if (!isnan(sp.axis[a]->settings.target)) {
				//debug("add %d %d %f -> %f", s, a, (sp.axis[a]->settings.target - sp.axis[a]->settings.current) * factor, sp.axis[a]->settings.current);
				sp.axis[a]->settings.current += (sp.axis[a]->settings.target - sp.axis[a]->settings.current) * factor;
			}
		}
	}
	if (factor < 1) {
		// Recalculate steps; ignore resulting factor.
		double dummy_factor = 1;
		//debug("redo steps %d", current_fragment);
		for (int s = 0; s < NUM_SPACES; ++s) {
			if (!settings.single && s == 2)
				continue;
			Space &sp = spaces[s];
			for (int a = 0; a < sp.num_axes; ++a) {
				if (!isnan(sp.axis[a]->settings.target))
					sp.axis[a]->settings.target = sp.axis[a]->settings.current;
			}
			move_axes(&sp, settings.last_time, dummy_factor);
		}
	}
	//debug("do steps %f", factor);
	int32_t the_last_time = settings.last_current_time;
	settings.last_current_time = current_time;
	// Adjust start time if factor < 1.
	if (factor < 1) {
		settings.start_time += (current_time - the_last_time) * ((1 - factor) * .99);
		movedebug("correct: %f %d", factor, int(settings.start_time));
	}
	else
		movedebug("no correct: %f %d", factor, int(settings.start_time));
	settings.last_time = current_time;
#ifdef DEBUG_PATH
	fprintf(stderr, "%d", current_time);
	for (int a = 0; a < spaces[0].num_axes; ++a) {
		if (isnan(spaces[0].axis[a]->settings.target))
			fprintf(stderr, "\t%f", spaces[0].axis[a]->settings.source);
		else
			fprintf(stderr, "\t%f", spaces[0].axis[a]->settings.target);
	}
	fprintf(stderr, "\n");
#endif
	// Move the motors.
	//debug("start move");
	for (int s = 0; s < NUM_SPACES; ++s) {
		if (!settings.single && s == 2)
			continue;
		Space &sp = spaces[s];
		for (int m = 0; m < sp.num_motors; ++m) {
			Motor &mtr = *sp.motor[m];
			if (isnan(mtr.settings.target_dist) || mtr.settings.target_dist == 0) {
				//debug("no %d %d", s, m);
				//if (mtr.target_v == 0)
					//mtr.last_v = 0;
				continue;
			}
			double target = mtr.settings.current_pos / mtr.steps_per_unit + mtr.settings.target_dist * factor;
			cpdebug(s, m, "ccp3 stopping %d target %f lastv %f spm %f tdist %f factor %f frag %d", stopping, target, mtr.settings.last_v, mtr.steps_per_unit, mtr.settings.target_dist, factor, current_fragment);
			double new_cp = target * mtr.steps_per_unit;
			if (arch_round_pos(s, m, mtr.settings.current_pos) != arch_round_pos(s, m, new_cp)) {
				have_steps = true;
				if (!mtr.active) {
					mtr.active = true;
					num_active_motors += 1;
				}
				int diff = arch_round_pos(s, m, new_cp) - arch_round_pos(s, m, mtr.settings.current_pos);
				DATA_SET(s, m, diff);
			}
			//debug("new cp: %d %d %f %d", s, m, new_cp, current_fragment_pos);
			if (!settings.single) {
				for (int mm = 0; mm < spaces[2].num_motors; ++mm) {
					int fm = space_types[spaces[2].type].follow(&spaces[2], mm);
					if (fm < 0)
						continue;
					int fs = fm >> 8;
					fm &= 0x7f;
					if (fs != s || fm != m || (fs == 2 && fm >= mm))
						continue;
					//debug("follow %d %d %d %d %d %f %f", s, m, fs, fm, mm, new_cp, mtr.settings.current_pos);
					spaces[2].motor[mm]->settings.current_pos += new_cp - mtr.settings.current_pos;
				}
			}
			mtr.settings.current_pos = new_cp;
			//cpdebug(s, m, "cp three %f", target);
			mtr.settings.last_v = mtr.settings.target_v * factor;
		}
	}
	current_fragment_pos += 1;
	return have_steps;
} // }}}
Ejemplo n.º 3
0
// Movement handling. {{{
static void check_distance(int sp, int mt, Motor *mtr, double distance, double dt, double &factor) { // {{{
	if (dt == 0) {
		factor = 0;
		return;
	}
	if (isnan(distance) || distance == 0) {
		mtr->settings.target_dist = 0;
		//mtr->last_v = 0;
		return;
	}
	//debug("cd %f %f", distance, dt);
	mtr->settings.target_dist = distance;
	mtr->settings.target_v = distance / dt;
	double v = fabs(mtr->settings.target_v);
	int s = (mtr->settings.target_v < 0 ? -1 : 1);
	// When turning around, ignore limits (they shouldn't have been violated anyway).
	if (mtr->settings.last_v * s < 0) {
		//debug("!");
		mtr->settings.last_v = 0;
	}
	// Limit v.
	if (v > mtr->limit_v) {
		//debug("v %f limit %f", v, mtr->limit_v);
		distance = (s * mtr->limit_v) * dt;
		v = fabs(distance / dt);
	}
	//debug("cd2 %f %f", distance, dt);
	// Limit a+.
	double limit_dv = mtr->limit_a * dt;
	if (v - mtr->settings.last_v * s > limit_dv) {
		//debug("a+ target v %f limit dv %f last v %f s %d", mtr->settings.target_v, limit_dv, mtr->settings.last_v, s);
		distance = (limit_dv * s + mtr->settings.last_v) * dt;
		v = fabs(distance / dt);
	}
	//debug("cd3 %f %f", distance, dt);
	// Limit a-.
	// Distance to travel until end of segment or connection.
	double max_dist = (mtr->settings.endpos - mtr->settings.current_pos / mtr->steps_per_unit) * s;
	// Find distance traveled when slowing down at maximum a.
	// x = 1/2 at²
	// t = sqrt(2x/a)
	// v = at
	// v² = 2a²x/a = 2ax
	// x = v²/2a
	double limit_dist = v * v / 2 / mtr->limit_a;
	//debug("max %f limit %f v %f a %f", max_dist, limit_dist, v, mtr->limit_a);
	if (max_dist > 0 && limit_dist > max_dist) {
		//debug("a- endpos %f limit a %f limit dist %f max dist %f v %f distance %f current pos %f s %d dt %f", mtr->settings.endpos, mtr->limit_a, limit_dist, max_dist, v, distance, mtr->settings.current_pos, s, dt);
		v = sqrt(max_dist * 2 * mtr->limit_a);
		distance = s * v * dt;
	}
	//debug("cd4 %f %f", distance, dt); */
	int steps = arch_round_pos(sp, mt, mtr->settings.current_pos + distance * mtr->steps_per_unit) - round(mtr->settings.current_pos);
	int targetsteps = steps;
	//cpdebug(s, m, "cf %d value %d", current_fragment, value);
	if (settings.probing && steps)
		steps = s;
	else {
		// Maximum depends on number of subfragments.  Be conservative and use 0x7e per subfragment; assume 128 subfragments (largest power of 2 smaller than 10000/75)
		// TODO: 10000 and 75 should follow the actual values for step_time in cdriver and TIME_PER_ISR in firmware.
		int max = 0x7e << 7;
		if (abs(steps) > max) {
			debug("overflow %d from cp %f dist %f steps/mm %f dt %f s %d max %d", steps, mtr->settings.current_pos, distance, mtr->steps_per_unit, dt, s, max);
			steps = max * s;
		}
	}
	if (abs(steps) < abs(targetsteps)) {
		distance = (arch_round_pos(sp, mt, mtr->settings.current_pos) + steps + s * .5 - mtr->settings.current_pos) / mtr->steps_per_unit;
		v = fabs(distance / dt);
	}
	//debug("=============");
	double f = distance / mtr->settings.target_dist;
	//debug("checked %f %f %f %f", mtr->settings.target_dist, distance, factor, f);
	if (f < factor)
		factor = f;
} // }}}
Ejemplo n.º 4
0
static void do_steps() { // {{{
	// Do the steps to arrive at the correct position. Update axis and motor current positions.
	// Set new current position.
	for (int s = 0; s < NUM_SPACES; ++s) {
		if (!settings.single && s == 2)
			continue;
		Space &sp = spaces[s];
		for (int a = 0; a < sp.num_axes; ++a) {
			if (!std::isnan(sp.axis[a]->settings.target))
				sp.axis[a]->settings.current = sp.axis[a]->settings.target;
		}
	}
	// Move the motors.
	//debug("start move");
	for (int s = 0; s < NUM_SPACES; ++s) {
		if (!settings.single && s == 2)
			continue;
		Space &sp = spaces[s];
		for (int m = 0; m < sp.num_motors; ++m) {
			Motor &mtr = *sp.motor[m];
			double target = mtr.settings.target_pos;
			if (std::isnan(target))
				continue;
			cpdebug(s, m, "ccp3 stopping %d target %f frag %d", stopping, target, current_fragment);
			double rounded_cp = arch_round_pos(s, m, mtr.settings.current_pos);
			double rounded_new_cp = arch_round_pos(s, m, target);
			if (rounded_cp != rounded_new_cp) {
				if (!mtr.active) {
					mtr.active = true;
					num_active_motors += 1;
					//debug("activating motor %d %d", s, m);
				}
				int diff = round((rounded_new_cp - rounded_cp) * mtr.steps_per_unit);
				if (diff > 0x7f) {
					debug("Error: trying to send more than 127 steps: %d", diff);
					int adjust = diff - 0x7f;
					settings.hwtime -= settings.hwtime_step * (adjust / diff);
					diff = 0x7f;
					target -= adjust;
					mtr.settings.target_pos = target;
				}
				if (diff < -0x80) {
					debug("Error: trying to send more than 128 steps: %d", -diff);
					int adjust = diff + 0x80;
					settings.hwtime -= settings.hwtime_step * (adjust / diff);
					diff = -0x80;
					target -= adjust;
					mtr.settings.target_pos = target;
				}
				//debug("sending %d %d steps %d", s, m, diff);
				DATA_SET(s, m, diff);
			}
			//debug("new cp: %d %d %f %d", s, m, target, current_fragment_pos);
			if (!settings.single) {
				for (int mm = 0; mm < spaces[2].num_motors; ++mm) {
					int fm = space_types[spaces[2].type].follow(&spaces[2], mm);
					if (fm < 0)
						continue;
					int fs = fm >> 8;
					fm &= 0x7f;
					if (fs != s || fm != m || (fs == 2 && fm >= mm))
						continue;
					//debug("follow %d %d %d %d %d %f %f", s, m, fs, fm, mm, target, mtr.settings.current_pos);
					spaces[2].motor[mm]->settings.current_pos += target - mtr.settings.current_pos;
				}
			}
			mtr.settings.current_pos = target;
			cpdebug(s, m, "cp three %f", target);
			mtr.settings.last_v = mtr.settings.target_v;
		}
	}
	int pattern_pos = (settings.hwtime - settings.hwtime_step / 2) / settings.hwtime_step;
	//debug("time %d step %d pattern pos %d size %d", settings.hwtime, settings.hwtime_step, pattern_pos, settings.pattern_size);
	if (pattern_pos < settings.pattern_size * 8) {
		if (!pattern.active) {
			pattern.active = true;
			num_active_motors += 1;
			//debug("activating pattern");
		}
		//debug("set pattern for %d at %d to %d", current_fragment, current_fragment_pos, settings.pattern[pattern_pos]);
		PATTERN_SET(settings.pattern[pattern_pos]);
	}
	current_fragment_pos += 1;
	if (current_fragment_pos >= SAMPLES_PER_FRAGMENT) {
		//debug("sending because fragment full (normal) %d %d %d", computing_move, current_fragment_pos, BYTES_PER_FRAGMENT);
		send_fragment();
	}
} // }}}
Ejemplo n.º 5
0
static void check_distance(int sp, int mt, Motor *mtr, Motor *limit_mtr, double distance, double dt, double &factor) { // {{{
	// Check motor limits for motor (sp,mt), which is mtr. For followers, limit_mtr is set to its leader unless settings.single is true.
	// distance is the distance to travel; dt is the time, factor is lowered if needed.
	//debug("check distance %d %d dist %f t %f current %f time step %d", sp, mt, distance, dt, mtr->settings.current_pos, settings.hwtime_step);
	if (dt == 0) {
		debug("check distance for 0 time");
		factor = 0;
		return;
	}
	if (std::isnan(distance) || distance == 0) {
		mtr->settings.last_v = 0;
		return;
	}
	double orig_distance = distance;
	//debug("cd %f %f", distance, dt);
	mtr->settings.target_v = distance / dt;
	double v = std::fabs(mtr->settings.target_v);
	int s = (mtr->settings.target_v < 0 ? -1 : 1);
	// When turning around, ignore limits (they shouldn't have been violated anyway).
	if (mtr->settings.last_v * s < 0) {
		//debug("direction change");
		mtr->settings.last_v = 0;
	}
	// Limit v.
	if (v > limit_mtr->limit_v) {
		debug("%d %d v %f limit %f dist %f t %f current %f factor %f", sp, mt, v, limit_mtr->limit_v, distance, dt, mtr->settings.current_pos, settings.factor);
		distance = (s * limit_mtr->limit_v) * dt;
		v = std::fabs(distance / dt);
	}
	// Limit a+.
	double limit_dv = limit_mtr->limit_a * dt;
	if (v - mtr->settings.last_v * s > limit_dv) {
		debug("a+ %d %d target v %f limit dv %f last v %f s %d current %f factor %f", sp, mt, mtr->settings.target_v, limit_dv, mtr->settings.last_v, s, mtr->settings.current_pos, settings.factor);
		distance = (limit_dv * s + mtr->settings.last_v) * dt;
		v = std::fabs(distance / dt);
	}
	int steps = round((arch_round_pos(sp, mt, mtr->settings.current_pos + distance) - arch_round_pos(sp, mt, mtr->settings.current_pos)) * mtr->steps_per_unit);
	int targetsteps = steps;
	//cpdebug(s, m, "cf %d value %d", current_fragment, value);
	if (settings.probing && steps)
		steps = s;
	else {
		int max = 0x78;	// Don't use 0x7f, because limiting isn't accurate.
		if (abs(steps) > max) {
			debug("overflow %d from cp %f dist %f steps/mm %f dt %f s %d max %d", steps, mtr->settings.current_pos, distance, mtr->steps_per_unit, dt, s, max);
			steps = max * s;
		}
	}
	if (abs(steps) < abs(targetsteps)) {
		distance = arch_round_pos(sp, mt, mtr->settings.current_pos) + (steps + s * .5) / mtr->steps_per_unit - mtr->settings.current_pos;
		v = std::fabs(distance / dt);
		debug("new distance = %f, old = %f", distance, orig_distance);
	}
	//debug("=============");
	double f = distance / orig_distance;
	//debug("checked %d %d src %f target %f target dist %f new dist %f old factor %f new factor %f", sp, mt, mtr->settings.current_pos, mtr->settings.target_pos, orig_distance, distance, factor, f);
	if (f < factor)
		factor = f;
	// Store adjusted value of v.
	mtr->settings.target_v = s * v;
} // }}}