From: zer0 Date: Sun, 25 Apr 2010 18:11:29 +0000 (+0200) Subject: cob detection X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=9d056416278ae5d772a0fc1f7d2d11a4461fb673 cob detection --- diff --git a/projects/microb2010/ballboard/i2c_protocol.c b/projects/microb2010/ballboard/i2c_protocol.c index f9e9b55..3f5a2eb 100644 --- a/projects/microb2010/ballboard/i2c_protocol.c +++ b/projects/microb2010/ballboard/i2c_protocol.c @@ -47,6 +47,7 @@ #include "../common/i2c_commands.h" #include "main.h" #include "state.h" +#include "sensor.h" #include "actuator.h" void i2c_protocol_init(void) @@ -75,6 +76,8 @@ void i2c_send_status(void) ans.hdr.cmd = I2C_ANS_BALLBOARD_STATUS; ans.status = 0x55; /* XXX */ ans.ball_count = state_get_ball_count(); + ans.lcob = cob_detect_left(); + ans.rcob = cob_detect_right(); i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, sizeof(ans), I2C_CTRL_GENERIC); diff --git a/projects/microb2010/ballboard/main.h b/projects/microb2010/ballboard/main.h index 0758279..a267d78 100755 --- a/projects/microb2010/ballboard/main.h +++ b/projects/microb2010/ballboard/main.h @@ -19,7 +19,7 @@ * */ -/* was sensorboard in 2009 */ +/* was mainboard in 2009 */ #define LED_TOGGLE(port, bit) do { \ if (port & _BV(bit)) \ diff --git a/projects/microb2010/ballboard/sensor.c b/projects/microb2010/ballboard/sensor.c index 6cfb491..fe7f92a 100644 --- a/projects/microb2010/ballboard/sensor.c +++ b/projects/microb2010/ballboard/sensor.c @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2009) * Olivier MATZ - * + * * 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 @@ -40,6 +40,7 @@ #include "main.h" #include "sensor.h" +#include "../common/i2c_commands.h" /************ ADC */ @@ -75,13 +76,13 @@ int16_t rii_strong(struct adc_infos *adc, int16_t val) #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 }, [ADC_CSENSE4] = { .config = ADC_CONF(3), .filter = rii_medium }, - + /* add adc on "cap" pins if needed */ /* [ADC_CAP1] = { .config = ADC_CONF(10) }, */ /* [ADC_CAP2] = { .config = ADC_CONF(11) }, */ @@ -92,7 +93,7 @@ static struct adc_infos adc_infos[ADC_MAX] = { 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); @@ -149,10 +150,10 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { [S_CAP2] = { 1, 0, 0, 1, 0, 0 }, /* 1 */ [S_CAP3] = { 1, 0, 0, 1, 0, 0 }, /* 2 */ [S_CAP4] = { 1, 0, 0, 1, 0, 0 }, /* 3 */ - [S_CAP5] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ - [S_CAP6] = { 1, 0, 0, 1, 0, 0 }, /* 5 */ - [S_CAP7] = { 1, 0, 0, 1, 0, 0 }, /* 6 */ - [S_CAP8] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ + [S_R_IR] = { 1, 0, 0, 1, 0, 0 }, /* 4 */ + [S_R_US] = { 1, 0, 0, 1, 0, 1 }, /* 5 */ + [S_L_US] = { 1, 0, 0, 1, 0, 1 }, /* 6 */ + [S_L_IR] = { 1, 0, 0, 1, 0, 0 }, /* 7 */ [S_RESERVED1] = { 10, 0, 3, 7, 0, 0 }, /* 8 */ [S_RESERVED2] = { 10, 0, 3, 7, 0, 0 }, /* 9 */ [S_RESERVED3] = { 1, 0, 0, 1, 0, 0 }, /* 10 */ @@ -166,7 +167,7 @@ static struct sensor_filter sensor_filter[SENSOR_MAX] = { /* 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) @@ -186,7 +187,7 @@ uint16_t sensor_get_all(void) uint8_t sensor_get(uint8_t i) { uint16_t tmp = sensor_get_all(); - return (tmp & _BV(i)); + return !!(tmp & _BV(i)); } /* get the physical value of pins */ @@ -221,8 +222,11 @@ static void do_boolean_sensors(void *dummy) if (sensor_filter[i].cpt <= sensor_filter[i].thres_off) sensor_filter[i].prev = 0; } - - if (sensor_filter[i].prev) { + + if (sensor_filter[i].prev && !sensor_filter[i].invert) { + tmp |= (1UL << i); + } + else if (!sensor_filter[i].prev && sensor_filter[i].invert) { tmp |= (1UL << i); } } @@ -231,7 +235,108 @@ static void do_boolean_sensors(void *dummy) IRQ_UNLOCK(flags); } +static volatile uint8_t lcob_seen = I2C_COB_NONE, rcob_seen = I2C_COB_NONE; + +uint8_t cob_detect_left(void) +{ + uint8_t flags; + uint8_t ret; + IRQ_LOCK(flags); + ret = lcob_seen; + lcob_seen = I2C_COB_NONE; + IRQ_UNLOCK(flags); + return ret; +} + +uint8_t cob_detect_right(void) +{ + uint8_t flags; + uint8_t ret; + IRQ_LOCK(flags); + ret = rcob_seen; + rcob_seen = I2C_COB_NONE; + IRQ_UNLOCK(flags); + return ret; +} + +#define COB_MIN_DETECT 4 +#define COB_MAX_DETECT 50 +static void do_cob_detection(void) +{ + uint8_t flags; + uint16_t tmp = sensor_get_all(); + uint8_t l_us = !!(tmp & _BV(S_L_US)); + uint8_t r_us = !!(tmp & _BV(S_R_US)); + uint8_t l_ir = !!(tmp & _BV(S_L_IR)); + uint8_t r_ir = !!(tmp & _BV(S_R_IR)); + static uint8_t l_us_prev, r_us_prev, l_ir_prev, r_ir_prev; + static uint8_t l_cpt_on, l_cpt_off; + static uint8_t r_cpt_on, r_cpt_off; + + /* rising edge on US */ + if (l_us_prev == 0 && l_us == 1) { + l_cpt_off = 0; + l_cpt_on = 0; + } + + /* us is on */ + if (l_us) { + if (l_ir && l_cpt_on < COB_MAX_DETECT) + l_cpt_on ++; + else if (l_cpt_off < COB_MAX_DETECT) + l_cpt_off ++; + } + + /* falling edge on US */ + if (l_us_prev == 1 && l_us == 0) { + /* detection should not be too short or too long */ + if ((l_cpt_off + l_cpt_on) < COB_MAX_DETECT && + (l_cpt_off + l_cpt_on) > COB_MIN_DETECT) { + IRQ_LOCK(flags); + if (l_cpt_on > l_cpt_off) + lcob_seen = I2C_COB_WHITE; + else + lcob_seen = I2C_COB_BLACK; + IRQ_UNLOCK(flags); + } + } + + /* rising edge on US */ + if (r_us_prev == 0 && r_us == 1) { + r_cpt_off = 0; + r_cpt_on = 0; + } + + /* us is on */ + if (r_us) { + if (r_ir && r_cpt_on < COB_MAX_DETECT) + r_cpt_on ++; + else if (r_cpt_off < COB_MAX_DETECT) + r_cpt_off ++; + } + + /* falling edge on US */ + if (r_us_prev == 1 && r_us == 0) { + + /* detection should not be too short or too long */ + if ((r_cpt_off + r_cpt_on) < COB_MAX_DETECT && + (r_cpt_off + r_cpt_on) > COB_MIN_DETECT) { + IRQ_LOCK(flags); + if (r_cpt_on > r_cpt_off) + rcob_seen = I2C_COB_WHITE; + else + rcob_seen = I2C_COB_BLACK; + IRQ_UNLOCK(flags); + } + } + + + l_us_prev = l_us; + r_us_prev = r_us; + l_ir_prev = l_ir; + r_ir_prev = r_ir; +} /************ global sensor init */ #define BACKGROUND_ADC 0 @@ -240,18 +345,19 @@ static void do_boolean_sensors(void *dummy) static void do_sensors(void *dummy) { if (BACKGROUND_ADC) - do_adc(NULL); + do_adc(NULL); do_boolean_sensors(NULL); + do_cob_detection(); } void sensor_init(void) { adc_init(); if (BACKGROUND_ADC) - adc_register_event(adc_event); + 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); } diff --git a/projects/microb2010/ballboard/sensor.h b/projects/microb2010/ballboard/sensor.h index c2b18a7..0d047d5 100644 --- a/projects/microb2010/ballboard/sensor.h +++ b/projects/microb2010/ballboard/sensor.h @@ -1,7 +1,7 @@ -/* +/* * Copyright Droids Corporation (2009) * Olivier MATZ - * + * * 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 @@ -32,10 +32,10 @@ #define S_CAP2 1 #define S_CAP3 2 #define S_CAP4 3 -#define S_CAP5 4 -#define S_CAP6 5 -#define S_CAP7 6 -#define S_CAP8 7 +#define S_R_IR 4 +#define S_R_US 5 +#define S_L_US 6 +#define S_L_IR 7 #define S_RESERVED1 8 #define S_RESERVED2 9 #define S_RESERVED3 10 @@ -54,3 +54,6 @@ int16_t sensor_get_adc(uint8_t i); /* get filtered values of boolean sensors */ uint16_t sensor_get_all(void); uint8_t sensor_get(uint8_t i); + +uint8_t cob_detect_left(void); +uint8_t cob_detect_right(void); diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index 105ca7d..4751285 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -19,7 +19,7 @@ * */ -/* was mainboard in 2009 */ +/* was sensorboard in 2009 */ #define LED_TOGGLE(port, bit) do { \ if (port & _BV(bit)) \ diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 7b04977..fa17fca 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -40,6 +40,7 @@ #define I2C_COB_WHITE 1 #define I2C_COB_UNKNOWN 2 #define I2C_COB_REMOVED 3 +#define I2C_COB_NONE 4 struct i2c_cmd_hdr { uint8_t cmd; @@ -151,6 +152,10 @@ struct i2c_ans_ballboard_status { uint8_t status; uint8_t ball_count; + + /* detection of cobs */ + uint8_t lcob; + uint8_t rcob; }; #endif /* _I2C_PROTOCOL_H_ */ diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c index 6ce17cf..d264622 100644 --- a/projects/microb2010/mainboard/i2c_protocol.c +++ b/projects/microb2010/mainboard/i2c_protocol.c @@ -284,6 +284,7 @@ void i2c_recvevent(uint8_t * buf, int8_t size) } case I2C_ANS_BALLBOARD_STATUS: { + uint8_t tmp; struct i2c_ans_ballboard_status * ans = (struct i2c_ans_ballboard_status *)buf; @@ -292,6 +293,12 @@ void i2c_recvevent(uint8_t * buf, int8_t size) ballboard.mode = ans->mode; ballboard.status = ans->status; ballboard.ball_count = ans->ball_count; + tmp = ans->lcob; + if (tmp != I2C_COB_NONE) + ballboard.lcob = tmp; + tmp = ans->rcob; + if (tmp != I2C_COB_NONE) + ballboard.rcob = tmp; break; } diff --git a/projects/microb2010/mainboard/main.c b/projects/microb2010/mainboard/main.c index 8616303..90e9ff1 100755 --- a/projects/microb2010/mainboard/main.c +++ b/projects/microb2010/mainboard/main.c @@ -185,6 +185,8 @@ int main(void) memset(&mainboard, 0, sizeof(mainboard)); mainboard.flags = DO_ENCODERS | DO_CS | DO_RS | DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING; + ballboard.lcob = I2C_COB_NONE; + ballboard.rcob = I2C_COB_NONE; /* UART */ uart_init(); diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h index 4fa3dd8..c38a234 100755 --- a/projects/microb2010/mainboard/main.h +++ b/projects/microb2010/mainboard/main.h @@ -208,6 +208,8 @@ struct ballboard { volatile uint8_t mode; uint8_t status; uint8_t ball_count; + uint8_t lcob; + uint8_t rcob; }; extern struct genboard gen; diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c index 52b151e..2c60428 100644 --- a/projects/microb2010/mainboard/strat.c +++ b/projects/microb2010/mainboard/strat.c @@ -113,9 +113,9 @@ void strat_init(void) time_reset(); interrupt_traj_reset(); - //i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); - //i2c_cobboard_harvest(I2C_LEFT_SIDE); - //i2c_cobboard_harvest(I2C_RIGHT_SIDE); + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST); + i2c_cobboard_harvest(I2C_LEFT_SIDE); + i2c_cobboard_harvest(I2C_RIGHT_SIDE); i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST); /* used in strat_base for END_TIMER */ @@ -147,17 +147,25 @@ void strat_exit(void) /* called periodically */ void strat_event(void *dummy) { -#if 0 - /* pack or deploy spickle */ - if (strat_infos.status.flags & STRAT_STATUS_LHARVEST) { - if (sensor_get(S_LCOB_PRESENT)) { - if (sensor_get(S_LCOB_WHITE)) - i2c_ballboard_set_mode(); - else - ; - } - } -#endif + uint8_t flags; + uint8_t lcob, rcob; + + IRQ_LOCK(flags); + lcob = ballboard.lcob; + ballboard.lcob = I2C_COB_NONE; + rcob = ballboard.rcob; + ballboard.rcob = I2C_COB_NONE; + IRQ_UNLOCK(flags); + + if (lcob == I2C_COB_WHITE) + DEBUG(E_USER_STRAT, "lcob white"); + if (lcob == I2C_COB_BLACK) + DEBUG(E_USER_STRAT, "lcob black"); + if (rcob == I2C_COB_WHITE) + DEBUG(E_USER_STRAT, "rcob white"); + if (rcob == I2C_COB_BLACK) + DEBUG(E_USER_STRAT, "rcob black"); + /* limit speed when opponent is close */ strat_limit_speed(); } @@ -171,20 +179,20 @@ static uint8_t strat_beginning(void) strat_set_speed(600, SPEED_ANGLE_FAST); #else /* 250 */ - strat_set_speed(600, SPEED_ANGLE_FAST); + strat_set_speed(250, SPEED_ANGLE_FAST); #endif - strat_set_speed(600, 60); /* OK */ - // strat_set_speed(250, 28); /* OK */ + // strat_set_speed(600, 60); /* OK */ + strat_set_speed(250, 28); /* OK */ - trajectory_d_a_rel(&mainboard.traj, 500, COLOR_A(20)); + trajectory_d_a_rel(&mainboard.traj, 1000, COLOR_A(20)); err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj), TRAJ_FLAGS_STD); strat_set_acc(ACC_DIST, ACC_ANGLE); -#if 0 +#if 1 l1: if (get_cob_count() >= 5) strat_set_speed(600, SPEED_ANGLE_FAST); diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h index ba51434..9fa720a 100644 --- a/projects/microb2010/mainboard/strat.h +++ b/projects/microb2010/mainboard/strat.h @@ -55,7 +55,7 @@ #define WAYPOINTS_NBY 8 /* - * Corn position and lines + * Corn position and lines, valid for YELLOW. * * vertical lines * O 1 2 3 4 5 @@ -68,7 +68,7 @@ * 1/ c0 c15 \1 * |-----+ +-----| * 2/ | | \2 - * | | | | + * | Yellow Blue | * 0 +-----+-----------------------------+-----+ * 0 x 3000 * @@ -83,10 +83,43 @@ * 2 | * 1-----+ +-----| * 0 | | | - * | | | | + * | Yellow Blue | * 0 +-----+-----------------------------+-----+ * 0 x 3000 * + * + * Corn position and lines, valid for BLUE. + * + * vertical lines + * 5 4 3 2 1 0 + * 0 +-----|-----|-----|-----|-----|-----|-----+ + * | c14 c9 c5 | + * | c17 c11 c7 c2 | diag + * | c13 c8 c4 | lines + * 0/ c16 c10 c6 c1 \0 + * y | c12 c3 | + * 1/ c15 c0 \1 + * |-----+ +-----| + * 2/ | | \2 + * | Yellow Blue | + * 2100 +-----+-----------------------------+-----+ + * 3000 x 0 + * + * Ball (tomato) and i,j coords (for waypoints) + * + * 0 +-12-11-10--9--8--7--6--5--4--3--2--1--0--+ + * 7 t9 t5 | + * 6 t11 t7 t3 | + * 5 t13 t8 t4 t1 | + * 4 t10 t6 t2 | + * y 3 t12 t0 | + * 2 | + * 1-----+ +-----| + * 0 | | | + * | Yellow Blue | + * 2100 +-----+-----------------------------+-----+ + * 3000 x 0 + * */ /* useful traj flags */ diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c index 31afd43..5224787 100644 --- a/projects/microb2010/mainboard/strat_corn.c +++ b/projects/microb2010/mainboard/strat_corn.c @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -140,15 +141,16 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, line_t ll1, ll2; point_t p; uint8_t err; + uint16_t a_speed, d_speed; /* convert to 2 points */ num2line(&l1, dir1, num1); num2line(&l2, dir2, num2); - printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); - printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), - l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); + DEBUG(E_USER_STRAT, "line1: (%2.2f, %2.2f) -> (%2.2f, %2.2f)", + l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); + DEBUG(E_USER_STRAT, "line2: (%2.2f, %2.2f) -> (%2.2f, %2.2f)", + l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); /* convert to line eq and find intersection */ pts2line(&l1.p1, &l1.p2, &ll1); @@ -213,7 +215,15 @@ uint8_t line2line(uint8_t dir1, uint8_t num1, } } - err = wait_traj_end(TRAJ_FLAGS_NO_NEAR); + err = WAIT_COND_OR_TRAJ_END(get_cob_count() == 5, 0xFF); + strat_get_speed(&d_speed, &a_speed); + + /* XXX 600 -> cste */ + if (err == 0 && d_speed < 600 && + mainboard.traj.state == RUNNING_CLITOID_LINE) + strat_set_speed(600, SPEED_ANGLE_FAST); + err = wait_traj_end(0xFF); + return err; } diff --git a/projects/microb2010/tests/tourel_beacon/.config.old b/projects/microb2010/tests/tourel_beacon/.config.old index 0182964..0db9b78 100644 --- a/projects/microb2010/tests/tourel_beacon/.config.old +++ b/projects/microb2010/tests/tourel_beacon/.config.old @@ -1,5 +1,5 @@ # -# Automatically generated by make menuconfig: don't edit +# Automatically generated make config: don't edit # # @@ -74,6 +74,10 @@ CONFIG_FORMAT_BINARY=y # # Base modules # + +# +# Enable math library in generation options to see all modules +# # CONFIG_MODULE_CIRBUF is not set # CONFIG_MODULE_CIRBUF_LARGE is not set # CONFIG_MODULE_FIXED_POINT is not set @@ -94,6 +98,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # Communication modules # + +# +# uart needs circular buffer, mf2 client may need scheduler +# # CONFIG_MODULE_UART is not set # CONFIG_MODULE_UART_9BITS is not set # CONFIG_MODULE_UART_CREATE_CONFIG is not set @@ -178,6 +186,10 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # Control system modules # # CONFIG_MODULE_CONTROL_SYSTEM_MANAGER is not set + +# +# Filters +# # CONFIG_MODULE_PID is not set # CONFIG_MODULE_PID_CREATE_CONFIG is not set # CONFIG_MODULE_RAMP is not set @@ -188,12 +200,20 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # 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 @@ -203,12 +223,20 @@ CONFIG_MODULE_SCHEDULER_TIMER0=y # # 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 is not set # CONFIG_MODULE_DIAGNOSTIC_CREATE_CONFIG is not set CONFIG_MODULE_ERROR=y diff --git a/projects/microb2010/tests/tourel_beacon/graph.py b/projects/microb2010/tests/tourel_beacon/graph.py index fa87953..0d97622 100644 --- a/projects/microb2010/tests/tourel_beacon/graph.py +++ b/projects/microb2010/tests/tourel_beacon/graph.py @@ -367,5 +367,5 @@ def do_graph_2d_move_error(): #do_random_test() #do_graph_2d_simple_error() #do_graph_2d_move_error() -#do_graph_2d_ad_error() -do_graph_2d_ad_error_mm() +do_graph_2d_ad_error() +#do_graph_2d_ad_error_mm()