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

Removed the stepper-robot platform

Browse source
This commit is contained in:
Adam Dunkels 2013-11-18 23:58:45 +01:00
parent 13117203e0
commit d68ce09fd5
27 changed files with 0 additions and 2792 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

25
platform/stepper-robot/Makefile
View file

@ -1,25 +0,0 @@
TARGET=stepper-robot
all: core
core:
$(MAKE) core.$(TARGET)
CONTIKI=../..
CORE=core-nosyms.$(TARGET)
CONTIKI_TARGET_MAIN=contiki-main.c
codeprop: $(CONTIKI)/tools/codeprop.c
cc -g -Wall $< -o $@
clean: stepper_robot_clean
.PHONY: stepper_robot_clean
stepper_robot_clean:
-rm codeprop
include $(CONTIKI)/Makefile.include

29
platform/stepper-robot/Makefile.stepper-robot
View file

@ -1,29 +0,0 @@
STEPPER_ROBOT = leds.c leds-arch.c sam7s-spi.c stepper-process.c stepper.c stepper-interrupt.c stepper-move.c
#CODEPROP_SOURCES = codeprop-otf.c ram-segments.c
UIPDRIVERS= cc2420.c cc2420-interrupt.c \
cc2420-spi.c slip.c
CONTIKI_TARGET_DIRS = . stepper
# Master clock frequency
MCK=23961600
CFLAGS+=-DAUTOSTART_ENABLE
ifndef CONTIKI_TARGET_MAIN
CONTIKI_TARGET_MAIN = robot-main.c
endif
CONTIKI_TARGET_SOURCEFILES += $(SENSORS) $(STEPPER_ROBOT) \
$(CODEPROP_SOURCES) $(CONTIKI_TARGET_MAIN)
# include $(CONTIKI)/platform/$(TARGET)/apps/Makefile.apps
include $(CONTIKI)/cpu/arm/at91sam7s/Makefile.at91sam7s
#contiki-$(TARGET).a: ${addprefix $(OBJECTDIR)/,symbols.o}
ifndef BASE_IP
BASE_IP := 172.16.1.1
endif

51
platform/stepper-robot/cc2420-interrupt.c
View file

@ -1,51 +0,0 @@
#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_interrupt();
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); */
}
#ifndef __MAKING_DEPS__
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
}
#endif /* __MAKING_DEPS__ */

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

@ -1,6 +0,0 @@
#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
View file

@ -1,23 +0,0 @@
#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);
}

98
platform/stepper-robot/contiki-conf.h
View file

@ -1,98 +0,0 @@
#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
/* These names are deprecated, use C99 names. */
typedef uint8_t u8_t;
typedef uint16_t u16_t;
typedef uint32_t u32_t;
typedef int8_t s8_t;
typedef int16_t s16_t;
typedef int32_t s32_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 */
/* LOOP count for waiting 20 symbols in the CC2420 code - same as MSP? */
#define CC2420_CONF_SYMBOL_LOOP_COUNT 800
#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 CCA_IS_1 1
#define SET_RESET_INACTIVE() setreg(CC2420_MAIN, 0xf800);
#define SET_RESET_ACTIVE() 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() (*AT91C_AIC_ICCR = (1 << AT91C_ID_IRQ1))
#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
#define UIP_CONF_BUFFER_SIZE 116
#define UIP_CONF_TCP_FORWARD 1
/* USB definitions */
#define USB_EP1_SIZE 64
#define USB_EP2_SIZE 64
#define USB_EP3_SIZE 64
#define USB_RBC_NUM_BLOCKS 16
/* Prefix for relocation sections in ELF files */
#define REL_SECT_PREFIX ".rel"
#define CC_BYTE_ALIGNED __attribute__ ((packed, aligned(1)))
#endif /* __CONTIKI_CONF_H__CDBB4VIH3I__ */

177
platform/stepper-robot/contiki-main.c
View file

