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; |
} |
} |