diff --git a/platform/stepper-robot/Makefile b/platform/stepper-robot/Makefile deleted file mode 100644 index beeb7adb3..000000000 --- a/platform/stepper-robot/Makefile +++ /dev/null @@ -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 - diff --git a/platform/stepper-robot/Makefile.stepper-robot b/platform/stepper-robot/Makefile.stepper-robot deleted file mode 100644 index b74e6180d..000000000 --- a/platform/stepper-robot/Makefile.stepper-robot +++ /dev/null @@ -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 - diff --git a/platform/stepper-robot/cc2420-interrupt.c b/platform/stepper-robot/cc2420-interrupt.c deleted file mode 100644 index 3b60711d8..000000000 --- a/platform/stepper-robot/cc2420-interrupt.c +++ /dev/null @@ -1,51 +0,0 @@ -#include -#include -#include -#include -#include -#include - -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__ */ diff --git a/platform/stepper-robot/cc2420-interrupt.h b/platform/stepper-robot/cc2420-interrupt.h deleted file mode 100644 index f36927850..000000000 --- a/platform/stepper-robot/cc2420-interrupt.h +++ /dev/null @@ -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__ */ diff --git a/platform/stepper-robot/cc2420-spi.c b/platform/stepper-robot/cc2420-spi.c deleted file mode 100644 index 04c5d6e17..000000000 --- a/platform/stepper-robot/cc2420-spi.c +++ /dev/null @@ -1,23 +0,0 @@ -#include -#include -#include -#include - -#define SPI_SPEED 1000000 /* 1MHz clock*/ -#define SPI_DLYBCT 1 -#define SPI_DLYBS 20 - - -#ifndef BV -#define BV(b) (1<<(b)) -#endif - -void -cc2420_arch_init(void) -{ - spi_init(); - - AT91C_SPI_CSR[CC2420_DEFAULT_DEV] = - ((SPI_DLYBCT<<24) | (SPI_DLYBS<<16) | (((MCK+SPI_SPEED/2)/SPI_SPEED)<<8) - | AT91C_SPI_NCPHA | AT91C_SPI_BITS_8 | AT91C_SPI_CSAAT); -} diff --git a/platform/stepper-robot/contiki-conf.h b/platform/stepper-robot/contiki-conf.h deleted file mode 100644 index c457801af..000000000 --- a/platform/stepper-robot/contiki-conf.h +++ /dev/null @@ -1,98 +0,0 @@ -#ifndef __CONTIKI_CONF_H__CDBB4VIH3I__ -#define __CONTIKI_CONF_H__CDBB4VIH3I__ - -#include -#include -#include -#include - -#define CCIF -#define CLIF - -#define WITH_UIP 1 -#define WITH_ASCII 1 - -#define CLOCK_CONF_SECOND 100 - -/* 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< -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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; -} diff --git a/platform/stepper-robot/core.c b/platform/stepper-robot/core.c deleted file mode 100644 index 0ac8437a8..000000000 --- a/platform/stepper-robot/core.c +++ /dev/null @@ -1,27 +0,0 @@ -#include -#include -#include -#include -#include -#include "codeprop-otf.h" -#include -#include - -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); diff --git a/platform/stepper-robot/gateway/Makefile b/platform/stepper-robot/gateway/Makefile deleted file mode 100644 index fd867a7b4..000000000 --- a/platform/stepper-robot/gateway/Makefile +++ /dev/null @@ -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 diff --git a/platform/stepper-robot/gateway/gateway.c b/platform/stepper-robot/gateway/gateway.c deleted file mode 100644 index dd13f4186..000000000 --- a/platform/stepper-robot/gateway/gateway.c +++ /dev/null @@ -1,134 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#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); diff --git a/platform/stepper-robot/leds-arch.c b/platform/stepper-robot/leds-arch.c deleted file mode 100644 index 8a51173fe..000000000 --- a/platform/stepper-robot/leds-arch.c +++ /dev/null @@ -1,38 +0,0 @@ -#include -#include - -#define GREEN_PIN AT91C_PIO_PA3 -#define YELLOW_PIN AT91C_PIO_PA4 -#define RED_PIN AT91C_PIO_PA8 - -#define ALL_PINS (GREEN_PIN | YELLOW_PIN | RED_PIN) - -void -leds_arch_init(void) -{ - *AT91C_PIOA_PER = ALL_PINS; - *AT91C_PIOA_OER = ALL_PINS; - *AT91C_PIOA_MDER = ALL_PINS; - *AT91C_PIOA_SODR = ALL_PINS; -} - -unsigned char -leds_arch_get(void) -{ - unsigned char on = 0; - if (*AT91C_PIOA_ODSR & GREEN_PIN) on |= LEDS_GREEN; - if (*AT91C_PIOA_ODSR & YELLOW_PIN) on |= LEDS_YELLOW; - if (*AT91C_PIOA_ODSR & RED_PIN) on |= LEDS_RED; - return on; -} - -void -leds_arch_set(unsigned char leds) -{ - unsigned int set = 0; - if (leds & LEDS_GREEN) set |= GREEN_PIN; - if (leds & LEDS_YELLOW) set |= YELLOW_PIN; - if (leds & LEDS_RED) set |= RED_PIN; - *AT91C_PIOA_CODR = set; - *AT91C_PIOA_SODR = (~set) & ALL_PINS; -} diff --git a/platform/stepper-robot/robot-main.c b/platform/stepper-robot/robot-main.c deleted file mode 100644 index c57de8dc0..000000000 --- a/platform/stepper-robot/robot-main.c +++ /dev/null @@ -1,183 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#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; -} diff --git a/platform/stepper-robot/sam7s-spi.c b/platform/stepper-robot/sam7s-spi.c deleted file mode 100644 index b1ff7bd8c..000000000 --- a/platform/stepper-robot/sam7s-spi.c +++ /dev/null @@ -1,126 +0,0 @@ -#include -#include -#include -#include - -/* Prevents interrupts using SPI at inappropriate times */ -unsigned char spi_busy = 0; - -#define SPI_SPEED 1000000 /* 1MHz clock*/ -#define SPI_DLYBCT 1 -#define SPI_DLYBS 20 - -#define SPI_TRANSFER (AT91C_PA12_MISO | AT91C_PA13_MOSI | AT91C_PA14_SPCK) - -#define SPI_CS (AT91C_PA11_NPCS0 | AT91C_PA31_NPCS1) - -void -spi_init() -{ - static uint8_t initialised = 0; - if (!initialised) { - *AT91C_SPI_CR = AT91C_SPI_SPIDIS | AT91C_SPI_SWRST; - *AT91C_PMC_PCER = (1 << AT91C_ID_SPI); - *AT91C_PIOA_ASR = SPI_TRANSFER | SPI_CS; - *AT91C_PIOA_PDR = SPI_TRANSFER | SPI_CS; - *AT91C_PIOA_PPUER = AT91C_PA12_MISO | SPI_CS; - *AT91C_SPI_MR = (AT91C_SPI_MSTR | AT91C_SPI_PS_FIXED - | AT91C_SPI_MODFDIS | AT91C_SPI_PCS); - - /* It seems necessary to set the clock speed for chip select 0 - even if it's not used. */ - AT91C_SPI_CSR[0] = (MCK/SPI_SPEED)<<8; - - *AT91C_SPI_CR = AT91C_SPI_SPIEN; - initialised = 1; - } -} - -void -spi_init_chip_select(unsigned int chip, unsigned int speed, - unsigned int dlybct, - unsigned int dlybs, unsigned int phase, - unsigned int polarity) -{ - spi_init(); - - AT91C_SPI_CSR[chip] = - ((dlybct<<24) | (dlybs<<16) | (((MCK+speed/2)/speed)<<8) - | (phase?AT91C_SPI_NCPHA:0) | (polarity?AT91C_SPI_CPOL:0) - | AT91C_SPI_BITS_8 | AT91C_SPI_CSAAT); -} - -#if 0 -#define DBG_SEND dbg_blocking_putchar('>'); -#define DBG_RECV dbg_blocking_putchar('<'); -#else -#define DBG_SEND -#define DBG_RECV -#endif - -void -spi_transfer(unsigned int chip, const struct spi_block *block, unsigned int blocks) -{ - spi_busy = 1; - while(!(*AT91C_SPI_SR & AT91C_SPI_TXEMPTY)); /* wait unti previous transfer is done */ - - /* Clear any data left in the receiver */ - (void)*AT91C_SPI_RDR; - (void)*AT91C_SPI_RDR; - - /* Select chip */ - *AT91C_SPI_MR = ((*AT91C_SPI_MR & ~AT91C_SPI_PCS) - | ((~(1< 0) { - struct spi_block current = *block++; - if (current.send) { - if (current.receive) { - /* Send and receive */ - while(current.len-- > 0) { - while(!(*AT91C_SPI_SR & AT91C_SPI_TDRE)); - *AT91C_SPI_TDR = *current.send++; - DBG_SEND; - while(!(*AT91C_SPI_SR & AT91C_SPI_RDRF)); - *current.receive++ = *AT91C_SPI_RDR; - DBG_RECV; - } - } else { - /* Send only */ - while(current.len-- > 0) { - while(!(*AT91C_SPI_SR & AT91C_SPI_TDRE)); - *AT91C_SPI_TDR = *current.send++; - DBG_SEND; - while(!(*AT91C_SPI_SR & AT91C_SPI_RDRF)); - (void)*AT91C_SPI_RDR; - DBG_RECV; - } - } - } else { - if (current.receive) { - /* Receive only */ - while(current.len-- > 0) { - while(!(*AT91C_SPI_SR & AT91C_SPI_TDRE)); - *AT91C_SPI_TDR = 0; - DBG_SEND; - while(!(*AT91C_SPI_SR & AT91C_SPI_RDRF)); - *current.receive++ = *AT91C_SPI_RDR; - DBG_RECV; - } - } else { - /* Clock only */ - while(current.len-- > 0) { - while(!(*AT91C_SPI_SR & AT91C_SPI_TDRE)); - *AT91C_SPI_TDR = 0; - DBG_SEND; - while(!(*AT91C_SPI_SR & AT91C_SPI_RDRF)); - (void)*AT91C_SPI_RDR; - DBG_RECV; - } - } - } - } - *AT91C_SPI_CR = AT91C_SPI_LASTXFER; - - spi_busy = 0; -} diff --git a/platform/stepper-robot/sam7s-spi.h b/platform/stepper-robot/sam7s-spi.h deleted file mode 100644 index 7a2f41f67..000000000 --- a/platform/stepper-robot/sam7s-spi.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef __CORE_SPI_H__KBMMOKI6CG__ -#define __CORE_SPI_H__KBMMOKI6CG__ -#include - -void -spi_init(); - -#define SPI_POLARITY_INACTIVE_HIGH 1 -#define SPI_POLARITY_INACTIVE_LOW 1 - -#define SPI_PHASE_CHANGE_CAPTURE 0 -#define SPI_PHASE_CAPTURE_CHANGE 1 - -struct spi_block { - const uint8_t *send; /* NULL for receive only */ - uint8_t *receive; /* NULL for send only */ - uint16_t len; /* transfer length, non-zero */ -}; - -void -spi_transfer(unsigned int chip, const struct spi_block *block, unsigned int blocks); - -void -spi_init_chip_select(unsigned int chip, unsigned int speed, - unsigned int dlybct, - unsigned int dlybs, unsigned int phase, - unsigned int polarity); - -#endif /* __CORE_SPI_H__KBMMOKI6CG__ */ diff --git a/platform/stepper-robot/stepper-process.c b/platform/stepper-robot/stepper-process.c deleted file mode 100644 index b371bf834..000000000 --- a/platform/stepper-robot/stepper-process.c +++ /dev/null @@ -1,517 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#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(); -} - diff --git a/platform/stepper-robot/stepper-process.h b/platform/stepper-robot/stepper-process.h deleted file mode 100644 index fc396749a..000000000 --- a/platform/stepper-robot/stepper-process.h +++ /dev/null @@ -1,7 +0,0 @@ -#ifndef __STEPPER_PROCESS_H__1OAHVG2XPP__ -#define __STEPPER_PROCESS_H__1OAHVG2XPP__ -#include - -PROCESS_NAME(stepper_process); - -#endif /* __STEPPER_PROCESS_H__1OAHVG2XPP__ */ diff --git a/platform/stepper-robot/stepper/stepper-interrupt.c b/platform/stepper-robot/stepper/stepper-interrupt.c deleted file mode 100644 index b89d8fabf..000000000 --- a/platform/stepper-robot/stepper/stepper-interrupt.c +++ /dev/null @@ -1,296 +0,0 @@ -#include -#include -#include -#include - - - -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(); -} diff --git a/platform/stepper-robot/stepper/stepper-interrupt.h b/platform/stepper-robot/stepper/stepper-interrupt.h deleted file mode 100644 index 1356004d7..000000000 --- a/platform/stepper-robot/stepper/stepper-interrupt.h +++ /dev/null @@ -1,80 +0,0 @@ -#ifndef __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ -#define __STEPPER3_INTERRUPT_H__2MHD6D6PQ1__ - -#include - -/* 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); diff --git a/platform/stepper-robot/stepper/stepper-move.c b/platform/stepper-robot/stepper/stepper-move.c deleted file mode 100644 index 8fe6df631..000000000 --- a/platform/stepper-robot/stepper/stepper-move.c +++ /dev/null @@ -1,193 +0,0 @@ -#include -#include -#include -#include - -#if 0 -#define PRINTF(...) printf(__VA_ARGS__) -#else -#define PRINTF(...) do {} while (0) -#endif - -static unsigned int -isqrt(unsigned long x) -{ - unsigned int r; - unsigned int b2 = 0x40000000; - unsigned int b = 0x8000; - while(x < b2) { - b2 >>= 2; - b >>= 1; - } - if (b == 0) return 0; - r = b; - b >>= 1; - while(b > 0) { - r += b; - unsigned int t = r*r; - if (t > x) { - r -= b; - } - b >>=1; - } - return r; -} - -#define ACC_FIRST_UP 0 -#define ACC_K1_UP 1 -#define ACC_LAST_UP 2 -#define ACC_TOP 3 -#define ACC_FIRST_DOWN 4 -#define ACC_K1_DOWN 5 -#define ACC_LAST_DOWN 6 -#define ACC_END 7 - -typedef struct _AccDiff AccDiff; -struct _AccDiff -{ - long diff; - unsigned long pos; -}; - - -static inline long -base_acc(unsigned long t,unsigned long n, unsigned long l, unsigned long a_max) -{ - long a; - if (t >= n) { - if (t >= n+l) { - a = -a_max; - } else { - a = 0; - } - } else { - a = a_max; - } - return a; -} - -static AccDiff acc[ACC_END+1]; -StepperResult -stepper_move(unsigned int stepper_index, unsigned long *periodp, - unsigned long a_max,unsigned long v_max, long s_end) -{ - unsigned long start_period = *periodp; - unsigned long s; - unsigned long ds; - unsigned long l; - unsigned long da0; - unsigned long k1 = 0; - unsigned long n = (v_max+a_max-1)/a_max; - unsigned long a_speed_adj = v_max - (n-1)*a_max; - unsigned long s_res; - long d; - if (s_end >= 0) { - s_res = s_end/2; - } else { - s_res = (-s_end)/2; - } - d = s_res - (long)a_max*(n*n-1) - (long)a_speed_adj; - - acc[ACC_END].diff = 0; - acc[ACC_END].pos = UINT_MAX; - if (d < 0) { - l = 0; - n = isqrt(s_res/a_max); - if (n*(unsigned long long)n*a_max < s_res) n++; - a_speed_adj = a_max; - acc[ACC_LAST_UP].diff=0; - acc[ACC_FIRST_DOWN].diff=0; - } else { - l = (d+v_max-1)/v_max; - acc[ACC_LAST_UP].diff= a_speed_adj - a_max; - acc[ACC_FIRST_DOWN].diff= a_max - a_speed_adj; - } - acc[ACC_LAST_UP].pos = n-1; - acc[ACC_FIRST_DOWN].pos = n+l; - - s = a_max*(n*n-1) + a_speed_adj + l * (a_max*(n-1) + a_speed_adj); - ds = s-s_res; - - da0 = ds/(2*n+l-1); - acc[ACC_FIRST_UP].diff = -da0; - acc[ACC_LAST_DOWN].diff = da0; - acc[ACC_FIRST_UP].pos = 0; - acc[ACC_LAST_DOWN].pos = 2*n+l-1; - ds -= da0*(2*n+l-1); - - acc[ACC_K1_UP].diff = 0; - acc[ACC_K1_DOWN].diff = 0; - acc[ACC_K1_UP].pos = 0; - acc[ACC_K1_DOWN].pos = 2*n+l-1; - - acc[ACC_TOP].diff = 0; - acc[ACC_TOP].pos = n; - - if (ds > 0) { - k1 = (2*n+l -ds)/2; - if (k1 < n) { - - acc[ACC_K1_UP].diff = -1; - acc[ACC_K1_DOWN].diff = 1; - acc[ACC_K1_UP].pos = k1; - acc[ACC_K1_DOWN].pos = 2*n+l-1 - k1; - ds -= (2*(n-k1)+l-1); - } - if (ds > 0) { - acc[ACC_LAST_UP].diff--; - acc[ACC_TOP].diff = 1; - acc[ACC_TOP].pos = n+ds-1; - } - } -#if 0 - { - unsigned int k; - PRINTF("n=%ld l=%ld a_max=%ld v_max=%ld s_res=%ld\n", - n,l ,a_max, v_max, s_res); - for (k = 0; k < 7; k++) { - PRINTF(" %ld@%ld", acc[k].diff, acc[k].pos); - } - PRINTF("\n"); - } -#endif - { - StepperResult res; - unsigned int k; - unsigned long t = 0; - long da = 0; - long a_prev = ULONG_MAX; - for (k = 0; k < ACC_END; k++) { - long a; - da += acc[k].diff; - if (acc[k].pos != acc[k+1].pos) { /* Next position is different */ - if (t != acc[k].pos) { - a = base_acc(t,n,l,a_max); - if (s_end < 0) a = -a; - if (a_prev != a) { - res = stepper_add_acc(stepper_index, t+start_period, a); - if (res != STEPPER_OK) return res; - PRINTF("%d: %ld@%ld\n", stepper_index, a, t+start_period); - a_prev = a; - } - t = acc[k].pos; - } - a = da + base_acc(t,n,l,a_max); - if (s_end < 0) a = -a; - if (a_prev != a) { - res = stepper_add_acc(stepper_index, t+start_period, a); - if (res != STEPPER_OK) return res; - PRINTF("%d: %ld@%ld\n", stepper_index, a, t+start_period); - a_prev = a; - } - t++; - da = 0; - } - } - res = stepper_add_acc(stepper_index, t+start_period, 0); - PRINTF("%d: %d@%ld\n", stepper_index, 0, t+start_period); - if (res != STEPPER_OK) return res; - *periodp += t; - } - return STEPPER_OK; -} - diff --git a/platform/stepper-robot/stepper/stepper-move.h b/platform/stepper-robot/stepper/stepper-move.h deleted file mode 100644 index 5bea1cd3f..000000000 --- a/platform/stepper-robot/stepper/stepper-move.h +++ /dev/null @@ -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__ */ diff --git a/platform/stepper-robot/stepper/stepper-steps.h b/platform/stepper-robot/stepper/stepper-steps.h deleted file mode 100644 index 35f778454..000000000 --- a/platform/stepper-robot/stepper/stepper-steps.h +++ /dev/null @@ -1,103 +0,0 @@ -#include - -#ifndef MAX -#define MIN(a,b) (((a) > (b)) ? (b) : (a)) -#endif - -/* Pins for stepper 0 */ -#define STEPPER0_I00 AT91C_PIO_PA17 -#define STEPPER0_I01 AT91C_PIO_PA18 -#define STEPPER0_PHASE0 AT91C_PIO_PA19 - -#define STEPPER0_I10 AT91C_PIO_PA21 -#define STEPPER0_I11 AT91C_PIO_PA22 -#define STEPPER0_PHASE1 AT91C_PIO_PA23 - -/* Pins for stepper 1 */ -#define STEPPER1_I00 AT91C_PIO_PA24 -#define STEPPER1_I01 AT91C_PIO_PA25 -#define STEPPER1_PHASE0 AT91C_PIO_PA26 - -#define STEPPER1_I10 AT91C_PIO_PA27 -#define STEPPER1_I11 AT91C_PIO_PA28 -#define STEPPER1_PHASE1 AT91C_PIO_PA29 - -/* Common pins */ -#define STEPPER_INHIBIT AT91C_PIO_PA7 - - -#define STEPPER_IOMASK(s) (STEPPER##s##_I00 | STEPPER##s##_I01 | STEPPER##s##_PHASE0 \ - | STEPPER##s##_I10 | STEPPER##s##_I11 | STEPPER##s##_PHASE1) -#define STEPPER_PHASE(s,p,l) ((((l) < 2) ? (STEPPER##s##_I##p##1) : 0) | (((l) & 1) ? 0 : (STEPPER##s##_I##p##0))) - -#define STEPPER_STEP(s, l0, l1) (\ -((l0 >= 0) ? STEPPER_PHASE(s,0, l0):(STEPPER_PHASE(s,0, -(l0))|STEPPER##s##_PHASE0)) | \ -((l1 >= 0) ? STEPPER_PHASE(s,1, l1):(STEPPER_PHASE(s,1,-(l1))|STEPPER##s##_PHASE1))) - - -#define FULL_STEP_BOTH(s,l) {\ - STEPPER_STEP(s,(l),(l)), \ - STEPPER_STEP(s,(l),-(l)), \ - STEPPER_STEP(s,-(l),-(l)), \ - STEPPER_STEP(s,-(l),(l))} - - -#define FULL_STEP_SINGLE(s,l) {\ - STEPPER_STEP(s,0,(l)), \ - STEPPER_STEP(s,(l),0), \ - STEPPER_STEP(s,0,-(l)), \ - STEPPER_STEP(s,-(l),0)} - -#define HALF_STEP(s,l) {\ - STEPPER_STEP(s,0,(l)), \ - STEPPER_STEP(s,(l),(l)), \ - STEPPER_STEP(s,(l),0), \ - STEPPER_STEP(s,(l),-(l)), \ - STEPPER_STEP(s,0,-(l)), \ - STEPPER_STEP(s,-(l),-(l)), \ - STEPPER_STEP(s,-(l),0), \ - STEPPER_STEP(s,-(l),(l))} - - -#define MINI_STEP(s,l) {\ - STEPPER_STEP(s,0,(l)), \ - STEPPER_STEP(s,1,MIN((l),2)), \ - STEPPER_STEP(s,MIN((l),2),1), \ - STEPPER_STEP(s,(l),0), \ - STEPPER_STEP(s,MIN((l),2),-1), \ - STEPPER_STEP(s,1,-MIN((l),2)), \ - STEPPER_STEP(s,0,-(l)), \ - STEPPER_STEP(s,-1,-MIN((l),2)), \ - STEPPER_STEP(s,-MIN((l),2),-1), \ - STEPPER_STEP(s,-(l),0), \ - STEPPER_STEP(s,-MIN((l),2),1), \ - STEPPER_STEP(s,-1,MIN((l),2))} - -#define MICRO_STEP(s,l) {\ - STEPPER_STEP(s,0,(l)), \ - STEPPER_STEP(s,1,(l)), \ - STEPPER_STEP(s,MIN((l),2),(l)), \ - STEPPER_STEP(s,(l),(l)), \ - STEPPER_STEP(s,(l),MIN((l),2)), \ - STEPPER_STEP(s,(l),1), \ - STEPPER_STEP(s,(l),0), \ - STEPPER_STEP(s,(l),-1), \ - STEPPER_STEP(s,(l),-MIN((l),2)), \ - STEPPER_STEP(s,(l),-(l)), \ - STEPPER_STEP(s,MIN((l),2),-(l)), \ - STEPPER_STEP(s,1,-(l)), \ - STEPPER_STEP(s,0,-(l)), \ - STEPPER_STEP(s,-1,-(l)), \ - STEPPER_STEP(s,-MIN((l),2),-(l)), \ - STEPPER_STEP(s,-(l),-(l)), \ - STEPPER_STEP(s,-(l),-MIN((l),2)), \ - STEPPER_STEP(s,-(l),-1), \ - STEPPER_STEP(s,-(l),0), \ - STEPPER_STEP(s,-(l),1), \ - STEPPER_STEP(s,-(l),MIN((l),2)), \ - STEPPER_STEP(s,-(l),(l)), \ - STEPPER_STEP(s,-MIN((l),2),(l)), \ - STEPPER_STEP(s,-1,(l))} - -#define STEPPER_ENABLE() (*AT91C_PIOA_SODR = STEPPER_INHIBIT) -#define STEPPER_DISABLE() (*AT91C_PIOA_CODR = STEPPER_INHIBIT) diff --git a/platform/stepper-robot/stepper/stepper.c b/platform/stepper-robot/stepper/stepper.c deleted file mode 100644 index 9f6f3c00d..000000000 --- a/platform/stepper-robot/stepper/stepper.c +++ /dev/null @@ -1,360 +0,0 @@ -#include -#include - -#ifndef NUL -#define NULL 0 -#endif - -static StepperAccSeq *free_seq = NULL; - -StepperAccSeq * -stepper_allocate_seq() -{ - StepperAccSeq *seq; - if (!free_seq) return NULL; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - seq = free_seq; - free_seq = seq->next; - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return seq; -} - - -void -stepper_free_seq(StepperAccSeq *seq) -{ - StepperAccSeq *s; - if (!seq) return; - s = seq; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - while(s->next) s = s->next; - s->next = free_seq; - free_seq = seq; - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; -} - -static void -stepper_state_init(StepperState *stepper) -{ - stepper->step_count = 0; - stepper->io_mask = 0; - stepper->acc_steps = NULL; - stepper->run_steps = NULL; - stepper->hold_steps = NULL; - stepper->current_step = 0; - stepper->sequence_length = 0; - - stepper->velocity = 0; - stepper->acceleration = 0; - stepper->step_full = 0; - stepper->step_frac = 0; - stepper->n_steps = 0; - -#ifdef TIMING_ERRORS - stepper->err_min = TIMER_FREQ; - stepper->err_max = -TIMER_FREQ; -#endif - -} - -void -stepper_init(AT91PS_TC timer, unsigned int id) -{ - unsigned int s; - stepper_context.flags = 0; - stepper_context.timer_channel = timer; - stepper_context.steps = NULL; - stepper_context.current_step = NULL; - stepper_context.period_count = 0; - stepper_context.user_callback = NULL; - - for (s = 0; s < NUM_STEPPERS; s++) { - stepper_state_init(&stepper_context.steppers[s]); - } - timer->TC_CMR = (AT91C_TC_WAVE | AT91C_TC_WAVESEL_UP_AUTO - | AT91C_TC_CLKS_TIMER_DIV3_CLOCK); - timer->TC_RC = TIMER_FREQ / PPS; - timer->TC_RA = 0xffff; - timer->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - *AT91C_PMC_PCER = (1 << id); - - AT91C_AIC_SMR[id] = AT91C_AIC_SRCTYPE_INT_POSITIVE_EDGE | 7; - AT91C_AIC_SVR[id] = (unsigned long)stepper_timer_interrupt; - *AT91C_AIC_IECR = (1 << id); - timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG; -} - -void -stepper_init_io(unsigned int stepper_index, uint32_t mask, - const uint32_t *acc, const uint32_t *run, - const uint32_t *hold, unsigned int nsteps) -{ - StepperState *state; - if (stepper_index >= NUM_STEPPERS) return; - state = &stepper_context.steppers[stepper_index]; - - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - - state->io_mask = mask; - state->acc_steps = acc; - state->run_steps = run; - state->hold_steps = hold; - state->current_step = 0; - state->sequence_length = nsteps; - *AT91C_PIOA_OWER = mask; - *AT91C_PIOA_MDDR = mask; - - *AT91C_PIOA_ODSR = ((*AT91C_PIOA_ODSR & ~mask) - | (state->hold_steps[0] & mask)); - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - *AT91C_PIOA_OER = mask; -} - -/** - Append an acceleration sequence - - Truncates the current acceleration sequence at the insertion time - and appends the new sequence at that position.. The insertion time - is the time of the first element of the new sequence. The - truncation takes place after any elements with the acceleration set - to STEPPER_ACC_INVALID (user callbacks) that has the same time as - the insertion time. All other elements with the same time is - replaced. - - \param stepper_index Index of the stepper the sequence is intended for. - \param new_seq A linked list of sequence elements to append. - */ -StepperResult -stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq) -{ - StepperResult res = STEPPER_ERR_TOO_LATE; - StepperAccSeq **seqp; - StepperState *state; - if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX; - state = &stepper_context.steppers[stepper_index]; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - seqp = &state->acceleration_sequence; - while(*seqp && ((*seqp)->period < new_seq->period || ((*seqp)->period == new_seq->period && (*seqp)->acceleration == STEPPER_ACC_INVALID))) { - seqp = &(*seqp)->next; - } - if (new_seq->period > stepper_context.period_count + 1) { - /* Replace tail of sequence */ - if (*seqp) stepper_free_seq(*seqp); - *seqp = new_seq; - res = STEPPER_OK; - } - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return res; -} - -/** - Insert a callback mark - - Inserts a callback mark at the indicated period. This will cause - the callback procedure to be called just before that period, - usually near the beginning of the previous period. Does not - truncate the current sequence. - - \param stepper_index Index of the stepper the callbak is intended for. - \param period When the callback should be invoked - - \sa stepper_set_callback_proc -*/ - -StepperResult -stepper_insert_callback(unsigned int stepper_index, unsigned int period) -{ - StepperResult res = STEPPER_ERR_TOO_LATE; - StepperAccSeq **seqp; - StepperState *state; - if (stepper_index >= NUM_STEPPERS) return STEPPER_ERR_INDEX; - state = &stepper_context.steppers[stepper_index]; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - seqp = &state->acceleration_sequence; - while(*seqp && (*seqp)->period < period) { - seqp = &(*seqp)->next; - } - if (period > stepper_context.period_count + 1) { - StepperAccSeq *new_seq = stepper_allocate_seq(); - if (!new_seq) { - res = STEPPER_ERR_MEM; - } else { - new_seq->next = *seqp; - *seqp = new_seq; - new_seq->period = period; - new_seq->acceleration = STEPPER_ACC_INVALID; - res = STEPPER_OK; - } - } - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return res; -} - -StepperResult -stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc) -{ - StepperAccSeq *seq = stepper_allocate_seq(); - /* printf("stepper_add_acc: %d %d %ld\n", stepper_index, period, acc); */ - if (!seq) return STEPPER_ERR_MEM; - seq->next = NULL; - seq->period = period; - seq->acceleration = acc; - return stepper_add_acc_seq(stepper_index, seq); -} - -void -stepper_set_callback_proc(StepperUserCallback callback) -{ - stepper_context.user_callback = callback; -} - -unsigned long -stepper_current_period() -{ - return stepper_context.period_count; -} - -long -stepper_current_step(unsigned int stepper_index) -{ - StepperState *state = &stepper_context.steppers[stepper_index]; - return state->step_count; -} - -long long -stepper_step_frac(unsigned int stepper_index) -{ - long long s; - StepperState *state = &stepper_context.steppers[stepper_index]; - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - s = state->step_full * DIST_SCALE + state->step_frac; - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return s; -} - -long -stepper_current_velocity(unsigned int stepper_index) -{ - StepperState *state = &stepper_context.steppers[stepper_index]; - return state->velocity; -} - -/* Calculate the speed at given current given the current acceleration - sequence. */ -unsigned long -stepper_velocity(unsigned int stepper_index, unsigned long period) -{ - long a; - long v; - unsigned long t; - StepperState *state; - StepperAccSeq *seq; - state = &stepper_context.steppers[stepper_index]; - - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - seq = state->acceleration_sequence; - a = state->acceleration; - v = state->velocity; - t = stepper_context.period_count + 2; - - while(seq && seq->period < period) { - v += a * (seq->period - t); - t = seq->period; - a = seq->acceleration; - seq = seq->next; - } - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - v += a * (period - t); - return v; -} - -/** - Calculate the speed and position at specified period given the - current acceleration sequence. - - \param stepper_index Index of the stepper the callbak is intended for. - \param period The period to calculate for - \param Speed return - \param Position return. In fractional steps - -*/ -StepperResult -stepper_state_at(unsigned int stepper_index, unsigned long period, - long *velocity, long long *position) -{ - long a; - long v; - long long s; - unsigned long t; - long dt; - StepperState *state; - StepperAccSeq *seq; - state = &stepper_context.steppers[stepper_index]; - - stepper_context.timer_channel->TC_IDR = AT91C_TC_CPCS | AT91C_TC_CPAS; - if (period < stepper_context.period_count + 2) { - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - return STEPPER_ERR_TOO_LATE; - } - seq = state->acceleration_sequence; - a = state->acceleration; - v = state->velocity; - t = stepper_context.period_count + 2; - s = state->step_full * (long long)DIST_SCALE + state->step_frac; - while(seq && seq->period < period) { - dt = seq->period - t; - s += (a * (long long)dt + 2 * v) * dt; - v += a * (seq->period - t); - t = seq->period; - a = seq->acceleration; - seq = seq->next; - } - stepper_context.timer_channel->TC_IER = AT91C_TC_CPCS | AT91C_TC_CPAS; - dt = period - t; - *position = s + (a * (long long)dt + (DIST_SCALE/VEL_SCALE) * v) * dt; - *velocity = v + a * dt; - - return STEPPER_OK; -} - - -StepperResult -stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp, - unsigned long max_acc, long final_speed) -{ - long start_period = *periodp; - long v = stepper_velocity(stepper_index, start_period); - /* printf("%ld @ %ld\n", v, start_period); */ - if (final_speed == v) { - return stepper_add_acc(stepper_index, start_period, 0); - } else { - StepperResult res; - long a = (final_speed > v) ? max_acc : -max_acc; - long t = ((long)(final_speed - v)) / a; - long diff = (final_speed - v) - t * a; - if (t > 0) { - res = stepper_add_acc(stepper_index, start_period, a); - if (res != STEPPER_OK) return res; - } - if (diff) { - res = stepper_add_acc(stepper_index, start_period+t, diff); - if (res != STEPPER_OK) return res; - t++; - } - *periodp = start_period+t; - return stepper_add_acc(stepper_index, start_period+t, 0); - } -} - -#ifdef TIMING_ERRORS -void -stepper_timing_errors(unsigned int stepper_index, long *min, long *max) -{ - StepperState *state; - state = &stepper_context.steppers[stepper_index]; - *min = state->err_min; - *max = state->err_max; - state->err_max = -TIMER_FREQ; - state->err_min = TIMER_FREQ; -} -#endif diff --git a/platform/stepper-robot/stepper/stepper.h b/platform/stepper-robot/stepper/stepper.h deleted file mode 100644 index 6e067641c..000000000 --- a/platform/stepper-robot/stepper/stepper.h +++ /dev/null @@ -1,98 +0,0 @@ -#ifndef __STEPPER_H__JPA916UOFT__ -#define __STEPPER_H__JPA916UOFT__ - -#include -#include -#include - -/* Define periods/second */ -#define PPS 128 - -/* Scaling factor for distance */ -#define DIST_SCALE (2 * PPS * PPS) - -/* Scaling factor for velocity */ -#define VEL_SCALE PPS - -typedef struct _StepperAccSeq StepperAccSeq; -struct _StepperAccSeq -{ - StepperAccSeq *next; - unsigned long period; - long acceleration; -}; - -#define STEPPER_ACC_INVALID LONG_MAX - -#define STEPPER_MAX_VELOCITY 4000 -#define STEPPER_MAX_ACCELRATION 4000 - -typedef void (*StepperUserCallback)(unsigned int stepper_index, - unsigned long period); - - -typedef unsigned int StepperResult; -#define STEPPER_OK 0 -#define STEPPER_ERR_MEM 1 -#define STEPPER_ERR_TOO_LATE 2 -#define STEPPER_ERR_INDEX 3 - -void -stepper_init(AT91PS_TC timer, unsigned int id); - -void -stepper_init_io(unsigned int stepper_index, uint32_t mask, - const uint32_t *acc, const uint32_t *run, - const uint32_t *hold, unsigned int nsteps); - -/* Returns true if the new sequence was actually added or false - if the index is illegal or the first step in the sequence is too soon */ - -StepperResult -stepper_add_acc_seq(unsigned int stepper_index, StepperAccSeq *new_seq); - -StepperResult -stepper_add_acc(unsigned int stepper_index, unsigned int period, long acc); - -StepperResult -stepper_insert_callback(unsigned int stepper_index, unsigned int period); - -void -stepper_set_callback_proc(StepperUserCallback callback); - -unsigned long -stepper_current_period(); - -long -stepper_current_step(unsigned int stepper_index); - -long long -stepper_step_frac(unsigned int stepper_index); - -long -stepper_current_velocity(unsigned int stepper_index); - -unsigned long -stepper_velocity(unsigned int stepper_index, unsigned long period); - -StepperResult -stepper_state_at(unsigned int stepper_index, unsigned long period, - long *velocity, long long *position); - -StepperResult -stepper_set_velocity(unsigned int stepper_index, unsigned long *periodp, - unsigned long max_acc, long final_speed); - -StepperAccSeq * -stepper_allocate_seq(); - -void -stepper_free_seq(StepperAccSeq *seq); - -#ifdef TIMING_ERRORS - -void -stepper_timing_errors(unsigned int stepper_index, long *min, long *max); -#endif - -#endif /* __STEPPER_H__JPA916UOFT__ */ diff --git a/platform/stepper-robot/sys-tst.c b/platform/stepper-robot/sys-tst.c deleted file mode 100644 index d8b7e6d88..000000000 --- a/platform/stepper-robot/sys-tst.c +++ /dev/null @@ -1,80 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -volatile const char * volatile input_line = NULL; -volatile unsigned int input_line_len = 0; - -static void -recv_input(const char *str, unsigned int len) -{ - /* Assume that the line is handled before any new characters is written - to the buffer */ - input_line = str; - input_line_len = len; -} -PROCESS(blink_process, "LED blink process"); - -PROCESS_THREAD(blink_process, ev , data) -{ - static struct etimer timer; - PROCESS_BEGIN(); - etimer_set(&timer, CLOCK_SECOND/2); - while(1) { - PROCESS_WAIT_EVENT_UNTIL(ev == PROCESS_EVENT_EXIT || - ev== PROCESS_EVENT_TIMER); - if (ev == PROCESS_EVENT_EXIT) break; - leds_invert(LEDS_RED); -#if 0 - { - DISABLE_FIFOP_INT(); - printf("FSMSTATE: %04x",cc2420_getreg(CC2420_FSMSTATE)); - ENABLE_FIFOP_INT(); - if (SFD_IS_1) printf(" SFD"); - if (FIFO_IS_1) printf(" FIFO"); - if (FIFOP_IS_1) printf(" FIFOP"); - putchar('\n'); - } -#endif - etimer_reset(&timer); - } - printf("Ended process\n"); - PROCESS_END(); -} -PROCINIT(&etimer_process, &blink_process); -int -main() -{ - disableIRQ(); - disableFIQ(); - *AT91C_AIC_IDCR = 0xffffffff; - *AT91C_PMC_PCDR = 0xffffffff; - *AT91C_PMC_PCER = (1 << AT91C_ID_PIOA); - - dbg_setup_uart(); - printf("Initialising\n"); - dbg_set_input_handler(recv_input); - leds_arch_init(); - clock_init(); - - process_init(); - printf("Started\n"); - - procinit_init(); - enableIRQ(); - printf("Processes running\n"); - while(1) { - do { - /* Reset watchdog. */ - } while(process_run() > 0); - /* Idle! */ - /* Stop processor clock */ - *AT91C_PMC_SCDR |= AT91C_PMC_PCK; - } - return 0; -} diff --git a/platform/stepper-robot/usb-main.c b/platform/stepper-robot/usb-main.c deleted file mode 100644 index cf10c1f11..000000000 --- a/platform/stepper-robot/usb-main.c +++ /dev/null @@ -1,32 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -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; -} diff --git a/platform/stepper-robot/usb-msc/Makefile b/platform/stepper-robot/usb-msc/Makefile deleted file mode 100644 index 83ad96b17..000000000 --- a/platform/stepper-robot/usb-msc/Makefile +++ /dev/null @@ -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 diff --git a/platform/stepper-robot/usb-msc/usbmsctst.c b/platform/stepper-robot/usb-msc/usbmsctst.c deleted file mode 100644 index 1746744ca..000000000 --- a/platform/stepper-robot/usb-msc/usbmsctst.c +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -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);