@ -1,177 +0,0 @@
#include <AT91SAM7S64.h>
#include <interrupt-utils.h>
#include <string.h>
#include <debug-uart.h>
#include <ctype.h>
#include <stdio.h>
#include <dev/cc2420.h>
#include <dev/cc2420_const.h>
#include <dev/spi.h>
#include <dev/leds.h>
#include <sys/process.h>
#include <sys/procinit.h>
#include <sys/autostart.h>
#include <sys/etimer.h>
#include <net/psock.h>
#include <unistd.h>
#include "net/mac/nullmac.h"
#include "net/rime.h"
#include "contiki-main.h"
#ifndef RF_CHANNEL
#define RF_CHANNEL 15
#endif
#if WITH_UIP
#include "net/uip.h"
#include "net/uip-fw.h"
#include "net/uip-fw-drv.h"
#include "net/uip-over-mesh.h"
static struct uip_fw_netif meshif =
{UIP_FW_NETIF(172,16,0,0, 255,255,0,0, uip_over_mesh_send)};
#define UIP_OVER_MESH_CHANNEL 9
uint8_t is_gateway = 0;
struct trickle_conn gateway_trickle;
#endif /* WITH_UIP */
extern char __heap_end__;
extern char __heap_start__;
rimeaddr_t node_addr __attribute__((weak)) = {{0,2}};
/*--------------------------------------------------------------------------*/
#if WITH_UIP
/* Receive address of gateway */
static void
trickle_recv(struct trickle_conn *c)
{
if (!is_gateway) {
struct gateway_msg *msg;
msg = rimebuf_dataptr();
printf("%d.%d: gateway message: %d.%d\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1],
msg->gateway.u8[0], msg->gateway.u8[1]);
uip_over_mesh_set_gateway(&msg->gateway);
}
}
const static struct trickle_callbacks trickle_call = {trickle_recv};
/*---------------------------------------------------------------------------*/
#endif
#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;
}
#if 0
static uip_ipaddr_t gw_addr = {{172,16,0,1}};
#endif
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");
leds_init();
clock_init();
process_init();
process_start(&etimer_process, NULL);
ctimer_init();
enableIRQ();
printf("Beginning CC2420 setup\n");
cc2420_init();
cc2420_set_pan_addr(0x2024, 0, NULL);
cc2420_set_channel(RF_CHANNEL);
rime_init(nullmac_init(&cc2420_driver));
printf("CC2420 setup done\n");
rimeaddr_set_node_addr(&node_addr);
printf("Rime started with address %d.%d\n", node_addr.u8[0], node_addr.u8[1]);
#if WITH_UIP
{
uip_ipaddr_t hostaddr, netmask;
uip_init();
uip_ipaddr(&hostaddr, 172,16,
rimeaddr_node_addr.u8[0],rimeaddr_node_addr.u8[1]);
uip_ipaddr(&netmask, 255,255,0,0);
uip_ipaddr_copy(&meshif.ipaddr, &hostaddr);
printf("Host addr\n");
uip_sethostaddr(&hostaddr);
uip_setnetmask(&netmask);
uip_over_mesh_set_net(&hostaddr, &netmask);
/* uip_fw_register(&slipif);*/
/*uip_over_mesh_set_gateway_netif(&slipif);*/
uip_fw_register(&meshif);
uip_fw_default(&meshif);
printf("Mesh init\n");
uip_over_mesh_init(UIP_OVER_MESH_CHANNEL);
printf("uIP started with IP address %d.%d.%d.%d\n",
uip_ipaddr_to_quad(&hostaddr));
}
#endif /* WITH_UIP */
#if WITH_UIP
process_start(&tcpip_process, NULL);
process_start(&uip_fw_process, NULL); /* Start IP output */
trickle_open(&gateway_trickle, CLOCK_SECOND * 4, GATEWAY_TRICKLE_CHANNEL,
&trickle_call);
#endif /* WITH_UIP */
printf("Heap size: %ld bytes\n", &__heap_end__ - (char*)sbrk(0));
printf("Started\n");
/* procinit_init(); */
autostart_start(autostart_processes);
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;
}

27
platform/stepper-robot/core.c
View file

@ -1,27 +0,0 @@
#include <stepper-process.h>
#include <dev/leds.h>
#include <loader/codeprop-otf.h>
#include <loader/ram-segments.h>
#include <sys/autostart.h>
#include "codeprop-otf.h"
#include <stdio.h>
#include <stdlib.h>
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);
etimer_reset(&timer);
}
PROCESS_END();
}
AUTOSTART_PROCESSES(&codeprop_process, &ram_segments_cleanup_process, &stepper_process,&blink_process);

