Removed the stepper-robot platform
This commit is contained in:
Adam Dunkels
parent
13117203e0
commit
d68ce09fd5
|
|
@ -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
|
|
||||||
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
||||||
|
|
@ -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__ */
|
|
||||||
|
|
@ -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__ */
|
|
||||||
|
|
@ -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);
|
|
||||||
}
|
|
||||||
|
|
@ -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__ */
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -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);
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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);
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -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__ */
|
|
||||||
|
|
@ -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();
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
@ -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__ */
|
|
||||||
|
|
@ -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();
|
|
||||||
}
|
|
||||||
|
|
@ -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);
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
@ -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__ */
|
|
||||||
|
|
@ -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)
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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__ */
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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);
|
|
||||||
Loading…
Reference in a new issue