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

Platform specific code for a robot using stepper motors.

Browse source
This commit is contained in:
ksb 2007-02-24 00:21:16 +00:00
parent 13af443115
commit 20eaa31eff
17 changed files with 2262 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

34
platform/stepper-robot/Makefile Normal file
View file

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

47
platform/stepper-robot/cc2420-interrupt.c Normal file
View file

@ -0,0 +1,47 @@
#include <AT91SAM7S64.h>
#include <contiki-conf.h>
#include <sys/process.h>
#include <net/uip.h>
#include <dev/cc2420.h>
#include <interrupt-utils.h>
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
}

6
platform/stepper-robot/cc2420-interrupt.h Normal file
View file

@ -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__ */

23
platform/stepper-robot/cc2420-spi.c Normal file
View file

@ -0,0 +1,23 @@
#include <AT91SAM7S64.h>
#include <net/uip.h>
#include <dev/cc2420.h>
#include <sam7s-spi.h>
#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);
}

77
platform/stepper-robot/contiki-conf.h Normal file
View file

@ -0,0 +1,77 @@
#ifndef __CONTIKI_CONF_H__CDBB4VIH3I__
#define __CONTIKI_CONF_H__CDBB4VIH3I__
#include <stdint.h>
#include <cc2420-interrupt.h>
#include <AT91SAM7S64.h>
#include <io.h>
#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<<CC2420_DEFAULT_DEV) & 0x0f) << 16));\
} while(0)
/* Automatically enabled when transmitting */
#define SPI_DISABLE() (*AT91C_SPI_CR = AT91C_SPI_LASTXFER)
#define CLEAR_FIFOP_INT() /* Cleared by the wrapper */
#define LOOP_20_SYMBOLS (MCK/28125) /* Assume the loop takes 9 cycles */
/* uIP configuration */
#define UIP_CONF_LLH_LEN 0
#define UIP_CONF_BROADCAST 1
#define UIP_CONF_LOGGING 1
/* Prefix for relocation sections in ELF files */
#define REL_SECT_PREFIX ".rel"
#endif /* __CONTIKI_CONF_H__CDBB4VIH3I__ */

38
platform/stepper-robot/leds-arch.c Normal file
View file

@ -0,0 +1,38 @@
#include <dev/leds.h>
#include <AT91SAM7S64.h>
#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;
}

187
platform/stepper-robot/robot-main.c Normal file
View file

@ -0,0 +1,187 @@
#include <AT91SAM7S64.h>
#include <interrupt-utils.h>
#include <string.h>
#include <debug-uart.h>
#include <ctype.h>
#include <stdio.h>
#include <net/uip.h>
#include "net/uip-fw-service.h"
#include "net/uaodv.h"
#include <dev/cc2420.h>
#include <dev/cc2420_const.h>
#include <dev/spi.h>
#include <sys/process.h>
#include <sys/procinit.h>
#include <sys/etimer.h>
#include <net/psock.h>
#include <stepper-process.h>
#include <dev/leds.h>
#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;
}

126
platform/stepper-robot/sam7s-spi.c Normal file
View file

@ -0,0 +1,126 @@
#include <AT91SAM7S64.h>
#include <stdint.h>
#include <dev/spi.h>
#include <sam7s-spi.h>
/* 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<<chip) & 0x0f) << 16));
while(blocks-- > 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;
}

29
platform/stepper-robot/sam7s-spi.h Normal file
View file

@ -0,0 +1,29 @@
#ifndef __CORE_SPI_H__KBMMOKI6CG__
#define __CORE_SPI_H__KBMMOKI6CG__
#include <stdint.h>
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__ */

500
platform/stepper-robot/stepper-process.c Normal file
View file

@ -0,0 +1,500 @@
#include <stepper-process.h>
#include <stepper-steps.h>
#include <stepper-interrupt.h>
#include <stepper-move.h>
#include <string.h>
#include <interrupt-utils.h>
#include <stdio.h>
#include <unistd.h>
#include <net/uip.h>
#include <dev/cc2420.h>
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();
}

7
platform/stepper-robot/stepper-process.h Normal file
View file

@ -0,0 +1,7 @@
#ifndef __STEPPER_PROCESS_H__1OAHVG2XPP__
#define __STEPPER_PROCESS_H__1OAHVG2XPP__
#include <sys/process.h>
PROCESS_NAME(stepper_process);
#endif /* __STEPPER_PROCESS_H__1OAHVG2XPP__ */

648
platform/stepper-robot/stepper/stepper-interrupt.c Normal file
View file

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

159
platform/stepper-robot/stepper/stepper-interrupt.h Normal file
View file

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

193
platform/stepper-robot/stepper/stepper-move.c Normal file
View file

@ -0,0 +1,193 @@
#include <stdio.h>
#include <stepper-interrupt.h>
#include <stepper-move.h>
#include <limits.h>
#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;
}

8
platform/stepper-robot/stepper/stepper-move.h Normal file
View file

@ -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__ */

100
platform/stepper-robot/stepper/stepper-steps.h Normal file
View file

@ -0,0 +1,100 @@
#include <AT91SAM7S64.h>
#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))}

80
platform/stepper-robot/sys-tst.c Normal file
View file

@ -0,0 +1,80 @@
#include <AT91SAM7S64.h>
#include <stdio.h>
#include <sys/process.h>
#include <sys/procinit.h>
#include <sys/etimer.h>
#include <dev/leds.h>
#include <debug-uart.h>
#include <interrupt-utils.h>
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;
}