11
platform/stepper-robot/gateway/Makefile
View file

@ -1,11 +0,0 @@
CONTIKI=../../../../contiki-2.x/
TARGET=stepper-robot
MCK=23961600
XSLTPROC=xsltproc
CONTIKI_TARGET_MAIN=contiki-main.c
all: gateway
include $(CONTIKI)/Makefile.include

134
platform/stepper-robot/gateway/gateway.c
View file

@ -1,134 +0,0 @@
#include <stdio.h>
#include <sys/etimer.h>
#include <sys/process.h>
#include <sys/autostart.h>
#include <usb/usb-api.h>
#include <usb/cdc-acm.h>
#include <dev/leds.h>
#include <debug-uart.h>
#include <net/uip-fw-drv.h>
#include <net/uip-over-mesh.h>
#include <dev/slip.h>
#include "contiki-main.h"
/* SLIP interface */
extern struct uip_fw_netif cc2420if;
rimeaddr_t node_addr = {{0,129}};
static struct uip_fw_netif slipif =
{UIP_FW_NETIF(0,0,0,0, 255,255,255,255, slip_send)};
/* USB buffers */
static unsigned char input_buffer[128];
static unsigned char output_buffer[128];
static unsigned char interrupt_buffer[16];
#define DEV_TO_HOST 0x81
#define HOST_TO_DEV 0x02
#define GATEWAY_TRICKLE_CHANNEL 8
void
slip_arch_init(unsigned long ubr)
{
}
void
slip_arch_writeb(unsigned char c)
{
while(usb_send_data(DEV_TO_HOST, &c, 1) != 1);
}
#if WITH_UIP
static void
set_gateway(void)
{
struct gateway_msg msg;
/* Make this node the gateway node, unless it already is the
gateway. */
if(!is_gateway) {
leds_on(LEDS_RED);
printf("%d.%d: making myself the gateway\n",
rimeaddr_node_addr.u8[0], rimeaddr_node_addr.u8[1]);
uip_over_mesh_set_gateway(&rimeaddr_node_addr);
rimeaddr_copy(&(msg.gateway), &rimeaddr_node_addr);
rimebuf_copyfrom(&msg, sizeof(struct gateway_msg));
trickle_send(&gateway_trickle);
is_gateway = 1;
}
}
#endif /* WITH_UIP */
PROCESS(gateway_process, "Gateway process");
PROCESS_THREAD(gateway_process, ev , data)
{
static struct etimer timer;
PROCESS_BEGIN();
usb_set_user_process(process_current);
usb_setup();
usb_cdc_acm_setup();
uip_fw_default(&slipif);
uip_over_mesh_set_gateway_netif(&slipif);
process_start(&slip_process, NULL);
set_gateway();
while(ev != PROCESS_EVENT_EXIT) {
PROCESS_WAIT_EVENT();
if (ev == PROCESS_EVENT_TIMER) {
leds_toggle(LEDS_YELLOW);
/* printf("FIFOP: %d\n", FIFOP_IS_1); */
etimer_restart(&timer);
} else if (ev == PROCESS_EVENT_MSG) {
const struct usb_user_msg * const msg = data;
switch(msg->type) {
case USB_USER_MSG_TYPE_CONFIG:
printf("User config\n");
if (msg->data.config != 0) {
usb_setup_bulk_endpoint(DEV_TO_HOST,
input_buffer, sizeof(input_buffer));
usb_setup_bulk_endpoint(HOST_TO_DEV,
output_buffer, sizeof(output_buffer));
usb_setup_interrupt_endpoint(0x83,interrupt_buffer,
sizeof(interrupt_buffer));
etimer_set(&timer, CLOCK_SECOND);
} else {
etimer_stop(&timer);
usb_disable_endpoint(DEV_TO_HOST);
usb_disable_endpoint(HOST_TO_DEV);
usb_disable_endpoint(0x83);
}
break;
case USB_USER_MSG_TYPE_EP_OUT(2):
{
/*unsigned int len = msg->data.length;
printf("Received %d:\n", len); */
{
unsigned char ch;
unsigned int xfer;
while((xfer = usb_recv_data(HOST_TO_DEV, &ch, 1)) > 0) {
/* printf(" %02x",ch); */
if (slip_input_byte(ch)) break;
}
/* printf("\n"); */
}
}
break;
}
}
}
printf("USB test process exited\n");
PROCESS_END();
}
AUTOSTART_PROCESSES(&gateway_process);

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

