// move to the given setpoint
void motionTarget (const Setpoint& s) {
LPC_TMR32B0->MR3 = ~0; // set timer count to max, so it won't change now
// determine target step to end on, rounding from a 24.8 to a 24.0 int
int target = posToStep(s.position);
if (s.relative)
target += motionPosition();
// enforce soft position limits
if (target < posToStep(motion.params.minPos)) {
target = posToStep(motion.params.minPos);
motion.errors |= 1<<F_HITMIN; // bumped into min position limit
}
if (target > posToStep(motion.params.maxPos)) {
target = posToStep(motion.params.maxPos);
motion.errors |= 1<<F_HITMAX; // bumped into max position limit
}
// set the next target
int stepDiff = target - motionPosition();
motion.direction = stepDiff > 0 ? 1 : -1;
motion.stepsToGo = stepDiff * motion.direction;
motion.targetStep = target;
if (motion.stepsToGo > 0) {
uint32_t clocks = MHZ * USPT * s.time + motion.residue;
uint32_t rate = (clocks + motion.stepsToGo / 2) / motion.stepsToGo;
motion.residue = clocks - rate * motion.stepsToGo;
if (rate < MHZ * 10) { // enforce a soft timer rate limit
rate = MHZ * 10; // at least 10 µs, i.e. max 100 KHz
motion.errors |= 1<<F_TOOFAST; // rate limiting took place
}
// TODO: could check that rate > TC, i.e. no overrun has occurred
LPC_TMR32B0->MR3 = rate;
palWritePad(GPIO1, GPIO1_MOTOR_DIR, stepDiff > 0 ? 1 : 0);
palClearPad(GPIO3, GPIO3_MOTOR_EN); // active low, on
LPC_TMR32B0->TCR = 1; // start timer if it wasn't running
while (motion.stepsToGo > 0) // move!
chThdYield(); // uses idle polling
} else if (s.time > 0) {
LPC_TMR32B0->TCR = 0; // stop timer
chThdSleepMilliseconds(s.time); // dwell
}
if (s.velocity == 0)
palSetPad(GPIO3, GPIO3_MOTOR_EN); // active low, off
}
void
CQRealSpin::
updateStep()
{
int pos = cursorPosition();
double s = posToStep(pos);
if (fabs(step() - s) >= 1E-6) {
step_ = s;
emit stepChanged(step());
}
}
