From 6a184e089758fcadfd57b85ecdbc20424b0cb471 Mon Sep 17 00:00:00 2001 From: ksb Date: Wed, 7 Mar 2007 16:15:00 +0000 Subject: [PATCH] Added codeprop loading to RAM. Moved some stepper code to Thumb so it could be dynamically linked to. --- platform/stepper-robot/Makefile | 18 +- platform/stepper-robot/cc2420-interrupt.c | 2 +- platform/stepper-robot/contiki-conf.h | 1 + platform/stepper-robot/robot-main.c | 29 +- platform/stepper-robot/stepper-process.c | 4 +- .../stepper-robot/stepper/stepper-interrupt.c | 359 +---------------- .../stepper-robot/stepper/stepper-interrupt.h | 91 +---- platform/stepper-robot/stepper/stepper.c | 360 ++++++++++++++++++ platform/stepper-robot/stepper/stepper.h | 98 +++++ 9 files changed, 500 insertions(+), 462 deletions(-) create mode 100644 platform/stepper-robot/stepper/stepper.c create mode 100644 platform/stepper-robot/stepper/stepper.h diff --git a/platform/stepper-robot/Makefile b/platform/stepper-robot/Makefile index a4c70b81f..35ee47aca 100644 --- a/platform/stepper-robot/Makefile +++ b/platform/stepper-robot/Makefile @@ -4,7 +4,7 @@ TARGET=stepper-robot CONTIKI_TARGET_DIRS=stepper -KERNELS= sys-tst.elf robot-main.elf +KERNELS= sys-tst.elf robot-main-syms.elf # Master clock frequency MCK=23961600 @@ -12,7 +12,8 @@ MCK=23961600 ARCH=debug-uart.o clock.o sys-interrupt.o interrupt-utils.o newlib-syscalls.o \ leds-arch.o sam7s-spi.o -SYSTEM=process.o procinit.o service.o clock.o etimer.o timer.o leds.o uip-log.o +SYSTEM=process.o procinit.o service.o clock.o etimer.o timer.o leds.o uip-log.o cfs.o +DEBUG_IO=dbg-printf.o dbg-puts.o dbg-putchar.o strformat.o UIP=uip.o uiplib.o tcpip.o uip-fw.o uip-fw-service.o uipbuf.o \ tcpdump.o psock.o uaodv.o uaodv-rt.o uip-udp-packet.o @@ -20,15 +21,22 @@ UIP=uip.o uiplib.o tcpip.o uip-fw.o uip-fw-service.o uipbuf.o \ UIPDRIVERS= cc2420.o cc2420_send_ip.o cc2420_send_uaodv.o cc2420-interrupt.o \ cc2420-spi.o -SYSLIB=memb.o list.o +SYSLIB=memb.o list.o malloc.o realloc.o + +STEPPER=stepper-interrupt.o stepper-move.o stepper.o + +ELFLOADER=elfloader-arm.o elfloader-otf.o symtab.o +CODEPROP=$(ELFLOADER) cfs-ram.o codeprop-otf.o ram-segments.o \ +autostart.o random.o -STEPPER=stepper-interrupt.o stepper-move.o CFLAGS+= -I stepper +cfs-ram.o: CFLAGS+= -DCFS_RAM_CONF_SIZE=4096 + all: $(KERNELS) sys-tst.elf: sys-tst.o $(ARCH) $(SYSTEM) -robot-main.elf: robot-main.o stepper-process.o $(ARCH) $(SYSTEM) $(SYSLIB) $(UIP) $(UIPDRIVERS) $(STEPPER) +robot-main-syms.elf: robot-main.o stepper-process.o $(ARCH) $(SYSTEM) $(SYSLIB) $(UIP) $(UIPDRIVERS) $(STEPPER) $(CODEPROP) $(DEBUG_IO) include $(CONTIKI)/cpu/at91sam7s/Makefile.at91sam7s diff --git a/platform/stepper-robot/cc2420-interrupt.c b/platform/stepper-robot/cc2420-interrupt.c index dd8f1f42f..68ba56d7a 100644 --- a/platform/stepper-robot/cc2420-interrupt.c +++ b/platform/stepper-robot/cc2420-interrupt.c @@ -32,7 +32,7 @@ inline int splhigh(void) #ifndef __THUMBEL__ asm("mrs %0, CPSR\n\torr r1,%0,#0x80\n\tmsr CPSR_c, r1" : "=r" (save)::"r1"); #else - #error Must be compiled in ARM mode +#error Must be compiled in ARM mode #endif return save; } diff --git a/platform/stepper-robot/contiki-conf.h b/platform/stepper-robot/contiki-conf.h index c35ad9ab9..10b045a80 100644 --- a/platform/stepper-robot/contiki-conf.h +++ b/platform/stepper-robot/contiki-conf.h @@ -70,6 +70,7 @@ do { \ #define UIP_CONF_LLH_LEN 0 #define UIP_CONF_BROADCAST 1 #define UIP_CONF_LOGGING 1 +#define UIP_CONF_BUFFER_SIZE 116 /* Prefix for relocation sections in ELF files */ #define REL_SECT_PREFIX ".rel" diff --git a/platform/stepper-robot/robot-main.c b/platform/stepper-robot/robot-main.c index fa1013546..e6e8b87d2 100644 --- a/platform/stepper-robot/robot-main.c +++ b/platform/stepper-robot/robot-main.c @@ -16,22 +16,18 @@ #include #include #include +#include +#include +#include +#include #ifndef RF_CHANNEL #define RF_CHANNEL 15 #endif -volatile const char * volatile input_line = NULL; -volatile unsigned int input_line_len = 0; +extern char __heap_end__; +extern char __heap_start__; -static void -recv_input(const char *str, unsigned int len) -{ - /* Assume that the line is handled before any new characters is written - to the buffer */ - input_line = str; - input_line_len = len; -} PROCESS(blink_process, "LED blink process"); @@ -65,6 +61,7 @@ PROCESS_THREAD(blink_process, ev , data) PROCESS_END(); } +#if 0 PROCESS(udprecv_process, "UDP recv process"); PROCESS_THREAD(udprecv_process, ev, data) @@ -100,7 +97,6 @@ PROCESS_THREAD(udprecv_process, ev, data) PROCESS_END(); } - PROCESS(wd_test_process, "Watchdog test process"); @@ -122,6 +118,7 @@ PROCESS_THREAD(wd_test_process, ev, data) PROCESS_END(); } +#endif #if 0 @@ -142,7 +139,11 @@ wdt_reset() static uip_ipaddr_t gw_addr = {{172,16,0,1}}; -PROCINIT(&etimer_process, &tcpip_process, &uip_fw_process, &cc2420_process,/* &uaodv_process, */ &udprecv_process, &blink_process, &stepper_process); +PROCINIT(&etimer_process, &tcpip_process, &uip_fw_process, &cc2420_process, + /* &uaodv_process, */ &cfs_ram_process, &codeprop_process, + &ram_segments_cleanup_process, + &blink_process, &stepper_process); + int main() @@ -155,8 +156,7 @@ main() dbg_setup_uart(); printf("Initialising\n"); - dbg_set_input_handler(recv_input); - leds_arch_init(); + leds_arch_init(); clock_init(); uip_sethostaddr(&cc2420if.ipaddr); uip_setnetmask(&cc2420if.netmask); @@ -168,6 +168,7 @@ main() uip_init(); uip_fw_default(&cc2420if); tcpip_set_forwarding(1); + printf("Heap size: %ld bytes\n", &__heap_end__ - (char*)sbrk(0)); printf("Started\n"); procinit_init(); diff --git a/platform/stepper-robot/stepper-process.c b/platform/stepper-robot/stepper-process.c index 5f2a5e7f5..d2aeb57f3 100644 --- a/platform/stepper-robot/stepper-process.c +++ b/platform/stepper-robot/stepper-process.c @@ -1,6 +1,6 @@ #include #include -#include +#include #include #include #include @@ -9,6 +9,8 @@ #include #include +#undef putchar + static const uint32_t stepper0_steps_acc[] = MICRO_STEP(0,3); static const uint32_t stepper0_steps_run[] = MICRO_STEP(0,2); static const uint32_t stepper0_steps_hold[] = MICRO_STEP(0,1); diff --git a/platform/stepper-robot/stepper/stepper-interrupt.c b/platform/stepper-robot/stepper/stepper-interrupt.c index 6ec8b94b3..b89d8fabf 100644 --- a/platform/stepper-robot/stepper/stepper-interrupt.c +++ b/platform/stepper-robot/stepper/stepper-interrupt.c @@ -1,39 +1,12 @@ #include #include #include +#include -/* Timer frequency */ -#define TIMER_FREQ 748800 -static StepperContext stepper_context; +StepperContext stepper_context; -static StepperAccSeq *free_seq = NULL; - -StepperAccSeq * -stepper_allocate_seq() -{ - StepperAccSeq *seq; - if (!free_seq) return NULL; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - seq = free_seq; - free_seq = seq->next; - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return seq; -} - -void -stepper_free_seq(StepperAccSeq *seq) -{ - StepperAccSeq *s; - if (!seq) return; - s = seq; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - while(s->next) s = s->next; - s->next = free_seq; - free_seq = seq; - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; -} static void do_step(StepperTimerStep *step) @@ -314,336 +287,10 @@ stepper_int_safe() } -void NACKEDFUNC stepper_int (void) { +void NACKEDFUNC stepper_timer_interrupt (void) { ISR_STORE(); ISR_ENABLE_NEST(); stepper_int_safe(); ISR_DISABLE_NEST(); ISR_RESTORE(); } - -static void -stepper_state_init(StepperState *stepper) -{ - stepper->step_count = 0; - stepper->io_mask = 0; - stepper->acc_steps = NULL; - stepper->run_steps = NULL; - stepper->hold_steps = NULL; - stepper->current_step = 0; - stepper->sequence_length = 0; - - stepper->velocity = 0; - stepper->acceleration = 0; - stepper->step_full = 0; - stepper->step_frac = 0; - stepper->n_steps = 0; - -#ifdef TIMING_ERRORS - stepper->err_min = TIMER_FREQ; - stepper->err_max = -TIMER_FREQ; -#endif - -} - -void -stepper_init(AT91PS_TC timer, unsigned int id) -{ - unsigned int s; - stepper_context.flags = 0; - stepper_context.timer_channel = timer; - stepper_context.steps = NULL; - stepper_context.current_step = NULL; - stepper_context.period_count = 0; - stepper_context.user_callback = NULL; - - for (s = 0; s < NUM_STEPPERS; s++) { - stepper_state_init(&stepper_context.steppers[s]); - } - timer->TC_CMR = (AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO - | AT91C_TC_CLKS_TIMER_DIV3_CLOCK); - timer->TC_RC = TIMER_FREQ / PPS; - timer->TC_RA = 0xffff; - timer->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - *AT91C_PMC_PCER = (1 << id); - - AT91C_AIC_SMR[id] = AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE | 7; - AT91C_AIC_SVR[id] = (unsigned long)stepper_int; - *AT91C_AIC_IECR = (1 << id); - timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; -} - -void -stepper_init_io(unsigned int stepper_index, uint32_t mask, - const uint32_t *acc, const uint32_t *run, - const uint32_t *hold, unsigned int nsteps) -{ - StepperState *state; - if (stepper_index >= NUM_STEPPERS) return; - state = &stepper_context.steppers[stepper_index]; - - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - - state->io_mask = mask; - state->acc_steps = acc; - state->run_steps = run; - state->hold_steps = hold; - state->current_step = 0; - state->sequence_length = nsteps; - *AT91C_PIOA_OWER = mask; - *AT91C_PIOA_MDDR = mask; - - *AT91C_PIOA_ODSR = ((*AT91C_PIOA_ODSR & ~mask) - | (state->hold_steps[0] & mask)); - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - *AT91C_PIOA_OER = mask; -} - -/** - Append an acceleration sequence - - Truncates the current acceleration sequence at the insertion time - and appends the new sequence at that position.. The insertion time - is the time of the first element of the new sequence. The - truncation takes place after any elements with the acceleration set - to STEPPER_ACC_INVALID (user callbacks) that has the same time as - the insertion time. All other elements with the same time is - replaced. - - \param stepper_index Index of the stepper the sequence is intended for. - \param new_seq A linked list of sequence elements to append. - */ -StepperResult -stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq) -{ - StepperResult res = STEPPER_ERR_TOO_LATE; - StepperAccSeq **seqp; - StepperState *state; - if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX; - state = &stepper_context.steppers[stepper_index]; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - seqp = &state->acceleration_sequence; - while(*seqp && ((*seqp)->period < new_seq->period || ((*seqp)->period == new_seq->period && (*seqp)->acceleration == STEPPER_ACC_INVALID))) { - seqp = &(*seqp)->next; - } - if (new_seq->period > stepper_context.period_count + 1) { - /* Replace tail of sequence */ - if (*seqp) stepper_free_seq(*seqp); - *seqp = new_seq; - res = STEPPER_OK; - } - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return res; -} - -/** - Insert a callback mark - - Inserts a callback mark at the indicated period. This will cause - the callback procedure to be called just before that period, - usually near the beginning of the previous period. Does not - truncate the current sequence. - - \param stepper_index Index of the stepper the callbak is intended for. - \param period When the callback should be invoked - - \sa stepper_set_callback_proc -*/ - -StepperResult -stepper_insert_callback(unsigned int stepper_index, unsigned int period) -{ - StepperResult res = STEPPER_ERR_TOO_LATE; - StepperAccSeq **seqp; - StepperState *state; - if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX; - state = &stepper_context.steppers[stepper_index]; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - seqp = &state->acceleration_sequence; - while(*seqp && (*seqp)->period < period) { - seqp = &(*seqp)->next; - } - if (period > stepper_context.period_count + 1) { - StepperAccSeq *new_seq = stepper_allocate_seq(); - if (!new_seq) { - res = STEPPER_ERR_MEM; - } else { - new_seq->next = *seqp; - *seqp = new_seq; - new_seq->period = period; - new_seq->acceleration = STEPPER_ACC_INVALID; - res = STEPPER_OK; - } - } - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return res; -} - -StepperResult -stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc) -{ - StepperAccSeq *seq = stepper_allocate_seq(); - /* printf("stepper_add_acc: %d %d %ld\n", stepper_index, period, acc); */ - if (!seq) return STEPPER_ERR_MEM; - seq->next = NULL; - seq->period = period; - seq->acceleration = acc; - return stepper_add_acc_seq(stepper_index, seq); -} - -void -stepper_set_callback_proc(StepperUserCallback callback) -{ - stepper_context.user_callback = callback; -} - -unsigned long -stepper_current_period() -{ - return stepper_context.period_count; -} - -long -stepper_current_step(unsigned int stepper_index) -{ - StepperState *state = &stepper_context.steppers[stepper_index]; - return state->step_count; -} - -long long -stepper_step_frac(unsigned int stepper_index) -{ - long long s; - StepperState *state = &stepper_context.steppers[stepper_index]; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - s = state->step_full * DIST_SCALE + state->step_frac; - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return s; -} - -long -stepper_current_velocity(unsigned int stepper_index) -{ - StepperState *state = &stepper_context.steppers[stepper_index]; - return state->velocity; -} - -/* Calculate the speed at given current given the current acceleration - sequence. */ -unsigned long -stepper_velocity(unsigned int stepper_index, unsigned long period) -{ - long a; - long v; - unsigned long t; - StepperState *state; - StepperAccSeq *seq; - state = &stepper_context.steppers[stepper_index]; - - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - seq = state->acceleration_sequence; - a = state->acceleration; - v = state->velocity; - t = stepper_context.period_count + 2; - - while(seq && seq->period < period) { - v += a * (seq->period - t); - t = seq->period; - a = seq->acceleration; - seq = seq->next; - } - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - v += a * (period - t); - return v; -} - -/** - Calculate the speed and position at specified period given the - current acceleration sequence. - - \param stepper_index Index of the stepper the callbak is intended for. - \param period The period to calculate for - \param Speed return - \param Position return. In fractional steps - -*/ -StepperResult -stepper_state_at(unsigned int stepper_index, unsigned long period, - long *velocity, long long *position) -{ - long a; - long v; - long long s; - unsigned long t; - long dt; - StepperState *state; - StepperAccSeq *seq; - state = &stepper_context.steppers[stepper_index]; - - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - if (period < stepper_context.period_count + 2) { - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return STEPPER_ERR_TOO_LATE; - } - seq = state->acceleration_sequence; - a = state->acceleration; - v = state->velocity; - t = stepper_context.period_count + 2; - s = state->step_full * (long long)DIST_SCALE + state->step_frac; - while(seq && seq->period < period) { - dt = seq->period - t; - s += (a * (long long)dt + 2 * v) * dt; - v += a * (seq->period - t); - t = seq->period; - a = seq->acceleration; - seq = seq->next; - } - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - dt = period - t; - *position = s + (a * (long long)dt + (DIST_SCALE/VEL_SCALE) * v) * dt; - *velocity = v + a * dt; - - return STEPPER_OK; -} - - -StepperResult -stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp, - unsigned long max_acc, long final_speed) -{ - long start_period = *periodp; - long v = stepper_velocity(stepper_index, start_period); - /* printf("%ld @ %ld\n", v, start_period); */ - if (final_speed == v) { - return stepper_add_acc(stepper_index, start_period, 0); - } else { - StepperResult res; - long a = (final_speed > v) ? max_acc : -max_acc; - long t = ((long)(final_speed - v)) / a; - long diff = (final_speed - v) - t * a; - if (t > 0) { - res = stepper_add_acc(stepper_index, start_period, a); - if (res != STEPPER_OK) return res; - } - if (diff) { - res = stepper_add_acc(stepper_index, start_period+t, diff); - if (res != STEPPER_OK) return res; - t++; - } - *periodp = start_period+t; - return stepper_add_acc(stepper_index, start_period+t, 0); - } -} - -#ifdef TIMING_ERRORS -void -stepper_timing_errors(unsigned int stepper_index, long *min, long *max) -{ - StepperState *state; - state = &stepper_context.steppers[stepper_index]; - *min = state->err_min; - *max = state->err_max; - state->err_max = -TIMER_FREQ; - state->err_min = TIMER_FREQ; -} -#endif diff --git a/platform/stepper-robot/stepper/stepper-interrupt.h b/platform/stepper-robot/stepper/stepper-interrupt.h index 96aac563b..1356004d7 100644 --- a/platform/stepper-robot/stepper/stepper-interrupt.h +++ b/platform/stepper-robot/stepper/stepper-interrupt.h @@ -1,22 +1,14 @@ #ifndef __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ #define __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ -#include -#include +#include -/* Define periods/second */ -#define PPS 128 - -/* Scaling factor for distance */ -#define DIST_SCALE (2 * PPS * PPS) - -/* Scaling factor for velocity */ -#define VEL_SCALE PPS +/* Timer frequency */ +#define TIMER_FREQ 748800 typedef struct _StepperContext StepperContext; typedef struct _StepperState StepperState; typedef struct _StepperTimerStep StepperTimerStep; -typedef struct _StepperAccSeq StepperAccSeq; #define MAX_STEPS_PER_PERIOD 40 #define NUM_STEPPERS 2 @@ -24,14 +16,6 @@ typedef struct _StepperAccSeq StepperAccSeq; #define STEPPER_MAX_VELOCITY 4000 #define STEPPER_MAX_ACCELRATION 4000 -struct _StepperAccSeq -{ - StepperAccSeq *next; - unsigned long period; - long acceleration; -}; - -#define STEPPER_ACC_INVALID LONG_MAX #define TIMING_ERRORS @@ -78,8 +62,6 @@ struct _StepperTimerStep uint8_t power; }; -typedef void (*StepperUserCallback)(unsigned int stepper_index, - unsigned long period); struct _StepperContext { @@ -92,68 +74,7 @@ struct _StepperContext StepperUserCallback user_callback; }; -typedef unsigned int StepperResult; -#define STEPPER_OK 0 -#define STEPPER_ERR_MEM 1 -#define STEPPER_ERR_TOO_LATE 2 -#define STEPPER_ERR_INDEX 3 - -void -stepper_init(AT91PS_TC timer, unsigned int id); - -void -stepper_init_io(unsigned int stepper_index, uint32_t mask, - const uint32_t *acc, const uint32_t *run, - const uint32_t *hold, unsigned int nsteps); - -/* Returns true if the new sequence was actually added or false - if the index is illegal or the first step in the sequence is too soon */ - -StepperResult -stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq); - -StepperResult -stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc); - -StepperResult -stepper_insert_callback(unsigned int stepper_index, unsigned int period); - -void -stepper_set_callback_proc(StepperUserCallback callback); - -unsigned long -stepper_current_period(); - -long -stepper_current_step(unsigned int stepper_index); - -long long -stepper_step_frac(unsigned int stepper_index); - -long -stepper_current_velocity(unsigned int stepper_index); - - -unsigned long -stepper_velocity(unsigned int stepper_index, unsigned long period); - -StepperResult -stepper_state_at(unsigned int stepper_index, unsigned long period, - long *velocity, long long *position); - -StepperResult -stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp, - unsigned long max_acc, long final_speed); - -StepperAccSeq * -stepper_allocate_seq(); - -void -stepper_free_seq(StepperAccSeq *seq); - -#ifdef TIMING_ERRORS -void -stepper_timing_errors(unsigned int stepper_index, long *min, long *max); -#endif - +extern StepperContext stepper_context; #endif /* __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ */ + +void stepper_timer_interrupt(void); diff --git a/platform/stepper-robot/stepper/stepper.c b/platform/stepper-robot/stepper/stepper.c new file mode 100644 index 000000000..9f6f3c00d --- /dev/null +++ b/platform/stepper-robot/stepper/stepper.c @@ -0,0 +1,360 @@ +#include +#include + +#ifndef NUL +#define NULL 0 +#endif + +static StepperAccSeq *free_seq = NULL; + +StepperAccSeq * +stepper_allocate_seq() +{ + StepperAccSeq *seq; + if (!free_seq) return NULL; + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + seq = free_seq; + free_seq = seq->next; + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + return seq; +} + + +void +stepper_free_seq(StepperAccSeq *seq) +{ + StepperAccSeq *s; + if (!seq) return; + s = seq; + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + while(s->next) s = s->next; + s->next = free_seq; + free_seq = seq; + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; +} + +static void +stepper_state_init(StepperState *stepper) +{ + stepper->step_count = 0; + stepper->io_mask = 0; + stepper->acc_steps = NULL; + stepper->run_steps = NULL; + stepper->hold_steps = NULL; + stepper->current_step = 0; + stepper->sequence_length = 0; + + stepper->velocity = 0; + stepper->acceleration = 0; + stepper->step_full = 0; + stepper->step_frac = 0; + stepper->n_steps = 0; + +#ifdef TIMING_ERRORS + stepper->err_min = TIMER_FREQ; + stepper->err_max = -TIMER_FREQ; +#endif + +} + +void +stepper_init(AT91PS_TC timer, unsigned int id) +{ + unsigned int s; + stepper_context.flags = 0; + stepper_context.timer_channel = timer; + stepper_context.steps = NULL; + stepper_context.current_step = NULL; + stepper_context.period_count = 0; + stepper_context.user_callback = NULL; + + for (s = 0; s < NUM_STEPPERS; s++) { + stepper_state_init(&stepper_context.steppers[s]); + } + timer->TC_CMR = (AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO + | AT91C_TC_CLKS_TIMER_DIV3_CLOCK); + timer->TC_RC = TIMER_FREQ / PPS; + timer->TC_RA = 0xffff; + timer->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + *AT91C_PMC_PCER = (1 << id); + + AT91C_AIC_SMR[id] = AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE | 7; + AT91C_AIC_SVR[id] = (unsigned long)stepper_timer_interrupt; + *AT91C_AIC_IECR = (1 << id); + timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; +} + +void +stepper_init_io(unsigned int stepper_index, uint32_t mask, + const uint32_t *acc, const uint32_t *run, + const uint32_t *hold, unsigned int nsteps) +{ + StepperState *state; + if (stepper_index >= NUM_STEPPERS) return; + state = &stepper_context.steppers[stepper_index]; + + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + + state->io_mask = mask; + state->acc_steps = acc; + state->run_steps = run; + state->hold_steps = hold; + state->current_step = 0; + state->sequence_length = nsteps; + *AT91C_PIOA_OWER = mask; + *AT91C_PIOA_MDDR = mask; + + *AT91C_PIOA_ODSR = ((*AT91C_PIOA_ODSR & ~mask) + | (state->hold_steps[0] & mask)); + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + *AT91C_PIOA_OER = mask; +} + +/** + Append an acceleration sequence + + Truncates the current acceleration sequence at the insertion time + and appends the new sequence at that position.. The insertion time + is the time of the first element of the new sequence. The + truncation takes place after any elements with the acceleration set + to STEPPER_ACC_INVALID (user callbacks) that has the same time as + the insertion time. All other elements with the same time is + replaced. + + \param stepper_index Index of the stepper the sequence is intended for. + \param new_seq A linked list of sequence elements to append. + */ +StepperResult +stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq) +{ + StepperResult res = STEPPER_ERR_TOO_LATE; + StepperAccSeq **seqp; + StepperState *state; + if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX; + state = &stepper_context.steppers[stepper_index]; + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + seqp = &state->acceleration_sequence; + while(*seqp && ((*seqp)->period < new_seq->period || ((*seqp)->period == new_seq->period && (*seqp)->acceleration == STEPPER_ACC_INVALID))) { + seqp = &(*seqp)->next; + } + if (new_seq->period > stepper_context.period_count + 1) { + /* Replace tail of sequence */ + if (*seqp) stepper_free_seq(*seqp); + *seqp = new_seq; + res = STEPPER_OK; + } + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + return res; +} + +/** + Insert a callback mark + + Inserts a callback mark at the indicated period. This will cause + the callback procedure to be called just before that period, + usually near the beginning of the previous period. Does not + truncate the current sequence. + + \param stepper_index Index of the stepper the callbak is intended for. + \param period When the callback should be invoked + + \sa stepper_set_callback_proc +*/ + +StepperResult +stepper_insert_callback(unsigned int stepper_index, unsigned int period) +{ + StepperResult res = STEPPER_ERR_TOO_LATE; + StepperAccSeq **seqp; + StepperState *state; + if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX; + state = &stepper_context.steppers[stepper_index]; + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + seqp = &state->acceleration_sequence; + while(*seqp && (*seqp)->period < period) { + seqp = &(*seqp)->next; + } + if (period > stepper_context.period_count + 1) { + StepperAccSeq *new_seq = stepper_allocate_seq(); + if (!new_seq) { + res = STEPPER_ERR_MEM; + } else { + new_seq->next = *seqp; + *seqp = new_seq; + new_seq->period = period; + new_seq->acceleration = STEPPER_ACC_INVALID; + res = STEPPER_OK; + } + } + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + return res; +} + +StepperResult +stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc) +{ + StepperAccSeq *seq = stepper_allocate_seq(); + /* printf("stepper_add_acc: %d %d %ld\n", stepper_index, period, acc); */ + if (!seq) return STEPPER_ERR_MEM; + seq->next = NULL; + seq->period = period; + seq->acceleration = acc; + return stepper_add_acc_seq(stepper_index, seq); +} + +void +stepper_set_callback_proc(StepperUserCallback callback) +{ + stepper_context.user_callback = callback; +} + +unsigned long +stepper_current_period() +{ + return stepper_context.period_count; +} + +long +stepper_current_step(unsigned int stepper_index) +{ + StepperState *state = &stepper_context.steppers[stepper_index]; + return state->step_count; +} + +long long +stepper_step_frac(unsigned int stepper_index) +{ + long long s; + StepperState *state = &stepper_context.steppers[stepper_index]; + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + s = state->step_full * DIST_SCALE + state->step_frac; + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + return s; +} + +long +stepper_current_velocity(unsigned int stepper_index) +{ + StepperState *state = &stepper_context.steppers[stepper_index]; + return state->velocity; +} + +/* Calculate the speed at given current given the current acceleration + sequence. */ +unsigned long +stepper_velocity(unsigned int stepper_index, unsigned long period) +{ + long a; + long v; + unsigned long t; + StepperState *state; + StepperAccSeq *seq; + state = &stepper_context.steppers[stepper_index]; + + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + seq = state->acceleration_sequence; + a = state->acceleration; + v = state->velocity; + t = stepper_context.period_count + 2; + + while(seq && seq->period < period) { + v += a * (seq->period - t); + t = seq->period; + a = seq->acceleration; + seq = seq->next; + } + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + v += a * (period - t); + return v; +} + +/** + Calculate the speed and position at specified period given the + current acceleration sequence. + + \param stepper_index Index of the stepper the callbak is intended for. + \param period The period to calculate for + \param Speed return + \param Position return. In fractional steps + +*/ +StepperResult +stepper_state_at(unsigned int stepper_index, unsigned long period, + long *velocity, long long *position) +{ + long a; + long v; + long long s; + unsigned long t; + long dt; + StepperState *state; + StepperAccSeq *seq; + state = &stepper_context.steppers[stepper_index]; + + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + if (period < stepper_context.period_count + 2) { + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + return STEPPER_ERR_TOO_LATE; + } + seq = state->acceleration_sequence; + a = state->acceleration; + v = state->velocity; + t = stepper_context.period_count + 2; + s = state->step_full * (long long)DIST_SCALE + state->step_frac; + while(seq && seq->period < period) { + dt = seq->period - t; + s += (a * (long long)dt + 2 * v) * dt; + v += a * (seq->period - t); + t = seq->period; + a = seq->acceleration; + seq = seq->next; + } + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + dt = period - t; + *position = s + (a * (long long)dt + (DIST_SCALE/VEL_SCALE) * v) * dt; + *velocity = v + a * dt; + + return STEPPER_OK; +} + + +StepperResult +stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp, + unsigned long max_acc, long final_speed) +{ + long start_period = *periodp; + long v = stepper_velocity(stepper_index, start_period); + /* printf("%ld @ %ld\n", v, start_period); */ + if (final_speed == v) { + return stepper_add_acc(stepper_index, start_period, 0); + } else { + StepperResult res; + long a = (final_speed > v) ? max_acc : -max_acc; + long t = ((long)(final_speed - v)) / a; + long diff = (final_speed - v) - t * a; + if (t > 0) { + res = stepper_add_acc(stepper_index, start_period, a); + if (res != STEPPER_OK) return res; + } + if (diff) { + res = stepper_add_acc(stepper_index, start_period+t, diff); + if (res != STEPPER_OK) return res; + t++; + } + *periodp = start_period+t; + return stepper_add_acc(stepper_index, start_period+t, 0); + } +} + +#ifdef TIMING_ERRORS +void +stepper_timing_errors(unsigned int stepper_index, long *min, long *max) +{ + StepperState *state; + state = &stepper_context.steppers[stepper_index]; + *min = state->err_min; + *max = state->err_max; + state->err_max = -TIMER_FREQ; + state->err_min = TIMER_FREQ; +} +#endif diff --git a/platform/stepper-robot/stepper/stepper.h b/platform/stepper-robot/stepper/stepper.h new file mode 100644 index 000000000..6e067641c --- /dev/null +++ b/platform/stepper-robot/stepper/stepper.h @@ -0,0 +1,98 @@ +#ifndef __STEPPER_H__JPA916UOFT__ +#define __STEPPER_H__JPA916UOFT__ + +#include +#include +#include + +/* Define periods/second */ +#define PPS 128 + +/* Scaling factor for distance */ +#define DIST_SCALE (2 * PPS * PPS) + +/* Scaling factor for velocity */ +#define VEL_SCALE PPS + +typedef struct _StepperAccSeq StepperAccSeq; +struct _StepperAccSeq +{ + StepperAccSeq *next; + unsigned long period; + long acceleration; +}; + +#define STEPPER_ACC_INVALID LONG_MAX + +#define STEPPER_MAX_VELOCITY 4000 +#define STEPPER_MAX_ACCELRATION 4000 + +typedef void (*StepperUserCallback)(unsigned int stepper_index, + unsigned long period); + + +typedef unsigned int StepperResult; +#define STEPPER_OK 0 +#define STEPPER_ERR_MEM 1 +#define STEPPER_ERR_TOO_LATE 2 +#define STEPPER_ERR_INDEX 3 + +void +stepper_init(AT91PS_TC timer, unsigned int id); + +void +stepper_init_io(unsigned int stepper_index, uint32_t mask, + const uint32_t *acc, const uint32_t *run, + const uint32_t *hold, unsigned int nsteps); + +/* Returns true if the new sequence was actually added or false + if the index is illegal or the first step in the sequence is too soon */ + +StepperResult +stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq); + +StepperResult +stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc); + +StepperResult +stepper_insert_callback(unsigned int stepper_index, unsigned int period); + +void +stepper_set_callback_proc(StepperUserCallback callback); + +unsigned long +stepper_current_period(); + +long +stepper_current_step(unsigned int stepper_index); + +long long +stepper_step_frac(unsigned int stepper_index); + +long +stepper_current_velocity(unsigned int stepper_index); + +unsigned long +stepper_velocity(unsigned int stepper_index, unsigned long period); + +StepperResult +stepper_state_at(unsigned int stepper_index, unsigned long period, + long *velocity, long long *position); + +StepperResult +stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp, + unsigned long max_acc, long final_speed); + +StepperAccSeq * +stepper_allocate_seq(); + +void +stepper_free_seq(StepperAccSeq *seq); + +#ifdef TIMING_ERRORS + +void +stepper_timing_errors(unsigned int stepper_index, long *min, long *max); +#endif + +#endif /* __STEPPER_H__JPA916UOFT__ */