@ -1,38 +0,0 @@
#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;
}

183
platform/stepper-robot/robot-main.c
View file

@ -1,183 +0,0 @@
#include <AT91SAM7S64.h>
#include <interrupt-utils.h>
#include <string.h>
#include <debug-uart.h>
#include <ctype.h>
#include <stdio.h>
#include <dev/cc2420.h>
#include <dev/cc2420_const.h>
#include <dev/spi.h>
#include <dev/leds.h>
#include <sys/process.h>
#include <sys/procinit.h>
#include <sys/autostart.h>
#include <sys/etimer.h>
#include <net/psock.h>
#include <unistd.h>
#include <stepper-steps.h>
#include <stepper.h>
#include <stepper-move.h>
#include "net/mac/nullmac.h"
#include "net/rime.h"
#ifndef RF_CHANNEL
#define RF_CHANNEL 15
#endif
#ifndef WITH_UIP
#define WITH_UIP 1
#endif
#if WITH_UIP
#include "net/uip.h"
#include "net/uip-fw.h"
#include "net/uip-fw-drv.h"
#include "net/uip-over-mesh.h"
static struct uip_fw_netif meshif =
{UIP_FW_NETIF(172,16,0,0, 255,255,0,0, uip_over_mesh_send)};
#define UIP_OVER_MESH_CHANNEL 9
#endif /* WITH_UIP */
static rimeaddr_t node_addr = {{0,2}};
extern char __heap_end__;
extern char __heap_start__;
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 void
robot_stepper_init()
{
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])));}
#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;
}
#if 0
static uip_ipaddr_t gw_addr = {{172,16,0,1}};
#endif
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");
leds_arch_init();
clock_init();
process_init();
process_start(&etimer_process, NULL);
ctimer_init();
robot_stepper_init();
enableIRQ();
cc2420_init();
cc2420_set_pan_addr(0x2024, 0, &uip_hostaddr.u16[1]);
cc2420_set_channel(RF_CHANNEL);
rime_init(nullmac_init(&cc2420_driver));
printf("CC2420 setup done\n");
rimeaddr_set_node_addr(&node_addr);
#if WITH_UIP
{
uip_ipaddr_t hostaddr, netmask;
uip_init();
uip_ipaddr(&hostaddr, 172,16,
rimeaddr_node_addr.u8[0],rimeaddr_node_addr.u8[1]);
uip_ipaddr(&netmask, 255,255,0,0);
uip_ipaddr_copy(&meshif.ipaddr, &hostaddr);
printf("Host addr\n");
uip_sethostaddr(&hostaddr);
uip_setnetmask(&netmask);
uip_over_mesh_set_net(&hostaddr, &netmask);
/* uip_fw_register(&slipif);*/
/*uip_over_mesh_set_gateway_netif(&slipif);*/
uip_fw_default(&meshif);
printf("Mesh init\n");
uip_over_mesh_init(UIP_OVER_MESH_CHANNEL);
printf("uIP started with IP address %d.%d.%d.%d\n",
uip_ipaddr_to_quad(&hostaddr));
}
#endif /* WITH_UIP */
#if WITH_UIP
process_start(&tcpip_process, NULL);
process_start(&uip_fw_process, NULL); /* Start IP output */
#endif /* WITH_UIP */
printf("Heap size: %ld bytes\n", &__heap_end__ - (char*)sbrk(0));
printf("Started\n");
autostart_start(autostart_processes);
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
View file

@ -1,126 +0,0 @@
#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
View file

@ -1,29 +0,0 @@
#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__ */

517
platform/stepper-robot/stepper-process.c
View file

