예제 #1
0
파일: motion.cpp 프로젝트: item28/tosqa-ssb
// 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
}
예제 #2
0
void
CQRealSpin::
updateStep()
{
  int pos = cursorPosition();

  double s = posToStep(pos);

  if (fabs(step() - s) >= 1E-6) {
    step_ = s;

    emit stepChanged(step());
  }
}