deac/osd-contiki
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Fixed error in calculation of future speed

Browse source
This commit is contained in:
ksb 2007-02-24 15:26:02 +00:00
parent 20eaa31eff
commit 5af5a5a9a5
1 changed files with 4 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

7
platform/stepper-robot/stepper/stepper-interrupt.c
View file

@ -483,7 +483,7 @@ StepperResult
stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc) stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc)
{ {
StepperAccSeq *seq = stepper_allocate_seq(); StepperAccSeq *seq = stepper_allocate_seq();
/* printf("stepper_add_acc: %d %d %ld\n", stepper_index, period, acc); */ /* printf("stepper_add_acc: %d %d %ld\n", stepper_index, period, acc); */
if (!seq) return STEPPER_ERR_MEM; if (!seq) return STEPPER_ERR_MEM;
seq->next = NULL; seq->next = NULL;
seq->period = period; seq->period = period;
@ -544,7 +544,7 @@ stepper_velocity(unsigned int stepper_index, unsigned long period)
seq = state->acceleration_sequence; seq = state->acceleration_sequence;
a = state->acceleration; a = state->acceleration;
v = state->velocity; v = state->velocity;
t = stepper_context.period_count + 1; t = stepper_context.period_count + 2;
while(seq && seq->period < period) { while(seq && seq->period < period) {
v += a * (seq->period - t); v += a * (seq->period - t);
@ -588,7 +588,7 @@ stepper_state_at(unsigned int stepper_index, unsigned long period,
seq = state->acceleration_sequence; seq = state->acceleration_sequence;
a = state->acceleration; a = state->acceleration;
v = state->velocity; v = state->velocity;
t = stepper_context.period_count + 1; t = stepper_context.period_count + 2;
s = state->step_full * (long long)DIST_SCALE + state->step_frac; s = state->step_full * (long long)DIST_SCALE + state->step_frac;
while(seq && seq->period < period) { while(seq && seq->period < period) {
dt = seq->period - t; dt = seq->period - t;
@ -613,6 +613,7 @@ stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp,
{ {
long start_period = *periodp; long start_period = *periodp;
long v = stepper_velocity(stepper_index, start_period); long v = stepper_velocity(stepper_index, start_period);
/* printf("%ld @ %ld\n", v, start_period); */
if (final_speed == v) { if (final_speed == v) {
return stepper_add_acc(stepper_index, start_period, 0); return stepper_add_acc(stepper_index, start_period, 0);
} else { } else {