@ -1,517 +0,0 @@
#include <stepper-process.h>
#include <stepper-steps.h>
#include <stepper.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>
#undef putchar
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;
#if 0
{
const char *p = input_line;
printf("Got line: '");
while(p < inend) {
putchar(*p++);
}
printf("'\n");
fsync(1);
}
#endif
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') {
STEPPER_ENABLE();
printf("Stepper enabled\n");
reply_str(reply, "OK\n");
} else if (input_line[0] == 'D') {
STEPPER_DISABLE();
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, UIP_HTONS(0), NULL);
if (!conn) goto exit;
uip_udp_bind(conn, UIP_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, UIP_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, UIP_HTONS(0), NULL);
if (!conn) goto exit;
uip_udp_bind(conn, UIP_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();
}
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 void
robot_stepper_init()
{
disableIRQ();
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])));
enableIRQ();
}
PROCESS(stepper_process, "Stepper control process");
PROCESS_THREAD(stepper_process, ev, data)
{
PROCESS_EXITHANDLER(goto exit);
PROCESS_BEGIN();
robot_stepper_init();
tcp_listen(UIP_HTONS(1010));
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
View file

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

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

@ -1,296 +0,0 @@
#include <stepper-interrupt.h>
#include <interrupt-utils.h>
#include <stdio.h>
#include <stepper.h>
StepperContext stepper_context;
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_timer_interrupt (void) {
ISR_STORE();
ISR_ENABLE_NEST();
stepper_int_safe();
ISR_DISABLE_NEST();
ISR_RESTORE();
}

80
platform/stepper-robot/stepper/stepper-interrupt.h
View file

@ -1,80 +0,0 @@
#ifndef __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__
#define __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__
#include <stepper.h>
/* Timer frequency */
#define TIMER_FREQ 748800
typedef struct _StepperContext StepperContext;
typedef struct _StepperState StepperState;
typedef struct _StepperTimerStep StepperTimerStep;
#define MAX_STEPS_PER_PERIOD 40
#define NUM_STEPPERS 2
#define STEPPER_MAX_VELOCITY 4000
#define STEPPER_MAX_ACCELRATION 4000
#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;
};
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;
};
extern StepperContext stepper_context;
#endif /* __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ */
void stepper_timer_interrupt(void);

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

@ -1,193 +0,0 @@
#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
View file

@ -1,8 +0,0 @@
#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__ */

103
platform/stepper-robot/stepper/stepper-steps.h
View file

@ -1,103 +0,0 @@
#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))}
#define STEPPER_ENABLE() (*AT91C_PIOA_SODR = STEPPER_INHIBIT)
#define STEPPER_DISABLE() (*AT91C_PIOA_CODR = STEPPER_INHIBIT)

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

