From 20eaa31eff51d6cfac3153e37c3c7ae614013e92 Mon Sep 17 00:00:00 2001 From: ksb Date: Sat, 24 Feb 2007 00:21:16 +0000 Subject: [PATCH] Platform specific code for a robot using stepper motors. --- platform/stepper-robot/Makefile | 34 + platform/stepper-robot/cc2420-interrupt.c | 47 ++ platform/stepper-robot/cc2420-interrupt.h | 6 + platform/stepper-robot/cc2420-spi.c | 23 + platform/stepper-robot/contiki-conf.h | 77 +++ platform/stepper-robot/leds-arch.c | 38 + platform/stepper-robot/robot-main.c | 187 +++++ platform/stepper-robot/sam7s-spi.c | 126 ++++ platform/stepper-robot/sam7s-spi.h | 29 + platform/stepper-robot/stepper-process.c | 500 ++++++++++++++ platform/stepper-robot/stepper-process.h | 7 + .../stepper-robot/stepper/stepper-interrupt.c | 648 ++++++++++++++++++ .../stepper-robot/stepper/stepper-interrupt.h | 159 +++++ platform/stepper-robot/stepper/stepper-move.c | 193 ++++++ platform/stepper-robot/stepper/stepper-move.h | 8 + .../stepper-robot/stepper/stepper-steps.h | 100 +++ platform/stepper-robot/sys-tst.c | 80 +++ 17 files changed, 2262 insertions(+) create mode 100644 platform/stepper-robot/Makefile create mode 100644 platform/stepper-robot/cc2420-interrupt.c create mode 100644 platform/stepper-robot/cc2420-interrupt.h create mode 100644 platform/stepper-robot/cc2420-spi.c create mode 100644 platform/stepper-robot/contiki-conf.h create mode 100644 platform/stepper-robot/leds-arch.c create mode 100644 platform/stepper-robot/robot-main.c create mode 100644 platform/stepper-robot/sam7s-spi.c create mode 100644 platform/stepper-robot/sam7s-spi.h create mode 100644 platform/stepper-robot/stepper-process.c create mode 100644 platform/stepper-robot/stepper-process.h create mode 100644 platform/stepper-robot/stepper/stepper-interrupt.c create mode 100644 platform/stepper-robot/stepper/stepper-interrupt.h create mode 100644 platform/stepper-robot/stepper/stepper-move.c create mode 100644 platform/stepper-robot/stepper/stepper-move.h create mode 100644 platform/stepper-robot/stepper/stepper-steps.h create mode 100644 platform/stepper-robot/sys-tst.c diff --git a/platform/stepper-robot/Makefile b/platform/stepper-robot/Makefile new file mode 100644 index 000000000..a4c70b81f --- /dev/null +++ b/platform/stepper-robot/Makefile @@ -0,0 +1,34 @@ +CONTIKI=../.. + +TARGET=stepper-robot + +CONTIKI_TARGET_DIRS=stepper + +KERNELS= sys-tst.elf robot-main.elf + +# Master clock frequency +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 + +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 + +UIPDRIVERS= cc2420.o cc2420_send_ip.o cc2420_send_uaodv.o cc2420-interrupt.o \ +cc2420-spi.o + +SYSLIB=memb.o list.o + +STEPPER=stepper-interrupt.o stepper-move.o +CFLAGS+= -I stepper + +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) + +include $(CONTIKI)/cpu/at91sam7s/Makefile.at91sam7s diff --git a/platform/stepper-robot/cc2420-interrupt.c b/platform/stepper-robot/cc2420-interrupt.c new file mode 100644 index 000000000..dd8f1f42f --- /dev/null +++ b/platform/stepper-robot/cc2420-interrupt.c @@ -0,0 +1,47 @@ +#include +#include +#include +#include +#include +#include + +static void NACKEDFUNC +cc2420_fifop_interrupt (void) /* System Interrupt Handler */ +{ + ISR_STORE(); + ISR_ENABLE_NEST(); + __cc2420_intr(); + ISR_DISABLE_NEST(); + *AT91C_AIC_EOICR = 0; /* End of Interrupt */ + ISR_RESTORE(); +} + +void +cc2420_interrupt_fifop_int_init(void) +{ + *AT91C_PIOA_ASR = AT91C_PA30_IRQ1; + *AT91C_PIOA_PDR = AT91C_PA30_IRQ1; + AT91C_AIC_SMR[AT91C_ID_IRQ1] = AT91C_AIC_SRCTYPE_POSITIVE_EDGE | 4; + AT91C_AIC_SVR[AT91C_ID_IRQ1] = (unsigned long)cc2420_fifop_interrupt; + /* *AT91C_AIC_IECR = (1 << AT91C_ID_IRQ1); */ +} + +inline int splhigh(void) +{ + int save; +#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 +#endif + return save; +} + +inline void splx(int saved) +{ +#ifndef __THUMBELL__ + asm("msr CPSR_c, %0" ::"r" (saved)); +#else +#error Must be compiled in ARM mode +#endif +} diff --git a/platform/stepper-robot/cc2420-interrupt.h b/platform/stepper-robot/cc2420-interrupt.h new file mode 100644 index 000000000..f36927850 --- /dev/null +++ b/platform/stepper-robot/cc2420-interrupt.h @@ -0,0 +1,6 @@ +#ifndef __CC2420_CORE_INTERRUPT_H__9499CTDNSK__ +#define __CC2420_CORE_INTERRUPT_H__9499CTDNSK__ + +void +cc2420_interrupt_fifop_int_init(void); +#endif /* __CC2420_CORE_INTERRUPT_H__9499CTDNSK__ */ diff --git a/platform/stepper-robot/cc2420-spi.c b/platform/stepper-robot/cc2420-spi.c new file mode 100644 index 000000000..f8aaaf91a --- /dev/null +++ b/platform/stepper-robot/cc2420-spi.c @@ -0,0 +1,23 @@ +#include +#include +#include +#include + +#define SPI_SPEED 1000000 /* 1MHz clock*/ +#define SPI_DLYBCT 1 +#define SPI_DLYBS 20 + + +#ifndef BV +#define BV(b) (1<<(b)) +#endif + +void +__cc2420_arch_init(void) +{ + spi_init(); + + AT91C_SPI_CSR[CC2420_DEFAULT_DEV] = + ((SPI_DLYBCT<<24) | (SPI_DLYBS<<16) | (((MCK+SPI_SPEED/2)/SPI_SPEED)<<8) + | AT91C_SPI_NCPHA | AT91C_SPI_BITS_8 | AT91C_SPI_CSAAT); +} diff --git a/platform/stepper-robot/contiki-conf.h b/platform/stepper-robot/contiki-conf.h new file mode 100644 index 000000000..c35ad9ab9 --- /dev/null +++ b/platform/stepper-robot/contiki-conf.h @@ -0,0 +1,77 @@ +#ifndef __CONTIKI_CONF_H__CDBB4VIH3I__ +#define __CONTIKI_CONF_H__CDBB4VIH3I__ + +#include +#include +#include +#include + +#define CCIF +#define CLIF + +#define WITH_UIP 1 +#define WITH_ASCII 1 + +#define CLOCK_CONF_SECOND 100 +typedef uint8_t u8_t; +typedef uint16_t u16_t; +typedef uint32_t u32_t; + +typedef unsigned int clock_time_t; +typedef unsigned int uip_stats_t; + +#ifndef BV +#define BV(x) (1<<(x)) +#endif + +/* SPI */ +#define SPI_TXBUF *AT91C_SPI_TDR + +#define SPI_RXBUF ((unsigned char)*AT91C_SPI_RDR) + +#define SPI_WAITFOREOTx() while ((*AT91C_SPI_SR & AT91C_SPI_TXEMPTY) == 0) + +#define SPI_WAITFOREORx() while ((*AT91C_SPI_SR & AT91C_SPI_RDRF) == 0) + +/* CC2420 control pins */ + +#define FIFO_IS_1 (*AT91C_PIOA_PDSR & AT91C_PIO_PA2) +#define VREG_IS_1 1 /* Hardwired */ +#define FIFOP_IS_1 (*AT91C_PIOA_PDSR & AT91C_PIO_PA30) +#define SFD_IS_1 (*AT91C_PIOA_PDSR & AT91C_PIO_PA15) + +#define SET_RESET_INACTIVE() cc2420_setreg(CC2420_MAIN, 0xf800); +#define SET_RESET_ACTIVE() cc2420_setreg(CC2420_MAIN, 0x0000); + +#define SET_VREG_ACTIVE() +#define SET_VREG_INACTIVE() + +#define FIFOP_INT_INIT() cc2420_interrupt_fifop_int_init() +#define DISABLE_FIFOP_INT() (*AT91C_AIC_IDCR = (1 << AT91C_ID_IRQ1)) +#define ENABLE_FIFOP_INT() (*AT91C_AIC_IECR = (1 << AT91C_ID_IRQ1)) + +#define CC2420_DEFAULT_DEV 1 + +#define SPI_ENABLE() \ +do { \ +*AT91C_SPI_MR = ((*AT91C_SPI_MR & ~AT91C_SPI_PCS) \ + | ((~(1< +#include + +#define GREEN_PIN AT91C_PIO_PA3 +#define YELLOW_PIN AT91C_PIO_PA4 +#define RED_PIN AT91C_PIO_PA8 + +#define ALL_PINS (GREEN_PIN | YELLOW_PIN | RED_PIN) + +void +leds_arch_init(void) +{ + *AT91C_PIOA_PER = ALL_PINS; + *AT91C_PIOA_OER = ALL_PINS; + *AT91C_PIOA_MDER = ALL_PINS; + *AT91C_PIOA_SODR = ALL_PINS; +} + +unsigned char +leds_arch_get(void) +{ + unsigned char on = 0; + if (*AT91C_PIOA_ODSR & GREEN_PIN) on |= LEDS_GREEN; + if (*AT91C_PIOA_ODSR & YELLOW_PIN) on |= LEDS_YELLOW; + if (*AT91C_PIOA_ODSR & RED_PIN) on |= LEDS_RED; + return on; +} + +void +leds_arch_set(unsigned char leds) +{ + unsigned int set = 0; + if (leds & LEDS_GREEN) set |= GREEN_PIN; + if (leds & LEDS_YELLOW) set |= YELLOW_PIN; + if (leds & LEDS_RED) set |= RED_PIN; + *AT91C_PIOA_CODR = set; + *AT91C_PIOA_SODR = (~set) & ALL_PINS; +} diff --git a/platform/stepper-robot/robot-main.c b/platform/stepper-robot/robot-main.c new file mode 100644 index 000000000..fa1013546 --- /dev/null +++ b/platform/stepper-robot/robot-main.c @@ -0,0 +1,187 @@ +#include +#include +#include +#include +#include +#include +#include +#include "net/uip-fw-service.h" +#include "net/uaodv.h" +#include +#include +#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; + +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"); + +struct uip_fw_netif cc2420if = + {UIP_FW_NETIF(172,16,0,2, 255,255,0,0, cc2420_send_ip)}; + +PROCESS_THREAD(blink_process, ev , data) +{ + static struct etimer timer; + PROCESS_BEGIN(); + etimer_set(&timer, CLOCK_SECOND/2); + while(1) { + PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_EXIT || + ev== PROCESS_EVENT_TIMER); + if (ev == PROCESS_EVENT_EXIT) break; + leds_invert(LEDS_RED); +#if 0 + { + DISABLE_FIFOP_INT(); + printf("FSMSTATE: %04x",cc2420_getreg(CC2420_FSMSTATE)); + ENABLE_FIFOP_INT(); + if (SFD_IS_1) printf(" SFD"); + if (FIFO_IS_1) printf(" FIFO"); + if (FIFOP_IS_1) printf(" FIFOP"); + putchar('\n'); + } +#endif + etimer_reset(&timer); + } + printf("Ended process\n"); + PROCESS_END(); +} + +PROCESS(udprecv_process, "UDP recv process"); + +PROCESS_THREAD(udprecv_process, ev, data) +{ + static struct uip_udp_conn *c; + + PROCESS_EXITHANDLER(goto exit); + PROCESS_BEGIN(); + + printf("udprecv_process starting\n"); + + { + uip_ipaddr_t any; + uip_ipaddr(&any, 0,0,0,0); + c = udp_new(&any, HTONS(0), NULL); + uip_udp_bind(c, HTONS(4321)); + } + + while(1) { + PROCESS_YIELD(); + + if(ev == tcpip_event && uip_newdata()) { + u8_t *src = ((struct uip_udpip_hdr *)uip_buf)->srcipaddr.u8; + printf("%d.%d.%d.%d: %s\n", + src[0], src[1], src[2], src[3], (char *)uip_appdata); + } + } + + exit: + /* Contiki does automatic garbage collection of uIP state and we + * need not worry about that. */ + printf("udprecv_process exiting\n"); + PROCESS_END(); +} + + +PROCESS(wd_test_process, "Watchdog test process"); + + +PROCESS_THREAD(wd_test_process, ev, data) +{ + static struct etimer timer; + PROCESS_BEGIN(); + + printf("tcp_test_process starting\n"); + + etimer_set(&timer, 25*CLOCK_SECOND); + while(1) { + PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_EXIT || + ev== PROCESS_EVENT_TIMER); + if (ev == PROCESS_EVENT_EXIT) break; + printf("Blocking execution\n"); + while(1); + } + + PROCESS_END(); +} + + +#if 0 +/* Wathcdog is already disabled in startup code */ +static void +wdt_setup() +{ + +} +#endif + +static void +wdt_reset() +{ + *AT91C_WDTC_WDCR = (0xa5<<24) | AT91C_WDTC_WDRSTT; +} + +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); + +int +main() +{ + disableIRQ(); + disableFIQ(); + *AT91C_AIC_IDCR = 0xffffffff; + *AT91C_PMC_PCDR = 0xffffffff; + *AT91C_PMC_PCER = (1 << AT91C_ID_PIOA); + + dbg_setup_uart(); + printf("Initialising\n"); + dbg_set_input_handler(recv_input); + leds_arch_init(); + clock_init(); + uip_sethostaddr(&cc2420if.ipaddr); + uip_setnetmask(&cc2420if.netmask); + + /*uip_setdraddr(&gw_addr);*/ + cc2420_init(); + cc2420_set_chan_pan_addr(RF_CHANNEL, HTONS(0x2024), uip_hostaddr.u16[1], NULL); + process_init(); + uip_init(); + uip_fw_default(&cc2420if); + tcpip_set_forwarding(1); + printf("Started\n"); + + procinit_init(); + enableIRQ(); + cc2420_on(); + printf("Processes running\n"); + while(1) { + do { + /* Reset watchdog. */ + wdt_reset(); + } while(process_run() > 0); + /* Idle! */ + /* Stop processor clock */ + *AT91C_PMC_SCDR |= AT91C_PMC_PCK; + } + return 0; +} diff --git a/platform/stepper-robot/sam7s-spi.c b/platform/stepper-robot/sam7s-spi.c new file mode 100644 index 000000000..b1ff7bd8c --- /dev/null +++ b/platform/stepper-robot/sam7s-spi.c @@ -0,0 +1,126 @@ +#include +#include +#include +#include + +/* Prevents interrupts using SPI at inappropriate times */ +unsigned char spi_busy = 0; + +#define SPI_SPEED 1000000 /* 1MHz clock*/ +#define SPI_DLYBCT 1 +#define SPI_DLYBS 20 + +#define SPI_TRANSFER (AT91C_PA12_MISO | AT91C_PA13_MOSI | AT91C_PA14_SPCK) + +#define SPI_CS (AT91C_PA11_NPCS0 | AT91C_PA31_NPCS1) + +void +spi_init() +{ + static uint8_t initialised = 0; + if (!initialised) { + *AT91C_SPI_CR = AT91C_SPI_SPIDIS | AT91C_SPI_SWRST; + *AT91C_PMC_PCER = (1 << AT91C_ID_SPI); + *AT91C_PIOA_ASR = SPI_TRANSFER | SPI_CS; + *AT91C_PIOA_PDR = SPI_TRANSFER | SPI_CS; + *AT91C_PIOA_PPUER = AT91C_PA12_MISO | SPI_CS; + *AT91C_SPI_MR = (AT91C_SPI_MSTR | AT91C_SPI_PS_FIXED + | AT91C_SPI_MODFDIS | AT91C_SPI_PCS); + + /* It seems necessary to set the clock speed for chip select 0 + even if it's not used. */ + AT91C_SPI_CSR[0] = (MCK/SPI_SPEED)<<8; + + *AT91C_SPI_CR = AT91C_SPI_SPIEN; + initialised = 1; + } +} + +void +spi_init_chip_select(unsigned int chip, unsigned int speed, + unsigned int dlybct, + unsigned int dlybs, unsigned int phase, + unsigned int polarity) +{ + spi_init(); + + AT91C_SPI_CSR[chip] = + ((dlybct<<24) | (dlybs<<16) | (((MCK+speed/2)/speed)<<8) + | (phase?AT91C_SPI_NCPHA:0) | (polarity?AT91C_SPI_CPOL:0) + | AT91C_SPI_BITS_8 | AT91C_SPI_CSAAT); +} + +#if 0 +#define DBG_SEND dbg_blocking_putchar('>'); +#define DBG_RECV dbg_blocking_putchar('<'); +#else +#define DBG_SEND +#define DBG_RECV +#endif + +void +spi_transfer(unsigned int chip, const struct spi_block *block, unsigned int blocks) +{ + spi_busy = 1; + while(!(*AT91C_SPI_SR & AT91C_SPI_TXEMPTY)); /* wait unti previous transfer is done */ + + /* Clear any data left in the receiver */ + (void)*AT91C_SPI_RDR; + (void)*AT91C_SPI_RDR; + + /* Select chip */ + *AT91C_SPI_MR = ((*AT91C_SPI_MR & ~AT91C_SPI_PCS) + | ((~(1< 0) { + struct spi_block current = *block++; + if (current.send) { + if (current.receive) { + /* Send and receive */ + while(current.len-- > 0) { + while(!(*AT91C_SPI_SR & AT91C_SPI_TDRE)); + *AT91C_SPI_TDR = *current.send++; + DBG_SEND; + while(!(*AT91C_SPI_SR & AT91C_SPI_RDRF)); + *current.receive++ = *AT91C_SPI_RDR; + DBG_RECV; + } + } else { + /* Send only */ + while(current.len-- > 0) { + while(!(*AT91C_SPI_SR & AT91C_SPI_TDRE)); + *AT91C_SPI_TDR = *current.send++; + DBG_SEND; + while(!(*AT91C_SPI_SR & AT91C_SPI_RDRF)); + (void)*AT91C_SPI_RDR; + DBG_RECV; + } + } + } else { + if (current.receive) { + /* Receive only */ + while(current.len-- > 0) { + while(!(*AT91C_SPI_SR & AT91C_SPI_TDRE)); + *AT91C_SPI_TDR = 0; + DBG_SEND; + while(!(*AT91C_SPI_SR & AT91C_SPI_RDRF)); + *current.receive++ = *AT91C_SPI_RDR; + DBG_RECV; + } + } else { + /* Clock only */ + while(current.len-- > 0) { + while(!(*AT91C_SPI_SR & AT91C_SPI_TDRE)); + *AT91C_SPI_TDR = 0; + DBG_SEND; + while(!(*AT91C_SPI_SR & AT91C_SPI_RDRF)); + (void)*AT91C_SPI_RDR; + DBG_RECV; + } + } + } + } + *AT91C_SPI_CR = AT91C_SPI_LASTXFER; + + spi_busy = 0; +} diff --git a/platform/stepper-robot/sam7s-spi.h b/platform/stepper-robot/sam7s-spi.h new file mode 100644 index 000000000..7a2f41f67 --- /dev/null +++ b/platform/stepper-robot/sam7s-spi.h @@ -0,0 +1,29 @@ +#ifndef __CORE_SPI_H__KBMMOKI6CG__ +#define __CORE_SPI_H__KBMMOKI6CG__ +#include + +void +spi_init(); + +#define SPI_POLARITY_INACTIVE_HIGH 1 +#define SPI_POLARITY_INACTIVE_LOW 1 + +#define SPI_PHASE_CHANGE_CAPTURE 0 +#define SPI_PHASE_CAPTURE_CHANGE 1 + +struct spi_block { + const uint8_t *send; /* NULL for receive only */ + uint8_t *receive; /* NULL for send only */ + uint16_t len; /* transfer length, non-zero */ +}; + +void +spi_transfer(unsigned int chip, const struct spi_block *block, unsigned int blocks); + +void +spi_init_chip_select(unsigned int chip, unsigned int speed, + unsigned int dlybct, + unsigned int dlybs, unsigned int phase, + unsigned int polarity); + +#endif /* __CORE_SPI_H__KBMMOKI6CG__ */ diff --git a/platform/stepper-robot/stepper-process.c b/platform/stepper-robot/stepper-process.c new file mode 100644 index 000000000..5f2a5e7f5 --- /dev/null +++ b/platform/stepper-robot/stepper-process.c @@ -0,0 +1,500 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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); + +static const uint32_t stepper1_steps_acc[] = MICRO_STEP(1,3); +static const uint32_t stepper1_steps_run[] = MICRO_STEP(1,2); +static const uint32_t stepper1_steps_hold[] = MICRO_STEP(1,1); + +static StepperAccSeq seq_heap[40]; + +static void +init_seq_heap() +{ + unsigned int i; + for(i = 0; i < sizeof(seq_heap)/sizeof(seq_heap[0]); i++) { + seq_heap[i].next = NULL; + stepper_free_seq(&seq_heap[i]); + } +} + +static unsigned int +parse_uint_hex(const char **pp, const char *end) +{ + unsigned int v = 0; + while(*pp < end) { + char ch; + if ((ch = **pp) >= '0' && ch <= '9') { + v = v* 16 + (ch - '0'); + } else if (ch >= 'A' && ch <= 'F') { + v = v* 16 + (ch - 'A') + 10; + } else break; + (*pp)++; + } + return v; +} + +static int +parse_int_hex(const char **pp, const char *end) +{ + if (*pp == end) return 0; + if (**pp == '-') { + (*pp)++; + return -parse_uint_hex(pp, end); + } else { + return parse_uint_hex(pp, end); + } +} + +static void +skip_white(const char **pp, const char *end) +{ + char ch; + while(*pp < end && ((ch = **pp) == ' ' || ch == '\t')) (*pp)++; +} + +static const char hex_chars[] = + {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'}; + +static void +format_uint_hex(char **str, char *end, unsigned int v) +{ + char buffer[10]; + char *p = buffer+10; + if (*str == end) return; + if (v == 0) { + *(*str)++ = '0'; + return; + } + while(v > 0) { + *--p = hex_chars[v&0xf]; + v >>= 4; + } + while((p < buffer+10) && (*str < end)) { + *(*str)++ = *p++; + } +} + +static void +format_int_hex(char **str, char *end, int v) +{ + if (v < 0) { + if (*str == end) return; + *(*str)++ = '-'; + v = -v; + } + format_uint_hex(str, end, v); +} + +static void +format_ull_hex(char **str, char *end, unsigned long long int v) +{ + char buffer[16]; + char *p = buffer+10; + if (*str == end) return; + if (v == 0) { + *(*str)++ = '0'; + return; + } + while(v > 0) { + *--p = hex_chars[v&0xf]; + v >>= 4; + } + while((p < buffer+10) && (*str < end)) { + *(*str)++ = *p++; + } +} +static void +format_ll_hex(char **str, char *end, long long v) +{ + if (v < 0) { + if (*str == end) return; + *(*str)++ = '-'; + v = -v; + } + format_ull_hex(str, end, v); +} + +typedef struct _ReplyBuffer ReplyBuffer; + +struct _ReplyBuffer +{ + char buffer[70]; /* Should be small enough to fit in one packet */ + char *write; +}; + +static ReplyBuffer tcp_reply; +static ReplyBuffer udp_reply; + +#define REPLY_BUFFER_END(reply) ((reply)->buffer+sizeof((reply)->buffer)) +#define REPLY_BUFFER_LEFT(reply) \ +((reply)->buffer+sizeof((reply)->buffer) - (reply)->write) + +static void +reply_char(ReplyBuffer *reply, char c) +{ + if (REPLY_BUFFER_LEFT(reply) > 0) { + *reply->write++ = c; + } +} + +static void +reply_str(ReplyBuffer *reply, char *str) +{ + while(reply->write < REPLY_BUFFER_END(reply) && *str != '\0') + *reply->write++ = *str++; +} + +static void +stepper_reply(ReplyBuffer *reply, StepperResult res) +{ + switch(res) { + case STEPPER_OK: + reply_str(reply, "OK"); + break; + case STEPPER_ERR_MEM: + reply_str(reply, "ERR MEM"); + break; + case STEPPER_ERR_TOO_LATE: + reply_str(reply, "ERR LATE"); + break; + case STEPPER_ERR_INDEX: /* Sholdn't happen here */ + reply_str(reply, "ERR INDEX"); + break; + default: + reply_str(reply, "ERR"); + } + reply_char(reply, '\n'); +} + +#define CHECK_INPUT_LEFT(x) \ +do {\ +if ((x) > inend - input_line) {reply_str(reply, "ERR\n");return 0;}\ +} while(0) + +static int +handle_line(const char *input_line, const char *inend, ReplyBuffer *reply) +{ + unsigned long when; + { + const char *p = input_line; + printf("Got line: '"); + while(p < inend) { + putchar(*p++); + } + printf("'\n"); + fsync(1); + } + skip_white(&input_line, inend); + CHECK_INPUT_LEFT(1); + if (*input_line == '#') { + input_line++; + reply_char(reply, '#'); + while (input_line < inend &&*input_line != ' ') { + reply_char(reply, *input_line++); + } + reply_char(reply, ' '); + } + skip_white(&input_line, inend); + + if (*input_line == '@') { + input_line++; + when = parse_uint_hex(&input_line, inend); + } else { + when = stepper_current_period() + 3; + } + skip_white(&input_line, inend); + CHECK_INPUT_LEFT(1); + if (input_line[0] == 'L' || input_line[0] == 'R') { + unsigned int stepper_index = (input_line[0] == 'R' ? 1 : 0); + CHECK_INPUT_LEFT(1); + input_line++; + if (input_line[0] == 'S') { + int speed; + input_line++; + if (input_line == inend) { + printf("Speed: %ld\n", + stepper_current_velocity(stepper_index)/VEL_SCALE); + reply_char(reply, input_line[-2]); + reply_char(reply, 'S'); + format_int_hex(&reply->write, REPLY_BUFFER_END(reply), + stepper_current_velocity(stepper_index)/VEL_SCALE); + reply_char(reply, '\n'); + } else { + speed = parse_int_hex(&input_line, inend); + if (*input_line == ',') { + StepperResult res; + unsigned int acc; + input_line++; + acc = parse_uint_hex(&input_line, inend); + printf("Speed=%d, Acc=%u\n", speed, acc); + res = stepper_set_velocity(stepper_index, &when, + acc, speed*VEL_SCALE); + + stepper_reply(reply, res); + } else { + reply_str(reply, "ERR\n"); + } + } + } else if (input_line[0] == 'C') { + reply_char(reply, input_line[-1]); + reply_char(reply, 'C'); + format_ll_hex(&reply->write, REPLY_BUFFER_END(reply), + stepper_current_step(stepper_index)); + reply_char(reply, '\n'); + } else if (input_line[0] == 'M') { + unsigned int speed; + unsigned int acc; + int move; + input_line++; + speed = parse_uint_hex(&input_line, inend); + CHECK_INPUT_LEFT(1); + if (*input_line == ',') { + input_line++; + acc = parse_uint_hex(&input_line, inend); + if (*input_line == ',') { + StepperResult res; + input_line++; + move = parse_int_hex(&input_line, inend); + printf("Speed=%u, Acc=%u, Move=%d\n", speed, acc, move); + res = stepper_move(stepper_index, &when, + acc,speed*VEL_SCALE,move*DIST_SCALE); + stepper_reply(reply, res); + } else { + reply_str(reply, "ERR\n"); + } + } else { + reply_str(reply, "ERR\n"); + } + } else { + reply_str(reply, "ERR\n"); + } + } else if (input_line[0] == 'E') { + *AT91C_PIOA_SODR = STEPPER_INHIBIT; + printf("Stepper enabled\n"); + reply_str(reply, "OK\n"); + } else if (input_line[0] == 'D') { + *AT91C_PIOA_CODR = STEPPER_INHIBIT; + printf("Stepper disabled\n"); + reply_str(reply, "OK\n"); + } else if (input_line[0] == 'p') { + reply_char(reply, 'p'); + format_int_hex(&reply->write, REPLY_BUFFER_END(reply), cc2420_last_rssi); + reply_char(reply, ','); + format_uint_hex(&reply->write, REPLY_BUFFER_END(reply), + cc2420_last_correlation); + reply_char(reply, '\n'); + } else if (input_line[0] == 'T') { + reply_char(reply, 'T'); + format_int_hex(&reply->write, REPLY_BUFFER_END(reply), + stepper_current_period()); + reply_char(reply, '\n'); + } else if (input_line[0] == 'q') { + return 1; + } else { + reply_str(reply, "ERR\n"); + } + return 0; +} +static unsigned int transmit_len = 0; + +static void +send_reply() +{ + if (transmit_len == 0) { + transmit_len = tcp_reply.write - tcp_reply.buffer; + if (transmit_len > 0) { + /* printf("Sending len = %d\n", transmit_len); */ + uip_send(tcp_reply.buffer, transmit_len); + } + } +} + + +static void +handle_connection() +{ + static char exiting = 0; + static char line_buffer[100]; + static char *line_end; + if (uip_connected()) { + exiting = 0; + transmit_len = 0; + line_end = line_buffer; + tcp_reply.write = tcp_reply.buffer; + reply_str(&tcp_reply, "Ready\n"); + send_reply(); + } + if (uip_acked()) { + if (tcp_reply.write - tcp_reply.buffer > transmit_len) { + memmove(tcp_reply.buffer, tcp_reply.buffer + transmit_len, + tcp_reply.write - tcp_reply.buffer - transmit_len); + } + tcp_reply.write -= transmit_len; + /* printf("Acked: %d left\n", reply_buffer.write-reply_buffer.buffer); */ + transmit_len = 0; + if (exiting && tcp_reply.write == tcp_reply.buffer) { + uip_close(); + exiting = 0; + } + } + if (uip_newdata()) { + const char *read_pos = uip_appdata; + const char *read_end = read_pos + uip_len; + printf("Got data\n"); + while(read_pos < read_end) { + if (line_end == line_buffer+sizeof(line_buffer)) { + /* Buffer too small, just discard everything */ + line_end = line_buffer; + } + *line_end++ = *read_pos++; + if (line_end[-1] == '\n' || line_end[-1] == '\r' || line_end[-1] == ';'){ + if (line_end - 1 != line_buffer) { + if (handle_line(line_buffer, line_end - 1, &tcp_reply)) { + send_reply(); + /* Postpone closing if there's reply data left to be sent. */ + if (transmit_len == 0) + uip_close(); + else + exiting = 1; + break; + } + } + line_end = line_buffer; + } + } + send_reply(); + } + + if (uip_poll()) { + send_reply(); + } + if(uip_rexmit()) { + printf("Retransmit\n"); + if (transmit_len > 0) + uip_send(tcp_reply.buffer, transmit_len); + } + +} + +PROCESS(udp_stepper_process, "UDP stepper process"); + +PROCESS_THREAD(udp_stepper_process, ev, data) +{ + static struct etimer timer; + static struct uip_udp_conn *conn; + static char listening = 1; /* Listen for connections from anyone */ + static uip_ipaddr_t any; + PROCESS_EXITHANDLER(goto exit); + PROCESS_BEGIN(); + + printf("udp_stepper_process starting\n"); + + uip_ipaddr(&any, 0,0,0,0); + conn = udp_new(&any, HTONS(0), NULL); + if (!conn) goto exit; + uip_udp_bind(conn, HTONS(1010)); + etimer_set(&timer, CLOCK_SECOND*2); + while(1) { + PROCESS_YIELD(); + + if(ev == tcpip_event) { + if (uip_newdata()) { + struct uip_udpip_hdr *header = (struct uip_udpip_hdr *)uip_buf; + const char *line_start = uip_appdata; + const char *line_end = line_start; + const char *packet_end = line_start + uip_len; + udp_reply.write = udp_reply.buffer; + while(line_end < packet_end) { + if (*line_end == '\n' || *line_end == '\r' || *line_end == ';' ) { + if (line_end != line_start) { + handle_line(line_start, line_end, &udp_reply); + } + line_start = line_end+1; + } + line_end++; + } + /* Check if we are connected to a client, if not reconnect */ + if (listening) { + uip_udp_remove(conn); + conn = udp_new(&header->srcipaddr, header->srcport, &conn); + if (!conn) goto exit; + uip_udp_bind(conn, HTONS(1010)); + listening = 0; + } + etimer_reset(&timer); + tcpip_poll_udp(conn); + } else if (uip_poll()) { + if (data == &conn) { + uip_send(udp_reply.buffer, udp_reply.write - udp_reply.buffer); + printf("sent %ld\n", udp_reply.write - udp_reply.buffer); + } + } + } else if (ev == PROCESS_EVENT_TIMER) { + uip_udp_remove(conn); + conn = udp_new(&any, HTONS(0), NULL); + if (!conn) goto exit; + uip_udp_bind(conn, HTONS(1010)); + listening = 1; + } + } + + exit: + /* Contiki does automatic garbage collection of uIP state and we + * need not worry about that. */ + printf("udprecv_process exiting\n"); + PROCESS_END(); +} + +PROCESS(stepper_process, "Stepper control process"); + +PROCESS_THREAD(stepper_process, ev, data) +{ + PROCESS_EXITHANDLER(goto exit); + PROCESS_BEGIN(); + tcp_listen(HTONS(1010)); + init_seq_heap(); + stepper_init(AT91C_BASE_TC0, AT91C_ID_TC0); + *AT91C_PIOA_OER = STEPPER_INHIBIT; + *AT91C_PIOA_MDER = STEPPER_INHIBIT; /* | STEPPER0_IOMASK; */ + *AT91C_PIOA_CODR = STEPPER_INHIBIT; + stepper_init_io(1, STEPPER_IOMASK(0), stepper0_steps_acc, + stepper0_steps_run, stepper0_steps_hold, + (sizeof(stepper0_steps_run) / sizeof(stepper0_steps_run[0]))); + stepper_init_io(0, STEPPER_IOMASK(1), stepper1_steps_acc, + stepper1_steps_run, stepper1_steps_hold, + (sizeof(stepper1_steps_run) / sizeof(stepper1_steps_run[0]))); + process_start(&udp_stepper_process, NULL); + printf("Stepper starting\n"); + + while(1) { + PROCESS_WAIT_EVENT_UNTIL(ev == tcpip_event); + if(uip_connected()) { + printf("connected\n"); + handle_connection(); /* Initialise parser */ + while(!(uip_aborted() || uip_closed() || uip_timedout())) { + PROCESS_WAIT_EVENT_UNTIL(ev == tcpip_event); + handle_connection(); + } + } + printf("disconnected\n"); + } + + exit: + /* Contiki does automatic garbage collection of uIP state and we + * need not worry about that. */ + printf("Stepper exiting\n"); + PROCESS_END(); +} + diff --git a/platform/stepper-robot/stepper-process.h b/platform/stepper-robot/stepper-process.h new file mode 100644 index 000000000..fc396749a --- /dev/null +++ b/platform/stepper-robot/stepper-process.h @@ -0,0 +1,7 @@ +#ifndef __STEPPER_PROCESS_H__1OAHVG2XPP__ +#define __STEPPER_PROCESS_H__1OAHVG2XPP__ +#include + +PROCESS_NAME(stepper_process); + +#endif /* __STEPPER_PROCESS_H__1OAHVG2XPP__ */ diff --git a/platform/stepper-robot/stepper/stepper-interrupt.c b/platform/stepper-robot/stepper/stepper-interrupt.c new file mode 100644 index 000000000..2a52b059c --- /dev/null +++ b/platform/stepper-robot/stepper/stepper-interrupt.c @@ -0,0 +1,648 @@ +#include +#include +#include + + +/* Timer frequency */ +#define TIMER_FREQ 748800 + +static 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) +{ + const uint32_t *io_steps; + StepperState *state = step->state; + + + if (step->power >= STEPPER_POWER_ACC) { + io_steps = state->acc_steps; + } else if (step->power >= STEPPER_POWER_RUN) { + io_steps = state->run_steps; + } else { + io_steps = state->hold_steps; + } + if (io_steps) { + if (step->direction == STEPPER_DIRECTION_FORWARD){ + state->step_count++; + /* dbg_putchar('+'); */ + if (++state->current_step == state->sequence_length) + state->current_step = 0; + } else { + state->step_count--; + /* dbg_putchar('-'); */ + if (state->current_step-- == 0) + state->current_step = state->sequence_length-1; + } + *AT91C_PIOA_ODSR = (*AT91C_PIOA_ODSR & ~state->io_mask) + | (io_steps[state->current_step] & state->io_mask); +#ifdef TIMING_ERRORS + { + long err = ((long)stepper_context.timer_channel->TC_CV - (long)step->time); + if (err >= (TIMER_FREQ/PPS/2)) { + err -= TIMER_FREQ/PPS; + } else if (err < -(TIMER_FREQ/PPS/2)) { + err += TIMER_FREQ/PPS; + } + if (err < state->err_min) state->err_min = err; + if (err > state->err_max) state->err_max = err; + } +#endif + } +} + +static void +set_hold(StepperState *state) { + *AT91C_PIOA_ODSR = (*AT91C_PIOA_ODSR & ~state->io_mask) + | (state->hold_steps[state->current_step] & state->io_mask); +} +static void +advance_step() +{ + StepperTimerStep *current =stepper_context.current_step; + AT91PS_TC timer = stepper_context.timer_channel; + unsigned int now = timer->TC_CV; + while (current && current->time <= now) { + do_step(current); + current = current->next; + if (!current) break; + timer->TC_RA = current->time; + now = timer->TC_CV; + } + stepper_context.current_step = current; +} + + +static inline int64_t +mulsu48_16(int64_t a, uint32_t b) +{ + return a*(int64_t)b; +} + +/* Find a solution for s = a*t*t +v * t in the interval [t_low, t_high[ */ +static unsigned long +solve_dist(long long s, long a, long long v, unsigned long t_low, unsigned long t_high) +{ + long long s_low = mulsu48_16((a*(long)t_low+ v), t_low); + long long s_high = mulsu48_16((a*(long)t_high + v), t_high); + if (s >= s_low && s <= s_high) { + while(t_low + 2 < t_high) { + unsigned long t = (t_high + t_low) / 2; + long long s_mid = mulsu48_16((a*(long)t + v), t); + if (s < s_mid) { + t_high = t; + s_high = s_mid; + } else { + t_low = t; + s_low = s_mid; + } + } + } else { + while(t_low + 1 < t_high) { + unsigned long t = (t_high + t_low) / 2; + long long s_mid = mulsu48_16((a*(long)t + v), t); + if (s > s_mid) { + t_high = t; + s_high = s_mid; + } else { + t_low = t; + s_low = s_mid; + } + } + } + return (t_high + t_low) / 2; +} + + +#define HEAP_SIZE 65 +static StepperTimerStep step_heap[2][HEAP_SIZE]; +static unsigned short heap_pos = 0; /* Next free position in heap */ +static unsigned char current_heap = 0; + +static StepperTimerStep * +allocate_step() +{ + if (heap_pos >= HEAP_SIZE) return NULL; + return &step_heap[current_heap][heap_pos++]; +} + +static void +switch_step_heap() +{ + current_heap ^= 1; + heap_pos = 0; +} + +StepperTimerStep ** +insert_step(StepperTimerStep **at, StepperState *state, + unsigned int time, uint8_t direction, uint8_t power) +{ + StepperTimerStep *new_step; + while(*at && (*at)->time <= time) { + at = &(*at)->next; + } + new_step = allocate_step(); + if (!new_step) return at; + new_step->next = *at; + new_step->state = state; + new_step->time = time; + new_step->direction = direction; + new_step->power = power; + *at = new_step; + /* dbg_putchar('!'); */ + return &new_step->next; +} + +/* Determine suitable power for the current state */ +static uint8_t +get_power(StepperState *state) +{ + if (state->acceleration != 0) return STEPPER_POWER_ACC; + if (state->velocity == 0) return STEPPER_POWER_HOLD; + return STEPPER_POWER_RUN; +} + +#define SQ(x) ((x)*(x)) +#define S_SCALING ((2LL*SQ((long long)TIMER_FREQ)) / DIST_SCALE ) +#define V_SCALING (2LL*TIMER_FREQ/VEL_SCALE) + + +static void +step_interval(StepperState *state) +{ + unsigned int i; + long long v = state->velocity * V_SCALING; + long long a = state->acceleration; + unsigned long t = 0; + StepperTimerStep **at = &stepper_context.steps; + if (state->n_steps >= 0) { + long long s = -state->step_frac * S_SCALING; + for (i = 0; i < state->n_steps; i++) { + s+= DIST_SCALE * S_SCALING; + t = solve_dist(s, a, v, t, TIMER_FREQ/PPS); + /* printf("F%ld\n", t); */ + at = insert_step(at, state, t, STEPPER_DIRECTION_FORWARD, get_power(state)); + } + } else { + long long s = (DIST_SCALE - state->step_frac) * S_SCALING; + for (i = 0; i < -state->n_steps; i++) { + s-= DIST_SCALE * S_SCALING; + t = solve_dist(s, a, v, t, TIMER_FREQ/PPS); + /* printf("B%ld\n", t); */ + at = insert_step(at, state, t, STEPPER_DIRECTION_BACKWARD, get_power(state)); + } + } +} +static void +setup_speed(StepperState *state) +{ + long steps; + long step_frac; + /* printf("%ld v= %ld s=%ld\n",stepper_context.period_count, */ +/* state->velocity, state->step_frac); */ + step_frac = (state->acceleration + 2 * state->velocity + + state->step_frac); + steps = step_frac / DIST_SCALE; + step_frac -= steps * DIST_SCALE; + if (step_frac <0) { + step_frac += DIST_SCALE; + steps--; + } + + /* printf("step_frac=%ld (%f) steps=%ld\n",step_frac, */ +/* (double)step_frac/(double)(DIST_SCALE), steps); */ + state->n_steps = steps; + step_interval(state); + state->velocity += state->acceleration; + state->step_frac = step_frac; + state->step_full += steps; +} + +static void +advance_period() +{ + unsigned int s; + StepperTimerStep *current =stepper_context.current_step; + /* Do any remaining step */ + while (current) { + do_step(current); + current = current->next; + } + /* Start from the beginning */ + stepper_context.current_step = stepper_context.steps; + stepper_context.steps = NULL; + if (stepper_context.current_step) { + stepper_context.timer_channel->TC_RA = stepper_context.current_step->time; + } else { + stepper_context.timer_channel->TC_RA = 0xffff; + } + /* In case there is a step very early in the period */ + advance_step(); + stepper_context.period_count++; + *AT91C_AIC_EOICR = 0; + for(s = 0; s < NUM_STEPPERS; s++) { + StepperState *state = &stepper_context.steppers[s]; + StepperAccSeq *acc_seq; + if (state->acceleration == 0 && state->velocity == 0) { + /* Set hold power if stationary */ + stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; + set_hold(state); + stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; + } + while ((acc_seq = state->acceleration_sequence) + && acc_seq->period == stepper_context.period_count + 1) { + state->acceleration_sequence = acc_seq->next; + if (acc_seq->acceleration == STEPPER_ACC_INVALID) { + if (stepper_context.user_callback) { + stepper_context.user_callback(s, stepper_context.period_count); + } + } else { + state->acceleration = acc_seq->acceleration; + } + acc_seq->next = NULL; /* Only free this one */ + stepper_free_seq(acc_seq); + } + setup_speed(&stepper_context.steppers[s]); + } + /* Prepare heap for next period */ + switch_step_heap(); +} + +/* Here we have a proper stack frame and can use local variables */ +static void stepper_int_safe() __attribute((noinline)); +static void +stepper_int_safe() +{ + unsigned int status; + status = stepper_context.timer_channel->TC_SR; + if (status & AT91C_TC_CPAS) { + advance_step(); + /* dbg_putchar('*'); */ + } + if (status & AT91C_TC_CPCS) { + advance_period(); + } else { + *AT91C_AIC_EOICR = 0; /* End of Interrupt */ + } + +} + +void NACKEDFUNC stepper_int (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 + 1; + + 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 + 1; + 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); + 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 new file mode 100644 index 000000000..96aac563b --- /dev/null +++ b/platform/stepper-robot/stepper/stepper-interrupt.h @@ -0,0 +1,159 @@ +#ifndef __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ +#define __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ + +#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 _StepperContext StepperContext; +typedef struct _StepperState StepperState; +typedef struct _StepperTimerStep StepperTimerStep; +typedef struct _StepperAccSeq StepperAccSeq; + +#define MAX_STEPS_PER_PERIOD 40 +#define NUM_STEPPERS 2 + +#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 + +struct _StepperState +{ + long step_count; + uint32_t io_mask; + const uint32_t *acc_steps; /* Stepping sequence when accelerating */ + const uint32_t *run_steps; /* Stepping sequence when running */ + const uint32_t *hold_steps; /* Stepping sequence when stationary */ + uint8_t current_step; /* in stepping sequence */ + uint8_t sequence_length; + + long velocity; /* steps/second * PPS */ + long acceleration; /* steps/second^2 */ + long step_full; /* steps, same as step_count at period boundaries */ + long step_frac; /* (steps * PPS^2 * 2) % steps * PPS^2 */ + + long n_steps; /* full steps to move during this period */ + + StepperAccSeq *acceleration_sequence; + +#ifdef TIMING_ERRORS + long err_max; + long err_min; +#endif +}; + +#define STEPPER_POWER_ACC 30 +#define STEPPER_POWER_RUN 20 +#define STEPPER_POWER_HOLD 10 +#define STEPPER_POWER_OFF 0 + +#define STEPPER_DIRECTION_NONE 0 +#define STEPPER_DIRECTION_FORWARD 1 +#define STEPPER_DIRECTION_BACKWARD 2 + +struct _StepperTimerStep +{ + StepperTimerStep *next; + StepperState *state; + uint16_t time; + uint8_t direction; + uint8_t power; +}; + +typedef void (*StepperUserCallback)(unsigned int stepper_index, + unsigned long period); + +struct _StepperContext +{ + unsigned int flags; + unsigned long period_count; + AT91PS_TC timer_channel; + StepperState steppers[NUM_STEPPERS]; + StepperTimerStep *steps; + StepperTimerStep *current_step; + 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 + +#endif /* __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ */ diff --git a/platform/stepper-robot/stepper/stepper-move.c b/platform/stepper-robot/stepper/stepper-move.c new file mode 100644 index 000000000..8fe6df631 --- /dev/null +++ b/platform/stepper-robot/stepper/stepper-move.c @@ -0,0 +1,193 @@ +#include +#include +#include +#include + +#if 0 +#define PRINTF(...) printf(__VA_ARGS__) +#else +#define PRINTF(...) do {} while (0) +#endif + +static unsigned int +isqrt(unsigned long x) +{ + unsigned int r; + unsigned int b2 = 0x40000000; + unsigned int b = 0x8000; + while(x < b2) { + b2 >>= 2; + b >>= 1; + } + if (b == 0) return 0; + r = b; + b >>= 1; + while(b > 0) { + r += b; + unsigned int t = r*r; + if (t > x) { + r -= b; + } + b >>=1; + } + return r; +} + +#define ACC_FIRST_UP 0 +#define ACC_K1_UP 1 +#define ACC_LAST_UP 2 +#define ACC_TOP 3 +#define ACC_FIRST_DOWN 4 +#define ACC_K1_DOWN 5 +#define ACC_LAST_DOWN 6 +#define ACC_END 7 + +typedef struct _AccDiff AccDiff; +struct _AccDiff +{ + long diff; + unsigned long pos; +}; + + +static inline long +base_acc(unsigned long t,unsigned long n, unsigned long l, unsigned long a_max) +{ + long a; + if (t >= n) { + if (t >= n+l) { + a = -a_max; + } else { + a = 0; + } + } else { + a = a_max; + } + return a; +} + +static AccDiff acc[ACC_END+1]; +StepperResult +stepper_move(unsigned int stepper_index, unsigned long *periodp, + unsigned long a_max,unsigned long v_max, long s_end) +{ + unsigned long start_period = *periodp; + unsigned long s; + unsigned long ds; + unsigned long l; + unsigned long da0; + unsigned long k1 = 0; + unsigned long n = (v_max+a_max-1)/a_max; + unsigned long a_speed_adj = v_max - (n-1)*a_max; + unsigned long s_res; + long d; + if (s_end >= 0) { + s_res = s_end/2; + } else { + s_res = (-s_end)/2; + } + d = s_res - (long)a_max*(n*n-1) - (long)a_speed_adj; + + acc[ACC_END].diff = 0; + acc[ACC_END].pos = UINT_MAX; + if (d < 0) { + l = 0; + n = isqrt(s_res/a_max); + if (n*(unsigned long long)n*a_max < s_res) n++; + a_speed_adj = a_max; + acc[ACC_LAST_UP].diff=0; + acc[ACC_FIRST_DOWN].diff=0; + } else { + l = (d+v_max-1)/v_max; + acc[ACC_LAST_UP].diff= a_speed_adj - a_max; + acc[ACC_FIRST_DOWN].diff= a_max - a_speed_adj; + } + acc[ACC_LAST_UP].pos = n-1; + acc[ACC_FIRST_DOWN].pos = n+l; + + s = a_max*(n*n-1) + a_speed_adj + l * (a_max*(n-1) + a_speed_adj); + ds = s-s_res; + + da0 = ds/(2*n+l-1); + acc[ACC_FIRST_UP].diff = -da0; + acc[ACC_LAST_DOWN].diff = da0; + acc[ACC_FIRST_UP].pos = 0; + acc[ACC_LAST_DOWN].pos = 2*n+l-1; + ds -= da0*(2*n+l-1); + + acc[ACC_K1_UP].diff = 0; + acc[ACC_K1_DOWN].diff = 0; + acc[ACC_K1_UP].pos = 0; + acc[ACC_K1_DOWN].pos = 2*n+l-1; + + acc[ACC_TOP].diff = 0; + acc[ACC_TOP].pos = n; + + if (ds > 0) { + k1 = (2*n+l -ds)/2; + if (k1 < n) { + + acc[ACC_K1_UP].diff = -1; + acc[ACC_K1_DOWN].diff = 1; + acc[ACC_K1_UP].pos = k1; + acc[ACC_K1_DOWN].pos = 2*n+l-1 - k1; + ds -= (2*(n-k1)+l-1); + } + if (ds > 0) { + acc[ACC_LAST_UP].diff--; + acc[ACC_TOP].diff = 1; + acc[ACC_TOP].pos = n+ds-1; + } + } +#if 0 + { + unsigned int k; + PRINTF("n=%ld l=%ld a_max=%ld v_max=%ld s_res=%ld\n", + n,l ,a_max, v_max, s_res); + for (k = 0; k < 7; k++) { + PRINTF(" %ld@%ld", acc[k].diff, acc[k].pos); + } + PRINTF("\n"); + } +#endif + { + StepperResult res; + unsigned int k; + unsigned long t = 0; + long da = 0; + long a_prev = ULONG_MAX; + for (k = 0; k < ACC_END; k++) { + long a; + da += acc[k].diff; + if (acc[k].pos != acc[k+1].pos) { /* Next position is different */ + if (t != acc[k].pos) { + a = base_acc(t,n,l,a_max); + if (s_end < 0) a = -a; + if (a_prev != a) { + res = stepper_add_acc(stepper_index, t+start_period, a); + if (res != STEPPER_OK) return res; + PRINTF("%d: %ld@%ld\n", stepper_index, a, t+start_period); + a_prev = a; + } + t = acc[k].pos; + } + a = da + base_acc(t,n,l,a_max); + if (s_end < 0) a = -a; + if (a_prev != a) { + res = stepper_add_acc(stepper_index, t+start_period, a); + if (res != STEPPER_OK) return res; + PRINTF("%d: %ld@%ld\n", stepper_index, a, t+start_period); + a_prev = a; + } + t++; + da = 0; + } + } + res = stepper_add_acc(stepper_index, t+start_period, 0); + PRINTF("%d: %d@%ld\n", stepper_index, 0, t+start_period); + if (res != STEPPER_OK) return res; + *periodp += t; + } + return STEPPER_OK; +} + diff --git a/platform/stepper-robot/stepper/stepper-move.h b/platform/stepper-robot/stepper/stepper-move.h new file mode 100644 index 000000000..5bea1cd3f --- /dev/null +++ b/platform/stepper-robot/stepper/stepper-move.h @@ -0,0 +1,8 @@ +#ifndef __STEPPER_MOVE_H__9UFUHMYMYS__ +#define __STEPPER_MOVE_H__9UFUHMYMYS__ + +StepperResult +stepper_move(unsigned int stepper_index, unsigned long *periodp, + unsigned long a_max,unsigned long v_max, long s_end); + +#endif /* __STEPPER_MOVE_H__9UFUHMYMYS__ */ diff --git a/platform/stepper-robot/stepper/stepper-steps.h b/platform/stepper-robot/stepper/stepper-steps.h new file mode 100644 index 000000000..7ff13b42d --- /dev/null +++ b/platform/stepper-robot/stepper/stepper-steps.h @@ -0,0 +1,100 @@ +#include + +#ifndef MAX +#define MIN(a,b) (((a) > (b)) ? (b) : (a)) +#endif + +/* Pins for stepper 0 */ +#define STEPPER0_I00 AT91C_PIO_PA17 +#define STEPPER0_I01 AT91C_PIO_PA18 +#define STEPPER0_PHASE0 AT91C_PIO_PA19 + +#define STEPPER0_I10 AT91C_PIO_PA21 +#define STEPPER0_I11 AT91C_PIO_PA22 +#define STEPPER0_PHASE1 AT91C_PIO_PA23 + +/* Pins for stepper 1 */ +#define STEPPER1_I00 AT91C_PIO_PA24 +#define STEPPER1_I01 AT91C_PIO_PA25 +#define STEPPER1_PHASE0 AT91C_PIO_PA26 + +#define STEPPER1_I10 AT91C_PIO_PA27 +#define STEPPER1_I11 AT91C_PIO_PA28 +#define STEPPER1_PHASE1 AT91C_PIO_PA29 + +/* Common pins */ +#define STEPPER_INHIBIT AT91C_PIO_PA7 + + +#define STEPPER_IOMASK(s) (STEPPER##s##_I00 | STEPPER##s##_I01 | STEPPER##s##_PHASE0 \ + | STEPPER##s##_I10 | STEPPER##s##_I11 | STEPPER##s##_PHASE1) +#define STEPPER_PHASE(s,p,l) ((((l) < 2) ? (STEPPER##s##_I##p##1) : 0) | (((l) & 1) ? 0 : (STEPPER##s##_I##p##0))) + +#define STEPPER_STEP(s, l0, l1) (\ +((l0 >= 0) ? STEPPER_PHASE(s,0, l0):(STEPPER_PHASE(s,0, -(l0))|STEPPER##s##_PHASE0)) | \ +((l1 >= 0) ? STEPPER_PHASE(s,1, l1):(STEPPER_PHASE(s,1,-(l1))|STEPPER##s##_PHASE1))) + + +#define FULL_STEP_BOTH(s,l) {\ + STEPPER_STEP(s,(l),(l)), \ + STEPPER_STEP(s,(l),-(l)), \ + STEPPER_STEP(s,-(l),-(l)), \ + STEPPER_STEP(s,-(l),(l))} + + +#define FULL_STEP_SINGLE(s,l) {\ + STEPPER_STEP(s,0,(l)), \ + STEPPER_STEP(s,(l),0), \ + STEPPER_STEP(s,0,-(l)), \ + STEPPER_STEP(s,-(l),0)} + +#define HALF_STEP(s,l) {\ + STEPPER_STEP(s,0,(l)), \ + STEPPER_STEP(s,(l),(l)), \ + STEPPER_STEP(s,(l),0), \ + STEPPER_STEP(s,(l),-(l)), \ + STEPPER_STEP(s,0,-(l)), \ + STEPPER_STEP(s,-(l),-(l)), \ + STEPPER_STEP(s,-(l),0), \ + STEPPER_STEP(s,-(l),(l))} + + +#define MINI_STEP(s,l) {\ + STEPPER_STEP(s,0,(l)), \ + STEPPER_STEP(s,1,MIN((l),2)), \ + STEPPER_STEP(s,MIN((l),2),1), \ + STEPPER_STEP(s,(l),0), \ + STEPPER_STEP(s,MIN((l),2),-1), \ + STEPPER_STEP(s,1,-MIN((l),2)), \ + STEPPER_STEP(s,0,-(l)), \ + STEPPER_STEP(s,-1,-MIN((l),2)), \ + STEPPER_STEP(s,-MIN((l),2),-1), \ + STEPPER_STEP(s,-(l),0), \ + STEPPER_STEP(s,-MIN((l),2),1), \ + STEPPER_STEP(s,-1,MIN((l),2))} + +#define MICRO_STEP(s,l) {\ + STEPPER_STEP(s,0,(l)), \ + STEPPER_STEP(s,1,(l)), \ + STEPPER_STEP(s,MIN((l),2),(l)), \ + STEPPER_STEP(s,(l),(l)), \ + STEPPER_STEP(s,(l),MIN((l),2)), \ + STEPPER_STEP(s,(l),1), \ + STEPPER_STEP(s,(l),0), \ + STEPPER_STEP(s,(l),-1), \ + STEPPER_STEP(s,(l),-MIN((l),2)), \ + STEPPER_STEP(s,(l),-(l)), \ + STEPPER_STEP(s,MIN((l),2),-(l)), \ + STEPPER_STEP(s,1,-(l)), \ + STEPPER_STEP(s,0,-(l)), \ + STEPPER_STEP(s,-1,-(l)), \ + STEPPER_STEP(s,-MIN((l),2),-(l)), \ + STEPPER_STEP(s,-(l),-(l)), \ + STEPPER_STEP(s,-(l),-MIN((l),2)), \ + STEPPER_STEP(s,-(l),-1), \ + STEPPER_STEP(s,-(l),0), \ + STEPPER_STEP(s,-(l),1), \ + STEPPER_STEP(s,-(l),MIN((l),2)), \ + STEPPER_STEP(s,-(l),(l)), \ + STEPPER_STEP(s,-MIN((l),2),(l)), \ + STEPPER_STEP(s,-1,(l))} diff --git a/platform/stepper-robot/sys-tst.c b/platform/stepper-robot/sys-tst.c new file mode 100644 index 000000000..d8b7e6d88 --- /dev/null +++ b/platform/stepper-robot/sys-tst.c @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +volatile const char * volatile input_line = NULL; +volatile unsigned int input_line_len = 0; + +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"); + +PROCESS_THREAD(blink_process, ev , data) +{ + static struct etimer timer; + PROCESS_BEGIN(); + etimer_set(&timer, CLOCK_SECOND/2); + while(1) { + PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_EXIT || + ev== PROCESS_EVENT_TIMER); + if (ev == PROCESS_EVENT_EXIT) break; + leds_invert(LEDS_RED); +#if 0 + { + DISABLE_FIFOP_INT(); + printf("FSMSTATE: %04x",cc2420_getreg(CC2420_FSMSTATE)); + ENABLE_FIFOP_INT(); + if (SFD_IS_1) printf(" SFD"); + if (FIFO_IS_1) printf(" FIFO"); + if (FIFOP_IS_1) printf(" FIFOP"); + putchar('\n'); + } +#endif + etimer_reset(&timer); + } + printf("Ended process\n"); + PROCESS_END(); +} +PROCINIT(&etimer_process, &blink_process); +int +main() +{ + disableIRQ(); + disableFIQ(); + *AT91C_AIC_IDCR = 0xffffffff; + *AT91C_PMC_PCDR = 0xffffffff; + *AT91C_PMC_PCER = (1 << AT91C_ID_PIOA); + + dbg_setup_uart(); + printf("Initialising\n"); + dbg_set_input_handler(recv_input); + leds_arch_init(); + clock_init(); + + process_init(); + printf("Started\n"); + + procinit_init(); + enableIRQ(); + printf("Processes running\n"); + while(1) { + do { + /* Reset watchdog. */ + } while(process_run() > 0); + /* Idle! */ + /* Stop processor clock */ + *AT91C_PMC_SCDR |= AT91C_PMC_PCK; + } + return 0; +}