| 0,0 → 1,765 |
|---|
| /* |
| * Copyright (C) 2004 Darren Hutchinson (dbh@gbdt.com.au) |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU Library General Public License as published by |
| * the Free Software Foundation; either version 2 of the License, or (at your |
| * option) any later version. |
| * |
| * This program is distributed in the hope that it will be useful, but |
| * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY |
| * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Library General Public |
| * License for more details. |
| * |
| * You should have received a copy of the GNU Library General Public License |
| * along with this software; see the file COPYING. If not, write to |
| * the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, |
| * MA 02111-1307, USA. |
| * |
| * $Id: stepper.c,v 1.9 2004/04/05 06:42:15 dbh Exp $ |
| */ |
| |
| /* This file converts the RA and DEC speed indications into drive values for |
| * the stepper motor coils. |
| */ |
| |
| #include <inttypes.h> |
| #include <avr/io.h> |
| #include <avr/interrupt.h> |
| |
| #include "eq6.h" |
| #include "combine.h" |
| |
| #include "stepper.h" |
| |
| int8_t trackingRate = 0; |
| uint8_t transRatio = DEF_TRANS_RATIO; |
| |
| #define STEPS_PER_CYCLE 32L /* Steps per cycle (complete set of phases) */ |
| #define CYCLES_PER_ROTN 12L /* Cycles per stepper motor rotation */ |
| #define SIDERIAL_LCM (long)(3 * 16) /* Divides to give all speeds */ |
| #define WORM_RATIO 180L /* Tooth on worm gear */ |
| |
| #define SECS_PER_SDAY 86164L /* 23h, 56m, 4s [Sidereal] */ |
| #define SECS_PER_DAY 86400L /* 24h, 0m, 0s [Solar] */ |
| #define SECS_PER_LDAY 89309L /* 1 + 1/27.3 sidereal days */ |
| |
| /* Structure holding information used to generate stepper pulses |
| * that generate motion at the siderial, solar, and lunar tracking |
| * rates |
| */ |
| struct trackRate_s |
| { |
| uint16_t div; // tint |
| uint16_t adj; // add/drop one int every .... |
| uint8_t doDropInt; // drop ints if true, add extra ints if false |
| } trackRateTable[3]; |
| |
| /* Define the stepping table. This defines the excitation to be used |
| * over a complete "cycle" of the stepper motor |
| * |
| * These are signed, four bit values. Coil 1 is the LSN, Coil 2 is the MSN |
| */ |
| |
| #if 1 |
| /* Step table. Values scaled such that one coil is always fully driven. |
| * Gives lots of torque, but the actual travel is lumpy |
| */ |
| uint8_t microTable[STEPS_PER_CYCLE] = |
| { |
| _EX_ENTRY(EX_0, EX_P_1), _EX_ENTRY(EX_P_0_2, EX_P_1), |
| _EX_ENTRY(EX_P_0_4, EX_P_1), _EX_ENTRY(EX_P_0_67, EX_P_1), |
| _EX_ENTRY(EX_P_1, EX_P_1), _EX_ENTRY(EX_P_1, EX_P_0_67), |
| _EX_ENTRY(EX_P_1, EX_P_0_4), _EX_ENTRY(EX_P_1, EX_P_0_2), |
| |
| _EX_ENTRY(EX_P_1, EX_0), _EX_ENTRY(EX_P_1, EX_M_0_2), |
| _EX_ENTRY(EX_P_1, EX_M_0_4), _EX_ENTRY(EX_P_1, EX_M_0_67), |
| _EX_ENTRY(EX_P_1, EX_M_1), _EX_ENTRY(EX_P_0_67, EX_M_1), |
| _EX_ENTRY(EX_P_0_4, EX_M_1), _EX_ENTRY(EX_P_0_2, EX_M_1), |
| |
| _EX_ENTRY(EX_0, EX_M_1), _EX_ENTRY(EX_M_0_2, EX_M_1), |
| _EX_ENTRY(EX_M_0_4, EX_M_1), _EX_ENTRY(EX_M_0_67, EX_M_1), |
| _EX_ENTRY(EX_M_1, EX_M_1), _EX_ENTRY(EX_M_1, EX_M_0_67), |
| _EX_ENTRY(EX_M_1, EX_M_0_4), _EX_ENTRY(EX_M_1, EX_M_0_2), |
| |
| _EX_ENTRY(EX_M_1, EX_0), _EX_ENTRY(EX_M_1, EX_P_0_2), |
| _EX_ENTRY(EX_M_1, EX_P_0_4), _EX_ENTRY(EX_M_1, EX_P_0_67), |
| _EX_ENTRY(EX_M_1, EX_P_1), _EX_ENTRY(EX_M_0_67, EX_P_1), |
| _EX_ENTRY(EX_M_0_4, EX_P_1), _EX_ENTRY(EX_M_0_2, EX_P_1), |
| }; |
| #else |
| /* Conventional microstep table. Torque vector with magnitude 1. Gives |
| * less torque that the first table, but the change in smoothness doesn't |
| * seem to be worth the loss of torque |
| */ |
| uint8_t microTable[STEPS_PER_CYCLE] = |
| { |
| _EX_ENTRY(EX_0, EX_P_1), _EX_ENTRY(EX_P_0_2, EX_P_1), |
| _EX_ENTRY(EX_P_0_2, EX_P_1), _EX_ENTRY(EX_P_0_4, EX_P_0_67), |
| _EX_ENTRY(EX_P_0_67, EX_P_0_67), _EX_ENTRY(EX_P_0_67, EX_P_0_4), |
| _EX_ENTRY(EX_P_1, EX_P_0_2), _EX_ENTRY(EX_P_1, EX_P_0_2), |
| |
| _EX_ENTRY(EX_P_1, EX_0), _EX_ENTRY(EX_P_1, EX_M_0_2), |
| _EX_ENTRY(EX_P_1, EX_M_0_2), _EX_ENTRY(EX_P_0_67, EX_M_0_4), |
| _EX_ENTRY(EX_P_0_67, EX_M_0_67), _EX_ENTRY(EX_P_0_4, EX_M_0_67), |
| _EX_ENTRY(EX_P_0_2, EX_M_1), _EX_ENTRY(EX_P_0_2, EX_M_1), |
| |
| _EX_ENTRY(EX_0, EX_M_1), _EX_ENTRY(EX_M_0_2, EX_M_1), |
| _EX_ENTRY(EX_M_0_2, EX_M_1), _EX_ENTRY(EX_M_0_4, EX_M_0_67), |
| _EX_ENTRY(EX_M_0_67, EX_M_0_67), _EX_ENTRY(EX_M_0_67, EX_M_0_4), |
| _EX_ENTRY(EX_M_1, EX_M_0_2), _EX_ENTRY(EX_M_1, EX_M_0_2), |
| |
| _EX_ENTRY(EX_M_1, EX_0), _EX_ENTRY(EX_M_1, EX_P_0_2), |
| _EX_ENTRY(EX_M_1, EX_P_0_2), _EX_ENTRY(EX_M_0_67, EX_P_0_4), |
| _EX_ENTRY(EX_M_0_67, EX_P_0_67), _EX_ENTRY(EX_M_0_4, EX_P_0_67), |
| _EX_ENTRY(EX_M_0_2, EX_P_1), _EX_ENTRY(EX_M_0_2, EX_P_1) |
| }; |
| #endif /* 0 */ |
| |
| uint8_t halfTable[STEPS_PER_CYCLE] = |
| { |
| _EX_ENTRY(EX_P_1, EX_P_1), _EX_ENTRY(EX_P_1, EX_P_1), |
| _EX_ENTRY(EX_P_1, EX_P_1), _EX_ENTRY(EX_P_1, EX_P_1), |
| |
| _EX_ENTRY(EX_P_1, EX_0), _EX_ENTRY(EX_P_1, EX_0), |
| _EX_ENTRY(EX_P_1, EX_0), _EX_ENTRY(EX_P_1, EX_0), |
| |
| _EX_ENTRY(EX_P_1, EX_M_1), _EX_ENTRY(EX_P_1, EX_M_1), |
| _EX_ENTRY(EX_P_1, EX_M_1), _EX_ENTRY(EX_P_1, EX_M_1), |
| |
| _EX_ENTRY(EX_0, EX_M_1), _EX_ENTRY(EX_0, EX_M_1), |
| _EX_ENTRY(EX_0, EX_M_1), _EX_ENTRY(EX_0, EX_M_1), |
| |
| _EX_ENTRY(EX_M_1, EX_M_1), _EX_ENTRY(EX_M_1, EX_M_1), |
| _EX_ENTRY(EX_M_1, EX_M_1), _EX_ENTRY(EX_M_1, EX_M_1), |
| |
| _EX_ENTRY(EX_M_1, EX_0), _EX_ENTRY(EX_M_1, EX_0), |
| _EX_ENTRY(EX_M_1, EX_0), _EX_ENTRY(EX_M_1, EX_0), |
| |
| _EX_ENTRY(EX_M_1, EX_P_1), _EX_ENTRY(EX_M_1, EX_P_1), |
| _EX_ENTRY(EX_M_1, EX_P_1), _EX_ENTRY(EX_M_1, EX_P_1), |
| |
| _EX_ENTRY(EX_0, EX_P_1), _EX_ENTRY(EX_0, EX_P_1), |
| _EX_ENTRY(EX_0, EX_P_1), _EX_ENTRY(EX_0, EX_P_1), |
| }; |
| |
| uint8_t fullTable[STEPS_PER_CYCLE] = |
| { |
| _EX_ENTRY(EX_P_1, EX_P_1), _EX_ENTRY(EX_P_1, EX_P_1), |
| _EX_ENTRY(EX_P_1, EX_P_1), _EX_ENTRY(EX_P_1, EX_P_1), |
| _EX_ENTRY(EX_P_1, EX_P_1), _EX_ENTRY(EX_P_1, EX_P_1), |
| _EX_ENTRY(EX_P_1, EX_P_1), _EX_ENTRY(EX_P_1, EX_P_1), |
| |
| _EX_ENTRY(EX_P_1, EX_M_1), _EX_ENTRY(EX_P_1, EX_M_1), |
| _EX_ENTRY(EX_P_1, EX_M_1), _EX_ENTRY(EX_P_1, EX_M_1), |
| _EX_ENTRY(EX_P_1, EX_M_1), _EX_ENTRY(EX_P_1, EX_M_1), |
| _EX_ENTRY(EX_P_1, EX_M_1), _EX_ENTRY(EX_P_1, EX_M_1), |
| |
| _EX_ENTRY(EX_M_1, EX_M_1), _EX_ENTRY(EX_M_1, EX_M_1), |
| _EX_ENTRY(EX_M_1, EX_M_1), _EX_ENTRY(EX_M_1, EX_M_1), |
| _EX_ENTRY(EX_M_1, EX_M_1), _EX_ENTRY(EX_M_1, EX_M_1), |
| _EX_ENTRY(EX_M_1, EX_M_1), _EX_ENTRY(EX_M_1, EX_M_1), |
| |
| _EX_ENTRY(EX_M_1, EX_P_1), _EX_ENTRY(EX_M_1, EX_P_1), |
| _EX_ENTRY(EX_M_1, EX_P_1), _EX_ENTRY(EX_M_1, EX_P_1), |
| _EX_ENTRY(EX_M_1, EX_P_1), _EX_ENTRY(EX_M_1, EX_P_1), |
| _EX_ENTRY(EX_M_1, EX_P_1), _EX_ENTRY(EX_M_1, EX_P_1) |
| }; |
| |
| /* Setup the table of divisors of the siderial interrupt use to |
| * achieve the required tracking rate. |
| */ |
| struct |
| { |
| uint8_t divisor; // Siderial interrupts per step |
| uint8_t flags; // Control flags |
| #define USE_RELAY 0 // Activate the magic relay [RA only] |
| #define USE_MICRO 1 // Use the microstep table |
| } rateConvert[] = |
| { |
| [SPEED_0_X] = {1, _BV(USE_MICRO)}, // Special value |
| [SPEED_0_33_X] = {3 * SIDERIAL_LCM, _BV(USE_MICRO)}, |
| [SPEED_0_67_X] = {(3 * SIDERIAL_LCM) / 2, _BV(USE_MICRO)}, |
| [SPEED_1_X] = {SIDERIAL_LCM, _BV(USE_MICRO)}, |
| [SPEED_1_33_X] = {(3 * SIDERIAL_LCM) / 4, _BV(USE_MICRO)}, |
| [SPEED_1_67_X] = {(3 * SIDERIAL_LCM) / 5, _BV(USE_MICRO)}, |
| [SPEED_2_X] = {SIDERIAL_LCM / 2, _BV(USE_MICRO) | _BV(USE_RELAY)}, |
| [SPEED_4_X] = {SIDERIAL_LCM / 4, _BV(USE_MICRO) | _BV(USE_RELAY)}, |
| [SPEED_8_X] = {SIDERIAL_LCM / 8, _BV(USE_MICRO) | _BV(USE_RELAY)}, |
| [SPEED_16_X] = {SIDERIAL_LCM / 16, _BV(USE_RELAY)}, |
| |
| [SPEED_SPIN] = {SIDERIAL_LCM / 16, 0} |
| }; |
| |
| /* Create the instance of the stepper excitation info |
| */ |
| struct excitation_s raExcitation; |
| struct excitation_s decExcitation; |
| |
| /* Define instances of stepper state info |
| */ |
| struct stepState_s raState; |
| struct stepState_s decState; |
| uint8_t doHalfStep = 0; |
| |
| /* Info for tracking rate correction */ |
| uint16_t adjCtr; |
| uint16_t adjLimit; |
| uint16_t doDropInt; |
| |
| /* stepperInit() initializes the state of the stepper code. |
| * |
| * The current implementation uses a single 16-bit timer with a fixed |
| * period shared between RA and DEC. |
| * |
| * Passed: |
| * Nothing |
| * |
| * Returns: |
| * Nothing |
| * |
| * Notes: |
| * An alternate implementation would use a pair of 16 bit timers with |
| * their timeouts set to the step period. This would minimize the |
| * number of interrupts, but would take an extra timer. |
| * |
| * The current implementation is preferred until we're sure the extra |
| * timer isn't needed elsewhere or until there is a performance |
| * problem caused by the extra interrupt load caused by having |
| * multiple interrupts per step. |
| */ |
| void |
| stepperInit(void) |
| { |
| /* Initialize the excitation state */ |
| raExcitation.excitation = EX_0; |
| raExcitation.useRelay = 0; |
| raState.pExcite = &raExcitation; |
| |
| decExcitation.excitation = EX_0; |
| decState.pExcite = &decExcitation; |
| |
| /* Initialize the siderial rate timer */ |
| |
| TIMSK |= _BV(OCIE1A); |
| } |
| |
| |
| /* calculateRateEntry() creates an entry in the rate table |
| * |
| * Passed: |
| * pEntry Pointer to entry |
| * transRatio Transmission (gearbox) ratio |
| * secPerDay Seconds per sideral/lunar/solar dat |
| * |
| * Returns: |
| * nothing |
| */ |
| static void |
| calculateRateEntry( struct trackRate_s *pEntry, |
| uint8_t transRatio, |
| uint32_t secsPerDay) |
| { |
| /* To do this calculation would (without optimization) would need about |
| * 40 bit arithmetic, which isn't available in this copmiler. |
| * |
| * To get the required precision down to below 32 bits the |
| * numerator and denominator are divided through by 1280. |
| * |
| * This gives an exact result for 8/16 MHz with a 180 tooth wormgear |
| * |
| * The formula gives the clock divisor for the siderial clock interrupt |
| * divisor: |
| * |
| * (CLK_RATE * SECS_PER_SDAY) / (MECH_DIV * STEPS_PER_CYCLE * SIDERIAL_LCM) |
| * |
| * This, by itself, does not give great accuracy because the divisor is |
| * an integer. With a typical divisor of about 1500 there is a maximum error |
| * of about 1 / 3000 (0.033%). |
| * |
| * For most purposes this should be accurate enough, but the accuracy can |
| * be improved by adding or dropping the occasional interrupt. |
| * |
| * This function calculates both the divisor and how often to |
| * drop an timer interrupt, or to insert an extra one to improve the |
| * timer accuracy |
| */ |
| #define CLK_FACTOR 1280L // Common divisor for numerator & divisor |
| #define WORM_CLK_LCM (WORM_RATIO * CYCLES_PER_ROTN * SIDERIAL_LCM) |
| #define SCALED_CLK (CLK_RATE / CLK_FACTOR) |
| |
| #define MIN_DIV 1000 /* Minimum allowed divisor to avoid |
| * interrupt saturation |
| */ |
| |
| long top; |
| long bottom; |
| long div; // Clock divisor for interrupt generation |
| long adj; // Add/drop adjustment |
| long adj_denom; |
| |
| top = SCALED_CLK * secsPerDay; |
| bottom = (WORM_CLK_LCM / CLK_FACTOR) * transRatio * STEPS_PER_CYCLE; |
| |
| /* Calculate divisor, round to nearest integer */ |
| div = (top + (bottom / 2)) / bottom; |
| |
| /* Calculate adjustment */ |
| adj = SCALED_CLK * secsPerDay; |
| adj_denom = (div * bottom) - (SCALED_CLK * secsPerDay); |
| |
| adj /= adj_denom; |
| |
| /* Fill in the entry */ |
| pEntry->div = (div > MIN_DIV) ? div : MIN_DIV; |
| |
| if (adj >= 0) |
| { |
| pEntry->doDropInt = 0; |
| pEntry->adj = (adj >= (1L << 16)) ? 0 : adj; |
| } |
| else |
| { |
| pEntry->doDropInt = 1; |
| pEntry->adj = (adj <= -(1L << 16)) ? 0 : -adj; |
| } |
| } |
| |
| /* setupRateTable() fills the tracking rate table with values |
| * that are correct for the transmission ratio of the system. |
| * |
| * Passed |
| * transRatio Transmission (gearbox) ratio |
| * |
| * Returns |
| * nothing |
| */ |
| void |
| setupRateTable(uint8_t transRatio) |
| { |
| calculateRateEntry(&trackRateTable[0], transRatio, SECS_PER_SDAY); |
| calculateRateEntry(&trackRateTable[1], transRatio, SECS_PER_DAY); |
| calculateRateEntry(&trackRateTable[2], transRatio, SECS_PER_LDAY); |
| } |
| |
| /* setTrackRate() sets the tracking rate used by the stepper module. |
| * |
| * Passed: |
| * rate The tracking rate (index) |
| * |
| * Returns: |
| * Nothing |
| * |
| * Note: |
| * If an illegal rate is entered the current rate will not be changed |
| */ |
| void |
| setTrackRate(int8_t rate) |
| { |
| /* If the track rate is <0 then disable siderial rate use in |
| * combine.c and return, leaving the current clock steup |
| */ |
| trackingRate = rate; |
| |
| if (rate < 0) |
| { |
| noTrack = 1; |
| return; |
| } |
| |
| /* Do nothing if the rate is not supported */ |
| if (rate >= (sizeof(trackRateTable) / sizeof(struct trackRate_s))) |
| return; |
| |
| /* Enable tracking */ |
| noTrack = 0; |
| |
| /* Update the tracking rate timer */ |
| OCR1A = trackRateTable[rate].div; |
| TCNT1 = 0; |
| TCCR1A = 0; |
| TCCR1B = _BV(WGM12) | _BV(CS10); |
| |
| /* Update adjustment data */ |
| adjCtr = 0; |
| adjLimit = trackRateTable[rate].adj; |
| doDropInt = trackRateTable[rate].doDropInt; |
| } |
| |
| /* setSpeed() is called by by the combiner to set the requested speed |
| * for the axis |
| * |
| * Passed: |
| * pState Axis state |
| * rate Requested rate |
| * |
| * Returns: |
| * Nothing |
| * |
| * Notes: |
| * setRaSpeed() and setDecSpeed() are wrappers used by the combiner |
| */ |
| static void |
| setSpeed(struct stepState_s *pState, int8_t speed) |
| { |
| /* If the current speed is zero then start the clock */ |
| if (pState->clkDivRatio == 0) |
| pState->clkDivRatio = 1; // Almost immediate clock |
| |
| pState->reqSpeed = speed; |
| |
| } |
| |
| void |
| setRaSpeed(int8_t speed) |
| { |
| setSpeed(&raState, speed); |
| } |
| |
| void |
| setDecSpeed(int8_t speed) |
| { |
| setSpeed(&decState, speed); |
| } |
| |
| /* setTickRate() is called by the state machine to set the clock interrupt |
| * rate. |
| * |
| * Passed |
| * pState The axis state |
| * tickRate The clock rate to set |
| * |
| * Returns |
| * nothing |
| */ |
| void |
| setTickRate(struct stepState_s *pState, uint8_t tickRate) |
| { |
| pState->clkDivRatio = rateConvert[tickRate].divisor; |
| } |
| |
| /* stepperProcess is the state machine that makes this whole thing |
| * work! It is executed each axis interrupt to run the state machine |
| * that handles operation and backlash processing. |
| * |
| * Like the other state machines in the program it takes advantage |
| * of the GNU computed goto to operate very efficiently. |
| * |
| * Passed |
| * pState The axis state |
| * |
| * Returns |
| * Nothing |
| */ |
| #define _GET_TABLE(f) ((f) ? (doHalfStep ? halfTable : microTable) : fullTable) |
| |
| void |
| stepperProcess(struct stepState_s *pState) |
| { |
| // Step up the initial state pointer |
| if (pState->pState == 0) |
| pState->pState = &&enter_idle_pos; |
| |
| /* Make sure both finPos and finNeg are not set - that will |
| * lead to a loop as the code tries to meet both! |
| */ |
| if (pState->finPos && pState->finNeg) |
| pState->finPos = pState->finNeg = 0; |
| |
| // Jump to the current state |
| goto *pState->pState; |
| |
| /* There are six states in the machine |
| * |
| * - idle_pos Idle (last move in positive direction) |
| * - spin_pos Taking up backlash in positive direction |
| * - move_pos Moving in the positive direction |
| * |
| * There are "negative" versions of these states. |
| * |
| * Just to make things simple we use the "idle" state as a central |
| * decision point. |
| */ |
| |
| enter_idle_pos: |
| /* We're about to move into the idle_pos state. We end up here if |
| * we're stopping or changing direction |
| */ |
| if (pState->reqSpeed == SPEED_0_X) |
| { |
| /* We're going to stop - if we're in the correct direction then |
| * stop, else start spinning in the other direction |
| */ |
| if (pState->finNeg) |
| goto enter_spin_neg; |
| else |
| { |
| /* Stop now! */ |
| setTickRate(pState, SPEED_0_X); |
| pState->pExcite->excitation = EX_0; |
| pState->pExcite->useRelay = 0; |
| |
| // For this state just call the entry point each interrupt |
| pState->pState = &&enter_idle_pos; |
| } |
| } |
| else if (pState->reqSpeed > SPEED_0_X) |
| { |
| /* We're now moving in the positive direction. As we are |
| * already engaged in the positive direction we can start |
| * running |
| */ |
| goto enter_move_pos; |
| } |
| else |
| { |
| /* Must be a negative move direction. Take up the backlash |
| * in the negative direction |
| */ |
| goto enter_spin_neg; |
| } |
| |
| return; |
| |
| enter_idle_neg: |
| /* We're about to move into the idle_neg state. We end up here if |
| * we're stopping or changing direction |
| */ |
| if (pState->reqSpeed == SPEED_0_X) |
| { |
| /* We're going to stop - if we're in the correct direction then |
| * stop, else start spinning in the other direction |
| */ |
| if (pState->finPos) |
| goto enter_spin_pos; |
| else |
| { |
| /* Stop now! */ |
| setTickRate(pState, SPEED_0_X); |
| pState->pExcite->excitation = EX_0; |
| pState->pExcite->useRelay = 0; |
| |
| // For this state just call the entry point each interrupt |
| pState->pState = &&enter_idle_neg; |
| } |
| } |
| else if (pState->reqSpeed < SPEED_0_X) |
| { |
| /* We're now moving in the negative direction. As we are |
| * already engaged in the negative direction we can start |
| * running |
| */ |
| goto enter_move_neg; |
| } |
| else |
| { |
| /* Must be a positive move direction. Take up the backlash |
| * in the positive direction |
| */ |
| goto enter_spin_pos; |
| } |
| return; |
| |
| enter_spin_pos: |
| /* Spin in the positive direction to take up backlash in the |
| * gear chain |
| */ |
| if (pState->backlash == 0) |
| { |
| /* No backlash - go to the idle_pos state which will take us |
| * to the correct place |
| */ |
| goto enter_idle_pos; |
| } |
| else |
| { |
| uint8_t flags = rateConvert[SPEED_SPIN].flags; |
| |
| /* There is a backlash setting - get ready to spin! */ |
| pState->count = 0; |
| |
| setTickRate(pState, SPEED_SPIN); |
| pState->pTable = _GET_TABLE(flags & _BV(USE_MICRO)); |
| pState->pExcite->useRelay = flags & _BV(USE_RELAY); |
| pState->pState = &&run_spin_pos; |
| |
| // Fall through to run the spin state |
| } |
| |
| run_spin_pos: |
| // Update excitation value |
| pState->pExcite->excitation = pState->pTable[pState->stepCtr]; |
| pState->stepCtr = (pState->stepCtr + 1) & (STEPS_PER_CYCLE - 1); |
| |
| /* Check the count. If we've spun enough then go back to the |
| * idle_pos state which will send us the right way |
| */ |
| if (++pState->count > pState->backlash) |
| goto enter_idle_pos; |
| return; |
| |
| enter_spin_neg: |
| /* Spin in the negative direction to take up backlash in the |
| * gear chain |
| */ |
| if (pState->backlash == 0) |
| { |
| /* No backlash - go to the idle_neg state which will take us |
| * to the correct place |
| */ |
| goto enter_idle_neg; |
| } |
| else |
| { |
| uint8_t flags = rateConvert[SPEED_SPIN].flags; |
| |
| /* There is a backlash setting - get ready to spin! */ |
| pState->count = 0; |
| |
| setTickRate(pState, SPEED_SPIN); |
| pState->pTable = _GET_TABLE(flags & _BV(USE_MICRO)); |
| pState->pExcite->useRelay = flags & _BV(USE_RELAY); |
| pState->pState = &&run_spin_neg; |
| |
| // Fall through to run the spin state |
| } |
| |
| run_spin_neg: |
| // Update excitation value |
| pState->pExcite->excitation = pState->pTable[pState->stepCtr]; |
| pState->stepCtr = (pState->stepCtr - 1) & (STEPS_PER_CYCLE - 1); |
| |
| /* Check the count. If we've spun enough then go back to the |
| * idle_neg state which will send us the right way |
| */ |
| if (++pState->count > pState->backlash) |
| goto enter_idle_neg; |
| |
| return; |
| |
| enter_move_pos: |
| /* Start moving in the positive direction. Save the requested |
| * speed as the current speed so we can detect changes in the |
| * requested speed |
| */ |
| if (pState->reqSpeed > SPEED_0_X) |
| { |
| uint8_t flags = rateConvert[pState->reqSpeed].flags; |
| |
| setTickRate(pState, pState->reqSpeed); |
| pState->pTable = _GET_TABLE(flags & _BV(USE_MICRO)); |
| pState->pExcite->useRelay = flags & _BV(USE_RELAY); |
| pState->pState = &&run_move_pos; |
| pState->curSpeed = pState->reqSpeed; |
| |
| /* Fall through to move action */ |
| } |
| else |
| { |
| /* We're not going in the positive direction any more */ |
| goto enter_idle_pos; |
| } |
| return; |
| |
| run_move_pos: |
| if (pState->curSpeed == pState->reqSpeed) |
| { |
| /* We're still moving at the same speed. Do it |
| */ |
| pState->pExcite->excitation = pState->pTable[pState->stepCtr]; |
| pState->stepCtr = (pState->stepCtr + 1) & (STEPS_PER_CYCLE - 1); |
| } |
| else |
| { |
| /* Go baxk to idle_pos that will decide the next state */ |
| goto enter_idle_pos; |
| } |
| return; |
| |
| enter_move_neg: |
| /* Start moving in the negative direction. Save the requested |
| * speed as the current speed so we can detect changes in the |
| * requested speed |
| */ |
| if (pState->reqSpeed < SPEED_0_X) |
| { |
| uint8_t flags = rateConvert[-pState->reqSpeed].flags; |
| |
| setTickRate(pState, -pState->reqSpeed); |
| pState->pTable = _GET_TABLE(flags & _BV(USE_MICRO)); |
| pState->pExcite->useRelay = flags & _BV(USE_RELAY); |
| pState->pState = &&run_move_neg; |
| pState->curSpeed = pState->reqSpeed; |
| |
| /* Fall through to move action */ |
| } |
| else |
| { |
| /* We're not going in the negative direction any more. Stop and |
| * continue from there |
| */ |
| goto enter_idle_neg; |
| } |
| return; |
| |
| run_move_neg: |
| if (pState->curSpeed == pState->reqSpeed) |
| { |
| /* We're still moving at the same speed. Do it |
| */ |
| pState->pExcite->excitation = pState->pTable[pState->stepCtr]; |
| pState->stepCtr = (pState->stepCtr - 1) & (STEPS_PER_CYCLE - 1); |
| } |
| else |
| { |
| /* Go back to the idle_neg. It will determine the next state */ |
| goto enter_idle_neg; |
| } |
| return; |
| } |
| |
| /* stepperInt() is called each siderial interrupt. This is divided down |
| * in software to derive the actual stepper timing. |
| * |
| * Passed: |
| * Nothing |
| * |
| * Returns: |
| * Nothing |
| */ |
| SIGNAL(SIG_OUTPUT_COMPARE1A) |
| { |
| /* Update the tracking rate adjustment counter */ |
| ++adjCtr; |
| |
| /* If we're dropping then drop if necessary */ |
| if (doDropInt && adjLimit && adjCtr >= adjLimit) |
| { |
| /* Drop interrupt */ |
| adjCtr = 0; |
| return; |
| } |
| |
| do_again: |
| /* Run the state machine for the DEC and RA axis */ |
| if (raState.clkDivRatio != 0 && ++raState.divCtr >= raState.clkDivRatio) |
| { |
| // Execute the RA state machine |
| raState.divCtr = 0; |
| stepperProcess(&raState); |
| } |
| |
| if (decState.clkDivRatio != 0 && ++decState.divCtr >= decState.clkDivRatio) |
| { |
| // Execute the DEC state machine |
| decState.divCtr = 0; |
| stepperProcess(&decState); |
| } |
| |
| /* If we need to "insert" an interrupt do it now */ |
| if (!doDropInt && adjLimit && adjCtr >= adjLimit) |
| { |
| adjCtr = 0; |
| goto do_again; |
| } |
| } |