@ -1,360 +0,0 @@
#include <stepper.h>
#include <stepper-interrupt.h>
#ifndef NUL
#define NULL 0
#endif
static StepperAccSeq *free_seq = NULL;
StepperAccSeq *
stepper_allocate_seq()
{
StepperAccSeq *seq;
if (!free_seq) return NULL;
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
seq = free_seq;
free_seq = seq->next;
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
return seq;
}
void
stepper_free_seq(StepperAccSeq *seq)
{
StepperAccSeq *s;
if (!seq) return;
s = seq;
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
while(s->next) s = s->next;
s->next = free_seq;
free_seq = seq;
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
}
static void
stepper_state_init(StepperState *stepper)
{
stepper->step_count = 0;
stepper->io_mask = 0;
stepper->acc_steps = NULL;
stepper->run_steps = NULL;
stepper->hold_steps = NULL;
stepper->current_step = 0;
stepper->sequence_length = 0;
stepper->velocity = 0;
stepper->acceleration = 0;
stepper->step_full = 0;
stepper->step_frac = 0;
stepper->n_steps = 0;
#ifdef TIMING_ERRORS
stepper->err_min = TIMER_FREQ;
stepper->err_max = -TIMER_FREQ;
#endif
}
void
stepper_init(AT91PS_TC timer, unsigned int id)
{
unsigned int s;
stepper_context.flags = 0;
stepper_context.timer_channel = timer;
stepper_context.steps = NULL;
stepper_context.current_step = NULL;
stepper_context.period_count = 0;
stepper_context.user_callback = NULL;
for (s = 0; s < NUM_STEPPERS; s++) {
stepper_state_init(&stepper_context.steppers[s]);
}
timer->TC_CMR = (AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO
| AT91C_TC_CLKS_TIMER_DIV3_CLOCK);
timer->TC_RC = TIMER_FREQ / PPS;
timer->TC_RA = 0xffff;
timer->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
*AT91C_PMC_PCER = (1 << id);
AT91C_AIC_SMR[id] = AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE | 7;
AT91C_AIC_SVR[id] = (unsigned long)stepper_timer_interrupt;
*AT91C_AIC_IECR = (1 << id);
timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
}
void
stepper_init_io(unsigned int stepper_index, uint32_t mask,
const uint32_t *acc, const uint32_t *run,
const uint32_t *hold, unsigned int nsteps)
{
StepperState *state;
if (stepper_index >= NUM_STEPPERS) return;
state = &stepper_context.steppers[stepper_index];
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
state->io_mask = mask;
state->acc_steps = acc;
state->run_steps = run;
state->hold_steps = hold;
state->current_step = 0;
state->sequence_length = nsteps;
*AT91C_PIOA_OWER = mask;
*AT91C_PIOA_MDDR = mask;
*AT91C_PIOA_ODSR = ((*AT91C_PIOA_ODSR & ~mask)
| (state->hold_steps[0] & mask));
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
*AT91C_PIOA_OER = mask;
}
/**
Append an acceleration sequence
Truncates the current acceleration sequence at the insertion time
and appends the new sequence at that position.. The insertion time
is the time of the first element of the new sequence. The
truncation takes place after any elements with the acceleration set
to STEPPER_ACC_INVALID (user callbacks) that has the same time as
the insertion time. All other elements with the same time is
replaced.
\param stepper_index Index of the stepper the sequence is intended for.
\param new_seq A linked list of sequence elements to append.
*/
StepperResult
stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq)
{
StepperResult res = STEPPER_ERR_TOO_LATE;
StepperAccSeq **seqp;
StepperState *state;
if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX;
state = &stepper_context.steppers[stepper_index];
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
seqp = &state->acceleration_sequence;
while(*seqp && ((*seqp)->period < new_seq->period || ((*seqp)->period == new_seq->period && (*seqp)->acceleration == STEPPER_ACC_INVALID))) {
seqp = &(*seqp)->next;
}
if (new_seq->period > stepper_context.period_count + 1) {
/* Replace tail of sequence */
if (*seqp) stepper_free_seq(*seqp);
*seqp = new_seq;
res = STEPPER_OK;
}
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
return res;
}
/**
Insert a callback mark
Inserts a callback mark at the indicated period. This will cause
the callback procedure to be called just before that period,
usually near the beginning of the previous period. Does not
truncate the current sequence.
\param stepper_index Index of the stepper the callbak is intended for.
\param period When the callback should be invoked
\sa stepper_set_callback_proc
*/
StepperResult
stepper_insert_callback(unsigned int stepper_index, unsigned int period)
{
StepperResult res = STEPPER_ERR_TOO_LATE;
StepperAccSeq **seqp;
StepperState *state;
if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX;
state = &stepper_context.steppers[stepper_index];
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
seqp = &state->acceleration_sequence;
while(*seqp && (*seqp)->period < period) {
seqp = &(*seqp)->next;
}
if (period > stepper_context.period_count + 1) {
StepperAccSeq *new_seq = stepper_allocate_seq();
if (!new_seq) {
res = STEPPER_ERR_MEM;
} else {
new_seq->next = *seqp;
*seqp = new_seq;
new_seq->period = period;
new_seq->acceleration = STEPPER_ACC_INVALID;
res = STEPPER_OK;
}
}
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
return res;
}
StepperResult
stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc)
{
StepperAccSeq *seq = stepper_allocate_seq();
/* printf("stepper_add_acc: %d %d %ld\n", stepper_index, period, acc); */
if (!seq) return STEPPER_ERR_MEM;
seq->next = NULL;
seq->period = period;
seq->acceleration = acc;
return stepper_add_acc_seq(stepper_index, seq);
}
void
stepper_set_callback_proc(StepperUserCallback callback)
{
stepper_context.user_callback = callback;
}
unsigned long
stepper_current_period()
{
return stepper_context.period_count;
}
long
stepper_current_step(unsigned int stepper_index)
{
StepperState *state = &stepper_context.steppers[stepper_index];
return state->step_count;
}
long long
stepper_step_frac(unsigned int stepper_index)
{
long long s;
StepperState *state = &stepper_context.steppers[stepper_index];
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
s = state->step_full * DIST_SCALE + state->step_frac;
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
return s;
}
long
stepper_current_velocity(unsigned int stepper_index)
{
StepperState *state = &stepper_context.steppers[stepper_index];
return state->velocity;
}
/* Calculate the speed at given current given the current acceleration
sequence. */
unsigned long
stepper_velocity(unsigned int stepper_index, unsigned long period)
{
long a;
long v;
unsigned long t;
StepperState *state;
StepperAccSeq *seq;
state = &stepper_context.steppers[stepper_index];
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
seq = state->acceleration_sequence;
a = state->acceleration;
v = state->velocity;
t = stepper_context.period_count + 2;
while(seq && seq->period < period) {
v += a * (seq->period - t);
t = seq->period;
a = seq->acceleration;
seq = seq->next;
}
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
v += a * (period - t);
return v;
}
/**
Calculate the speed and position at specified period given the
current acceleration sequence.
\param stepper_index Index of the stepper the callbak is intended for.
\param period The period to calculate for
\param Speed return
\param Position return. In fractional steps
*/
StepperResult
stepper_state_at(unsigned int stepper_index, unsigned long period,
long *velocity, long long *position)
{
long a;
long v;
long long s;
unsigned long t;
long dt;
StepperState *state;
StepperAccSeq *seq;
state = &stepper_context.steppers[stepper_index];
stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS;
if (period < stepper_context.period_count + 2) {
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
return STEPPER_ERR_TOO_LATE;
}
seq = state->acceleration_sequence;
a = state->acceleration;
v = state->velocity;
t = stepper_context.period_count + 2;
s = state->step_full * (long long)DIST_SCALE + state->step_frac;
while(seq && seq->period < period) {
dt = seq->period - t;
s += (a * (long long)dt + 2 * v) * dt;
v += a * (seq->period - t);
t = seq->period;
a = seq->acceleration;
seq = seq->next;
}
stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS;
dt = period - t;
*position = s + (a * (long long)dt + (DIST_SCALE/VEL_SCALE) * v) * dt;
*velocity = v + a * dt;
return STEPPER_OK;
}
StepperResult
stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp,
unsigned long max_acc, long final_speed)
{
long start_period = *periodp;
long v = stepper_velocity(stepper_index, start_period);
/* printf("%ld @ %ld\n", v, start_period); */
if (final_speed == v) {
return stepper_add_acc(stepper_index, start_period, 0);
} else {
StepperResult res;
long a = (final_speed > v) ? max_acc : -max_acc;
long t = ((long)(final_speed - v)) / a;
long diff = (final_speed - v) - t * a;
if (t > 0) {
res = stepper_add_acc(stepper_index, start_period, a);
if (res != STEPPER_OK) return res;
}
if (diff) {
res = stepper_add_acc(stepper_index, start_period+t, diff);
if (res != STEPPER_OK) return res;
t++;
}
*periodp = start_period+t;
return stepper_add_acc(stepper_index, start_period+t, 0);
}
}
#ifdef TIMING_ERRORS
void
stepper_timing_errors(unsigned int stepper_index, long *min, long *max)
{
StepperState *state;
state = &stepper_context.steppers[stepper_index];
*min = state->err_min;
*max = state->err_max;
state->err_max = -TIMER_FREQ;
state->err_min = TIMER_FREQ;
}
#endif

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

