From 25b9d1245aa263ce269289d698d9fc56cdd0f230 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Tue, 19 Jul 2016 00:36:41 +0200
Subject: [PATCH] Added custom printf and scanf functions to avoid unnecessary
text send via the serial port in the real robot. It also helps redirect the
stdin and stdout streams to a virtual serial port in simulation. Added a
macro definition __REAL__ in all Makefile's Added a new example of
communication throw a vurtual serial port and visualization of the robot
camera outside ROS.
---
communications/Makefile | 2 +-
communications/include/serial_console.h | 4 +-
communications/src/examples/Makefile | 2 +-
communications/src/examples/main.c | 4 +-
communications/src/serial_console.c | 23 +++++++-
controllers/Makefile | 2 +-
controllers/src/examples/Makefile | 2 +-
controllers/src/examples/main.c | 4 +-
dyn_devices/Makefile | 2 +-
dyn_devices/src/examples/Makefile | 2 +-
dyn_devices/src/examples/exp_board_ex.c | 56 +++++++++---------
dyn_devices/src/examples/servos_ex.c | 4 +-
examples/get_up/Makefile | 2 +-
examples/movements/Makefile | 2 +-
examples/pan_tilt/Makefile | 2 +-
examples/pan_tilt/pan_tilt.c | 2 +-
examples/sensors/Makefile | 2 +-
examples/sensors/sensors.c | 10 ++--
examples/stairs/Makefile | 2 +-
examples/vision/Makefile | 63 ++++++++++++++++++++
examples/vision/cm510_vision.c | 77 ++++++++++++++++++++++++
examples/vision/vision.cpp | 78 +++++++++++++++++++++++++
examples/walk_straight/Makefile | 2 +-
motion/Makefile | 2 +-
motion/src/examples/Makefile | 2 +-
motion/src/examples/main.c | 10 ++--
26 files changed, 302 insertions(+), 61 deletions(-)
create mode 100644 examples/vision/Makefile
create mode 100644 examples/vision/cm510_vision.c
create mode 100644 examples/vision/vision.cpp
diff --git a/communications/Makefile b/communications/Makefile
index 17a0a64..31e3571 100644
--- a/communications/Makefile
+++ b/communications/Makefile
@@ -15,7 +15,7 @@ CC=avr-gcc
OBJCOPY=avr-ar
MMCU=atmega2561
-CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
ARFLAGS=rsc
diff --git a/communications/include/serial_console.h b/communications/include/serial_console.h
index dae9cb9..e21d13f 100644
--- a/communications/include/serial_console.h
+++ b/communications/include/serial_console.h
@@ -9,7 +9,9 @@ extern "C" {
#include <avr/interrupt.h>
void serial_console_init(uint32_t baudrate);
-int printf(const char *fmt, ...);
+uint8_t serial_console_get_num_data(void);
+int cm510_printf(const char *fmt, ...);
+int cm510_scanf(const char *fmt, ... );
#ifdef __cplusplus
}
diff --git a/communications/src/examples/Makefile b/communications/src/examples/Makefile
index fd3b35d..1c56a7a 100644
--- a/communications/src/examples/Makefile
+++ b/communications/src/examples/Makefile
@@ -14,7 +14,7 @@ MMCU=atmega2561
LIBS=$(COMM_DIR)libcomm.a
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/communications/src/examples/main.c b/communications/src/examples/main.c
index 6188a35..a3d6baa 100755
--- a/communications/src/examples/main.c
+++ b/communications/src/examples/main.c
@@ -19,12 +19,12 @@ int16_t main(void)
dyn_master_scan(&num,ids);
if(num==1)
{
- printf("Found one device\n");
+ cm510_printf("Found one device\n");
PORTC=0x3F;
}
else
{
- printf("No device found\n");
+ cm510_printf("No device found\n");
PORTC=0x7F;
}
}
diff --git a/communications/src/serial_console.c b/communications/src/serial_console.c
index 89bfd52..97dd048 100644
--- a/communications/src/serial_console.c
+++ b/communications/src/serial_console.c
@@ -140,8 +140,16 @@ void serial_console_init(uint32_t baudrate)
device=fdevopen(serial_console_putchar,serial_console_getchar);
}
-int printf(const char *fmt, ...)
+uint8_t serial_console_get_num_data(void)
+{
+ return serial_console_rx_num_data;
+}
+
+int cm510_printf(const char *fmt, ...)
{
+ #ifdef __REAL__
+ return 0;
+ #else
va_list ap;
int i;
@@ -149,4 +157,17 @@ int printf(const char *fmt, ...)
i = vfprintf(stdout, fmt, ap);
va_end(ap);
return i;
+ #endif
+}
+
+int cm510_scanf(const char *fmt, ...)
+{
+ va_list arg;
+ int done;
+
+ va_start (arg, fmt);
+ done = vfscanf (stdin, fmt, arg);
+ va_end (arg);
+
+ return done;
}
diff --git a/controllers/Makefile b/controllers/Makefile
index 06de89a..9315f6f 100755
--- a/controllers/Makefile
+++ b/controllers/Makefile
@@ -18,7 +18,7 @@ INC_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(DEV_DIR)include -I$(MAN_
LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a $(MAN_DIR)lib/libmotion_manager.a
-CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
ARFLAGS=rsc
diff --git a/controllers/src/examples/Makefile b/controllers/src/examples/Makefile
index 7c70888..7a53003 100644
--- a/controllers/src/examples/Makefile
+++ b/controllers/src/examples/Makefile
@@ -18,7 +18,7 @@ INCLUDE_DIRS=-I$(CONT_DIR)include/ -I$(COMM_DIR)include -I$(MAN_DIR)include -I$(
LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/controllers/src/examples/main.c b/controllers/src/examples/main.c
index a1d6216..73a75bd 100755
--- a/controllers/src/examples/main.c
+++ b/controllers/src/examples/main.c
@@ -29,8 +29,8 @@ void user_loop(void)
{
if(user_time_is_period_done())
{
- printf("Gyro X: %d\n",get_adc_channel(ADC_PORT_3));
- printf("Gyro Y: %d\n",get_adc_channel(ADC_PORT_4));
+ cm510_printf("Gyro X: %d\n",get_adc_channel(ADC_PORT_3));
+ cm510_printf("Gyro Y: %d\n",get_adc_channel(ADC_PORT_4));
}
if(is_button_pressed(BTN_START))
turn_led_on(LED_AUX);
diff --git a/dyn_devices/Makefile b/dyn_devices/Makefile
index c0bcc5b..0536f19 100755
--- a/dyn_devices/Makefile
+++ b/dyn_devices/Makefile
@@ -14,7 +14,7 @@ INC_DIRS=-I./include/ -I$(COMM_DIR)/include -I$(CONT_DIR)/include
LIBS=$(COMM_DIR)/lib/libcomm.a
-CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
ARFLAGS=rsc
diff --git a/dyn_devices/src/examples/Makefile b/dyn_devices/src/examples/Makefile
index e7b85a0..05e3b27 100755
--- a/dyn_devices/src/examples/Makefile
+++ b/dyn_devices/src/examples/Makefile
@@ -19,7 +19,7 @@ LIBS=$(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/dyn_devices/src/examples/exp_board_ex.c b/dyn_devices/src/examples/exp_board_ex.c
index bff254a..2d75700 100755
--- a/dyn_devices/src/examples/exp_board_ex.c
+++ b/dyn_devices/src/examples/exp_board_ex.c
@@ -32,41 +32,41 @@ int main(void)
_delay_ms(4000);
if(exp_board_init(0xC0)==0x00)
{
- printf("expansion baord found\n");
+ cm510_printf("expansion baord found\n");
/* GPIO test */
/* if(exp_gpio_config(GPIO0,GPIO_OUT))
- printf("Error configuring the GPIO0 pin\n");
+ cm510_printf("Error configuring the GPIO0 pin\n");
if(exp_gpio_config(GPIO1,GPIO_IN))
- printf("Error configuring the GPIO1 pin\n");
+ cm510_printf("Error configuring the GPIO1 pin\n");
for(i=0;i<10;i++)
{
if(exp_gpio_set_value(0,1))
- printf("Error Error setting the value of the GPIO0 pin\n");
+ cm510_printf("Error Error setting the value of the GPIO0 pin\n");
if(exp_gpio_get_value(GPIO1)==0x01)
- printf("GPIO pin is HIGH\n");
+ cm510_printf("GPIO pin is HIGH\n");
else if(exp_gpio_get_value(GPIO1)==0x00)
- printf("GPIO pin is LOW\n");
+ cm510_printf("GPIO pin is LOW\n");
else
- printf("Error getting the GPIO1 value\n");
+ cm510_printf("Error getting the GPIO1 value\n");
_delay_ms(1000);
if(exp_gpio_set_value(0,0))
- printf("Error Error setting the value of the GPIO1 pin\n");
+ cm510_printf("Error Error setting the value of the GPIO1 pin\n");
if(exp_gpio_get_value(GPIO1)==0x00)
- printf("GPIO pin is LOW\n");
+ cm510_printf("GPIO pin is LOW\n");
else if(exp_gpio_get_value(GPIO1)==0x01)
- printf("GPIO pin is HIGH\n");
+ cm510_printf("GPIO pin is HIGH\n");
else
- printf("Error getting the GPIO1 value\n");
+ cm510_printf("Error getting the GPIO1 value\n");
_delay_ms(1000);
}*/
/* LED and switches test */
/* for(i=0;i<10;i++)
{
exp_gpio_set_led();
- printf("Switches value: %d\n",exp_gpio_get_switches());
+ cm510_printf("Switches value: %d\n",exp_gpio_get_switches());
_delay_ms(1000);
exp_gpio_clear_led();
- printf("Switches value: %d\n",exp_gpio_get_switches());
+ cm510_printf("Switches value: %d\n",exp_gpio_get_switches());
_delay_ms(1000);
}*/
/* PWM test */
@@ -89,58 +89,58 @@ int main(void)
for(i=0;i<10;i++)
{
exp_dac_set_voltage(DAC0,1000);
- printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
+ cm510_printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
_delay_ms(1000);
exp_dac_set_voltage(DAC0,2000);
- printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
+ cm510_printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
_delay_ms(1000);
exp_dac_set_voltage(DAC0,3000);
- printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
+ cm510_printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
_delay_ms(1000);
exp_dac_set_voltage(DAC0,4000);
- printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
+ cm510_printf("Volatge: %d mV\n",exp_dac_get_voltage(DAC0));
_delay_ms(1000);
}
dac_stop();*/
/* compass test */
exp_compass_start();
- printf("Number of samples to average: %d\n",exp_compass_get_avg_samples());
+ cm510_printf("Number of samples to average: %d\n",exp_compass_get_avg_samples());
for(i=0;i<100;i++)
{
- printf("Current heading: %d\n",exp_compass_get_heading());
- printf("Current averaged heading: %d\n",exp_compass_get_avg_heading());
+ cm510_printf("Current heading: %d\n",exp_compass_get_heading());
+ cm510_printf("Current averaged heading: %d\n",exp_compass_get_avg_heading());
_delay_ms(1000);
}
exp_compass_stop();
/* ADC test */
/* exp_adc_start();
- printf("Number of samples to average: %d\n",exp_adc_get_average_samples());
- printf("Sampling period: %d ms\n",exp_adc_get_sample_period());
+ cm510_printf("Number of samples to average: %d\n",exp_adc_get_average_samples());
+ cm510_printf("Sampling period: %d ms\n",exp_adc_get_sample_period());
for(i=0;i<100;i++)
{
- printf("Current voltage at ADC0: %d mV\n",exp_adc_get_channel(ADC0));
- printf("Current averaged voltage at ADC0: %d mV\n",exp_adc_get_avg_channel(ADC0));
+ cm510_printf("Current voltage at ADC0: %d mV\n",exp_adc_get_channel(ADC0));
+ cm510_printf("Current averaged voltage at ADC0: %d mV\n",exp_adc_get_avg_channel(ADC0));
_delay_ms(1000);
}
exp_adc_stop();*/
/* UART USB test */
/* exp_uart_usb_start();
- printf("current baudrate: %d\n",exp_uart_usb_get_baudrate());
+ cm510_printf("current baudrate: %d\n",exp_uart_usb_get_baudrate());
for(i=0;i<10;i++)
{
while(!exp_uart_usb_is_data_available())
_delay_ms(100);
data=exp_uart_usb_receive_data();
- printf("Received data: %d\n",data);
+ cm510_printf("Received data: %d\n",data);
exp_uart_usb_send_byte(data);
while(exp_uart_usb_is_sending())
_delay_ms(100);
- printf("data sent\n");
+ cm510_printf("data sent\n");
}
exp_uart_usb_stop();*/
}
else
- printf("expansion board not found\n");
+ cm510_printf("expansion board not found\n");
while(1);
}
diff --git a/dyn_devices/src/examples/servos_ex.c b/dyn_devices/src/examples/servos_ex.c
index f9ad49f..3dbb1a2 100755
--- a/dyn_devices/src/examples/servos_ex.c
+++ b/dyn_devices/src/examples/servos_ex.c
@@ -35,7 +35,7 @@ int main(void)
if(num>0)
{
model=get_model_number(ids[0]);
- printf("%d\n",model);
+ cm510_printf("%d\n",model);
set_target_speed(ids[0],100);
for(;;)
{
@@ -50,7 +50,7 @@ int main(void)
}
}
else
- printf("No device found");
+ cm510_printf("No device found");
while(1);
}
diff --git a/examples/get_up/Makefile b/examples/get_up/Makefile
index 0a5a21b..3dcbe47 100644
--- a/examples/get_up/Makefile
+++ b/examples/get_up/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/examples/movements/Makefile b/examples/movements/Makefile
index 6d668f0..84f98f1 100644
--- a/examples/movements/Makefile
+++ b/examples/movements/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/examples/pan_tilt/Makefile b/examples/pan_tilt/Makefile
index 4190866..f3d9520 100644
--- a/examples/pan_tilt/Makefile
+++ b/examples/pan_tilt/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include -I../movements
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/examples/pan_tilt/pan_tilt.c b/examples/pan_tilt/pan_tilt.c
index 9c0e486..8445616 100644
--- a/examples/pan_tilt/pan_tilt.c
+++ b/examples/pan_tilt/pan_tilt.c
@@ -35,7 +35,7 @@ void user_loop(void)
}
else
state=wait_start;
- printf("wait_start %d\n",(int)state);
+ cm510_printf("wait_start %d\n",(int)state);
break;
case wait_cmd: if(is_button_rising_edge(BTN_LEFT))
{
diff --git a/examples/sensors/Makefile b/examples/sensors/Makefile
index 28b3e0c..dc31ef4 100644
--- a/examples/sensors/Makefile
+++ b/examples/sensors/Makefile
@@ -18,7 +18,7 @@ LIBS=$(CONT_DIR)lib/libcontrollers.a $(MAN_DIR)lib/libmotion_manager.a $(DEV_DIR
INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/examples/sensors/sensors.c b/examples/sensors/sensors.c
index 09e3d42..44011f3 100644
--- a/examples/sensors/sensors.c
+++ b/examples/sensors/sensors.c
@@ -21,7 +21,7 @@ void user_loop(void)
{
static main_states state=wait_start;
-/* switch(state)
+ switch(state)
{
case wait_start: if(is_button_rising_edge(BTN_START))
{
@@ -37,9 +37,9 @@ void user_loop(void)
else
state=read_sensors;
break;
- case read_sensors: printf("CM510 ADC port 1: %d\n",get_adc_avg_channel(ADC_PORT_2));
- printf("Exp. Board compass: %d\n",exp_compass_get_avg_heading());
- printf("Exp. Board ADC port 7: %d\n",exp_adc_get_avg_channel(ADC7));
+ case read_sensors: cm510_printf("CM510 ADC port 1: %d\n",get_adc_avg_channel(ADC_PORT_2));
+ cm510_printf("Exp. Board compass: %d\n",exp_compass_get_avg_heading());
+ cm510_printf("Exp. Board ADC port 7: %d\n",exp_adc_get_avg_channel(ADC7));
break;
- }*/
+ }
}
diff --git a/examples/stairs/Makefile b/examples/stairs/Makefile
index 746c9e6..09812e8 100644
--- a/examples/stairs/Makefile
+++ b/examples/stairs/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include -I../movements
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/examples/vision/Makefile b/examples/vision/Makefile
new file mode 100644
index 0000000..2181ffc
--- /dev/null
+++ b/examples/vision/Makefile
@@ -0,0 +1,63 @@
+# computer code
+INPUT_FILE = vision.cpp
+OUTPUT_FILE = vision
+LIBS = -lpthread -L/usr/lib -lopencv_calib3d -lopencv_contrib -lopencv_core -lopencv_features2d -lopencv_flann -lopencv_gpu -lopencv_highgui -lopencv_imgproc -lopencv_legacy -lopencv_ml -lopencv_objdetect -lopencv_ocl -lopencv_photo -lopencv_stitching -lopencv_superres -lopencv_ts -lopencv_video -lopencv_videostab -L/usr/local/lib/iridrivers -ldetectqrcode -lcomm -liriutils
+INCLUDES = -I. -I/usr/include/opencv -I/usr/local/include/iridrivers
+
+# microcontroller code
+CM510_PROJECT = cm510_vision
+CM510_SOURCES = cm510_vision.c
+
+CM510_OBJS=$(CM510_SOURCES:.c=.o)
+CM510_SRC_DIR=./
+CM510_DEV_DIR=../../dyn_devices/
+CM510_COMM_DIR=../../communications/
+CM510_CONT_DIR=../../controllers/
+CM510_MAN_DIR=../../motion/
+CC=avr-gcc
+OBJCOPY=avr-objcopy
+MMCU=atmega2561
+
+CM510_LIBS=$(CM510_MAN_DIR)lib/libmotion_manager.a $(CM510_CONT_DIR)lib/libcontrollers.a $(CM510_COMM_DIR)lib/libcomm.a $(CM510_DEV_DIR)lib/libdyn_devices.a
+
+CM510_INCLUDE_DIRS=-I$(CM510_DEV_DIR)include -I$(CM510_COMM_DIR)include -I$(CM510_CONT_DIR)include -I$(CM510_MAN_DIR)include -I../movements
+
+CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+
+LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(CM510_PROJECT).map -DF_CPU=16000000UL
+
+HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
+
+all: communications dyn_devices controllers motion_manager $(CM510_PROJECT).hex
+ $(CXX) $(INPUT_FILE) $(LIBS) $(INCLUDES) -o $(OUTPUT_FILE)
+
+$(CM510_PROJECT).hex: $(CM510_PROJECT).elf
+ $(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS) $< $@
+$(CM510_PROJECT).elf: $(CM510_OBJS)
+ $(CC) $(LDFLAGS) $(CM510_OBJS) $(CM510_LIBS) -o $(CM510_PROJECT).elf
+%.o:%.c
+ $(CC) -c $(CFLAGS) $(CM510_INCLUDE_DIRS) -o $@ $<
+
+communications:
+ $(MAKE) -C $(CM510_COMM_DIR)
+
+dyn_devices:
+ $(MAKE) -C $(CM510_DEV_DIR)
+
+controllers:
+ $(MAKE) -C $(CM510_CONT_DIR)
+
+motion_manager:
+ $(MAKE) -C $(CM510_MAN_DIR)
+
+download: $(MAIN_OUT_HEX)
+ fw_downloader -d /dev/ttyUSB0 -f ./$(CM510_PROJECT).hex -p cm510
+
+clean:
+ -rm $(CM510_PROJECT).map
+ -rm $(CM510_PROJECT).hex
+ -rm $(CM510_PROJECT).elf
+ -rm $(CM510_OBJS)
+ $(RM) $(OUTPUT_FILE)
+
+.PHONY: all clean
diff --git a/examples/vision/cm510_vision.c b/examples/vision/cm510_vision.c
new file mode 100644
index 0000000..9c86cca
--- /dev/null
+++ b/examples/vision/cm510_vision.c
@@ -0,0 +1,77 @@
+#include <util/delay.h>
+#include "cm510.h"
+#include "balance.h"
+#include "exp_board.h"
+#include "pan_tilt.h"
+#include <stdio.h>
+
+typedef enum {wait_start,wait_cmd,wait_pan_left,wait_pan_right} main_states;
+
+#define PAN_SERVO_ID 19
+#define TILT_SERVO_ID 20
+
+void user_init(void)
+{
+ serial_console_init(57600);
+ balance_init();
+ balance_calibrate_gyro();
+ balance_enable_gyro();
+ user_time_set_period(100);
+ pan_tilt_init(PAN_SERVO_ID,TILT_SERVO_ID);
+}
+
+void user_loop(void)
+{
+ static main_states state=wait_start;
+ uint8_t cmd;
+
+ if(user_time_is_period_done())
+ {
+ switch(state)
+ {
+ case wait_start: if(is_button_rising_edge(BTN_START))
+ {
+ action_set_page(31);
+ action_start_page();
+ state=wait_cmd;
+ }
+ else
+ state=wait_start;
+ break;
+ case wait_cmd: if(serial_console_get_num_data()>0)
+ {
+ printf("New data received\n");
+ cm510_scanf("%c",&cmd);
+ switch(cmd)
+ {
+ case 'l': pan_set_speed(200);
+ pan_move_angle(-70);
+ state=wait_pan_right;
+ printf("move left\n");
+ break;
+ case 'r': pan_set_speed(200);
+ pan_move_angle(70);
+ state=wait_pan_left;
+ printf("move right\n");
+ break;
+ default: state=wait_cmd;
+ break;
+ }
+ }
+ else
+ state=wait_cmd;
+ break;
+ case wait_pan_left: if(pan_is_moving())
+ state=wait_pan_left;
+ else
+ state=wait_cmd;
+ break;
+ case wait_pan_right: if(pan_is_moving())
+ state=wait_pan_right;
+ else
+ state=wait_cmd;
+ break;
+ }
+ }
+}
+
diff --git a/examples/vision/vision.cpp b/examples/vision/vision.cpp
new file mode 100644
index 0000000..dbf2bab
--- /dev/null
+++ b/examples/vision/vision.cpp
@@ -0,0 +1,78 @@
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <iostream>
+#include "detectqrcode.h"
+#include "rs232.h"
+#include "exceptions.h"
+
+using namespace cv;
+using namespace std;
+
+int main()
+{
+ cv::Mat gray;
+ cv::Mat frame;
+ CDetectqrcode detector;
+ std::vector<std::vector<cv::Point> > squares;
+ CRS232 serial_console("bioloid_serial_port");
+ std::string serial_console_device="/dev/pts/27";
+ TRS232_config serial_console_config;
+ std::string cmd;
+
+ cv::VideoCapture cap(0); //0 is the id of video device.0 if you have only one camera.
+
+ if (!cap.isOpened())
+ { //check if video device has been initialised
+ cout << "cannot open camera";
+ return 0;
+ }
+
+ detector.init(721.977446f,717.128980f,334.800583f,242.691968f,0.0475f,0.0475f);
+
+ try{
+ serial_console_config.baud=57600;
+ serial_console_config.num_bits=8;
+ serial_console_config.parity=none;
+ serial_console_config.stop_bits=1;
+ serial_console.open((void *)&serial_console_device);
+ serial_console.config(&serial_console_config);
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ return 0;
+ }
+
+ cv::namedWindow("QRcode",1);
+ //unconditional loop
+
+ for(unsigned int i=0;i<2;i++)
+ {
+ std::cout << "move left" << std::endl;
+ cmd="l\n";
+ serial_console.write((unsigned char *)cmd.c_str(),2);
+ sleep(2);
+ std::cout << "move right" << std::endl;
+ cmd="r\n";
+ serial_console.write((unsigned char *)cmd.c_str(),2);
+ sleep(2);
+ }
+
+ while (true)
+ {
+ cap.read(frame);
+ std::vector<TQRInfo> tags;
+ cv::cvtColor(frame,gray,CV_BGR2GRAY);
+ detector.findSquares(gray, squares);
+ detector.findQR(gray,tags);
+ for(unsigned int i=0;i<tags.size();i++)
+ {
+ std::cout << "Tag ID: " << tags[i].tag_id << std::endl;
+ std::cout << "Tag position: X:" << tags[i].tvec[0] << " Y: " << tags[i].tvec[1] << " Z: " << tags[i].tvec[2] << std::endl;
+ }
+ detector.drawSquares(frame, squares);
+ cv::imshow("QRcode",frame);
+ if (waitKey(30) >= 0)
+ break;
+ }
+
+ return 0;
+}
diff --git a/examples/walk_straight/Makefile b/examples/walk_straight/Makefile
index 428ca81..bf76cb5 100644
--- a/examples/walk_straight/Makefile
+++ b/examples/walk_straight/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/motion/Makefile b/motion/Makefile
index 2b667ac..9ae73ca 100755
--- a/motion/Makefile
+++ b/motion/Makefile
@@ -15,7 +15,7 @@ INC_DIRS=-I./include/ -I$(DEV_DIR)include/ -I$(COMM_DIR)include/ -I$(CONT_DIR)in
LIBS=$(CONT_DIR)lib/libcontrollers.a $(COMM_DIR)lib/libcomm.a $(DEV_DIR)lib/libdyn_devices.a
-CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -O3 -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
ARFLAGS= rsc
diff --git a/motion/src/examples/Makefile b/motion/src/examples/Makefile
index b773bdd..d88b371 100644
--- a/motion/src/examples/Makefile
+++ b/motion/src/examples/Makefile
@@ -18,7 +18,7 @@ LIBS=$(MAN_DIR)lib/libmotion_manager.a $(CONT_DIR)lib/libcontrollers.a $(COMM_DI
INCLUDE_DIRS=-I$(DEV_DIR)include -I$(COMM_DIR)include -I$(CONT_DIR)include -I$(MAN_DIR)include
-CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
+CFLAGS=-mmcu=$(MMCU) -Wall -Os $(defines) -DF_CPU=16000000UL -D__REAL__ -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
diff --git a/motion/src/examples/main.c b/motion/src/examples/main.c
index fc603d7..eeab8f8 100644
--- a/motion/src/examples/main.c
+++ b/motion/src/examples/main.c
@@ -13,22 +13,22 @@ void user_loop(void)
{
if(is_action_running()==0x00)
{
- printf("Walk ready\n");
+ cm510_printf("Walk ready\n");
if(action_set_page(25)==0)
action_start_page();
else
- printf("Error loading page\n");
+ cm510_printf("Error loading page\n");
}
}
else if(is_button_falling_edge(BTN_RIGHT))
{
if(is_action_running()==0x00)
{
- printf("start walking\n");
+ cm510_printf("start walking\n");
if(action_set_page(26)==0)
action_start_page();
else
- printf("Error loading page\n");
+ cm510_printf("Error loading page\n");
}
}
else
@@ -37,7 +37,7 @@ void user_loop(void)
{
if(is_action_running())
{
- printf("stop action\n");
+ cm510_printf("stop action\n");
action_stop_page();
}
}
--
GitLab