Contiki 2.6

stepper.c

00001 #include <stepper.h>
00002 #include <stepper-interrupt.h>
00003 
00004 #ifndef NUL
00005 #define NULL 0
00006 #endif
00007 
00008 static StepperAccSeq *free_seq = NULL;
00009 
00010 StepperAccSeq *
00011 stepper_allocate_seq()
00012 {
00013   StepperAccSeq *seq;
00014   if (!free_seq) return NULL;
00015   stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
00016   seq = free_seq;
00017   free_seq = seq->next;
00018   stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00019   return seq;
00020 }
00021 
00022 
00023 void
00024 stepper_free_seq(StepperAccSeq *seq)
00025 {
00026   StepperAccSeq *s;
00027   if (!seq) return;
00028   s = seq;
00029   stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
00030   while(s->next) s = s->next;
00031   s->next = free_seq;
00032   free_seq = seq;
00033   stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00034 }
00035 
00036 static void
00037 stepper_state_init(StepperState *stepper)
00038 {
00039   stepper->step_count = 0;
00040   stepper->io_mask = 0;
00041   stepper->acc_steps = NULL;
00042   stepper->run_steps = NULL;
00043   stepper->hold_steps = NULL;
00044   stepper->current_step = 0;
00045   stepper->sequence_length = 0;
00046 
00047   stepper->velocity = 0;
00048   stepper->acceleration = 0;
00049   stepper->step_full = 0;
00050   stepper->step_frac = 0;
00051   stepper->n_steps = 0;
00052 
00053 #ifdef TIMING_ERRORS
00054   stepper->err_min = TIMER_FREQ;
00055   stepper->err_max = -TIMER_FREQ;
00056 #endif
00057   
00058 }
00059 
00060 void
00061 stepper_init(AT91PS_TC timer, unsigned int id)
00062 {
00063   unsigned int s;
00064   stepper_context.flags = 0;
00065   stepper_context.timer_channel = timer;
00066   stepper_context.steps = NULL;
00067   stepper_context.current_step = NULL;
00068   stepper_context.period_count = 0;
00069   stepper_context.user_callback = NULL;
00070   
00071   for (s = 0; s < NUM_STEPPERS; s++) {
00072     stepper_state_init(&stepper_context.steppers[s]);
00073   }
00074   timer->TC_CMR = (AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO
00075                    | AT91C_TC_CLKS_TIMER_DIV3_CLOCK);
00076   timer->TC_RC = TIMER_FREQ / PPS;
00077   timer->TC_RA = 0xffff;
00078   timer->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00079   *AT91C_PMC_PCER = (1 << id);
00080   
00081   AT91C_AIC_SMR[id] = AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE | 7;
00082   AT91C_AIC_SVR[id] =  (unsigned long)stepper_timer_interrupt;
00083   *AT91C_AIC_IECR = (1 << id);
00084   timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
00085 }
00086 
00087 void
00088 stepper_init_io(unsigned int stepper_index, uint32_t mask,
00089                 const uint32_t *acc, const uint32_t *run,
00090                 const uint32_t *hold, unsigned int nsteps)
00091 {
00092   StepperState *state;
00093   if (stepper_index >= NUM_STEPPERS) return;
00094   state = &stepper_context.steppers[stepper_index];
00095   
00096   stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
00097   
00098   state->io_mask = mask;
00099   state->acc_steps = acc;
00100   state->run_steps = run;
00101   state->hold_steps = hold;
00102   state->current_step = 0;
00103   state->sequence_length = nsteps;
00104   *AT91C_PIOA_OWER = mask;
00105   *AT91C_PIOA_MDDR = mask;
00106 
00107   *AT91C_PIOA_ODSR = ((*AT91C_PIOA_ODSR & ~mask)
00108                       | (state->hold_steps[0] & mask));
00109   stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00110   *AT91C_PIOA_OER = mask;
00111 }
00112 
00113 /**
00114    Append an acceleration sequence
00115    
00116    Truncates the current acceleration sequence at the insertion time
00117    and appends the new sequence at that position.. The insertion time
00118    is the time of the first element of the new sequence. The
00119    truncation takes place after any elements with the acceleration set
00120    to STEPPER_ACC_INVALID (user callbacks) that has the same time as
00121    the insertion time. All other elements with the same time is
00122    replaced.
00123 
00124    \param stepper_index Index of the stepper the sequence is intended for.
00125    \param new_seq A linked list of sequence elements to append.
00126  */
00127 StepperResult
00128 stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq)
00129 {
00130   StepperResult res = STEPPER_ERR_TOO_LATE;
00131   StepperAccSeq **seqp;
00132   StepperState *state;
00133   if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX;
00134   state = &stepper_context.steppers[stepper_index];
00135   stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
00136   seqp = &state->acceleration_sequence;
00137   while(*seqp && ((*seqp)->period < new_seq->period || ((*seqp)->period == new_seq->period && (*seqp)->acceleration == STEPPER_ACC_INVALID))) {
00138     seqp = &(*seqp)->next;
00139   }
00140   if (new_seq->period > stepper_context.period_count + 1) {
00141     /* Replace tail of sequence */
00142     if (*seqp) stepper_free_seq(*seqp);
00143     *seqp = new_seq;
00144     res = STEPPER_OK;
00145   }
00146   stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00147   return res;
00148 }
00149   
00150 /**
00151    Insert a callback mark
00152    
00153    Inserts a callback mark at the indicated period. This will cause
00154    the callback procedure to be called just before that period,
00155    usually near the beginning of the previous period. Does not
00156    truncate the current sequence.
00157 
00158    \param stepper_index Index of the stepper the callbak is intended for.
00159    \param period When the callback should be invoked
00160 
00161    \sa stepper_set_callback_proc
00162 */
00163   
00164 StepperResult
00165 stepper_insert_callback(unsigned int stepper_index, unsigned int period)
00166 {
00167   StepperResult res = STEPPER_ERR_TOO_LATE;
00168   StepperAccSeq **seqp;
00169   StepperState *state;
00170   if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX;
00171   state = &stepper_context.steppers[stepper_index];
00172   stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
00173   seqp = &state->acceleration_sequence;
00174   while(*seqp && (*seqp)->period < period) {
00175     seqp = &(*seqp)->next;
00176   }
00177   if (period > stepper_context.period_count + 1) {
00178     StepperAccSeq *new_seq = stepper_allocate_seq();
00179     if (!new_seq) {
00180       res = STEPPER_ERR_MEM;
00181     } else {
00182       new_seq->next = *seqp;
00183       *seqp = new_seq;
00184       new_seq->period = period;
00185       new_seq->acceleration = STEPPER_ACC_INVALID;
00186       res = STEPPER_OK;
00187     }
00188   }
00189   stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00190   return res;
00191 }
00192 
00193 StepperResult
00194 stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc)
00195 {
00196   StepperAccSeq *seq = stepper_allocate_seq();
00197   /* printf("stepper_add_acc: %d %d %ld\n", stepper_index, period, acc);  */
00198   if (!seq) return STEPPER_ERR_MEM;
00199   seq->next = NULL;
00200   seq->period = period;
00201   seq->acceleration = acc;
00202   return stepper_add_acc_seq(stepper_index, seq);
00203 }
00204 
00205 void
00206 stepper_set_callback_proc(StepperUserCallback callback)
00207 {
00208   stepper_context.user_callback = callback;
00209 }
00210 
00211 unsigned long
00212 stepper_current_period()
00213 {
00214   return stepper_context.period_count;
00215 }
00216 
00217 long
00218 stepper_current_step(unsigned int stepper_index)
00219 {
00220   StepperState *state = &stepper_context.steppers[stepper_index];
00221   return state->step_count;
00222 }
00223 
00224 long long
00225 stepper_step_frac(unsigned int stepper_index)
00226 {
00227   long long s;
00228   StepperState *state = &stepper_context.steppers[stepper_index];
00229   stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
00230   s = state->step_full * DIST_SCALE + state->step_frac;
00231   stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00232   return s;
00233 }
00234 
00235 long
00236 stepper_current_velocity(unsigned int stepper_index)
00237 {
00238   StepperState *state = &stepper_context.steppers[stepper_index];
00239   return state->velocity;
00240 }
00241 
00242 /* Calculate the speed at given current given the current acceleration
00243    sequence. */
00244 unsigned long
00245 stepper_velocity(unsigned int stepper_index, unsigned long period)
00246 {
00247   long a;
00248   long v;
00249   unsigned long t;
00250   StepperState *state;
00251   StepperAccSeq *seq;
00252   state = &stepper_context.steppers[stepper_index];
00253   
00254   stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
00255   seq = state->acceleration_sequence;
00256   a = state->acceleration;
00257   v = state->velocity;
00258   t = stepper_context.period_count + 2;
00259 
00260   while(seq && seq->period < period) {
00261     v += a * (seq->period - t);
00262     t = seq->period;
00263     a = seq->acceleration;
00264     seq = seq->next;
00265   }
00266   stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00267   v += a * (period - t);
00268   return v;
00269 }
00270 
00271 /**
00272    Calculate the speed and position at specified period given the
00273    current acceleration sequence.
00274 
00275    \param stepper_index Index of the stepper the callbak is intended for.
00276    \param period The period to calculate for
00277    \param Speed return
00278    \param Position return. In fractional steps
00279 
00280 */
00281 StepperResult
00282 stepper_state_at(unsigned int stepper_index, unsigned long period,
00283                  long *velocity, long long *position)
00284 {
00285   long a;
00286   long v;
00287   long long s;
00288   unsigned long t;
00289   long dt;
00290   StepperState *state;
00291   StepperAccSeq *seq;
00292   state = &stepper_context.steppers[stepper_index];
00293   
00294   stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
00295   if (period < stepper_context.period_count + 2) {
00296     stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00297     return STEPPER_ERR_TOO_LATE;
00298   }
00299   seq = state->acceleration_sequence;
00300   a = state->acceleration;
00301   v = state->velocity;
00302   t = stepper_context.period_count + 2;
00303   s = state->step_full * (long long)DIST_SCALE + state->step_frac;
00304   while(seq && seq->period < period) {
00305     dt = seq->period - t;
00306     s += (a * (long long)dt + 2 * v) * dt;
00307     v += a * (seq->period - t);
00308     t = seq->period;
00309     a = seq->acceleration;
00310     seq = seq->next;
00311   }
00312   stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
00313   dt = period - t;
00314   *position = s + (a * (long long)dt + (DIST_SCALE/VEL_SCALE) * v) * dt;
00315   *velocity = v + a * dt;
00316   
00317   return STEPPER_OK;
00318 }
00319 
00320 
00321 StepperResult
00322 stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp,
00323                      unsigned long max_acc, long final_speed)
00324 {
00325   long start_period = *periodp;
00326   long v = stepper_velocity(stepper_index, start_period);
00327   /* printf("%ld @ %ld\n", v, start_period); */
00328   if (final_speed == v) {
00329     return stepper_add_acc(stepper_index, start_period, 0);
00330   } else {
00331     StepperResult res;
00332     long a = (final_speed > v) ? max_acc : -max_acc;
00333     long t = ((long)(final_speed - v)) / a;
00334     long diff = (final_speed - v) - t * a;
00335     if (t > 0) {
00336       res = stepper_add_acc(stepper_index, start_period, a);
00337       if (res != STEPPER_OK) return res;
00338     }
00339     if (diff) {
00340       res = stepper_add_acc(stepper_index, start_period+t, diff);
00341       if (res != STEPPER_OK) return res;
00342       t++;
00343     }
00344     *periodp = start_period+t;
00345     return stepper_add_acc(stepper_index, start_period+t, 0);
00346   }
00347 }
00348 
00349 #ifdef TIMING_ERRORS
00350 void
00351 stepper_timing_errors(unsigned int stepper_index, long *min, long *max)
00352 {
00353   StepperState *state;
00354   state = &stepper_context.steppers[stepper_index];
00355   *min = state->err_min;
00356   *max = state->err_max;
00357   state->err_max = -TIMER_FREQ;
00358   state->err_min = TIMER_FREQ;
00359 }
00360 #endif