*
*/
+#include <aversive.h>
#include <stdio.h>
#include <string.h>
#include <scheduler_config.h>
#include <scheduler_private.h>
+#include <scheduler_stats.h>
/* this file is compiled for host version only */
/** declared in scheduler.c in case of AVR version */
struct event_t g_tab_event[SCHEDULER_NB_MAX_EVENT];
+#ifdef CONFIG_MODULE_SCHEDULER_STATS
+struct scheduler_stats sched_stats;
+#endif
+
/** init all global data */
void scheduler_init(void)
{
TARGET = i2c
# List C source files here. (C dependencies are automatically generated.)
+ifeq ($(HOST),avr)
SRC = i2c.c
+else
+SRC = i2c_host.c
+endif
include $(AVERSIVE_DIR)/mk/aversive_module.mk
-/*
+/*
* Copyright Droids Corporation (2007)
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* params are : hwstatus, index of byte in frame, byte value */
static void (*g_recv_byte_event)(uint8_t, uint8_t, uint8_t) = NULL;
-/** send event, called when transmit is complete
+/** send event, called when transmit is complete
* param is error code : 0 if success */
static void (*g_send_event)(int8_t) = NULL;
/**
* mode is I2C_MODE_UNINIT, I2C_MODE_MASTER, I2C_MODE_MULTIMASTER or
* I2C_MODE_SLAVE. Parameter add is the address in slave mode, it is
- * composed from:
+ * composed from:
* b7 : true if the uC can be addressed with GENCALL
* b0-6: slave address
*/
-void
+void
i2c_init(i2c_mode_t mode, uint8_t add)
{
uint8_t flags;
IRQ_LOCK(flags);
-
+
if (mode == I2C_MODE_UNINIT) {
/* disable all */
TWCR = 0;
}
TWBR = I2C_BITRATE;
-
+
/* prescaler */
if (I2C_PRESCALER & 1)
sbi(TWSR, TWPS0);
if (I2C_PRESCALER & 2)
sbi(TWSR, TWPS1);
-
+
/* change for TWAR format */
TWAR = add << 1 ;
-
+
/* general call */
if (add & 0x80)
sbi(TWAR, TWGCE);
-
+
/* init vars */
g_mode = mode;
g_status = I2C_STATUS_READY;
}
-/**
+/**
* Register a function that is called when a buffer is received. The
* user application is always notified when data frame is received.
* Arguments of the callback are:
- * - (recv_buf, n>0) if transmission succedded. The first parameter
+ * - (recv_buf, n>0) if transmission succedded. The first parameter
* contains the address of the reception buffer and
* the second contains the number of received bytes.
* - (NULL, err<0) if the transmission failed (slave not answering
- * or arbiteration lost). The first parameter is
+ * or arbiteration lost). The first parameter is
* NULL and the second contains the error code.
*/
-void
+void
i2c_register_recv_event(void (*event)(uint8_t *, int8_t))
{
uint8_t flags;
IRQ_UNLOCK(flags);
}
-/**
+/**
* Register a function that is called when a byte is received.
* Arguments of the callback are: (hwstatus, numbyte, byte). The user
* app can modify the g_recv_size value, which is the number of bytes
* to be received in the frame: this can be done by calling
* i2c_set_recv_size().
*/
-void
+void
i2c_register_recv_byte_event(void (*event)(uint8_t, uint8_t, uint8_t))
{
uint8_t flags;
* - Else, the number of transmitted bytes is given, including the
* one that was not acked.
*/
-void
+void
i2c_register_send_event(void (*event)(int8_t))
{
uint8_t flags;
/**
* Send a buffer. Return 0 if xmit starts correctly.
* On error, return < 0.
- * - If mode is slave, dest_add should be I2C_ADD_MASTER, and transmission
- * starts when the master transmits a clk.
- * - If mode is master and if dest_add != I2C_ADD_MASTER, it will transmit
- * a START condition if bus is available (the uc will act as a
+ * - If mode is slave, dest_add should be I2C_ADD_MASTER, and transmission
+ * starts when the master transmits a clk.
+ * - If mode is master and if dest_add != I2C_ADD_MASTER, it will transmit
+ * a START condition if bus is available (the uc will act as a
* master)
* - If mode is master and if dest_add == I2C_ADD_MASTER, the uC will
* act as a slave, and data will be sent when the uC will be
* addressed.
* The transmission will be processed with these params until a
- * i2c_flush() is called.
- * The 'ctrl' parameter is composed by the flags I2C_CTRL_SYNC and
+ * i2c_flush() is called.
+ * The 'ctrl' parameter is composed by the flags I2C_CTRL_SYNC and
* I2C_CTRL_DONT_RELEASE_BUS
*/
-int8_t
-i2c_send(uint8_t dest_add, uint8_t *buf, uint8_t size, uint8_t ctrl)
+int8_t
+i2c_send(uint8_t dest_add, uint8_t *buf, uint8_t size, uint8_t ctrl)
{
uint8_t flags;
-
+
IRQ_LOCK(flags);
if (g_mode == I2C_MODE_UNINIT) {
IRQ_UNLOCK(flags);
return -ENXIO;
}
-
+
if (g_status & (I2C_STATUS_MASTER_XMIT |
I2C_STATUS_MASTER_RECV |
I2C_STATUS_SLAVE_XMIT_WAIT |
return 0;
return g_sync_res;
}
-
+
return -ESUCCESS;
}
* the same parameters as the last call. It safe to call it from the
* send_event, but else the send buffer may have been overwritten.
*/
-int8_t
-i2c_resend(void)
+int8_t
+i2c_resend(void)
{
return i2c_send(g_dest, g_send_buf, g_send_size, g_ctrl);
}
/**
* same but for recv
*/
-int8_t
-i2c_rerecv(void)
+int8_t
+i2c_rerecv(void)
{
return i2c_recv(g_dest, g_recv_size, g_ctrl);
}
TWCR = 0;
g_status = I2C_STATUS_READY;
#ifdef CONFIG_MODULE_I2C_MASTER
- if (g_mode == I2C_MODE_MASTER)
+ if (g_mode == I2C_MODE_MASTER)
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
else
#endif
- TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) |
+ TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) |
(1<<TWSTO) | (1<<TWEA);
IRQ_UNLOCK(flags);
}
IRQ_UNLOCK(flags);
return -ENXIO;
}
-
+
if (g_status != I2C_STATUS_READY) {
IRQ_UNLOCK(flags);
return -EBUSY;
g_ctrl = ctrl;
g_recv_size = size;
g_status |= I2C_STATUS_MASTER_RECV;
- g_dest = dest_add ;
+ g_dest = dest_add ;
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWSTA);
IRQ_UNLOCK(flags);
return 0;
return g_sync_res;
}
-
+
return -ESUCCESS;
#endif
}
/**
* Try to flush the current operation, before it is started. The
- * i2c module is then tagged as ready. If it returns 0, the flush was
- * a success, and i2c_send() can be called. Else, it means that
- * a transmission was running.
+ * i2c module is then tagged as ready. If it returns 0, the flush was
+ * a success, and i2c_send() can be called. Else, it means that
+ * a transmission was running.
*/
int8_t i2c_flush(void)
{
g_status &= ~(I2C_STATUS_SLAVE_XMIT_WAIT);
IRQ_UNLOCK(flags);
-
+
return -ESUCCESS;
}
IRQ_UNLOCK(flags);
return -EINVAL;
}
-
+
g_recv_size = size;
IRQ_UNLOCK(flags);
return -EBUSY;
}
- if (size > g_recv_nbytes)
+ if (size > g_recv_nbytes)
size = g_recv_nbytes;
memcpy(buf, g_recv_buf, size);
switch(hard_status) {
#ifdef CONFIG_MODULE_I2C_MASTER
- case TW_START:
+ case TW_START:
case TW_REP_START:
/* a start has been transmitted, transmit SLA+W which is :
- * b7-1: slave address
+ * b7-1: slave address
* b0 : 0 (write operation) or 1 (read) */
if (g_status & I2C_STATUS_MASTER_RECV) {
TWDR = (g_dest << 1) | (0x01);
break;
case TW_MT_DATA_NACK:
- /* we transmitted data but slave sent us a NACK.
+ /* we transmitted data but slave sent us a NACK.
* Notify the number of bytes sent, including the one
* that were not acked, and send a stop condition */
g_status |= (I2C_STATUS_OP_FINISHED | I2C_STATUS_NEED_XMIT_EVT);
break;
-
+
/* MASTER RECEIVER */
if (g_recv_size > 1)
command |= (1<<TWEA);
break;
-
+
case TW_MR_SLA_NACK:
/* the slave does not answer, send a stop condition */
g_recv_nbytes = -ENOENT;
}
else if (g_status & I2C_STATUS_MASTER_RECV) {
g_send_nbytes = -EAGAIN;
- g_status |= I2C_STATUS_NEED_XMIT_EVT;
+ g_status |= I2C_STATUS_NEED_XMIT_EVT;
}
/* g_status |= I2C_STATUS_OP_FINISHED; */ /* ?? or not ? */
break;
-#endif
+#endif
+
-
/* SLAVE RECEIVER */
case TW_SR_ARB_LOST_SLA_ACK:
break;
case TW_SR_GCALL_DATA_NACK:
- case TW_SR_DATA_NACK:
+ case TW_SR_DATA_NACK:
/* receive last data byte (our buffer is full) */
if (g_recv_nbytes < g_recv_size) {
g_recv_buf[g_recv_nbytes] = TWDR;
g_recv_nbytes++;
}
break;
-
+
case TW_SR_STOP:
/* the master sent a stop condition, notify app */
g_status |= (I2C_STATUS_OP_FINISHED | I2C_STATUS_NEED_RECV_EVT);
break;
-
+
/* SLAVE TRANSMITTER */
case TW_ST_ARB_LOST_SLA_ACK:
- case TW_ST_SLA_ACK:
+ case TW_ST_SLA_ACK:
/* slave is addressed. If it is not ready, send a 0 as
* last byte. */
g_send_nbytes = 0;
TWDR = 0;
g_send_size=0;
}
- /* else:
+ /* else:
* if there is only 1 byte to transmit, we don't
- * need to send ack, else set TWEA. */
+ * need to send ack, else set TWEA. */
else {
if (g_send_size > 1) {
command |= (1<<TWEA);
g_status &= ~(I2C_STATUS_SLAVE_XMIT_WAIT);
g_status |= I2C_STATUS_SLAVE_XMIT;
break;
-
+
case TW_ST_DATA_ACK:
/* transmitting data, if there is more than one byte
* to send, send ack */
g_status |= (I2C_STATUS_OP_FINISHED | I2C_STATUS_NEED_XMIT_EVT);
break;
-
+
/* COMMON */
case TW_BUS_ERROR:
command |= (1<<TWSTO);
g_status |= I2C_STATUS_OP_FINISHED;
break;
-
+
default :
/* default ... what can we do ? */
g_status |= I2C_STATUS_OP_FINISHED;
break;
}
-
+
#if I2C_DEBUG == 1
g_prev_status = g_status;
#endif
I2C_STATUS_OP_FINISHED);
}
}
-
+
/* Callback events if necessary (if not sync) */
if ( ! (g_ctrl & I2C_CTRL_SYNC) ) {
if ( (g_status & I2C_STATUS_NEED_XMIT_EVT) && g_send_event) {
else {
if ( g_status & (I2C_STATUS_MASTER_XMIT | I2C_STATUS_SLAVE_XMIT) )
g_sync_res = g_send_nbytes;
- else
+ else
g_sync_res = g_recv_nbytes;
}
g_status &= ~(I2C_STATUS_NEED_XMIT_EVT | I2C_STATUS_NEED_RECV_EVT);
-
+
#if I2C_DEBUG == 1
g_command = command;
#endif
* (by calling i2c_send() or i2c_recv(), we don't need to
* send it (we are back in MASTER_SEND or MASTER_RECV mode) */
if (TWCR & (1<<TWINT))
- TWCR = command;
+ TWCR = command;
}
--- /dev/null
+/* empty */
TARGET = diagnostic
# List C source files here. (C dependencies are automatically generated.)
+ifeq ($(HOST),avr)
SRC = stack_space.c int_show.c
+else
+SRC = diag_host.c
+endif
include $(AVERSIVE_DIR)/mk/aversive_module.mk
--- /dev/null
+/* empty */
TARGET = adc
# List C source files here. (C dependencies are automatically generated.)
+ifeq ($(HOST),avr)
SRC = adc.c
+else
+SRC = adc_host.c
+endif
include $(AVERSIVE_DIR)/mk/aversive_module.mk
/**
* Initialisation of ADC internal registers
- * Can be called for a wake up after a shutdown command
+ * Can be called for a wake up after a shutdown command
*/
void adc_init(void)
{
return;
result = ADC;
-
+
/* sign extension to fill the 16 bits when negative
* (for the 16 bits output format, the output is
* already right.) */
&& !(g_adc_previous_config & ADC_MODE_16_BITS)
&& (result & 0x0200) )
result |= 0xFE00;
-
+
adc_event(result);
}
--- /dev/null
+/* empty */
#
-# Automatically generated make config: don't edit
+# Automatically generated by make menuconfig: don't edit
#
#
#
# Base modules
#
-
-#
-# Enable math library in generation options to see all modules
-#
CONFIG_MODULE_CIRBUF=y
# CONFIG_MODULE_CIRBUF_LARGE is not set
CONFIG_MODULE_FIXED_POINT=y
CONFIG_MODULE_VECT2=y
CONFIG_MODULE_GEOMETRY=y
-# CONFIG_MODULE_HOSTSIM is not set
+CONFIG_MODULE_HOSTSIM=y
CONFIG_MODULE_SCHEDULER=y
CONFIG_MODULE_SCHEDULER_STATS=y
# CONFIG_MODULE_SCHEDULER_CREATE_CONFIG is not set
#
# Communication modules
#
-
-#
-# uart needs circular buffer, mf2 client may need scheduler
-#
CONFIG_MODULE_UART=y
# CONFIG_MODULE_UART_9BITS is not set
CONFIG_MODULE_UART_CREATE_CONFIG=y
# Control system modules
#
CONFIG_MODULE_CONTROL_SYSTEM_MANAGER=y
-
-#
-# Filters
-#
CONFIG_MODULE_PID=y
# CONFIG_MODULE_PID_CREATE_CONFIG is not set
# CONFIG_MODULE_RAMP is not set
#
# Radio devices
#
-
-#
-# Some radio devices require SPI to be activated
-#
# CONFIG_MODULE_CC2420 is not set
# CONFIG_MODULE_CC2420_CREATE_CONFIG is not set
#
# Crypto modules
#
-
-#
-# Crypto modules depend on utils module
-#
# CONFIG_MODULE_AES is not set
# CONFIG_MODULE_AES_CTR is not set
# CONFIG_MODULE_MD5 is not set
#
# Encodings modules
#
-
-#
-# Encoding modules depend on utils module
-#
# CONFIG_MODULE_BASE64 is not set
# CONFIG_MODULE_HAMMING is not set
#
# Debug modules
#
-
-#
-# Debug modules depend on utils module
-#
CONFIG_MODULE_DIAGNOSTIC=y
CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG=y
CONFIG_MODULE_ERROR=y
# repertoire des modules
AVERSIVE_DIR = ../../..
-SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
+SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
SRC += strat_utils.c strat_base.c strat.c
+ifneq ($(HOST),avr)
+SRC += robotsim.c
+endif
-ASRC =
+ASRC =
CFLAGS += -Wall -Werror
+ifeq ($(HOST),avr)
LDFLAGS = -T ../common/avr6.x
+endif
########################################
#include <rdline.h>
#include "main.h"
+#include "robotsim.h"
void pwm_set_and_save(void *pwm, int32_t val)
{
mainboard.pwm_l = val;
else if (pwm == RIGHT_PWM)
mainboard.pwm_r = val;
+#ifdef HOST_VERSION
+ robotsim_pwm(pwm, val);
+#else
pwm_ng_set(pwm, val);
+#endif
}
void support_balls_deploy(void)
{
+#ifndef HOST_VERSION
pwm_ng_set(SUPPORT_BALLS_R_SERVO, 510);
pwm_ng_set(SUPPORT_BALLS_L_SERVO, 205);
+#endif
}
void support_balls_pack(void)
{
+#ifndef HOST_VERSION
pwm_ng_set(SUPPORT_BALLS_R_SERVO, 250);
pwm_ng_set(SUPPORT_BALLS_L_SERVO, 480);
+#endif
}
void actuator_init(void)
* DDR to manage the port directions.
*/
+#ifndef HOST_VERSION
+
static volatile uint8_t ax12_state = AX12_STATE_READ;
extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */
static volatile uint8_t ax12_nsent = 0;
IRQ_UNLOCK(flags);
}
}
-
+#endif
void ax12_user_init(void)
{
+#ifdef HOST_VERSION
+ return;
+#else
/* AX12 */
AX12_init(&gen.ax12);
AX12_set_hardware_send(&gen.ax12, ax12_send_char);
AX12_set_hardware_recv(&gen.ax12, ax12_recv_char);
AX12_set_hardware_switch(&gen.ax12, ax12_switch_uart);
uart_register_tx_event(UART_AX12_NUM, ax12_send_callback);
+#endif
}
-/*
+/*
* Copyright Droids Corporation
* Olivier Matz <zer0@droids-corp.org>
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
#include <aversive.h>
#include <aversive/error.h>
+#include <hostsim.h>
+
#include <parse.h>
#include <rdline.h>
#include <ax12.h>
/******** See in commands.c for the list of commands. */
extern parse_pgm_ctx_t main_ctx[];
-static void write_char(char c)
+static void write_char(char c)
{
uart_send(CMDLINE_UART, c);
}
-static void
-valid_buffer(const char *buf, uint8_t size)
+static void
+valid_buffer(const char *buf, uint8_t size)
{
int8_t ret;
printf_P(PSTR("Bad arguments\r\n"));
}
-static int8_t
+static int8_t
complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
int16_t *state)
{
void emergency(char c)
{
static uint8_t i = 0;
-
+
/* interrupt traj here */
if (c == '\003')
interrupt_traj();
-
+
if ((i == 0 && c == 'p') ||
(i == 1 && c == 'o') ||
- (i == 2 && c == 'p'))
+ (i == 2 && c == 'p'))
i++;
else if ( !(i == 1 && c == 'p') )
i = 0;
- if (i == 3)
+ if (i == 3) {
+#ifdef HOST_VERSION
+ hostsim_exit();
+#endif
reset();
+ }
}
/* log function, add a command to configure
* it dynamically */
-void mylog(struct error * e, ...)
+void mylog(struct error * e, ...)
{
va_list ap;
+#ifndef HOST_VERSION
u16 stream_flags = stdout->flags;
+#endif
uint8_t i;
time_h tv;
if (e->severity > ERROR_SEVERITY_ERROR) {
if (gen.log_level < e->severity)
return;
-
+
for (i=0; i<NB_LOGS+1; i++)
if (gen.logs[i] == e->err_num)
break;
va_start(ap, e);
tv = time_get_time();
- printf_P(PSTR("%ld.%.3ld: "), tv.s, (tv.us/1000UL));
-
+ printf_P(PSTR("%d.%.3d: "), (int)tv.s, (int)(tv.us/1000UL));
+
printf_P(PSTR("(%d,%d,%d) "),
position_get_x_s16(&mainboard.pos),
position_get_y_s16(&mainboard.pos),
position_get_a_deg_s16(&mainboard.pos));
-
+
vfprintf_P(stdout, e->text, ap);
printf_P(PSTR("\r\n"));
va_end(ap);
+#ifndef HOST_VERSION
stdout->flags = stream_flags;
+#endif
}
int cmdline_interact(void)
const char *history, *buffer;
int8_t ret, same = 0;
int16_t c;
-
+
rdline_init(&gen.rdl, write_char, valid_buffer, complete_buffer);
- snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > ");
+ snprintf(gen.prompt, sizeof(gen.prompt), "mainboard > ");
rdline_newline(&gen.rdl, gen.prompt);
while (1) {
c = uart_recv_nowait(CMDLINE_UART);
- if (c == -1)
+ if (c == -1)
continue;
ret = rdline_char_in(&gen.rdl, c);
if (ret != 2 && ret != 0) {
*
*/
+#ifdef HOST_VERSION
+#define CMDLINE_UART 0
+#else
#define CMDLINE_UART 1
+#endif
/* uart rx callback for reset() */
void emergency(char c);
/* function called when cmd_baudrate is parsed successfully */
static void cmd_baudrate_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_baudrate_result *res = parsed_result;
struct uart_config c;
c.baudrate = res->arg1;
uart_setconf(1, &c);
printf_P(PSTR("%d %d\r\n"), UBRR1H, UBRR1L);
+#endif
}
prog_char str_baudrate_arg0[] = "baudrate";
#include <aversive/wait.h>
#include <aversive/error.h>
+#include <hostsim.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
if (!show)
ramp_set_vars(&csb->ramp, res->s, res->s); /* set speed */
- printf_P(PSTR("%s %lu\r\n"),
+ printf_P(PSTR("%s %"PRIu32"\r\n"),
res->cs.csname,
ext.r_b.var_pos);
#else
if (!show)
pid_set_maximums(&csb->pid, res->in, res->i, res->out);
- printf_P(PSTR("maximum %s %lu %lu %lu\r\n"),
+ printf_P(PSTR("maximum %s %"PRIu32" %"PRIu32" %"PRIu32"\r\n"),
res->cs.csname,
pid_get_max_in(&csb->pid),
pid_get_max_I(&csb->pid),
quadramp_set_2nd_order_vars(&csb->qr, res->ap, res->an);
}
- printf_P(PSTR("quadramp %s %ld %ld %ld %ld\r\n"),
+ printf_P(PSTR("quadramp %s %"PRIi32" %"PRIi32" %"PRIi32" %"PRIi32"\r\n"),
res->cs.csname,
csb->qr.var_2nd_ord_pos,
csb->qr.var_2nd_ord_neg,
bd_set_current_thresholds(&csb->bd, res->k1, res->k2,
res->i, res->cpt);
- printf_P(PSTR("%s %s %ld %ld %ld %d\r\n"),
+ printf_P(PSTR("%s %s %"PRIi32" %"PRIi32" %"PRIi32" %d\r\n"),
res->cs.cmdname,
res->cs.csname,
csb->bd.k1,
#include <aversive/wait.h>
#include <aversive/error.h>
+#include <hostsim.h>
#include <ax12.h>
#include <uart.h>
#include <pwm_ng.h>
/* function called when cmd_reset is parsed successfully */
static void cmd_reset_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ hostsim_exit();
+#endif
reset();
}
/* function called when cmd_bootloader is parsed successfully */
static void cmd_bootloader_parsed(void *parsed_result, void *data)
{
+#ifndef HOST_VERSION
bootloader();
+#else
+ printf("not implemented\n");
+#endif
}
prog_char str_bootloader_arg0[] = "bootloader";
/* function called when cmd_encoders is parsed successfully */
static void cmd_encoders_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_encoders_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("reset"))) {
encoders_spi_get_value((void *)3));
wait_ms(100);
}
+#endif
}
prog_char str_encoders_arg0[] = "encoders";
/* function called when cmd_pwm is parsed successfully */
static void cmd_pwm_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
void * pwm_ptr = NULL;
struct cmd_pwm_result * res = parsed_result;
pwm_ng_set(pwm_ptr, res->arg2);
printf_P(PSTR("done\r\n"));
+#endif
}
prog_char str_pwm_arg0[] = "pwm";
/* function called when cmd_adc is parsed successfully */
static void cmd_adc_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_adc_result *res = parsed_result;
uint8_t i, loop = 0;
printf_P(PSTR("\r\n"));
wait_ms(100);
} while (loop && !cmdline_keypressed());
+#endif
}
prog_char str_adc_arg0[] = "adc";
/* function called when cmd_sensor is parsed successfully */
static void cmd_sensor_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_sensor_result *res = parsed_result;
uint8_t i, loop = 0;
printf_P(PSTR("\r\n"));
wait_ms(100);
} while (loop && !cmdline_keypressed());
+#endif
}
prog_char str_sensor_arg0[] = "sensor";
for (i=0; i<NB_LOGS; i++) {
name = log_num2name(gen.logs[i]);
if (name) {
+#ifdef HOST_VERSION
+ printf_P(PSTR("log type %s is on\r\n"), name);
+#else
printf_P(PSTR("log type %S is on\r\n"), name);
+#endif
empty = 0;
}
}
if (empty)
- printf_P(PSTR("no log configured\r\n"), gen.logs[i]);
+ printf_P(PSTR("no log configured\r\n"));
}
/* function called when cmd_log is parsed successfully */
parse_pgm_token_string_t cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
prog_char str_log_arg1[] = "level";
parse_pgm_token_string_t cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
-parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT32);
+parse_pgm_token_num_t cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT8);
prog_char help_log[] = "Set log options: level (0 -> 5)";
parse_pgm_inst_t cmd_log = {
/* function called when cmd_stack_space is parsed successfully */
static void cmd_stack_space_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
printf("res stack: %d\r\n", min_stack_space_available());
-
+#endif
}
prog_char str_stack_space_arg0[] = "stack_space";
#include <stdio.h>
#include <string.h>
+#include <hostsim.h>
#include <aversive/pgmspace.h>
#include <aversive/wait.h>
#include <aversive/error.h>
#include "../common/eeprom_mapping.h"
#include "main.h"
+#include "robotsim.h"
#include "sensor.h"
#include "cmdline.h"
#include "strat.h"
mainboard.flags |= bit;
else if (!strcmp_P(res->arg2, PSTR("off"))) {
if (!strcmp_P(res->arg1, PSTR("cs"))) {
+#ifdef HOST_VERSION
+ robotsim_pwm(LEFT_PWM, 0);
+ robotsim_pwm(RIGHT_PWM, 0);
+#else
pwm_ng_set(LEFT_PWM, 0);
pwm_ng_set(RIGHT_PWM, 0);
+#endif
}
mainboard.flags &= (~bit);
}
/* function called when cmd_spi_test is parsed successfully */
static void cmd_spi_test_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
uint16_t i = 0, ret = 0, ret2 = 0;
if (mainboard.flags & DO_ENCODERS) {
i++;
} while(!cmdline_keypressed());
+#endif
}
prog_char str_spi_test_arg0[] = "spi_test";
/* function called when cmd_start is parsed successfully */
static void cmd_start_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_start_result *res = parsed_result;
uint8_t old_level = gen.log_level;
gen.logs[NB_LOGS] = 0;
gen.log_level = old_level;
+#endif
}
prog_char str_start_arg0[] = "start";
static void print_cs(void)
{
- printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld "
- "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"),
+ printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
+ "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
cs_get_consign(&mainboard.distance.cs),
cs_get_consign(&mainboard.angle.cs),
cs_get_filtered_consign(&mainboard.distance.cs),
static void print_pid(void)
{
- printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | "
- "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | "
+ "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid),
pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid),
pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid),
/* function called when cmd_color is parsed successfully */
static void cmd_color_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_color_result *res = (struct cmd_color_result *) parsed_result;
if (!strcmp_P(res->color, PSTR("yellow"))) {
mainboard.our_color = I2C_COLOR_YELLOW;
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
}
printf_P(PSTR("Done\r\n"));
+#endif
}
prog_char str_color_arg0[] = "color";
{
// struct cmd_rs_result *res = parsed_result;
do {
- printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "),
+ printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
cs_get_consign(&mainboard.angle.cs),
cs_get_filtered_feedback(&mainboard.angle.cs),
cs_get_out(&mainboard.angle.cs));
- printf_P(PSTR("distance cons=% .6ld in=% .6ld out=% .6ld / "),
+ printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
cs_get_consign(&mainboard.distance.cs),
cs_get_filtered_feedback(&mainboard.distance.cs),
cs_get_out(&mainboard.distance.cs));
- printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l,
+ printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l,
mainboard.pwm_r);
wait_ms(100);
} while(!cmdline_keypressed());
/* function called when cmd_i2cdebug is parsed successfully */
static void cmd_i2cdebug_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
i2c_debug();
i2c_protocol_debug();
+#endif
}
prog_char str_i2cdebug_arg0[] = "i2cdebug";
/* function called when cmd_cobboard_show is parsed successfully */
static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
printf_P(PSTR("status = %x\r\n"), cobboard.status);
printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
+#endif
}
prog_char str_cobboard_show_arg0[] = "cobboard";
/* function called when cmd_cobboard_setmode1 is parsed successfully */
static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_cobboard_setmode1_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("init")))
i2c_cobboard_mode_init();
else if (!strcmp_P(res->arg1, PSTR("eject")))
i2c_cobboard_mode_eject();
+#endif
}
prog_char str_cobboard_setmode1_arg0[] = "cobboard";
/* function called when cmd_cobboard_setmode2 is parsed successfully */
static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_cobboard_setmode2_result *res = parsed_result;
uint8_t side = I2C_LEFT_SIDE;
i2c_cobboard_mode_harvest(side);
else if (!strcmp_P(res->arg1, PSTR("pack")))
i2c_cobboard_mode_pack(side);
+#endif
}
prog_char str_cobboard_setmode2_arg0[] = "cobboard";
/* function called when cmd_cobboard_setmode3 is parsed successfully */
static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_cobboard_setmode3_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("xxx")))
printf("faux\r\n");
+#endif
}
prog_char str_cobboard_setmode3_arg0[] = "cobboard";
/* function called when cmd_ballboard_show is parsed successfully */
static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
printf_P(PSTR("status = %x\r\n"), ballboard.status);
printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
+#endif
}
prog_char str_ballboard_show_arg0[] = "ballboard";
/* function called when cmd_ballboard_setmode1 is parsed successfully */
static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_ballboard_setmode1_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("init")))
i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
/* other commands */
+#endif
}
prog_char str_ballboard_setmode1_arg0[] = "ballboard";
/* function called when cmd_ballboard_setmode2 is parsed successfully */
static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_ballboard_setmode2_result *res = parsed_result;
uint8_t mode = I2C_BALLBOARD_MODE_INIT;
mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
}
i2c_ballboard_set_mode(mode);
+#endif
}
prog_char str_ballboard_setmode2_arg0[] = "ballboard";
/* function called when cmd_ballboard_setmode3 is parsed successfully */
static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_ballboard_setmode3_result *res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("xxx")))
printf("faux\r\n");
+#endif
}
prog_char str_ballboard_setmode3_arg0[] = "ballboard";
#include <stdio.h>
#include <string.h>
+#include <hostsim.h>
#include <aversive/pgmspace.h>
#include <aversive/wait.h>
#include <aversive/error.h>
trajectory_set_circle_coef(&mainboard.traj, res->circle_coef);
}
- printf_P(PSTR("circle_coef %2.2f\r\n"), mainboard.traj.circle_coef);
+ printf_P(PSTR("circle_coef set %2.2f\r\n"), mainboard.traj.circle_coef);
}
prog_char str_circle_coef_arg0[] = "circle_coef";
/* function called when cmd_rs_gains is parsed successfully */
static void cmd_rs_gains_parsed(void * parsed_result, void * data)
{
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
struct cmd_rs_gains_result * res = parsed_result;
if (!strcmp_P(res->arg1, PSTR("set"))) {
}
printf_P(PSTR("rs_gains set %2.2f %2.2f\r\n"),
mainboard.rs.left_ext_gain, mainboard.rs.right_ext_gain);
+#endif
}
prog_char str_rs_gains_arg0[] = "rs_gains";
struct cmd_pt_list_result * res = parsed_result;
uint8_t i, why=0;
- if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
- printf_P(PSTR("not implemented\r\n"));
- return;
- }
-
if (!strcmp_P(res->arg1, PSTR("append"))) {
res->arg2 = pt_list_len;
}
+ if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
+ printf_P(PSTR("removed\r\n"));
+ return;
+ }
if (!strcmp_P(res->arg1, PSTR("insert")) ||
!strcmp_P(res->arg1, PSTR("append"))) {
printf_P(PSTR("List empty\r\n"));
return;
}
+ restart:
for (i=0 ; i<pt_list_len ; i++) {
printf_P(PSTR("%d: x=%d y=%d\r\n"), i, pt_list[i].x, pt_list[i].y);
if (!strcmp_P(res->arg1, PSTR("start"))) {
trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
why = wait_traj_end(0xFF); /* all */
}
+ else if (!strcmp_P(res->arg1, PSTR("loop_start"))) {
+ trajectory_goto_xy_abs(&mainboard.traj, pt_list[i].x, pt_list[i].y);
+ why = wait_traj_end(0xFF); /* all */
+ }
#if 0
else if (!strcmp_P(res->arg1, PSTR("avoid_start"))) {
while (1) {
#endif
if (why & (~(END_TRAJ | END_NEAR)))
trajectory_stop(&mainboard.traj);
+ if (why & END_INTR)
+ break;
}
+ if (why & END_INTR)
+ return;
+ if (!strcmp_P(res->arg1, PSTR("loop_start")))
+ goto restart;
}
prog_char str_pt_list_arg0[] = "pt_list";
};
/* show */
-prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start";
+prog_char str_pt_list_show_arg[] = "show#reset#start#avoid_start#loop_start";
parse_pgm_token_string_t cmd_pt_list_show_arg = TOKEN_STRING_INITIALIZER(struct cmd_pt_list_result, arg1, str_pt_list_show_arg);
prog_char help_pt_list_show[] = "Show, start or reset pt_list";
}
else if (!strcmp_P(res->arg1, PSTR("autoset_blue"))) {
mainboard.our_color = I2C_COLOR_BLUE;
+#ifndef HOST_VERSION
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_BLUE);
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE);
+#endif
auto_position();
}
else if (!strcmp_P(res->arg1, PSTR("autoset_red"))) {
mainboard.our_color = I2C_COLOR_YELLOW;
+#ifndef HOST_VERSION
i2c_set_color(I2C_COBBOARD_ADDR, I2C_COLOR_YELLOW);
i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_YELLOW);
+#endif
auto_position();
}
-/*
+/*
* Copyright Droids Corporation
* Olivier Matz <zer0@droids-corp.org>
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
#include <stdio.h>
#include <string.h>
+#include <stdint.h>
#include <aversive.h>
#include <aversive/error.h>
#include <rdline.h>
#include "main.h"
+#include "robotsim.h"
#include "strat.h"
#include "actuator.h"
+#ifndef HOST_VERSION
int32_t encoders_left_cobroller_speed(void *number)
{
static volatile int32_t roller_pos;
roller_pos = tmp;
return speed;
}
+#endif
/* called every 5 ms */
-static void do_cs(void *dummy)
+static void do_cs(void *dummy)
{
static uint16_t cpt = 0;
static int32_t old_a = 0, old_d = 0;
+#ifdef HOST_VERSION
+ robotsim_update();
+#else
/* read encoders */
if (mainboard.flags & DO_ENCODERS) {
encoders_spi_manage(NULL);
}
+#endif
- /* XXX there is an issue which is probably related to avr-libc
- * 1.6.2 (debian): this code using fixed_point lib does not
- * work with it */
/* robot system, conversion to angle,distance */
if (mainboard.flags & DO_RS) {
int16_t a,d;
cs_manage(&mainboard.angle.cs);
if (mainboard.distance.on)
cs_manage(&mainboard.distance.cs);
+#ifndef HOST_VERSION
if (mainboard.left_cobroller.on)
cs_manage(&mainboard.left_cobroller.cs);
if (mainboard.right_cobroller.on)
cs_manage(&mainboard.right_cobroller.cs);
+#endif
}
if ((cpt & 1) && (mainboard.flags & DO_POS)) {
/* about 1.5ms (worst case without centrifugal force
if (mainboard.flags & DO_BD) {
bd_manage_from_cs(&mainboard.angle.bd, &mainboard.angle.cs);
bd_manage_from_cs(&mainboard.distance.bd, &mainboard.distance.cs);
+#ifndef HOST_VERSION
bd_manage_from_cs(&mainboard.left_cobroller.bd, &mainboard.left_cobroller.cs);
bd_manage_from_cs(&mainboard.right_cobroller.bd, &mainboard.right_cobroller.cs);
+#endif
}
+#ifndef HOST_VERSION
if (mainboard.flags & DO_TIMER) {
uint8_t second;
/* the robot should stop correctly in the strat, but
while(1);
}
}
+#endif
/* brakes */
if (mainboard.flags & DO_POWER)
BRAKE_OFF();
else
BRAKE_ON();
cpt++;
+
+#ifdef HOST_VERSION
+ if ((cpt & 7) == 0)
+ robotsim_dump();
+#endif
}
void dump_cs_debug(const char *name, struct cs *cs)
{
- DEBUG(E_USER_CS, "%s cons=% .5ld fcons=% .5ld err=% .5ld "
- "in=% .5ld out=% .5ld",
+ DEBUG(E_USER_CS, "%s cons=% .5"PRIi32" fcons=% .5"PRIi32" err=% .5"PRIi32" "
+ "in=% .5"PRIi32" out=% .5"PRIi32"",
name, cs_get_consign(cs), cs_get_filtered_consign(cs),
cs_get_error(cs), cs_get_filtered_feedback(cs),
cs_get_out(cs));
void dump_cs(const char *name, struct cs *cs)
{
- printf_P(PSTR("%s cons=% .5ld fcons=% .5ld err=% .5ld "
- "in=% .5ld out=% .5ld\r\n"),
+ printf_P(PSTR("%s cons=% .5"PRIi32" fcons=% .5"PRIi32" err=% .5"PRIi32" "
+ "in=% .5"PRIi32" out=% .5"PRIi32"\r\n"),
name, cs_get_consign(cs), cs_get_filtered_consign(cs),
cs_get_error(cs), cs_get_filtered_feedback(cs),
cs_get_out(cs));
void dump_pid(const char *name, struct pid_filter *pid)
{
- printf_P(PSTR("%s P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"),
+ printf_P(PSTR("%s P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"),
name,
pid_get_value_in(pid) * pid_get_gain_P(pid),
pid_get_value_I(pid) * pid_get_gain_I(pid),
rs_set_left_pwm(&mainboard.rs, pwm_set_and_save, LEFT_PWM);
rs_set_right_pwm(&mainboard.rs, pwm_set_and_save, RIGHT_PWM);
/* increase gain to decrease dist, increase left and it will turn more left */
- rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+#ifdef HOST_VERSION
+ rs_set_left_ext_encoder(&mainboard.rs, robotsim_encoder_get,
+ LEFT_ENCODER, IMP_COEF);
+ rs_set_right_ext_encoder(&mainboard.rs, robotsim_encoder_get,
+ RIGHT_ENCODER, IMP_COEF);
+#else
+ rs_set_left_ext_encoder(&mainboard.rs, encoders_spi_get_value,
LEFT_ENCODER, IMP_COEF * -1.00);
- rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
+ rs_set_right_ext_encoder(&mainboard.rs, encoders_spi_get_value,
RIGHT_ENCODER, IMP_COEF * 1.00);
+#endif
/* rs will use external encoders */
rs_set_flags(&mainboard.rs, RS_USE_EXT);
bd_set_speed_threshold(&mainboard.distance.bd, 60);
bd_set_current_thresholds(&mainboard.distance.bd, 500, 8000, 1000000, 50);
+#ifndef HOST_VERSION
/* ---- CS left_cobroller */
/* PID */
pid_init(&mainboard.left_cobroller.pid);
bd_init(&mainboard.right_cobroller.bd);
bd_set_speed_threshold(&mainboard.right_cobroller.bd, 60);
bd_set_current_thresholds(&mainboard.right_cobroller.bd, 500, 8000, 1000000, 50);
+#endif /* !HOST_VERSION */
/* set them on !! */
mainboard.angle.on = 0;
--- /dev/null
+import math, sys, time, os, random
+from visual import *
+
+AREA_X = 3000.
+AREA_Y = 2100.
+
+area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
+areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
+area_box = box(size=areasize, color=(0.0, 1.0, 0.0))
+
+scene.autoscale=0
+
+# all positions of robot every 5ms
+save_pos = []
+
+robot = box(pos = (0, 0, 150),
+ size = (250,320,350),
+ color = (1, 0, 0) )
+
+last_pos = robot.pos.x, robot.pos.y, robot.pos.z
+hcenter_line = curve()
+hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
+vcenter_line = curve()
+vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
+
+def square(sz):
+ sq = curve()
+ sq.pos = [(-sz, -sz, 0.3),
+ (-sz, sz, 0.3),
+ (sz, sz, 0.3),
+ (sz, -sz, 0.3),
+ (-sz, -sz, 0.3),]
+ return sq
+
+sq1 = square(250)
+sq2 = square(500)
+
+robot_trail = curve()
+robot_trail_list = []
+max_trail = 500
+
+area_objects = []
+
+OFFSET_CORN_X=150
+OFFSET_CORN_Y=222
+STEP_CORN_X=225
+STEP_CORN_Y=250
+
+WAYPOINTS_NBX = 13
+WAYPOINTS_NBY = 8
+
+TYPE_WAYPOINT=0
+TYPE_DANGEROUS=1
+TYPE_WHITE_CORN=2
+TYPE_BLACK_CORN=3
+TYPE_OBSTACLE=4
+TYPE_NEIGH=5
+
+col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
+waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
+corn_table = [TYPE_WHITE_CORN]*18
+
+corn_side_confs = [
+ [ 1, 4 ],
+ [ 0, 4 ],
+ [ 2, 4 ],
+ [ 2, 3 ],
+ [ 0, 3 ],
+ [ 1, 3 ],
+ [ 1, 6 ],
+ [ 0, 6 ],
+ [ 2, 6 ],
+ ]
+corn_center_confs = [
+ [ 5, 8 ],
+ [ 7, 8 ],
+ [ 5, 9 ],
+ [ 7, 8 ],
+ ]
+
+def pt2corn(i,j):
+ if i == 0 and j == 2: return 0
+ if i == 0 and j == 4: return 1
+ if i == 0 and j == 6: return 2
+ if i == 2 and j == 3: return 3
+ if i == 2 and j == 5: return 4
+ if i == 2 and j == 7: return 5
+ if i == 4 and j == 4: return 6
+ if i == 4 and j == 6: return 7
+ if i == 6 and j == 5: return 8
+ if i == 6 and j == 7: return 9
+ if i == 8 and j == 4: return 10
+ if i == 8 and j == 6: return 11
+ if i == 10 and j == 3: return 12
+ if i == 10 and j == 5: return 13
+ if i == 10 and j == 7: return 14
+ if i == 12 and j == 2: return 15
+ if i == 12 and j == 4: return 16
+ if i == 12 and j == 6: return 17
+ return -1
+
+def corn_get_sym(i):
+ sym = [15, 16, 17, 12, 13, 14, 10, 11, 8, 9, 6, 7, 3, 4, 5, 0, 1, 2]
+ return sym[i]
+
+def init_corn_table(conf_side, conf_center):
+ global corn_table, corn_side_confs, corn_center_confs
+ print "confs = %d, %d"%(conf_side, conf_center)
+ for i in range(18):
+ if i in corn_side_confs[conf_side]:
+ corn_table[i] = TYPE_BLACK_CORN
+ continue
+ if corn_get_sym(i) in corn_side_confs[conf_side]:
+ corn_table[i] = TYPE_BLACK_CORN
+ continue
+ if i in corn_center_confs[conf_center]:
+ corn_table[i] = TYPE_BLACK_CORN
+ continue
+ if corn_get_sym(i) in corn_center_confs[conf_center]:
+ corn_table[i] = TYPE_BLACK_CORN
+ continue
+ corn_table[i] = TYPE_WHITE_CORN
+
+def init_waypoints():
+ global waypoints, corn_table
+
+ for i in range(WAYPOINTS_NBX):
+ for j in range(WAYPOINTS_NBY):
+ # corn
+ c = pt2corn(i, j)
+ if c >= 0:
+ waypoints[i][j] = corn_table[c]
+ continue
+ # too close of border
+ if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
+ waypoints[i][j] = TYPE_OBSTACLE
+ continue
+ # hill
+ if i >= 2 and i < WAYPOINTS_NBX - 2 and j < 2:
+ waypoints[i][j] = TYPE_OBSTACLE
+ continue
+ # dangerous points
+ if i == 0 or i == WAYPOINTS_NBX-1:
+ waypoints[i][j] = TYPE_DANGEROUS
+ continue
+ if (i&1) == 0 and j == WAYPOINTS_NBY-1:
+ waypoints[i][j] = TYPE_DANGEROUS
+ continue
+
+ waypoints[i][j] = TYPE_WAYPOINT
+
+ print i, waypoints[i]
+ return waypoints
+
+
+def toggle_obj_disp():
+ global area_objects
+
+ if area_objects == []:
+ i = 0
+ j = 0
+ x = OFFSET_CORN_X
+ while x < 3000:
+ if (i & 1) == 0:
+ y = OFFSET_CORN_Y
+ else:
+ y = OFFSET_CORN_Y + STEP_CORN_Y/2
+ j = 0
+ while y < 2100:
+ print x,y
+ if waypoints[i][j] == TYPE_WHITE_CORN:
+ c = cylinder(axis=(0,0,1), length=150,
+ radius=25, color=(0.8,0.8,0.8),
+ pos=(x-AREA_X/2,y-AREA_Y/2,75))
+ area_objects.append(c)
+ elif waypoints[i][j] == TYPE_BLACK_CORN:
+ c = cylinder(axis=(0,0,1), length=150,
+ radius=25, color=(0.2,0.2,0.2),
+ pos=(x-AREA_X/2,y-AREA_Y/2,75))
+ area_objects.append(c)
+ j += 1
+ y += STEP_CORN_Y
+ i += 1
+ x += STEP_CORN_X
+ else:
+ for o in area_objects:
+ if o.visible:
+ o.visible = 0
+ else:
+ o.visible = 1
+
+
+def set_robot(x, y, a):
+ global robot, last_pos, robot_trail, robot_trail_list
+ global save_pos
+
+ robot.pos = (x - AREA_X/2, y - AREA_Y/2, 150)
+ robot.axis = (math.cos(a*math.pi/180),
+ math.sin(a*math.pi/180),
+ 0)
+ robot.size = (250, 320, 350)
+
+ # save position
+ save_pos.append((robot.pos.x, robot.pos, a))
+
+ pos = robot.pos.x, robot.pos.y, 0.3
+ if pos != last_pos:
+ robot_trail_list.append(pos)
+ last_pos = pos
+ robot_trail_l = len(robot_trail_list)
+ if robot_trail_l > max_trail:
+ robot_trail_list = robot_trail_list[robot_trail_l - max_trail:]
+ robot_trail.pos = robot_trail_list
+
+def graph():
+ pass
+
+def save():
+ f = open("/tmp/robot_save", "w")
+ for p in save_pos:
+ f.write("%f %f %f\n"%(p[0], p[1], p[2]))
+ f.close()
+
+def silent_mkfifo(f):
+ try:
+ os.mkfifo(f)
+ except:
+ pass
+
+init_corn_table(random.randint(0,8), random.randint(0,3))
+waypoints = init_waypoints()
+toggle_obj_disp()
+
+while True:
+ silent_mkfifo("/tmp/.robot_sim2dis")
+ silent_mkfifo("/tmp/.robot_dis2sim")
+ while True:
+ fr = open("/tmp/.robot_sim2dis", "r")
+ fw = open("/tmp/.robot_dis2sim", "w", 0)
+ while True:
+ l = fr.readline()
+ try:
+ x,y,a = map(lambda x:int(x), l[:-1].split(" "))
+ set_robot(x,y,a)
+ except ValueError:
+ pass
+
+ if scene.kb.keys == 0:
+ continue
+
+ k = scene.kb.getkey()
+ x,y,z = scene.center
+ if k == "left":
+ scene.center = x-10,y,z
+ elif k == "right":
+ scene.center = x+10,y,z
+ elif k == "up":
+ scene.center = x,y+10,z
+ elif k == "down":
+ scene.center = x,y-10,z
+ elif k == "l":
+ fw.write("l")
+ elif k == "r":
+ fw.write("r")
+ elif k == "c":
+ robot_trail_list = []
+ elif k == "x":
+ save_pos = []
+ elif k == "g":
+ graph()
+ elif k == "s":
+ save()
+ elif k == "h":
+ toggle_obj_disp()
+
+ # EOF
+ if l == "":
+ break
+
+ fr.close()
+ fw.close()
+
/*
* Copyright Droids Corporation (2009)
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
static volatile uint8_t running_op = OP_READY;
#define I2C_MAX_LOG 3
+#ifndef HOST_VERSION
static uint8_t error_log = 0;
+static int8_t i2c_req_cobboard_status(void);
+static int8_t i2c_req_ballboard_status(void);
+
+#endif
+
/* used for commands */
uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
volatile int8_t command_dest=-1;
volatile uint8_t command_size=0;
-static int8_t i2c_req_cobboard_status(void);
-static int8_t i2c_req_ballboard_status(void);
-
#define I2C_ERROR(args...) do { \
if (error_log < I2C_MAX_LOG) { \
ERROR(E_USER_I2C_PROTO, args); \
void i2c_protocol_debug(void)
{
+#ifdef HOST_VERSION
+ return;
+#else
printf_P(PSTR("I2C protocol debug infos:\r\n"));
printf_P(PSTR(" i2c_state=%d\r\n"), i2c_state);
printf_P(PSTR(" i2c_errors=%d\r\n"), i2c_errors);
printf_P(PSTR(" command_size=%d\r\n"), command_size);
printf_P(PSTR(" command_dest=%d\r\n"), command_dest);
printf_P(PSTR(" i2c_status=%x\r\n"), i2c_status());
+#endif
}
+#ifndef HOST_VERSION
static void i2cproto_next_state(uint8_t inc)
{
i2c_state += inc;
uint8_t flags;
int8_t err;
static uint8_t a = 0;
-
+
a++;
if (a & 0x4)
LED2_TOGGLE();
-
+
/* already running */
IRQ_LOCK(flags);
if (running_op != OP_READY) {
#define I2C_ANS_COBBOARD 1
case I2C_ANS_COBBOARD:
- if ((err = i2c_recv(I2C_COBBOARD_ADDR,
- sizeof(struct i2c_ans_cobboard_status),
+ if ((err = i2c_recv(I2C_COBBOARD_ADDR,
+ sizeof(struct i2c_ans_cobboard_status),
I2C_CTRL_GENERIC)))
goto error;
break;
#define I2C_ANS_BALLBOARD 3
case I2C_ANS_BALLBOARD:
- if ((err = i2c_recv(I2C_BALLBOARD_ADDR,
- sizeof(struct i2c_ans_ballboard_status),
+ if ((err = i2c_recv(I2C_BALLBOARD_ADDR,
+ sizeof(struct i2c_ans_ballboard_status),
I2C_CTRL_GENERIC)))
goto error;
break;
IRQ_UNLOCK(flags);
i2c_errors++;
if (i2c_errors > I2C_MAX_ERRORS) {
- I2C_ERROR("I2C send is_cmd=%d proto_state=%d "
+ I2C_ERROR("I2C send is_cmd=%d proto_state=%d "
"err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
i2c_reset();
i2c_errors = 0;
i2c_reset();
i2c_errors = 0;
}
-
+
if (running_op == OP_POLL) {
/* skip associated answer */
i2cproto_next_state(2);
/* recv is only trigged after a poll */
running_op = OP_READY;
-
+
if (size < 0) {
goto error;
}
switch (buf[0]) {
case I2C_ANS_COBBOARD_STATUS: {
- struct i2c_ans_cobboard_status * ans =
+ struct i2c_ans_cobboard_status * ans =
(struct i2c_ans_cobboard_status *)buf;
-
+
if (size != sizeof (*ans))
goto error;
}
case I2C_ANS_BALLBOARD_STATUS: {
- struct i2c_ans_ballboard_status * ans =
+ struct i2c_ans_ballboard_status * ans =
(struct i2c_ans_ballboard_status *)buf;
if (size != sizeof (*ans))
return;
error:
i2c_errors++;
- NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",
+ NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",
i2c_state, running_op);
if (i2c_errors > I2C_MAX_ERRORS) {
I2C_ERROR("I2C error, slave not ready");
void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
{
}
+#endif /* !HOST_VERSION */
/* ******** ******** ******** ******** */
/* commands */
static int8_t
-i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
+i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
{
+#ifdef HOST_VERSION
+ return 0;
+#else
uint8_t flags;
microseconds us = time_get_us2();
* interrupt context, but it's forbidden */
I2C_ERROR("I2C command send failed");
return -EBUSY;
+#endif
}
+#ifndef HOST_VERSION
static int8_t i2c_req_cobboard_status(void)
{
struct i2c_req_cobboard_status buf;
static int8_t i2c_req_ballboard_status(void)
{
struct i2c_req_ballboard_status buf;
-
+
buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS;
return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf,
sizeof(buf), I2C_CTRL_GENERIC);
}
+#endif /* !HOST_VERSION */
int8_t i2c_set_color(uint8_t addr, uint8_t color)
{
#include "../common/i2c_commands.h"
#include "main.h"
+#include "robotsim.h"
#include "ax12_user.h"
#include "strat.h"
#include "cmdline.h"
#include "sensor.h"
#include "actuator.h"
#include "cs.h"
+#include "strat_base.h"
#include "i2c_protocol.h"
/* 0 means "programmed"
struct cobboard cobboard;
struct ballboard ballboard;
+#ifndef HOST_VERSION
/***********************/
void bootloader(void)
if ((cpt & 0x3) == 0)
scheduler_interrupt();
}
+#endif
int main(void)
{
- uint16_t seconds;
-
+#ifndef HOST_VERSION
/* brake */
BRAKE_DDR();
BRAKE_OFF();
LED2_OFF();
LED3_OFF();
LED4_OFF();
+#endif
memset(&gen, 0, sizeof(gen));
memset(&mainboard, 0, sizeof(mainboard));
/* UART */
uart_init();
+ uart_register_rx_event(CMDLINE_UART, emergency);
+#ifndef HOST_VERSION
#if CMDLINE_UART == 3
fdevopen(uart3_dev_send, uart3_dev_recv);
- uart_register_rx_event(3, emergency);
#elif CMDLINE_UART == 1
fdevopen(uart1_dev_send, uart1_dev_recv);
- uart_register_rx_event(1, emergency);
-#else
-# error not supported
#endif
- eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD);
+ //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD);
/* check eeprom to avoid to run the bad program */
if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
EEPROM_MAGIC_MAINBOARD) {
printf_P(PSTR("Bad eeprom value\r\n"));
while(1);
}
+#endif /* ! HOST_VERSION */
/* LOGS */
error_register_emerg(mylog);
error_register_notice(mylog);
error_register_debug(mylog);
+#ifndef HOST_VERSION
/* SPI + ENCODERS */
encoders_spi_init(); /* this will also init spi hardware */
PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
NULL, 0);
support_balls_deploy(); /* init pwm for servos */
+#endif /* !HOST_VERSION */
/* SCHEDULER */
scheduler_init();
+#ifdef HOST_VERSION
+ hostsim_init();
+ robotsim_init();
+#endif
+#ifndef HOST_VERSION
scheduler_add_periodical_event_priority(do_led_blink, NULL,
100000L / SCHEDULER_UNIT,
LED_PRIO);
+#endif /* !HOST_VERSION */
+
/* all cs management */
microb_cs_init();
+#ifndef HOST_VERSION
/* sensors, will also init hardware adc */
sensor_init();
/* start i2c slave polling */
scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
8000L / SCHEDULER_UNIT, I2C_POLL_PRIO);
+#endif /* !HOST_VERSION */
/* strat */
gen.logs[0] = E_USER_STRAT;
25000L / SCHEDULER_UNIT,
STRAT_PRIO);
+#ifndef HOST_VERSION
/* eeprom time monitor */
scheduler_add_periodical_event_priority(do_time_monitor, NULL,
1000000L / SCHEDULER_UNIT,
EEPROM_TIME_PRIO);
+#endif /* !HOST_VERSION */
sei();
printf_P(PSTR("\r\n"));
printf_P(PSTR("Respect et robustesse.\r\n"));
- seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
- printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
+#ifndef HOST_VERSION
+ {
+ uint16_t seconds;
+ seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
+ printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
+ }
+#endif
+
+#ifdef HOST_VERSION
+ strat_reset_pos(1000, 1000, -90);
+#endif
+
cmdline_interact();
return 0;
port |= _BV(bit); \
} while(0)
+#ifdef HOST_VERSION
+#define LED1_ON()
+#define LED1_OFF()
+#define LED1_TOGGLE()
+
+#define LED2_ON()
+#define LED2_OFF()
+#define LED2_TOGGLE()
+
+#define LED3_ON()
+#define LED3_OFF()
+#define LED3_TOGGLE()
+
+#define LED4_ON()
+#define LED4_OFF()
+#define LED4_TOGGLE()
+
+#define BRAKE_DDR()
+#define BRAKE_ON()
+#define BRAKE_OFF()
+
+#else
+
#define LED1_ON() sbi(PORTJ, 2)
#define LED1_OFF() cbi(PORTJ, 2)
#define LED1_TOGGLE() LED_TOGGLE(PORTJ, 2)
#define BRAKE_DDR() do { DDRJ |= 0xF0; } while(0)
#define BRAKE_ON() do { PORTJ |= 0xF0; } while(0)
#define BRAKE_OFF() do { PORTJ &= 0x0F; } while(0)
+#endif
/* only 90 seconds, don't forget it :) */
#define MATCH_TIME 89
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: main.c,v 1.9.4.5 2007-06-01 09:37:22 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <timer.h>
+#include <scheduler.h>
+#include <time.h>
+
+#include <ax12.h>
+#include <pwm_ng.h>
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <parse.h>
+#include <rdline.h>
+
+#include "strat.h"
+#include "strat_utils.h"
+#include "main.h"
+
+static int32_t l_pwm, r_pwm;
+static int32_t l_enc, r_enc;
+
+static int fdr, fdw;
+
+/* */
+#define FILTER 97
+#define FILTER2 (100-FILTER)
+#define SHIFT 4
+
+void robotsim_dump(void)
+{
+ char buf[BUFSIZ];
+ int len;
+
+ len = snprintf(buf, sizeof(buf), "%d %d %d\n",
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+ hostsim_lock();
+ write(fdw, buf, len);
+ hostsim_unlock();
+}
+
+/* must be called periodically */
+void robotsim_update(void)
+{
+ static int32_t l_pwm_shift[SHIFT];
+ static int32_t r_pwm_shift[SHIFT];
+ static int32_t l_speed, r_speed;
+ static unsigned i = 0;
+ static unsigned cpt = 0;
+ int32_t local_l_pwm, local_r_pwm;
+ double x, y, a, a2, d;
+ char cmd = 0;
+
+ /* corners of the robot */
+ double xfl, yfl; /* front left */
+ double xrl, yrl; /* rear left */
+ double xrr, yrr; /* rear right */
+ double xfr, yfr; /* front right */
+
+ /* time shift the command */
+ l_pwm_shift[i] = l_pwm;
+ r_pwm_shift[i] = r_pwm;
+ i ++;
+ i %= SHIFT;
+ local_l_pwm = l_pwm_shift[i];
+ local_r_pwm = r_pwm_shift[i];
+
+ /* read command */
+ if (((cpt ++) & 0x7) == 0) {
+ if (read(fdr, &cmd, 1) != 1)
+ cmd = 0;
+ }
+
+ x = position_get_x_double(&mainboard.pos);
+ y = position_get_y_double(&mainboard.pos);
+ a = position_get_a_rad_double(&mainboard.pos);
+
+ l_speed = ((l_speed * FILTER) / 100) +
+ ((local_l_pwm * 1000 * FILTER2)/1000);
+ r_speed = ((r_speed * FILTER) / 100) +
+ ((local_r_pwm * 1000 * FILTER2)/1000);
+
+ /* basic collision detection */
+ a2 = atan2(ROBOT_WIDTH/2, ROBOT_LENGTH/2);
+ d = norm(ROBOT_WIDTH/2, ROBOT_LENGTH/2);
+
+ xfl = x + cos(a+a2) * d;
+ yfl = y + sin(a+a2) * d;
+ if (!is_in_area(xfl, yfl, 0) && l_speed > 0)
+ l_speed = 0;
+
+ xrl = x + cos(a+M_PI-a2) * d;
+ yrl = y + sin(a+M_PI-a2) * d;
+ if (!is_in_area(xrl, yrl, 0) && l_speed < 0)
+ l_speed = 0;
+
+ xrr = x + cos(a+M_PI+a2) * d;
+ yrr = y + sin(a+M_PI+a2) * d;
+ if (!is_in_area(xrr, yrr, 0) && r_speed < 0)
+ r_speed = 0;
+
+ xfr = x + cos(a-a2) * d;
+ yfr = y + sin(a-a2) * d;
+ if (!is_in_area(xfr, yfr, 0) && r_speed > 0)
+ r_speed = 0;
+
+ /* perturbation */
+ if (cmd == 'l')
+ l_enc += 5000; /* push 1 cm */
+ if (cmd == 'r')
+ r_enc += 5000; /* push 1 cm */
+
+ /* XXX should lock */
+ l_enc += (l_speed / 1000);
+ r_enc += (r_speed / 1000);
+}
+
+void robotsim_pwm(void *arg, int32_t val)
+{
+ // printf("%p, %d\n", arg, val);
+ if (arg == LEFT_PWM)
+ l_pwm = val;
+ else if (arg == RIGHT_PWM)
+ r_pwm = val;
+}
+
+int32_t robotsim_encoder_get(void *arg)
+{
+ if (arg == LEFT_ENCODER)
+ return l_enc;
+ else if (arg == RIGHT_ENCODER)
+ return r_enc;
+ return 0;
+}
+
+int robotsim_init(void)
+{
+ mkfifo("/tmp/.robot_sim2dis", 0600);
+ mkfifo("/tmp/.robot_dis2sim", 0600);
+ fdw = open("/tmp/.robot_sim2dis", O_WRONLY, 0);
+ if (fdw < 0)
+ return -1;
+ fdr = open("/tmp/.robot_dis2sim", O_RDONLY | O_NONBLOCK, 0);
+ if (fdr < 0) {
+ close(fdw);
+ return -1;
+ }
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: main.c,v 1.9.4.5 2007-06-01 09:37:22 zer0 Exp $
+ *
+ */
+
+void robotsim_update(void);
+void robotsim_pwm(void *arg, int32_t val);
+int32_t robotsim_encoder_get(void *arg);
+int robotsim_init(void);
+void robotsim_dump(void);
/** maximum number of allocated events */
#define SCHEDULER_NB_MAX_EVENT 10
-
+#ifdef HOST_VERSION
+#define SCHEDULER_UNIT_FLOAT 1000.0
+#define SCHEDULER_UNIT 1000UL
+#else
#define SCHEDULER_UNIT_FLOAT 512.0
#define SCHEDULER_UNIT 512L
+#endif
/** number of allowed imbricated scheduler interrupts. The maximum
* should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate
-/*
+/*
* Copyright Droids Corporation (2009)
* Olivier MATZ <zer0@droids-corp.org>
- *
+ *
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
#include "main.h"
#include "sensor.h"
+#ifndef HOST_VERSION
+
/************ ADC */
struct adc_infos {
#define ADC_CONF(x) ( ADC_REF_AVCC | ADC_MODE_INT | MUX_ADC##x )
/* define which ADC to poll, see in sensor.h */
-static struct adc_infos adc_infos[ADC_MAX] = {
+static struct adc_infos adc_infos[ADC_MAX] = {
[ADC_CSENSE1] = { .config = ADC_CONF(0), .filter = rii_medium },
[ADC_CSENSE2] = { .config = ADC_CONF(1), .filter = rii_medium },
[ADC_CSENSE3] = { .config = ADC_CONF(2), .filter = rii_medium },
static void adc_event(int16_t result);
/* called every 10 ms, see init below */
-static void do_adc(void *dummy)
+static void do_adc(void *dummy)
{
/* launch first conversion */
adc_launch(adc_infos[0].config);
else
adc_launch(adc_infos[i].config);
}
+#endif /* !HOST_VERSION */
int16_t sensor_get_adc(uint8_t i)
{
+#ifdef HOST_VERSION
+ return 0;
+#else
int16_t tmp;
uint8_t flags;
tmp = adc_infos[i].value;
IRQ_UNLOCK(flags);
return tmp;
+#endif
}
/************ boolean sensors */
-
+#ifndef HOST_VERSION
struct sensor_filter {
uint8_t filter;
uint8_t prev;
[S_RESERVED7] = { 1, 0, 0, 1, 0, 0 }, /* 14 */
[S_RESERVED8] = { 1, 0, 0, 1, 0, 0 }, /* 15 */
};
+#endif /* !HOST_VERSION */
/* value of filtered sensors */
static uint16_t sensor_filtered = 0;
-/* sensor mapping :
+/* sensor mapping :
* 0-3: PORTK 2->5 (cap1 -> cap4) (adc10 -> adc13)
* 4-5: PORTL 0->1 (cap5 -> cap6)
* 6-7: PORTE 3->4 (cap7 -> cap8)
uint8_t sensor_get(uint8_t i)
{
+#ifdef HOST_VERSION
+ return 0;
+#else
uint16_t tmp = sensor_get_all();
return (tmp & _BV(i));
+#endif
}
+#ifndef HOST_VERSION
/* get the physical value of pins */
static uint16_t sensor_read(void)
{
if (sensor_filter[i].cpt <= sensor_filter[i].thres_off)
sensor_filter[i].prev = 0;
}
-
+
if (sensor_filter[i].prev) {
tmp |= (1UL << i);
}
sensor_filtered = tmp;
IRQ_UNLOCK(flags);
}
+#endif /* !HOST_VERSION */
/* virtual obstacle */
#define DISABLE_CPT_MAX 200
-static uint8_t disable = 0; /* used to disable obstacle detection
+static uint8_t disable = 0; /* used to disable obstacle detection
* during some time */
/* called every 10 ms */
/************ global sensor init */
+#ifndef HOST_VERSION
/* called every 10 ms, see init below */
static void do_sensors(void *dummy)
{
do_boolean_sensors(NULL);
sensor_obstacle_update();
}
+#endif
void sensor_init(void)
{
+#ifdef HOST_VERSION
+ return;
+#else
adc_init();
adc_register_event(adc_event);
/* CS EVENT */
- scheduler_add_periodical_event_priority(do_sensors, NULL,
- 10000L / SCHEDULER_UNIT,
+ scheduler_add_periodical_event_priority(do_sensors, NULL,
+ 10000L / SCHEDULER_UNIT,
ADC_PRIO);
+#endif
}
git.droids-corp.org / aversive.git / commitdiff