@ -1,98 +0,0 @@
#ifndef __STEPPER_H__JPA916UOFT__
#define __STEPPER_H__JPA916UOFT__
#include <AT91SAM7S64.h>
#include <inttypes.h>
#include <limits.h>
/* Define periods/second */
#define PPS 128
/* Scaling factor for distance */
#define DIST_SCALE (2 * PPS * PPS)
/* Scaling factor for velocity */
#define VEL_SCALE PPS
typedef struct _StepperAccSeq StepperAccSeq;
struct _StepperAccSeq
{
StepperAccSeq *next;
unsigned long period;
long acceleration;
};
#define STEPPER_ACC_INVALID LONG_MAX
#define STEPPER_MAX_VELOCITY 4000
#define STEPPER_MAX_ACCELRATION 4000
typedef void (*StepperUserCallback)(unsigned int stepper_index,
unsigned long period);
typedef unsigned int StepperResult;
#define STEPPER_OK 0
#define STEPPER_ERR_MEM 1
#define STEPPER_ERR_TOO_LATE 2
#define STEPPER_ERR_INDEX 3
void
stepper_init(AT91PS_TC timer, unsigned int id);
void
stepper_init_io(unsigned int stepper_index, uint32_t mask,
const uint32_t *acc, const uint32_t *run,
const uint32_t *hold, unsigned int nsteps);
/* Returns true if the new sequence was actually added or false
if the index is illegal or the first step in the sequence is too soon */
StepperResult
stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq);
StepperResult
stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc);
StepperResult
stepper_insert_callback(unsigned int stepper_index, unsigned int period);
void
stepper_set_callback_proc(StepperUserCallback callback);
unsigned long
stepper_current_period();
long
stepper_current_step(unsigned int stepper_index);
long long
stepper_step_frac(unsigned int stepper_index);
long
stepper_current_velocity(unsigned int stepper_index);
unsigned long
stepper_velocity(unsigned int stepper_index, unsigned long period);
StepperResult
stepper_state_at(unsigned int stepper_index, unsigned long period,
long *velocity, long long *position);
StepperResult
stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp,
unsigned long max_acc, long final_speed);
StepperAccSeq *
stepper_allocate_seq();
void
stepper_free_seq(StepperAccSeq *seq);
#ifdef TIMING_ERRORS
void
stepper_timing_errors(unsigned int stepper_index, long *min, long *max);
#endif
#endif /* __STEPPER_H__JPA916UOFT__ */

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

