giomba
/
nes-proj
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Added codeprop loading to RAM. 

Moved some stepper code to Thumb so it could be dynamically linked to.
This commit is contained in:
ksb 2007-03-07 16:15:00 +00:00
parent b105b40e9a
commit 6a184e0897
9 changed files with 500 additions and 462 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
platform/stepper-robot/Makefile
View File

@ -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

2
platform/stepper-robot/cc2420-interrupt.c
View File

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

1
platform/stepper-robot/contiki-conf.h
View File

@ -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"

29
platform/stepper-robot/robot-main.c
View File

@ -16,22 +16,18 @@
#include <net/psock.h>
#include <stepper-process.h>
#include <dev/leds.h>
#include <cfs/cfs-ram.h>
#include <loader/codeprop-otf.h>
#include <loader/ram-segments.h>
#include <unistd.h>
#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();

4
platform/stepper-robot/stepper-process.c
View File

@ -1,6 +1,6 @@
#include <stepper-process.h>
#include <stepper-steps.h>
#include <stepper-interrupt.h>
#include <stepper.h>
#include <stepper-move.h>
#include <string.h>
#include <interrupt-utils.h>
@ -9,6 +9,8 @@
#include <net/uip.h>
#include <dev/cc2420.h>
#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);

359
platform/stepper-robot/stepper/stepper-interrupt.c
View File

@ -1,39 +1,12 @@
#include <stepper-interrupt.h>
#include <interrupt-utils.h>
#include <stdio.h>
#include <stepper.h>
/* 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

91
platform/stepper-robot/stepper/stepper-interrupt.h
View File

@ -1,22 +1,14 @@
#ifndef __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__
#define __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__
#include <AT91SAM7S64.h>
#include <inttypes.h>
#include <stepper.h>
/* 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);

360
platform/stepper-robot/stepper/stepper.c Normal file
View File

@ -0,0 +1,360 @@
#include <stepper.h>
#include <stepper-interrupt.h>
#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

98
platform/stepper-robot/stepper/stepper.h Normal file
View File

@ -0,0 +1,98 @@
#ifndef __STEPPER_H__JPA916UOFT__
#define __STEPPER_H__JPA916UOFT__
#include <AT91SAM7S64.h>
#include <inttypes.h>
#include <limits.h>
/* 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__ */