@ -1,80 +0,0 @@
#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;
}

32
platform/stepper-robot/usb-main.c
View file

@ -1,32 +0,0 @@
#include <stdint.h>
#include <stdio.h>
#include <debug-uart.h>
#include <sys/process.h>
#include <sys/procinit.h>
#include <etimer.h>
#include <sys/autostart.h>
#include <clock.h>
unsigned int idle_count = 0;
int
main()
{
dbg_setup_uart();
printf("Initialising\n");
clock_init();
process_init();
process_start(&etimer_process, NULL);
autostart_start(autostart_processes);
printf("Processes running\n");
while(1) {
do {
} while(process_run() > 0);
idle_count++;
/* Idle! */
/* Stop processor clock */
*AT91C_PMC_SCDR |= AT91C_PMC_PCK;
}
return 0;
}

21
platform/stepper-robot/usb-msc/Makefile
View file

@ -1,21 +0,0 @@
TARGET=stepper-robot
all: usbmsctst
CONTIKI=../../..
CONTIKI_TARGET_MAIN=usb-main.c
PROJECT_SOURCEFILES =
USB_MASS_STORAGE_CLASS=1
clean: stepper_robot_clean
.PHONY: stepper_robot_clean
stepper_robot_clean:
include $(CONTIKI)/Makefile.include

40
platform/stepper-robot/usb-msc/usbmsctst.c
View file

@ -1,40 +0,0 @@
#include <string.h>
#include <stdio.h>
#include <ctype.h>
#include <sys/process.h>
#include <sys/autostart.h>
#include <sys/etimer.h>
#include <usb-api.h>
#include <msc/usb-msc-bulk.h>
PROCESS(usb_masstest_process, "USB massstorage test process");
#if 0
extern unsigned int idle_count;
#endif
PROCESS_THREAD(usb_masstest_process, ev , data)
{
static unsigned int last_count;
PROCESS_BEGIN();
static struct etimer timer;
usb_msc_bulk_setup();
etimer_set(&timer, CLOCK_SECOND);
#if 0
last_count = idle_count;
#endif
while(1) {
PROCESS_WAIT_EVENT();
if (ev == PROCESS_EVENT_TIMER) {
#if 0
printf("Idle count: %d\n", idle_count - last_count);
last_count = idle_count;
#endif
etimer_reset(&timer);
}
}
PROCESS_END();
}
AUTOSTART_PROCESSES(&usb_masstest_process);