From 40f4332510d5f582e2b5dd19af5809e775f41ed0 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 8 May 2011 18:53:58 +0200 Subject: host/simu/robots/robospierre: move US sensors to right position --- host/simu/robots/robospierre/model/bag.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index ce55f997..4d87192c 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -43,13 +43,13 @@ class Bag: link_bag.mimot.aux[1], self.clamping_motor) self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, - (20, -20), -pi * 10 / 180, (self.position, ), 2), - DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), + DistanceSensorSensopart (link_bag.io_hub.adc[1], scheduler, table, + (20, -20), -pi * 10 / 180, (self.position, ), 2), DistanceSensorSensopart (link_bag.io_hub.adc[2], scheduler, table, - (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), - DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, -20), pi + pi * 10 / 180, (self.position, ), 2), + DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, + (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] for adc in link_bag.io_hub.adc[4:]: adc.value = 0 -- cgit v1.2.3 From 5c549e44c62e754b76bc9000ca9c8a12fb02c6ae Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 8 May 2011 19:32:24 +0200 Subject: eurobot/2011: add cabling schematics --- eurobot/2011/io-hub-cabling.svg | 1238 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 1238 insertions(+) create mode 100644 eurobot/2011/io-hub-cabling.svg diff --git a/eurobot/2011/io-hub-cabling.svg b/eurobot/2011/io-hub-cabling.svg new file mode 100644 index 00000000..e2774091 --- /dev/null +++ b/eurobot/2011/io-hub-cabling.svg @@ -0,0 +1,1238 @@ + + + + + + + + + + + + + + image/svg+xml + + + + + + + + + + ADC0 + + ADC1 + + OC0 + + OC1 + + IO0 + io-hub + + + + + + 0 + spoke-indus + + 1 + + 2 + + 3 + + + + front left US + front right US + + + back right US + back left US + + + + + + + + + + 0 + spoke-indus + + 1 + + 2 + + 3 + + + + front bottom pawn presence + back bottom pawn presence + + + front top pawn presence + team color switch + + + + + front middle pawn presence + back middle pawn presence + back top pawn presence + jack + + + + + + + + + + + + + 0 + spoke-motor + + 1 + + + back bottom door + + back top door + + + 24V motor + + + + + + side pushing servo + + + + + + + side pawn presence + + + + 0 + spoke-servo + + 1 + + 2 + + 3 + + 4 + + 5 + + 6 + + 7 + + + + 6V servo + + + + + + + 0 + spoke-motor + + 1 + + + front bottom door + + front top door + + + 24V motor + + + + + + + 0 + spoke-motor + + X + + + clamping + + + 24V motor + + + 5V elec + + 24V indus + + DEV + + -- cgit v1.2.3 From 60d3060306a09f301b09201cfaefc9a1f88096c4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 10 May 2011 19:37:43 +0200 Subject: digital/{ai,io}: add define for backward/forward --- digital/ai/src/common/defs.h | 7 +++++++ digital/ai/src/twi_master/asserv.c | 6 +++--- digital/ai/src/twi_master/asserv.h | 8 ++++---- digital/io/src/move.c | 6 +++--- 4 files changed, 17 insertions(+), 10 deletions(-) diff --git a/digital/ai/src/common/defs.h b/digital/ai/src/common/defs.h index f16e9e52..beec5608 100644 --- a/digital/ai/src/common/defs.h +++ b/digital/ai/src/common/defs.h @@ -43,6 +43,13 @@ typedef struct position_t position_t; /** Convert degrees to an angle usable in position_t. */ #define POSITION_A_DEG(a) G_ANGLE_UF016_DEG (a) +/** No particular direction. */ +#define DIRECTION_NONE 0 +/** Forward direction, along the robot X axis. */ +#define DIRECTION_FORWARD 1 +/** Backward, opposite the robot X axis. */ +#define DIRECTION_BACKWARD 2 + /** Team color, determine the start zone side. */ enum team_color_e { diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c index d066737b..d9ae9330 100644 --- a/digital/ai/src/twi_master/asserv.c +++ b/digital/ai/src/twi_master/asserv.c @@ -202,12 +202,12 @@ asserv_get_moving_direction (void) { /* Foward move? */ if (asserv_status.status & _BV (asserv_status_flag_move_forward)) - return 1; + return DIRECTION_FORWARD; /* Backward move? */ if (asserv_status.status & _BV (asserv_status_flag_move_backward)) - return 2; + return DIRECTION_BACKWARD; /* Not moving */ - return 0; + return DIRECTION_NONE; } uint8_t diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h index 0d29ba96..2aa7a6f8 100644 --- a/digital/ai/src/twi_master/asserv.h +++ b/digital/ai/src/twi_master/asserv.h @@ -121,16 +121,16 @@ asserv_get_motor1_position (void); /** * Are we moving forward/backward? * @return - * - 0 we are not moving; - * - 1 we are moving forward; - * - 2 we are moving backward. + * - DIRECTION_NONE we are not moving; + * - DIRECTION_FORWARD we are moving forward; + * - DIRECTION_BACKWARD we are moving backward. */ uint8_t asserv_get_moving_direction (void); /** * Get the last moving direction of the bot. - * @return 1 is forward, 2 is backward. + * @return DIRECTION_FORWARD or DIRECTION_BACKWARD. */ uint8_t asserv_get_last_moving_direction (void); diff --git a/digital/io/src/move.c b/digital/io/src/move.c index 60db0272..81c554fc 100644 --- a/digital/io/src/move.c +++ b/digital/io/src/move.c @@ -397,7 +397,7 @@ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY () position_t robot_pos; asserv_get_position (&robot_pos); vect_t obstacle_pos; - int16_t dist = asserv_get_last_moving_direction () == 1 + int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS); vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a); @@ -405,8 +405,8 @@ move_MOVE_MOVING_bot_move_failed_MOVE_MOVING_BACKWARD_TO_TURN_FREELY () path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0, MOVE_OBSTACLE_VALIDITY); /* Move backward to turn freely. */ - asserv_move_linearly (asserv_get_last_moving_direction () == 1 ? - - 300 : 300); + asserv_move_linearly (asserv_get_last_moving_direction () + == DIRECTION_FORWARD ? -300 : 300); } FSM_TRANS (MOVE_MOVING, -- cgit v1.2.3 From 2533936e43d199f2ee270a2014eccc5d3a7af2ea Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 11 May 2011 01:44:02 +0200 Subject: digital/cms-intro: add eurocircuits order --- .../orders/eurocircuits_2011-05-11/cms_intro.GBL | 2556 ++++++++++++++ .../orders/eurocircuits_2011-05-11/cms_intro.GBO | 2103 ++++++++++++ .../orders/eurocircuits_2011-05-11/cms_intro.GBS | 66 + .../orders/eurocircuits_2011-05-11/cms_intro.GTL | 3546 ++++++++++++++++++++ .../orders/eurocircuits_2011-05-11/cms_intro.GTO | 244 ++ .../orders/eurocircuits_2011-05-11/cms_intro.GTS | 137 + .../orders/eurocircuits_2011-05-11/cms_intro.TXT | 76 + .../orders/eurocircuits_2011-05-11/cms_intro.brd | Bin 0 -> 39187 bytes .../orders/eurocircuits_2011-05-11/cms_intro.sch | Bin 0 -> 127669 bytes 9 files changed, 8728 insertions(+) create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBL create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBO create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBS create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTL create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTO create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTS create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.TXT create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.brd create mode 100644 digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.sch diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBL b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBL new file mode 100644 index 00000000..b0ed3258 --- /dev/null +++ b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBL @@ -0,0 +1,2556 @@ +G75* +G70* +%OFA0B0*% +%FSLAX24Y24*% +%IPPOS*% +%LPD*% +%AMOC8* +5,1,8,0,0,1.08239X$1,22.5* +% +%ADD10OC8,0.0640*% +%ADD11C,0.0436*% +%ADD12C,0.0100*% +%ADD13C,0.0400*% +D10* +X000651Y011568D03* +X001651Y011568D03* +X002651Y011568D03* +X003651Y011568D03* +X004651Y011568D03* +X005651Y011568D03* +X006651Y011568D03* +X007651Y011568D03* +X008651Y011568D03* +X009651Y011568D03* +X010651Y011568D03* +X011651Y011568D03* +X012651Y011568D03* +X013651Y011568D03* +X014651Y011568D03* +X015651Y011568D03* +X016651Y011568D03* +X016651Y010568D03* +X015651Y010568D03* +X015651Y009568D03* +X016651Y009568D03* +X016651Y008568D03* +X015651Y008568D03* +X015651Y007568D03* +X016651Y007568D03* +X016651Y006568D03* +X015651Y006568D03* +X015651Y005568D03* +X016651Y005568D03* +X016651Y004568D03* +X015651Y004568D03* +X015651Y003568D03* +X016651Y003568D03* +X016651Y002568D03* +X015651Y002568D03* +X015651Y001568D03* +X016651Y001568D03* +X016651Y012568D03* +X015651Y012568D03* +X014651Y012568D03* +X013651Y012568D03* +X012651Y012568D03* +X011651Y012568D03* +X010651Y012568D03* +X009651Y012568D03* +X008651Y012568D03* +X007651Y012568D03* +X006651Y012568D03* +X005651Y012568D03* +X004651Y012568D03* +X003651Y012568D03* +X002651Y012568D03* +X001651Y012568D03* +X000651Y012568D03* +D11* +X006801Y009718D03* +X008101Y010318D03* +X005301Y007568D03* +X005301Y005568D03* +X006151Y003518D03* +X009351Y004218D03* +X008601Y006718D03* +X013151Y003518D03* +X001501Y003668D03* +D12* +X000301Y001218D02* +X000301Y011254D01* +X000456Y011098D01* +X000846Y011098D01* +X001121Y011374D01* +X001121Y011763D01* +X000846Y012038D01* +X000456Y012038D01* +X000301Y011883D01* +X000301Y012254D01* +X000456Y012098D01* +X000601Y012098D01* +X000601Y012518D01* +X000701Y012518D01* +X000701Y012098D01* +X000846Y012098D01* +X001121Y012374D01* +X001121Y012518D01* +X000701Y012518D01* +X000701Y012618D01* +X001121Y012618D01* +X001121Y012763D01* +X000966Y012918D01* +X001337Y012918D01* +X001181Y012763D01* +X001181Y012618D01* +X001601Y012618D01* +X001601Y012518D01* +X001701Y012518D01* +X001701Y012098D01* +X001846Y012098D01* +X002121Y012374D01* +X002121Y012518D01* +X001701Y012518D01* +X001701Y012618D01* +X002121Y012618D01* +X002121Y012763D01* +X001966Y012918D01* +X002337Y012918D01* +X002181Y012763D01* +X002181Y012618D01* +X002601Y012618D01* +X002601Y012518D01* +X002701Y012518D01* +X002701Y012098D01* +X002846Y012098D01* +X003121Y012374D01* +X003121Y012518D01* +X002701Y012518D01* +X002701Y012618D01* +X003121Y012618D01* +X003121Y012763D01* +X002966Y012918D01* +X003337Y012918D01* +X003181Y012763D01* +X003181Y012618D01* +X003601Y012618D01* +X003601Y012518D01* +X003701Y012518D01* +X003701Y012098D01* +X003846Y012098D01* +X004121Y012374D01* +X004121Y012518D01* +X003701Y012518D01* +X003701Y012618D01* +X004121Y012618D01* +X004121Y012763D01* +X003966Y012918D01* +X004337Y012918D01* +X004181Y012763D01* +X004181Y012618D01* +X004601Y012618D01* +X004601Y012518D01* +X004701Y012518D01* +X004701Y012098D01* +X004846Y012098D01* +X005121Y012374D01* +X005121Y012518D01* +X004701Y012518D01* +X004701Y012618D01* +X005121Y012618D01* +X005121Y012763D01* +X004966Y012918D01* +X005337Y012918D01* +X005181Y012763D01* +X005181Y012618D01* +X005601Y012618D01* +X005601Y012518D01* +X005701Y012518D01* +X005701Y012098D01* +X005846Y012098D01* +X006121Y012374D01* +X006121Y012518D01* +X005701Y012518D01* +X005701Y012618D01* +X006121Y012618D01* +X006121Y012763D01* +X005966Y012918D01* +X006337Y012918D01* +X006181Y012763D01* +X006181Y012618D01* +X006601Y012618D01* +X006601Y012518D01* +X006701Y012518D01* +X006701Y012098D01* +X006846Y012098D01* +X007121Y012374D01* +X007121Y012518D01* +X006701Y012518D01* +X006701Y012618D01* +X007121Y012618D01* +X007121Y012763D01* +X006966Y012918D01* +X007337Y012918D01* +X007181Y012763D01* +X007181Y012618D01* +X007601Y012618D01* +X007601Y012518D01* +X007701Y012518D01* +X007701Y012098D01* +X007846Y012098D01* +X008121Y012374D01* +X008121Y012518D01* +X007701Y012518D01* +X007701Y012618D01* +X008121Y012618D01* +X008121Y012763D01* +X007966Y012918D01* +X008337Y012918D01* +X008181Y012763D01* +X008181Y012618D01* +X008601Y012618D01* +X008601Y012518D01* +X008701Y012518D01* +X008701Y012098D01* +X008846Y012098D01* +X009121Y012374D01* +X009121Y012518D01* +X008701Y012518D01* +X008701Y012618D01* +X009121Y012618D01* +X009121Y012763D01* +X008966Y012918D01* +X009337Y012918D01* +X009181Y012763D01* +X009181Y012618D01* +X009601Y012618D01* +X009601Y012518D01* +X009701Y012518D01* +X009701Y012098D01* +X009846Y012098D01* +X010121Y012374D01* +X010121Y012518D01* +X009701Y012518D01* +X009701Y012618D01* +X010121Y012618D01* +X010121Y012763D01* +X009966Y012918D01* +X010337Y012918D01* +X010181Y012763D01* +X010181Y012618D01* +X010601Y012618D01* +X010601Y012518D01* +X010701Y012518D01* +X010701Y012098D01* +X010846Y012098D01* +X011121Y012374D01* +X011121Y012518D01* +X010701Y012518D01* +X010701Y012618D01* +X011121Y012618D01* +X011121Y012763D01* +X010966Y012918D01* +X011337Y012918D01* +X011181Y012763D01* +X011181Y012618D01* +X011601Y012618D01* +X011601Y012518D01* +X011701Y012518D01* +X011701Y012098D01* +X011846Y012098D01* +X012121Y012374D01* +X012121Y012518D01* +X011701Y012518D01* +X011701Y012618D01* +X012121Y012618D01* +X012121Y012763D01* +X011966Y012918D01* +X012337Y012918D01* +X012181Y012763D01* +X012181Y012618D01* +X012601Y012618D01* +X012601Y012518D01* +X012701Y012518D01* +X012701Y012098D01* +X012846Y012098D01* +X013121Y012374D01* +X013121Y012518D01* +X012701Y012518D01* +X012701Y012618D01* +X013121Y012618D01* +X013121Y012763D01* +X012966Y012918D01* +X013337Y012918D01* +X013181Y012763D01* +X013181Y012618D01* +X013601Y012618D01* +X013601Y012518D01* +X013701Y012518D01* +X013701Y012098D01* +X013846Y012098D01* +X014121Y012374D01* +X014121Y012518D01* +X013701Y012518D01* +X013701Y012618D01* +X014121Y012618D01* +X014121Y012763D01* +X013966Y012918D01* +X014337Y012918D01* +X014181Y012763D01* +X014181Y012618D01* +X014601Y012618D01* +X014601Y012518D01* +X014701Y012518D01* +X014701Y012098D01* +X014846Y012098D01* +X015121Y012374D01* +X015121Y012518D01* +X014701Y012518D01* +X014701Y012618D01* +X015121Y012618D01* +X015121Y012763D01* +X014966Y012918D01* +X015337Y012918D01* +X015181Y012763D01* +X015181Y012618D01* +X015601Y012618D01* +X015601Y012518D01* +X015701Y012518D01* +X015701Y012098D01* +X015846Y012098D01* +X016121Y012374D01* +X016121Y012518D01* +X015701Y012518D01* +X015701Y012618D01* +X016121Y012618D01* +X016121Y012763D01* +X015966Y012918D01* +X016337Y012918D01* +X016181Y012763D01* +X016181Y012618D01* +X016601Y012618D01* +X016601Y012518D01* +X016701Y012518D01* +X016701Y012098D01* +X016846Y012098D01* +X017001Y012254D01* +X017001Y011883D01* +X016846Y012038D01* +X016701Y012038D01* +X016701Y011619D01* +X016601Y011619D01* +X016601Y012038D01* +X016456Y012038D01* +X016181Y011763D01* +X016181Y011618D01* +X016601Y011618D01* +X016601Y011518D01* +X016701Y011518D01* +X016701Y011098D01* +X016846Y011098D01* +X017001Y011254D01* +X017001Y010883D01* +X016846Y011038D01* +X016701Y011038D01* +X016701Y010619D01* +X016601Y010619D01* +X016601Y011038D01* +X016456Y011038D01* +X016181Y010763D01* +X016181Y010618D01* +X016601Y010618D01* +X016601Y010518D01* +X016701Y010518D01* +X016701Y010098D01* +X016846Y010098D01* +X017001Y010254D01* +X017001Y009883D01* +X016846Y010038D01* +X016701Y010038D01* +X016701Y009619D01* +X016601Y009619D01* +X016601Y010038D01* +X016456Y010038D01* +X016181Y009763D01* +X016181Y009618D01* +X016601Y009618D01* +X016601Y009518D01* +X016701Y009518D01* +X016701Y009098D01* +X016846Y009098D01* +X017001Y009254D01* +X017001Y008883D01* +X016846Y009038D01* +X016701Y009038D01* +X016701Y008619D01* +X016601Y008619D01* +X016601Y009038D01* +X016456Y009038D01* +X016181Y008763D01* +X016181Y008618D01* +X016601Y008618D01* +X016601Y008518D01* +X016701Y008518D01* +X016701Y008098D01* +X016846Y008098D01* +X017001Y008254D01* +X017001Y007883D01* +X016846Y008038D01* +X016701Y008038D01* +X016701Y007619D01* +X016601Y007619D01* +X016601Y008038D01* +X016456Y008038D01* +X016181Y007763D01* +X016181Y007618D01* +X016601Y007618D01* +X016601Y007518D01* +X016701Y007518D01* +X016701Y007098D01* +X016846Y007098D01* +X017001Y007254D01* +X017001Y006883D01* +X016846Y007038D01* +X016701Y007038D01* +X016701Y006619D01* +X016601Y006619D01* +X016601Y007038D01* +X016456Y007038D01* +X016181Y006763D01* +X016181Y006618D01* +X016601Y006618D01* +X016601Y006518D01* +X016701Y006518D01* +X016701Y006098D01* +X016846Y006098D01* +X017001Y006254D01* +X017001Y005883D01* +X016846Y006038D01* +X016701Y006038D01* +X016701Y005619D01* +X016601Y005619D01* +X016601Y006038D01* +X016456Y006038D01* +X016181Y005763D01* +X016181Y005618D01* +X016601Y005618D01* +X016601Y005518D01* +X016701Y005518D01* +X016701Y005098D01* +X016846Y005098D01* +X017001Y005254D01* +X017001Y004883D01* +X016846Y005038D01* +X016701Y005038D01* +X016701Y004619D01* +X016601Y004619D01* +X016601Y005038D01* +X016456Y005038D01* +X016181Y004763D01* +X016181Y004618D01* +X016601Y004618D01* +X016601Y004518D01* +X016701Y004518D01* +X016701Y004098D01* +X016846Y004098D01* +X017001Y004254D01* +X017001Y003883D01* +X016846Y004038D01* +X016701Y004038D01* +X016701Y003619D01* +X016601Y003619D01* +X016601Y004038D01* +X016456Y004038D01* +X016181Y003763D01* +X016181Y003618D01* +X016601Y003618D01* +X016601Y003518D01* +X016701Y003518D01* +X016701Y003098D01* +X016846Y003098D01* +X017001Y003254D01* +X017001Y002883D01* +X016846Y003038D01* +X016701Y003038D01* +X016701Y002619D01* +X016601Y002619D01* +X016601Y003038D01* +X016456Y003038D01* +X016181Y002763D01* +X016181Y002618D01* +X016601Y002618D01* +X016601Y002518D01* +X016701Y002518D01* +X016701Y002098D01* +X016846Y002098D01* +X017001Y002254D01* +X017001Y001883D01* +X016846Y002038D01* +X016701Y002038D01* +X016701Y001619D01* +X016601Y001619D01* +X016601Y002038D01* +X016456Y002038D01* +X016181Y001763D01* +X016181Y001618D01* +X016601Y001618D01* +X016601Y001518D01* +X016181Y001518D01* +X016181Y001374D01* +X016337Y001218D01* +X015966Y001218D01* +X016121Y001374D01* +X016121Y001518D01* +X015701Y001518D01* +X015701Y001618D01* +X016121Y001618D01* +X016121Y001763D01* +X015846Y002038D01* +X015701Y002038D01* +X015701Y001619D01* +X015601Y001619D01* +X015601Y002038D01* +X015456Y002038D01* +X015181Y001763D01* +X015181Y001618D01* +X015601Y001618D01* +X015601Y001518D01* +X015181Y001518D01* +X015181Y001374D01* +X015337Y001218D01* +X000301Y001218D01* +X000301Y001265D02* +X015289Y001265D01* +X015191Y001364D02* +X000301Y001364D01* +X000301Y001463D02* +X015181Y001463D01* +X015181Y001660D02* +X000301Y001660D01* +X000301Y001758D02* +X015181Y001758D01* +X015275Y001857D02* +X000301Y001857D01* +X000301Y001955D02* +X001981Y001955D01* +X002068Y001868D02* +X001868Y002068D01* +X001868Y002068D01* +X001751Y002185D01* +X001751Y002973D01* +X001654Y003070D01* +X001601Y003199D01* +X001601Y003338D01* +X001654Y003467D01* +X001753Y003565D01* +X001882Y003618D01* +X002021Y003618D01* +X002149Y003565D01* +X002248Y003467D01* +X002301Y003338D01* +X002301Y003199D01* +X002248Y003070D01* +X002151Y002973D01* +X002151Y002351D01* +X002234Y002268D01* +X002768Y002268D01* +X002851Y002351D01* +X002851Y003473D01* +X002754Y003570D01* +X002701Y003699D01* +X002701Y003838D01* +X002754Y003967D01* +X002853Y004065D01* +X002982Y004118D01* +X003121Y004118D01* +X003249Y004065D01* +X003348Y003967D01* +X003401Y003838D01* +X003401Y003699D01* +X003348Y003570D01* +X003251Y003473D01* +X003251Y002351D01* +X003251Y002185D01* +X002934Y001868D01* +X002768Y001868D01* +X002234Y001868D01* +X002068Y001868D01* +X002151Y002068D02* +X001951Y002268D01* +X001951Y003268D01* +X001641Y003433D02* +X000301Y003433D01* +X000301Y003335D02* +X001601Y003335D01* +X001601Y003236D02* +X000301Y003236D01* +X000301Y003138D02* +X001626Y003138D01* +X001685Y003039D02* +X000301Y003039D01* +X000301Y002941D02* +X001751Y002941D01* +X001751Y002842D02* +X000301Y002842D01* +X000301Y002744D02* +X001751Y002744D01* +X001751Y002645D02* +X000301Y002645D01* +X000301Y002546D02* +X001751Y002546D01* +X001751Y002448D02* +X000301Y002448D01* +X000301Y002349D02* +X001751Y002349D01* +X001751Y002251D02* +X000301Y002251D01* +X000301Y002152D02* +X001784Y002152D01* +X001883Y002054D02* +X000301Y002054D01* +X000301Y001561D02* +X015601Y001561D01* +X015601Y001660D02* +X015701Y001660D01* +X015701Y001758D02* +X015601Y001758D01* +X015601Y001857D02* +X015701Y001857D01* +X015701Y001955D02* +X015601Y001955D01* +X015456Y002098D02* +X015846Y002098D01* +X016121Y002374D01* +X016121Y002763D01* +X015846Y003038D01* +X015456Y003038D01* +X015181Y002763D01* +X015181Y002374D01* +X015456Y002098D01* +X015403Y002152D02* +X003218Y002152D01* +X003251Y002251D02* +X015304Y002251D01* +X015205Y002349D02* +X003251Y002349D01* +X003251Y002448D02* +X015181Y002448D01* +X015181Y002546D02* +X003251Y002546D01* +X003251Y002645D02* +X015181Y002645D01* +X015181Y002744D02* +X003251Y002744D01* +X003251Y002842D02* +X015260Y002842D01* +X015359Y002941D02* +X003251Y002941D01* +X003251Y003039D02* +X017001Y003039D01* +X017001Y002941D02* +X016943Y002941D01* +X016701Y002941D02* +X016601Y002941D01* +X016601Y002842D02* +X016701Y002842D01* +X016701Y002744D02* +X016601Y002744D01* +X016601Y002645D02* +X016701Y002645D01* +X016601Y002546D02* +X016121Y002546D01* +X016181Y002518D02* +X016181Y002374D01* +X016456Y002098D01* +X016601Y002098D01* +X016601Y002518D01* +X016181Y002518D01* +X016181Y002448D02* +X016121Y002448D01* +X016097Y002349D02* +X016205Y002349D01* +X016304Y002251D02* +X015998Y002251D01* +X015900Y002152D02* +X016403Y002152D01* +X016601Y002152D02* +X016701Y002152D01* +X016701Y002251D02* +X016601Y002251D01* +X016601Y002349D02* +X016701Y002349D01* +X016701Y002448D02* +X016601Y002448D01* +X016900Y002152D02* +X017001Y002152D01* +X017001Y002054D02* +X003119Y002054D01* +X003021Y001955D02* +X015373Y001955D01* +X015701Y001561D02* +X016601Y001561D01* +X016601Y001660D02* +X016701Y001660D01* +X016701Y001758D02* +X016601Y001758D01* +X016601Y001857D02* +X016701Y001857D01* +X016701Y001955D02* +X016601Y001955D01* +X016373Y001955D02* +X015929Y001955D01* +X016027Y001857D02* +X016275Y001857D01* +X016181Y001758D02* +X016121Y001758D01* +X016121Y001660D02* +X016181Y001660D01* +X016181Y001463D02* +X016121Y001463D01* +X016111Y001364D02* +X016191Y001364D01* +X016289Y001265D02* +X016013Y001265D01* +X016929Y001955D02* +X017001Y001955D01* +X016998Y002251D02* +X017001Y002251D01* +X016181Y002645D02* +X016121Y002645D01* +X016121Y002744D02* +X016181Y002744D01* +X016260Y002842D02* +X016042Y002842D01* +X015943Y002941D02* +X016359Y002941D01* +X016456Y003098D02* +X016601Y003098D01* +X016601Y003518D01* +X016181Y003518D01* +X016181Y003374D01* +X016456Y003098D01* +X016417Y003138D02* +X015885Y003138D01* +X015846Y003098D02* +X016121Y003374D01* +X016121Y003763D01* +X015846Y004038D01* +X015456Y004038D01* +X015181Y003763D01* +X015181Y003374D01* +X015456Y003098D01* +X015846Y003098D01* +X015984Y003236D02* +X016319Y003236D01* +X016220Y003335D02* +X016082Y003335D01* +X016121Y003433D02* +X016181Y003433D01* +X016121Y003532D02* +X016601Y003532D01* +X016601Y003630D02* +X016701Y003630D01* +X016701Y003729D02* +X016601Y003729D01* +X016601Y003828D02* +X016701Y003828D01* +X016701Y003926D02* +X016601Y003926D01* +X016601Y004025D02* +X016701Y004025D01* +X016701Y004123D02* +X016601Y004123D01* +X016601Y004098D02* +X016601Y004518D01* +X016181Y004518D01* +X016181Y004374D01* +X016456Y004098D01* +X016601Y004098D01* +X016601Y004222D02* +X016701Y004222D01* +X016701Y004320D02* +X016601Y004320D01* +X016601Y004419D02* +X016701Y004419D01* +X016701Y004517D02* +X016601Y004517D01* +X016601Y004616D02* +X016121Y004616D01* +X016121Y004714D02* +X016181Y004714D01* +X016121Y004763D02* +X015846Y005038D01* +X015456Y005038D01* +X015181Y004763D01* +X015181Y004374D01* +X015456Y004098D01* +X015846Y004098D01* +X016121Y004374D01* +X016121Y004763D01* +X016071Y004813D02* +X016231Y004813D01* +X016330Y004912D02* +X015973Y004912D01* +X015874Y005010D02* +X016428Y005010D01* +X016456Y005098D02* +X016601Y005098D01* +X016601Y005518D01* +X016181Y005518D01* +X016181Y005374D01* +X016456Y005098D01* +X016446Y005109D02* +X015856Y005109D01* +X015846Y005098D02* +X016121Y005374D01* +X016121Y005763D01* +X015846Y006038D01* +X015456Y006038D01* +X015181Y005763D01* +X015181Y005374D01* +X015456Y005098D01* +X015846Y005098D01* +X015955Y005207D02* +X016348Y005207D01* +X016249Y005306D02* +X016053Y005306D01* +X016121Y005404D02* +X016181Y005404D01* +X016181Y005503D02* +X016121Y005503D01* +X016121Y005601D02* +X016601Y005601D01* +X016601Y005503D02* +X016701Y005503D01* +X016701Y005404D02* +X016601Y005404D01* +X016601Y005306D02* +X016701Y005306D01* +X016701Y005207D02* +X016601Y005207D01* +X016601Y005109D02* +X016701Y005109D01* +X016701Y005010D02* +X016601Y005010D01* +X016601Y004912D02* +X016701Y004912D01* +X016701Y004813D02* +X016601Y004813D01* +X016601Y004714D02* +X016701Y004714D01* +X016973Y004912D02* +X017001Y004912D01* +X017001Y005010D02* +X016874Y005010D01* +X016856Y005109D02* +X017001Y005109D01* +X017001Y005207D02* +X016955Y005207D01* +X016701Y005700D02* +X016601Y005700D01* +X016601Y005798D02* +X016701Y005798D01* +X016701Y005897D02* +X016601Y005897D01* +X016601Y005996D02* +X016701Y005996D01* +X016601Y006098D02* +X016601Y006518D01* +X016181Y006518D01* +X016181Y006374D01* +X016456Y006098D01* +X016601Y006098D01* +X016601Y006193D02* +X016701Y006193D01* +X016701Y006291D02* +X016601Y006291D01* +X016601Y006390D02* +X016701Y006390D01* +X016701Y006488D02* +X016601Y006488D01* +X016601Y006587D02* +X016121Y006587D01* +X016121Y006685D02* +X016181Y006685D01* +X016121Y006763D02* +X015846Y007038D01* +X015456Y007038D01* +X015181Y006763D01* +X015181Y006374D01* +X015456Y006098D01* +X015846Y006098D01* +X016121Y006374D01* +X016121Y006763D01* +X016100Y006784D02* +X016202Y006784D01* +X016301Y006882D02* +X016002Y006882D01* +X015903Y006981D02* +X016399Y006981D01* +X016456Y007098D02* +X016601Y007098D01* +X016601Y007518D01* +X016181Y007518D01* +X016181Y007374D01* +X016456Y007098D01* +X016377Y007178D02* +X015926Y007178D01* +X015846Y007098D02* +X016121Y007374D01* +X016121Y007763D01* +X015846Y008038D01* +X015456Y008038D01* +X015181Y007763D01* +X015181Y007374D01* +X015456Y007098D01* +X015846Y007098D01* +X016024Y007277D02* +X016278Y007277D01* +X016181Y007375D02* +X016121Y007375D01* +X016121Y007474D02* +X016181Y007474D01* +X016121Y007572D02* +X016601Y007572D01* +X016601Y007474D02* +X016701Y007474D01* +X016701Y007375D02* +X016601Y007375D01* +X016601Y007277D02* +X016701Y007277D01* +X016701Y007178D02* +X016601Y007178D01* +X016601Y006981D02* +X016701Y006981D01* +X016701Y006882D02* +X016601Y006882D01* +X016601Y006784D02* +X016701Y006784D01* +X016701Y006685D02* +X016601Y006685D01* +X016903Y006981D02* +X017001Y006981D01* +X017001Y007079D02* +X000301Y007079D01* +X000301Y006981D02* +X015399Y006981D01* +X015301Y006882D02* +X000301Y006882D01* +X000301Y006784D02* +X015202Y006784D01* +X015181Y006685D02* +X000301Y006685D01* +X000301Y006587D02* +X015181Y006587D01* +X015181Y006488D02* +X000301Y006488D01* +X000301Y006390D02* +X015181Y006390D01* +X015264Y006291D02* +X000301Y006291D01* +X000301Y006193D02* +X015362Y006193D01* +X015414Y005996D02* +X000301Y005996D01* +X000301Y006094D02* +X017001Y006094D01* +X017001Y005996D02* +X016889Y005996D01* +X016987Y005897D02* +X017001Y005897D01* +X017001Y006193D02* +X016940Y006193D01* +X016414Y005996D02* +X015889Y005996D01* +X015987Y005897D02* +X016315Y005897D01* +X016217Y005798D02* +X016086Y005798D01* +X016121Y005700D02* +X016181Y005700D01* +X016362Y006193D02* +X015940Y006193D01* +X016039Y006291D02* +X016264Y006291D01* +X016181Y006390D02* +X016121Y006390D01* +X016121Y006488D02* +X016181Y006488D01* +X016925Y007178D02* +X017001Y007178D01* +X016701Y007671D02* +X016601Y007671D01* +X016601Y007769D02* +X016701Y007769D01* +X016701Y007868D02* +X016601Y007868D01* +X016601Y007966D02* +X016701Y007966D01* +X016601Y008098D02* +X016601Y008518D01* +X016181Y008518D01* +X016181Y008374D01* +X016456Y008098D01* +X016601Y008098D01* +X016601Y008163D02* +X016701Y008163D01* +X016701Y008262D02* +X016601Y008262D01* +X016601Y008361D02* +X016701Y008361D01* +X016701Y008459D02* +X016601Y008459D01* +X016601Y008558D02* +X016121Y008558D01* +X016121Y008656D02* +X016181Y008656D01* +X016181Y008755D02* +X016121Y008755D01* +X016121Y008763D02* +X015846Y009038D01* +X015456Y009038D01* +X015181Y008763D01* +X015181Y008374D01* +X015456Y008098D01* +X015846Y008098D01* +X016121Y008374D01* +X016121Y008763D01* +X016031Y008853D02* +X016271Y008853D01* +X016370Y008952D02* +X015932Y008952D01* +X015846Y009098D02* +X016121Y009374D01* +X016121Y009763D01* +X015846Y010038D01* +X015456Y010038D01* +X015181Y009763D01* +X015181Y009374D01* +X015456Y009098D01* +X015846Y009098D01* +X015896Y009149D02* +X016406Y009149D01* +X016456Y009098D02* +X016181Y009374D01* +X016181Y009518D01* +X016601Y009518D01* +X016601Y009098D01* +X016456Y009098D01* +X016601Y009149D02* +X016701Y009149D01* +X016701Y009247D02* +X016601Y009247D01* +X016601Y009346D02* +X016701Y009346D01* +X016701Y009445D02* +X016601Y009445D01* +X016601Y009543D02* +X016121Y009543D01* +X016121Y009445D02* +X016181Y009445D01* +X016209Y009346D02* +X016093Y009346D01* +X015995Y009247D02* +X016307Y009247D01* +X016601Y008952D02* +X016701Y008952D01* +X016701Y008853D02* +X016601Y008853D01* +X016601Y008755D02* +X016701Y008755D01* +X016701Y008656D02* +X016601Y008656D01* +X016932Y008952D02* +X017001Y008952D01* +X017001Y009050D02* +X000301Y009050D01* +X000301Y008952D02* +X015370Y008952D01* +X015271Y008853D02* +X000301Y008853D01* +X000301Y008755D02* +X015181Y008755D01* +X015181Y008656D02* +X000301Y008656D01* +X000301Y008558D02* +X015181Y008558D01* +X015181Y008459D02* +X000301Y008459D01* +X000301Y008361D02* +X015194Y008361D01* +X015293Y008262D02* +X000301Y008262D01* +X000301Y008163D02* +X015391Y008163D01* +X015385Y007966D02* +X000301Y007966D01* +X000301Y007868D02* +X015286Y007868D01* +X015187Y007769D02* +X000301Y007769D01* +X000301Y007671D02* +X015181Y007671D01* +X015181Y007572D02* +X000301Y007572D01* +X000301Y007474D02* +X015181Y007474D01* +X015181Y007375D02* +X000301Y007375D01* +X000301Y007277D02* +X015278Y007277D01* +X015377Y007178D02* +X000301Y007178D01* +X000301Y008065D02* +X017001Y008065D01* +X017001Y008163D02* +X016911Y008163D01* +X016918Y007966D02* +X017001Y007966D01* +X016385Y007966D02* +X015918Y007966D01* +X016016Y007868D02* +X016286Y007868D01* +X016187Y007769D02* +X016115Y007769D01* +X016121Y007671D02* +X016181Y007671D01* +X016391Y008163D02* +X015911Y008163D01* +X016009Y008262D02* +X016293Y008262D01* +X016194Y008361D02* +X016108Y008361D01* +X016121Y008459D02* +X016181Y008459D01* +X016896Y009149D02* +X017001Y009149D01* +X016995Y009247D02* +X017001Y009247D01* +X016701Y009642D02* +X016601Y009642D01* +X016601Y009740D02* +X016701Y009740D01* +X016701Y009839D02* +X016601Y009839D01* +X016601Y009937D02* +X016701Y009937D01* +X016701Y010036D02* +X016601Y010036D01* +X016601Y010098D02* +X016601Y010518D01* +X016181Y010518D01* +X016181Y010374D01* +X016456Y010098D01* +X016601Y010098D01* +X016601Y010134D02* +X016701Y010134D01* +X016701Y010233D02* +X016601Y010233D01* +X016601Y010331D02* +X016701Y010331D01* +X016701Y010430D02* +X016601Y010430D01* +X016601Y010529D02* +X016121Y010529D01* +X016121Y010627D02* +X016181Y010627D01* +X016181Y010726D02* +X016121Y010726D01* +X016121Y010763D02* +X015846Y011038D01* +X015456Y011038D01* +X015181Y010763D01* +X015181Y010374D01* +X015456Y010098D01* +X015846Y010098D01* +X016121Y010374D01* +X016121Y010763D01* +X016060Y010824D02* +X016242Y010824D01* +X016341Y010923D02* +X015961Y010923D01* +X015863Y011021D02* +X016439Y011021D01* +X016456Y011098D02* +X016601Y011098D01* +X016601Y011518D01* +X016181Y011518D01* +X016181Y011374D01* +X016456Y011098D01* +X016435Y011120D02* +X015867Y011120D01* +X015846Y011098D02* +X016121Y011374D01* +X016121Y011763D01* +X015846Y012038D01* +X015456Y012038D01* +X015181Y011763D01* +X015181Y011374D01* +X015456Y011098D01* +X015846Y011098D01* +X015966Y011218D02* +X016337Y011218D01* +X016238Y011317D02* +X016064Y011317D01* +X016121Y011415D02* +X016181Y011415D01* +X016181Y011514D02* +X016121Y011514D01* +X016121Y011612D02* +X016601Y011612D01* +X016601Y011514D02* +X016701Y011514D01* +X016701Y011415D02* +X016601Y011415D01* +X016601Y011317D02* +X016701Y011317D01* +X016701Y011218D02* +X016601Y011218D01* +X016601Y011120D02* +X016701Y011120D01* +X016701Y011021D02* +X016601Y011021D01* +X016601Y010923D02* +X016701Y010923D01* +X016701Y010824D02* +X016601Y010824D01* +X016601Y010726D02* +X016701Y010726D01* +X016701Y010627D02* +X016601Y010627D01* +X016181Y010430D02* +X016121Y010430D01* +X016079Y010331D02* +X016223Y010331D01* +X016322Y010233D02* +X015980Y010233D01* +X015882Y010134D02* +X016421Y010134D01* +X016454Y010036D02* +X015848Y010036D01* +X015947Y009937D02* +X016355Y009937D01* +X016257Y009839D02* +X016045Y009839D01* +X016121Y009740D02* +X016181Y009740D01* +X016181Y009642D02* +X016121Y009642D01* +X015454Y010036D02* +X008339Y010036D01* +X008310Y010006D02* +X008174Y009950D01* +X008028Y009950D01* +X007893Y010006D01* +X007789Y010110D01* +X007786Y010118D01* +X007484Y010118D01* +X007166Y009800D01* +X007169Y009792D01* +X007169Y009645D01* +X007113Y009510D01* +X007010Y009406D01* +X006874Y009350D01* +X006728Y009350D01* +X006593Y009406D01* +X006489Y009510D01* +X006433Y009645D01* +X006433Y009792D01* +X006489Y009927D01* +X006593Y010030D01* +X006728Y010086D01* +X006874Y010086D01* +X006883Y010083D01* +X007318Y010518D01* +X007484Y010518D01* +X007786Y010518D01* +X007789Y010527D01* +X007893Y010630D01* +X008028Y010686D01* +X008174Y010686D01* +X008310Y010630D01* +X008413Y010527D01* +X008469Y010392D01* +X008469Y010245D01* +X008413Y010110D01* +X008310Y010006D01* +X008423Y010134D02* +X015421Y010134D01* +X015322Y010233D02* +X008464Y010233D01* +X008469Y010331D02* +X015223Y010331D01* +X015181Y010430D02* +X008453Y010430D01* +X008412Y010529D02* +X015181Y010529D01* +X015181Y010627D02* +X008313Y010627D01* +X008101Y010318D02* +X007401Y010318D01* +X006801Y009718D01* +X006554Y009445D02* +X000301Y009445D01* +X000301Y009543D02* +X006475Y009543D01* +X006434Y009642D02* +X000301Y009642D01* +X000301Y009740D02* +X006433Y009740D01* +X006453Y009839D02* +X000301Y009839D01* +X000301Y009937D02* +X006499Y009937D01* +X006606Y010036D02* +X000301Y010036D01* +X000301Y010134D02* +X006934Y010134D01* +X007033Y010233D02* +X000301Y010233D01* +X000301Y010331D02* +X007131Y010331D01* +X007230Y010430D02* +X000301Y010430D01* +X000301Y010529D02* +X007791Y010529D01* +X007889Y010627D02* +X000301Y010627D01* +X000301Y010726D02* +X015181Y010726D01* +X015242Y010824D02* +X000301Y010824D01* +X000301Y010923D02* +X015341Y010923D01* +X015439Y011021D02* +X000301Y011021D01* +X000301Y011120D02* +X000435Y011120D01* +X000337Y011218D02* +X000301Y011218D01* +X000867Y011120D02* +X001435Y011120D01* +X001456Y011098D02* +X001846Y011098D01* +X002121Y011374D01* +X002121Y011763D01* +X001846Y012038D01* +X001456Y012038D01* +X001181Y011763D01* +X001181Y011374D01* +X001456Y011098D01* +X001337Y011218D02* +X000966Y011218D01* +X001064Y011317D02* +X001238Y011317D01* +X001181Y011415D02* +X001121Y011415D01* +X001121Y011514D02* +X001181Y011514D01* +X001181Y011612D02* +X001121Y011612D01* +X001121Y011711D02* +X001181Y011711D01* +X001228Y011810D02* +X001075Y011810D01* +X000976Y011908D02* +X001326Y011908D01* +X001425Y012007D02* +X000877Y012007D01* +X000853Y012105D02* +X001450Y012105D01* +X001456Y012098D02* +X001181Y012374D01* +X001181Y012518D01* +X001601Y012518D01* +X001601Y012098D01* +X001456Y012098D01* +X001601Y012105D02* +X001701Y012105D01* +X001701Y012204D02* +X001601Y012204D01* +X001601Y012302D02* +X001701Y012302D01* +X001701Y012401D02* +X001601Y012401D01* +X001601Y012499D02* +X001701Y012499D01* +X001701Y012598D02* +X002601Y012598D01* +X002601Y012518D02* +X002181Y012518D01* +X002181Y012374D01* +X002456Y012098D01* +X002601Y012098D01* +X002601Y012518D01* +X002601Y012499D02* +X002701Y012499D01* +X002701Y012401D02* +X002601Y012401D01* +X002601Y012302D02* +X002701Y012302D01* +X002701Y012204D02* +X002601Y012204D01* +X002601Y012105D02* +X002701Y012105D01* +X002853Y012105D02* +X003450Y012105D01* +X003456Y012098D02* +X003601Y012098D01* +X003601Y012518D01* +X003181Y012518D01* +X003181Y012374D01* +X003456Y012098D01* +X003456Y012038D02* +X003181Y011763D01* +X003181Y011374D01* +X003456Y011098D01* +X003846Y011098D01* +X004121Y011374D01* +X004121Y011763D01* +X003846Y012038D01* +X003456Y012038D01* +X003425Y012007D02* +X002877Y012007D01* +X002846Y012038D02* +X002456Y012038D01* +X002181Y011763D01* +X002181Y011374D01* +X002456Y011098D01* +X002846Y011098D01* +X003121Y011374D01* +X003121Y011763D01* +X002846Y012038D01* +X002976Y011908D02* +X003326Y011908D01* +X003228Y011810D02* +X003075Y011810D01* +X003121Y011711D02* +X003181Y011711D01* +X003181Y011612D02* +X003121Y011612D01* +X003121Y011514D02* +X003181Y011514D01* +X003181Y011415D02* +X003121Y011415D01* +X003064Y011317D02* +X003238Y011317D01* +X003337Y011218D02* +X002966Y011218D01* +X002867Y011120D02* +X003435Y011120D01* +X003867Y011120D02* +X004435Y011120D01* +X004456Y011098D02* +X004846Y011098D01* +X005121Y011374D01* +X005121Y011763D01* +X004846Y012038D01* +X004456Y012038D01* +X004181Y011763D01* +X004181Y011374D01* +X004456Y011098D01* +X004337Y011218D02* +X003966Y011218D01* +X004064Y011317D02* +X004238Y011317D01* +X004181Y011415D02* +X004121Y011415D01* +X004121Y011514D02* +X004181Y011514D01* +X004181Y011612D02* +X004121Y011612D01* +X004121Y011711D02* +X004181Y011711D01* +X004228Y011810D02* +X004075Y011810D01* +X003976Y011908D02* +X004326Y011908D01* +X004425Y012007D02* +X003877Y012007D01* +X003853Y012105D02* +X004450Y012105D01* +X004456Y012098D02* +X004601Y012098D01* +X004601Y012518D01* +X004181Y012518D01* +X004181Y012374D01* +X004456Y012098D01* +X004601Y012105D02* +X004701Y012105D01* +X004701Y012204D02* +X004601Y012204D01* +X004601Y012302D02* +X004701Y012302D01* +X004701Y012401D02* +X004601Y012401D01* +X004601Y012499D02* +X004701Y012499D01* +X004701Y012598D02* +X005601Y012598D01* +X005601Y012518D02* +X005181Y012518D01* +X005181Y012374D01* +X005456Y012098D01* +X005601Y012098D01* +X005601Y012518D01* +X005601Y012499D02* +X005701Y012499D01* +X005701Y012401D02* +X005601Y012401D01* +X005601Y012302D02* +X005701Y012302D01* +X005701Y012204D02* +X005601Y012204D01* +X005601Y012105D02* +X005701Y012105D01* +X005853Y012105D02* +X006450Y012105D01* +X006456Y012098D02* +X006601Y012098D01* +X006601Y012518D01* +X006181Y012518D01* +X006181Y012374D01* +X006456Y012098D01* +X006456Y012038D02* +X006181Y011763D01* +X006181Y011618D01* +X006601Y011618D01* +X006601Y011518D01* +X006701Y011518D01* +X006701Y011098D01* +X006846Y011098D01* +X007121Y011374D01* +X007121Y011518D01* +X006701Y011518D01* +X006701Y011618D01* +X007121Y011618D01* +X007121Y011763D01* +X006846Y012038D01* +X006701Y012038D01* +X006701Y011619D01* +X006601Y011619D01* +X006601Y012038D01* +X006456Y012038D01* +X006425Y012007D02* +X005877Y012007D01* +X005846Y012038D02* +X005456Y012038D01* +X005181Y011763D01* +X005181Y011374D01* +X005456Y011098D01* +X005846Y011098D01* +X006121Y011374D01* +X006121Y011763D01* +X005846Y012038D01* +X005976Y011908D02* +X006326Y011908D01* +X006228Y011810D02* +X006075Y011810D01* +X006121Y011711D02* +X006181Y011711D01* +X006121Y011612D02* +X006601Y011612D01* +X006601Y011518D02* +X006181Y011518D01* +X006181Y011374D01* +X006456Y011098D01* +X006601Y011098D01* +X006601Y011518D01* +X006601Y011514D02* +X006701Y011514D01* +X006701Y011612D02* +X007181Y011612D01* +X007181Y011514D02* +X007121Y011514D01* +X007121Y011415D02* +X007181Y011415D01* +X007181Y011374D02* +X007456Y011098D01* +X007846Y011098D01* +X008121Y011374D01* +X008121Y011763D01* +X007846Y012038D01* +X007456Y012038D01* +X007181Y011763D01* +X007181Y011374D01* +X007238Y011317D02* +X007064Y011317D01* +X006966Y011218D02* +X007337Y011218D01* +X007435Y011120D02* +X006867Y011120D01* +X006701Y011120D02* +X006601Y011120D01* +X006601Y011218D02* +X006701Y011218D01* +X006701Y011317D02* +X006601Y011317D01* +X006601Y011415D02* +X006701Y011415D01* +X006701Y011711D02* +X006601Y011711D01* +X006601Y011810D02* +X006701Y011810D01* +X006701Y011908D02* +X006601Y011908D01* +X006601Y012007D02* +X006701Y012007D01* +X006701Y012105D02* +X006601Y012105D01* +X006601Y012204D02* +X006701Y012204D01* +X006701Y012302D02* +X006601Y012302D01* +X006601Y012401D02* +X006701Y012401D01* +X006701Y012499D02* +X006601Y012499D01* +X006601Y012598D02* +X005701Y012598D01* +X005991Y012894D02* +X006312Y012894D01* +X006213Y012795D02* +X006089Y012795D01* +X006121Y012696D02* +X006181Y012696D01* +X006181Y012499D02* +X006121Y012499D01* +X006121Y012401D02* +X006181Y012401D01* +X006253Y012302D02* +X006050Y012302D01* +X005951Y012204D02* +X006351Y012204D01* +X006853Y012105D02* +X007450Y012105D01* +X007456Y012098D02* +X007601Y012098D01* +X007601Y012518D01* +X007181Y012518D01* +X007181Y012374D01* +X007456Y012098D01* +X007425Y012007D02* +X006877Y012007D01* +X006976Y011908D02* +X007326Y011908D01* +X007228Y011810D02* +X007075Y011810D01* +X007121Y011711D02* +X007181Y011711D01* +X007351Y012204D02* +X006951Y012204D01* +X007050Y012302D02* +X007253Y012302D01* +X007181Y012401D02* +X007121Y012401D01* +X007121Y012499D02* +X007181Y012499D01* +X007181Y012696D02* +X007121Y012696D01* +X007089Y012795D02* +X007213Y012795D01* +X007312Y012894D02* +X006991Y012894D01* +X006701Y012598D02* +X007601Y012598D01* +X007601Y012499D02* +X007701Y012499D01* +X007701Y012401D02* +X007601Y012401D01* +X007601Y012302D02* +X007701Y012302D01* +X007701Y012204D02* +X007601Y012204D01* +X007601Y012105D02* +X007701Y012105D01* +X007853Y012105D02* +X008450Y012105D01* +X008456Y012098D02* +X008601Y012098D01* +X008601Y012518D01* +X008181Y012518D01* +X008181Y012374D01* +X008456Y012098D01* +X008456Y012038D02* +X008181Y011763D01* +X008181Y011374D01* +X008456Y011098D01* +X008846Y011098D01* +X009121Y011374D01* +X009121Y011763D01* +X008846Y012038D01* +X008456Y012038D01* +X008425Y012007D02* +X007877Y012007D01* +X007976Y011908D02* +X008326Y011908D01* +X008228Y011810D02* +X008075Y011810D01* +X008121Y011711D02* +X008181Y011711D01* +X008181Y011612D02* +X008121Y011612D01* +X008121Y011514D02* +X008181Y011514D01* +X008181Y011415D02* +X008121Y011415D01* +X008064Y011317D02* +X008238Y011317D01* +X008337Y011218D02* +X007966Y011218D01* +X007867Y011120D02* +X008435Y011120D01* +X008867Y011120D02* +X009435Y011120D01* +X009456Y011098D02* +X009846Y011098D01* +X010121Y011374D01* +X010121Y011763D01* +X009846Y012038D01* +X009456Y012038D01* +X009181Y011763D01* +X009181Y011374D01* +X009456Y011098D01* +X009337Y011218D02* +X008966Y011218D01* +X009064Y011317D02* +X009238Y011317D01* +X009181Y011415D02* +X009121Y011415D01* +X009121Y011514D02* +X009181Y011514D01* +X009181Y011612D02* +X009121Y011612D01* +X009121Y011711D02* +X009181Y011711D01* +X009228Y011810D02* +X009075Y011810D01* +X008976Y011908D02* +X009326Y011908D01* +X009425Y012007D02* +X008877Y012007D01* +X008853Y012105D02* +X009450Y012105D01* +X009456Y012098D02* +X009601Y012098D01* +X009601Y012518D01* +X009181Y012518D01* +X009181Y012374D01* +X009456Y012098D01* +X009601Y012105D02* +X009701Y012105D01* +X009701Y012204D02* +X009601Y012204D01* +X009601Y012302D02* +X009701Y012302D01* +X009701Y012401D02* +X009601Y012401D01* +X009601Y012499D02* +X009701Y012499D01* +X009701Y012598D02* +X010601Y012598D01* +X010601Y012518D02* +X010181Y012518D01* +X010181Y012374D01* +X010456Y012098D01* +X010601Y012098D01* +X010601Y012518D01* +X010601Y012499D02* +X010701Y012499D01* +X010701Y012401D02* +X010601Y012401D01* +X010601Y012302D02* +X010701Y012302D01* +X010701Y012204D02* +X010601Y012204D01* +X010601Y012105D02* +X010701Y012105D01* +X010853Y012105D02* +X011450Y012105D01* +X011456Y012098D02* +X011601Y012098D01* +X011601Y012518D01* +X011181Y012518D01* +X011181Y012374D01* +X011456Y012098D01* +X011456Y012038D02* +X011181Y011763D01* +X011181Y011374D01* +X011456Y011098D01* +X011846Y011098D01* +X012121Y011374D01* +X012121Y011763D01* +X011846Y012038D01* +X011456Y012038D01* +X011425Y012007D02* +X010877Y012007D01* +X010846Y012038D02* +X010456Y012038D01* +X010181Y011763D01* +X010181Y011374D01* +X010456Y011098D01* +X010846Y011098D01* +X011121Y011374D01* +X011121Y011763D01* +X010846Y012038D01* +X010976Y011908D02* +X011326Y011908D01* +X011228Y011810D02* +X011075Y011810D01* +X011121Y011711D02* +X011181Y011711D01* +X011181Y011612D02* +X011121Y011612D01* +X011121Y011514D02* +X011181Y011514D01* +X011181Y011415D02* +X011121Y011415D01* +X011064Y011317D02* +X011238Y011317D01* +X011337Y011218D02* +X010966Y011218D01* +X010867Y011120D02* +X011435Y011120D01* +X011867Y011120D02* +X012435Y011120D01* +X012456Y011098D02* +X012846Y011098D01* +X013121Y011374D01* +X013121Y011763D01* +X012846Y012038D01* +X012456Y012038D01* +X012181Y011763D01* +X012181Y011374D01* +X012456Y011098D01* +X012337Y011218D02* +X011966Y011218D01* +X012064Y011317D02* +X012238Y011317D01* +X012181Y011415D02* +X012121Y011415D01* +X012121Y011514D02* +X012181Y011514D01* +X012181Y011612D02* +X012121Y011612D01* +X012121Y011711D02* +X012181Y011711D01* +X012228Y011810D02* +X012075Y011810D01* +X011976Y011908D02* +X012326Y011908D01* +X012425Y012007D02* +X011877Y012007D01* +X011853Y012105D02* +X012450Y012105D01* +X012456Y012098D02* +X012601Y012098D01* +X012601Y012518D01* +X012181Y012518D01* +X012181Y012374D01* +X012456Y012098D01* +X012601Y012105D02* +X012701Y012105D01* +X012701Y012204D02* +X012601Y012204D01* +X012601Y012302D02* +X012701Y012302D01* +X012701Y012401D02* +X012601Y012401D01* +X012601Y012499D02* +X012701Y012499D01* +X012701Y012598D02* +X013601Y012598D01* +X013601Y012518D02* +X013181Y012518D01* +X013181Y012374D01* +X013456Y012098D01* +X013601Y012098D01* +X013601Y012518D01* +X013601Y012499D02* +X013701Y012499D01* +X013701Y012401D02* +X013601Y012401D01* +X013601Y012302D02* +X013701Y012302D01* +X013701Y012204D02* +X013601Y012204D01* +X013601Y012105D02* +X013701Y012105D01* +X013853Y012105D02* +X014450Y012105D01* +X014456Y012098D02* +X014601Y012098D01* +X014601Y012518D01* +X014181Y012518D01* +X014181Y012374D01* +X014456Y012098D01* +X014456Y012038D02* +X014181Y011763D01* +X014181Y011374D01* +X014456Y011098D01* +X014846Y011098D01* +X015121Y011374D01* +X015121Y011763D01* +X014846Y012038D01* +X014456Y012038D01* +X014425Y012007D02* +X013877Y012007D01* +X013846Y012038D02* +X013456Y012038D01* +X013181Y011763D01* +X013181Y011374D01* +X013456Y011098D01* +X013846Y011098D01* +X014121Y011374D01* +X014121Y011763D01* +X013846Y012038D01* +X013976Y011908D02* +X014326Y011908D01* +X014228Y011810D02* +X014075Y011810D01* +X014121Y011711D02* +X014181Y011711D01* +X014181Y011612D02* +X014121Y011612D01* +X014121Y011514D02* +X014181Y011514D01* +X014181Y011415D02* +X014121Y011415D01* +X014064Y011317D02* +X014238Y011317D01* +X014337Y011218D02* +X013966Y011218D01* +X013867Y011120D02* +X014435Y011120D01* +X014867Y011120D02* +X015435Y011120D01* +X015337Y011218D02* +X014966Y011218D01* +X015064Y011317D02* +X015238Y011317D01* +X015181Y011415D02* +X015121Y011415D01* +X015121Y011514D02* +X015181Y011514D01* +X015181Y011612D02* +X015121Y011612D01* +X015121Y011711D02* +X015181Y011711D01* +X015228Y011810D02* +X015075Y011810D01* +X014976Y011908D02* +X015326Y011908D01* +X015425Y012007D02* +X014877Y012007D01* +X014853Y012105D02* +X015450Y012105D01* +X015456Y012098D02* +X015601Y012098D01* +X015601Y012518D01* +X015181Y012518D01* +X015181Y012374D01* +X015456Y012098D01* +X015601Y012105D02* +X015701Y012105D01* +X015701Y012204D02* +X015601Y012204D01* +X015601Y012302D02* +X015701Y012302D01* +X015701Y012401D02* +X015601Y012401D01* +X015601Y012499D02* +X015701Y012499D01* +X015701Y012598D02* +X016601Y012598D01* +X016601Y012518D02* +X016181Y012518D01* +X016181Y012374D01* +X016456Y012098D01* +X016601Y012098D01* +X016601Y012518D01* +X016601Y012499D02* +X016701Y012499D01* +X016701Y012401D02* +X016601Y012401D01* +X016601Y012302D02* +X016701Y012302D01* +X016701Y012204D02* +X016601Y012204D01* +X016601Y012105D02* +X016701Y012105D01* +X016701Y012007D02* +X016601Y012007D01* +X016601Y011908D02* +X016701Y011908D01* +X016701Y011810D02* +X016601Y011810D01* +X016601Y011711D02* +X016701Y011711D01* +X016976Y011908D02* +X017001Y011908D01* +X017001Y012007D02* +X016877Y012007D01* +X016853Y012105D02* +X017001Y012105D01* +X017001Y012204D02* +X016951Y012204D01* +X016450Y012105D02* +X015853Y012105D01* +X015877Y012007D02* +X016425Y012007D01* +X016326Y011908D02* +X015976Y011908D01* +X016075Y011810D02* +X016228Y011810D01* +X016181Y011711D02* +X016121Y011711D01* +X015951Y012204D02* +X016351Y012204D01* +X016253Y012302D02* +X016050Y012302D01* +X016121Y012401D02* +X016181Y012401D01* +X016181Y012499D02* +X016121Y012499D01* +X016121Y012696D02* +X016181Y012696D01* +X016213Y012795D02* +X016089Y012795D01* +X015991Y012894D02* +X016312Y012894D01* +X015601Y012598D02* +X014701Y012598D01* +X014701Y012499D02* +X014601Y012499D01* +X014601Y012401D02* +X014701Y012401D01* +X014701Y012302D02* +X014601Y012302D01* +X014601Y012204D02* +X014701Y012204D01* +X014701Y012105D02* +X014601Y012105D01* +X014351Y012204D02* +X013951Y012204D01* +X014050Y012302D02* +X014253Y012302D01* +X014181Y012401D02* +X014121Y012401D01* +X014121Y012499D02* +X014181Y012499D01* +X014181Y012696D02* +X014121Y012696D01* +X014089Y012795D02* +X014213Y012795D01* +X014312Y012894D02* +X013991Y012894D01* +X013701Y012598D02* +X014601Y012598D01* +X014991Y012894D02* +X015312Y012894D01* +X015213Y012795D02* +X015089Y012795D01* +X015121Y012696D02* +X015181Y012696D01* +X015181Y012499D02* +X015121Y012499D01* +X015121Y012401D02* +X015181Y012401D01* +X015253Y012302D02* +X015050Y012302D01* +X014951Y012204D02* +X015351Y012204D01* +X016867Y011120D02* +X017001Y011120D01* +X017001Y011218D02* +X016966Y011218D01* +X017001Y011021D02* +X016863Y011021D01* +X016961Y010923D02* +X017001Y010923D01* +X016980Y010233D02* +X017001Y010233D01* +X017001Y010134D02* +X016882Y010134D01* +X016848Y010036D02* +X017001Y010036D01* +X017001Y009937D02* +X016947Y009937D01* +X015406Y009149D02* +X000301Y009149D01* +X000301Y009247D02* +X015307Y009247D01* +X015209Y009346D02* +X000301Y009346D01* +X001867Y011120D02* +X002435Y011120D01* +X002337Y011218D02* +X001966Y011218D01* +X002064Y011317D02* +X002238Y011317D01* +X002181Y011415D02* +X002121Y011415D01* +X002121Y011514D02* +X002181Y011514D01* +X002181Y011612D02* +X002121Y011612D01* +X002121Y011711D02* +X002181Y011711D01* +X002228Y011810D02* +X002075Y011810D01* +X001976Y011908D02* +X002326Y011908D01* +X002425Y012007D02* +X001877Y012007D01* +X001853Y012105D02* +X002450Y012105D01* +X002351Y012204D02* +X001951Y012204D01* +X002050Y012302D02* +X002253Y012302D01* +X002181Y012401D02* +X002121Y012401D01* +X002121Y012499D02* +X002181Y012499D01* +X002181Y012696D02* +X002121Y012696D01* +X002089Y012795D02* +X002213Y012795D01* +X002312Y012894D02* +X001991Y012894D01* +X001601Y012598D02* +X000701Y012598D01* +X000701Y012499D02* +X000601Y012499D01* +X000601Y012401D02* +X000701Y012401D01* +X000701Y012302D02* +X000601Y012302D01* +X000601Y012204D02* +X000701Y012204D01* +X000701Y012105D02* +X000601Y012105D01* +X000450Y012105D02* +X000301Y012105D01* +X000301Y012007D02* +X000425Y012007D01* +X000326Y011908D02* +X000301Y011908D01* +X000301Y012204D02* +X000351Y012204D01* +X000951Y012204D02* +X001351Y012204D01* +X001253Y012302D02* +X001050Y012302D01* +X001121Y012401D02* +X001181Y012401D01* +X001181Y012499D02* +X001121Y012499D01* +X001121Y012696D02* +X001181Y012696D01* +X001213Y012795D02* +X001089Y012795D01* +X000991Y012894D02* +X001312Y012894D01* +X002701Y012598D02* +X003601Y012598D01* +X003601Y012499D02* +X003701Y012499D01* +X003701Y012401D02* +X003601Y012401D01* +X003601Y012302D02* +X003701Y012302D01* +X003701Y012204D02* +X003601Y012204D01* +X003601Y012105D02* +X003701Y012105D01* +X003951Y012204D02* +X004351Y012204D01* +X004253Y012302D02* +X004050Y012302D01* +X004121Y012401D02* +X004181Y012401D01* +X004181Y012499D02* +X004121Y012499D01* +X004121Y012696D02* +X004181Y012696D01* +X004213Y012795D02* +X004089Y012795D01* +X003991Y012894D02* +X004312Y012894D01* +X004601Y012598D02* +X003701Y012598D01* +X003312Y012894D02* +X002991Y012894D01* +X003089Y012795D02* +X003213Y012795D01* +X003181Y012696D02* +X003121Y012696D01* +X003121Y012499D02* +X003181Y012499D01* +X003181Y012401D02* +X003121Y012401D01* +X003050Y012302D02* +X003253Y012302D01* +X003351Y012204D02* +X002951Y012204D01* +X004853Y012105D02* +X005450Y012105D01* +X005425Y012007D02* +X004877Y012007D01* +X004976Y011908D02* +X005326Y011908D01* +X005228Y011810D02* +X005075Y011810D01* +X005121Y011711D02* +X005181Y011711D01* +X005181Y011612D02* +X005121Y011612D01* +X005121Y011514D02* +X005181Y011514D01* +X005181Y011415D02* +X005121Y011415D01* +X005064Y011317D02* +X005238Y011317D01* +X005337Y011218D02* +X004966Y011218D01* +X004867Y011120D02* +X005435Y011120D01* +X005867Y011120D02* +X006435Y011120D01* +X006337Y011218D02* +X005966Y011218D01* +X006064Y011317D02* +X006238Y011317D01* +X006181Y011415D02* +X006121Y011415D01* +X006121Y011514D02* +X006181Y011514D01* +X005351Y012204D02* +X004951Y012204D01* +X005050Y012302D02* +X005253Y012302D01* +X005181Y012401D02* +X005121Y012401D01* +X005121Y012499D02* +X005181Y012499D01* +X005181Y012696D02* +X005121Y012696D01* +X005089Y012795D02* +X005213Y012795D01* +X005312Y012894D02* +X004991Y012894D01* +X007701Y012598D02* +X008601Y012598D01* +X008601Y012499D02* +X008701Y012499D01* +X008701Y012401D02* +X008601Y012401D01* +X008601Y012302D02* +X008701Y012302D01* +X008701Y012204D02* +X008601Y012204D01* +X008601Y012105D02* +X008701Y012105D01* +X008951Y012204D02* +X009351Y012204D01* +X009253Y012302D02* +X009050Y012302D01* +X009121Y012401D02* +X009181Y012401D01* +X009181Y012499D02* +X009121Y012499D01* +X009121Y012696D02* +X009181Y012696D01* +X009213Y012795D02* +X009089Y012795D01* +X008991Y012894D02* +X009312Y012894D01* +X009601Y012598D02* +X008701Y012598D01* +X008312Y012894D02* +X007991Y012894D01* +X008089Y012795D02* +X008213Y012795D01* +X008181Y012696D02* +X008121Y012696D01* +X008121Y012499D02* +X008181Y012499D01* +X008181Y012401D02* +X008121Y012401D01* +X008050Y012302D02* +X008253Y012302D01* +X008351Y012204D02* +X007951Y012204D01* +X009853Y012105D02* +X010450Y012105D01* +X010425Y012007D02* +X009877Y012007D01* +X009976Y011908D02* +X010326Y011908D01* +X010228Y011810D02* +X010075Y011810D01* +X010121Y011711D02* +X010181Y011711D01* +X010181Y011612D02* +X010121Y011612D01* +X010121Y011514D02* +X010181Y011514D01* +X010181Y011415D02* +X010121Y011415D01* +X010064Y011317D02* +X010238Y011317D01* +X010337Y011218D02* +X009966Y011218D01* +X009867Y011120D02* +X010435Y011120D01* +X010351Y012204D02* +X009951Y012204D01* +X010050Y012302D02* +X010253Y012302D01* +X010181Y012401D02* +X010121Y012401D01* +X010121Y012499D02* +X010181Y012499D01* +X010181Y012696D02* +X010121Y012696D01* +X010089Y012795D02* +X010213Y012795D01* +X010312Y012894D02* +X009991Y012894D01* +X010701Y012598D02* +X011601Y012598D01* +X011601Y012499D02* +X011701Y012499D01* +X011701Y012401D02* +X011601Y012401D01* +X011601Y012302D02* +X011701Y012302D01* +X011701Y012204D02* +X011601Y012204D01* +X011601Y012105D02* +X011701Y012105D01* +X011951Y012204D02* +X012351Y012204D01* +X012253Y012302D02* +X012050Y012302D01* +X012121Y012401D02* +X012181Y012401D01* +X012181Y012499D02* +X012121Y012499D01* +X012121Y012696D02* +X012181Y012696D01* +X012213Y012795D02* +X012089Y012795D01* +X011991Y012894D02* +X012312Y012894D01* +X012601Y012598D02* +X011701Y012598D01* +X011312Y012894D02* +X010991Y012894D01* +X011089Y012795D02* +X011213Y012795D01* +X011181Y012696D02* +X011121Y012696D01* +X011121Y012499D02* +X011181Y012499D01* +X011181Y012401D02* +X011121Y012401D01* +X011050Y012302D02* +X011253Y012302D01* +X011351Y012204D02* +X010951Y012204D01* +X012853Y012105D02* +X013450Y012105D01* +X013425Y012007D02* +X012877Y012007D01* +X012976Y011908D02* +X013326Y011908D01* +X013228Y011810D02* +X013075Y011810D01* +X013121Y011711D02* +X013181Y011711D01* +X013181Y011612D02* +X013121Y011612D01* +X013121Y011514D02* +X013181Y011514D01* +X013181Y011415D02* +X013121Y011415D01* +X013064Y011317D02* +X013238Y011317D01* +X013337Y011218D02* +X012966Y011218D01* +X012867Y011120D02* +X013435Y011120D01* +X013351Y012204D02* +X012951Y012204D01* +X013050Y012302D02* +X013253Y012302D01* +X013181Y012401D02* +X013121Y012401D01* +X013121Y012499D02* +X013181Y012499D01* +X013181Y012696D02* +X013121Y012696D01* +X013089Y012795D02* +X013213Y012795D01* +X013312Y012894D02* +X012991Y012894D01* +X015355Y009937D02* +X007303Y009937D01* +X007204Y009839D02* +X015257Y009839D01* +X015181Y009740D02* +X007169Y009740D01* +X007168Y009642D02* +X015181Y009642D01* +X015181Y009543D02* +X007127Y009543D01* +X007048Y009445D02* +X015181Y009445D01* +X015315Y005897D02* +X000301Y005897D01* +X000301Y005798D02* +X015217Y005798D01* +X015181Y005700D02* +X000301Y005700D01* +X000301Y005601D02* +X015181Y005601D01* +X015181Y005503D02* +X000301Y005503D01* +X000301Y005404D02* +X015181Y005404D01* +X015249Y005306D02* +X000301Y005306D01* +X000301Y005207D02* +X015348Y005207D01* +X015446Y005109D02* +X000301Y005109D01* +X000301Y005010D02* +X015428Y005010D01* +X015330Y004912D02* +X000301Y004912D01* +X000301Y004813D02* +X015231Y004813D01* +X015181Y004714D02* +X000301Y004714D01* +X000301Y004616D02* +X015181Y004616D01* +X015181Y004517D02* +X000301Y004517D01* +X000301Y004419D02* +X015181Y004419D01* +X015235Y004320D02* +X000301Y004320D01* +X000301Y004222D02* +X015333Y004222D01* +X015432Y004123D02* +X000301Y004123D01* +X000301Y004025D02* +X002813Y004025D01* +X002738Y003926D02* +X000301Y003926D01* +X000301Y003828D02* +X002701Y003828D01* +X002701Y003729D02* +X000301Y003729D01* +X000301Y003630D02* +X002729Y003630D01* +X002793Y003532D02* +X002183Y003532D01* +X002262Y003433D02* +X002851Y003433D01* +X002851Y003335D02* +X002301Y003335D01* +X002301Y003236D02* +X002851Y003236D01* +X002851Y003138D02* +X002276Y003138D01* +X002217Y003039D02* +X002851Y003039D01* +X002851Y002941D02* +X002151Y002941D01* +X002151Y002842D02* +X002851Y002842D01* +X002851Y002744D02* +X002151Y002744D01* +X002151Y002645D02* +X002851Y002645D01* +X002851Y002546D02* +X002151Y002546D01* +X002151Y002448D02* +X002851Y002448D01* +X002849Y002349D02* +X002153Y002349D01* +X002151Y002068D02* +X002851Y002068D01* +X003051Y002268D01* +X003051Y003768D01* +X003373Y003630D02* +X015181Y003630D01* +X015181Y003532D02* +X003310Y003532D01* +X003251Y003433D02* +X015181Y003433D01* +X015220Y003335D02* +X003251Y003335D01* +X003251Y003236D02* +X015319Y003236D01* +X015417Y003138D02* +X003251Y003138D01* +X003401Y003729D02* +X015181Y003729D01* +X015246Y003828D02* +X003401Y003828D01* +X003365Y003926D02* +X015344Y003926D01* +X015443Y004025D02* +X003290Y004025D01* +X001720Y003532D02* +X000301Y003532D01* +X007401Y010036D02* +X007863Y010036D01* +X015871Y004123D02* +X016432Y004123D01* +X016443Y004025D02* +X015859Y004025D01* +X015958Y003926D02* +X016344Y003926D01* +X016246Y003828D02* +X016057Y003828D01* +X016121Y003729D02* +X016181Y003729D01* +X016181Y003630D02* +X016121Y003630D01* +X016601Y003433D02* +X016701Y003433D01* +X016701Y003335D02* +X016601Y003335D01* +X016601Y003236D02* +X016701Y003236D01* +X016701Y003138D02* +X016601Y003138D01* +X016885Y003138D02* +X017001Y003138D01* +X017001Y003236D02* +X016984Y003236D01* +X016958Y003926D02* +X017001Y003926D01* +X017001Y004025D02* +X016859Y004025D01* +X016871Y004123D02* +X017001Y004123D01* +X017001Y004222D02* +X016969Y004222D01* +X016333Y004222D02* +X015969Y004222D01* +X016068Y004320D02* +X016235Y004320D01* +X016181Y004419D02* +X016121Y004419D01* +X016121Y004517D02* +X016181Y004517D01* +D13* +X003051Y003768D03* +X001951Y003268D03* +M02* diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBO b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBO new file mode 100644 index 00000000..fa114b37 --- /dev/null +++ b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBO @@ -0,0 +1,2103 @@ +G75* +G70* +%OFA0B0*% +%FSLAX24Y24*% +%IPPOS*% +%LPD*% +%AMOC8* +5,1,8,0,0,1.08239X$1,22.5* +% +%ADD10C,0.0080*% +%ADD11C,0.0060*% +D10* +X003901Y003528D02* +X004061Y003428D01* +X005821Y002628D01* +X006621Y002348D01* +X007061Y002228D01* +X007301Y002228D01* +X007501Y002268D01* +X010941Y003388D01* +X010901Y003388D02* +X009981Y003668D01* +X009861Y003628D02* +X009541Y003428D01* +X009141Y003268D01* +X008861Y003188D01* +X008581Y003148D01* +X008261Y003108D01* +X007861Y003148D01* +X007501Y003228D01* +X007141Y003348D01* +X006821Y003508D01* +X006581Y003748D01* +X006381Y004028D01* +X006261Y004308D01* +X006241Y004468D01* +X006301Y004668D01* +X006241Y004688D01* +X006241Y004468D01* +X006481Y004428D02* +X006481Y004328D01* +X006581Y004208D01* +X006701Y004148D01* +X006801Y004188D01* +X006801Y004248D01* +X006721Y004268D01* +X006641Y004368D01* +X006621Y004528D01* +X006541Y004508D01* +X006481Y004428D01* +X006494Y004446D02* +X006632Y004446D01* +X006661Y004388D02* +X006741Y004268D01* +X006861Y004228D01* +X008581Y003428D01* +X008741Y003388D01* +X008901Y003428D01* +X009301Y003588D01* +X009781Y003828D01* +X009861Y003908D01* +X009881Y003988D01* +X009981Y004088D01* +X009981Y004168D01* +X009921Y004228D01* +X009981Y004308D02* +X009821Y004188D01* +X009661Y004268D02* +X009861Y004428D01* +X009861Y004588D01* +X009856Y004603D02* +X009642Y004603D01* +X009641Y004608D02* +X009721Y004688D01* +X009821Y004748D01* +X009901Y004768D01* +X009941Y005148D01* +X010041Y005088D01* +X010001Y004748D01* +X010041Y004748D01* +X010121Y004688D01* +X010141Y004628D01* +X010121Y004548D01* +X010061Y004508D01* +X009881Y004628D01* +X009841Y004588D01* +X009841Y004508D01* +X009741Y004468D01* +X009661Y004508D01* +X009641Y004608D01* +X009658Y004524D02* +X009841Y004524D01* +X009920Y004603D02* +X010135Y004603D01* +X010124Y004681D02* +X009714Y004681D01* +X009601Y004808D02* +X009561Y004768D01* +X009401Y004868D01* +X009361Y004828D01* +X009341Y004728D01* +X009261Y004708D01* +X009181Y004768D01* +X009181Y004888D01* +X009261Y004968D01* +X009381Y005028D01* +X009421Y005068D01* +X009441Y005468D01* +X009541Y005428D01* +X009501Y005028D01* +X009561Y004988D01* +X009621Y004928D01* +X009621Y004868D01* +X009601Y004808D01* +X009611Y004838D02* +X009449Y004838D01* +X009381Y004828D02* +X009381Y004668D01* +X009181Y004508D01* +X009261Y004588D02* +X008961Y004748D01* +X009021Y004788D02* +X009061Y004828D01* +X009061Y004988D01* +X009061Y005008D02* +X008921Y005108D01* +X008881Y005048D01* +X008841Y004928D01* +X008721Y004928D01* +X008661Y004988D01* +X008661Y005108D01* +X008761Y005208D01* +X008881Y005268D01* +X008921Y005308D01* +X008941Y005628D01* +X009041Y005628D01* +X009021Y005408D01* +X009001Y005268D01* +X009061Y005228D01* +X009141Y005168D01* +X009141Y005108D01* +X009101Y005028D01* +X009061Y005008D01* +X009124Y005074D02* +X008969Y005074D01* +X008901Y005068D02* +X008901Y004908D01* +X008801Y004828D01* +X008501Y004988D01* +X008541Y005068D02* +X008381Y004908D01* +X008221Y004988D02* +X008381Y005148D01* +X008381Y005308D01* +X008363Y005310D02* +X008208Y005310D01* +X008201Y005288D02* +X008241Y005408D01* +X008301Y005448D01* +X008421Y005468D01* +X008421Y005668D01* +X008541Y005668D01* +X008541Y005468D01* +X008641Y005428D01* +X008661Y005348D01* +X008641Y005248D01* +X008581Y005188D01* +X008561Y005248D01* +X008401Y005348D01* +X008341Y005288D01* +X008321Y005148D01* +X008241Y005168D01* +X008201Y005288D01* +X008220Y005231D02* +X008333Y005231D01* +X008322Y005153D02* +X008304Y005153D01* +X008301Y005088D02* +X008021Y005228D01* +X007941Y005268D01* +X007861Y005268D01* +X007701Y005228D01* +X007341Y005068D01* +X007461Y005128D01* +X006981Y005428D01* +X006861Y005368D01* +X007341Y005068D01* +X007021Y004908D01* +X006781Y004708D01* +X006621Y004548D01* +X006661Y004388D01* +X006642Y004367D02* +X006481Y004367D01* +X006514Y004289D02* +X006705Y004289D01* +X006801Y004210D02* +X006580Y004210D01* +X006861Y004228D02* +X006861Y004348D01* +X007101Y004548D01* +X007701Y004828D01* +X007901Y004908D01* +X007981Y004908D01* +X009881Y003988D01* +X010061Y003828D02* +X009861Y003628D01* +X010061Y003828D02* +X010221Y004068D01* +X010261Y004268D01* +X010261Y004508D01* +X010181Y004868D01* +X009941Y005148D01* +X009661Y005348D01* +X009341Y005548D01* +X008941Y005668D01* +X008501Y005708D01* +X007981Y005708D01* +X007541Y005628D01* +X007141Y005508D01* +X006741Y005268D01* +X006421Y004988D01* +X006341Y004808D01* +X006301Y004668D01* +X006305Y004681D02* +X006262Y004681D01* +X006241Y004688D02* +X006301Y005068D01* +X006361Y005208D01* +X006461Y005348D01* +X006621Y005528D01* +X006781Y005668D01* +X007181Y005908D01* +X007621Y006028D01* +X008021Y006108D01* +X008381Y006148D01* +X008941Y006068D01* +X009501Y005828D01* +X009541Y005788D01* +X009501Y005708D01* +X009501Y005628D01* +X009541Y005508D01* +X009661Y005468D01* +X009781Y005468D01* +X009901Y005548D01* +X010061Y005348D01* +X010221Y005028D01* +X010261Y004828D01* +X010261Y004508D01* +X010258Y004524D02* +X010261Y004524D01* +X010261Y004603D02* +X010240Y004603D01* +X010223Y004681D02* +X010261Y004681D01* +X010261Y004760D02* +X010205Y004760D01* +X010188Y004838D02* +X010259Y004838D01* +X010243Y004917D02* +X010140Y004917D01* +X010072Y004996D02* +X010228Y004996D01* +X010198Y005074D02* +X010005Y005074D01* +X010040Y005074D02* +X009933Y005074D01* +X009935Y005153D02* +X010159Y005153D01* +X010120Y005231D02* +X009825Y005231D01* +X009715Y005310D02* +X010081Y005310D01* +X010029Y005388D02* +X009597Y005388D01* +X009537Y005388D02* +X009437Y005388D01* +X009433Y005310D02* +X009529Y005310D01* +X009521Y005231D02* +X009429Y005231D01* +X009425Y005153D02* +X009514Y005153D01* +X009506Y005074D02* +X009421Y005074D01* +X009316Y004996D02* +X009550Y004996D01* +X009621Y004917D02* +X009210Y004917D01* +X009181Y004838D02* +X009371Y004838D01* +X009348Y004760D02* +X009192Y004760D01* +X009021Y004788D02* +X008861Y004668D01* +X008801Y004828D02* +X008701Y004748D01* +X008661Y004996D02* +X008864Y004996D01* +X008898Y005074D02* +X008661Y005074D01* +X008705Y005153D02* +X009141Y005153D01* +X009057Y005231D02* +X008807Y005231D01* +X008921Y005310D02* +X009007Y005310D01* +X009018Y005388D02* +X008926Y005388D01* +X008931Y005467D02* +X009026Y005467D01* +X009034Y005545D02* +X008936Y005545D01* +X008941Y005624D02* +X009041Y005624D01* +X009089Y005624D02* +X009503Y005624D01* +X009501Y005702D02* +X008567Y005702D01* +X008541Y005624D02* +X008421Y005624D01* +X008421Y005545D02* +X008541Y005545D01* +X008545Y005467D02* +X008412Y005467D01* +X008463Y005310D02* +X008653Y005310D01* +X008651Y005388D02* +X008234Y005388D01* +X008021Y005228D02* +X008021Y005108D01* +X007981Y004948D01* +X007721Y005248D02* +X007601Y005188D01* +X007081Y005508D01* +X007201Y005568D01* +X007721Y005248D01* +X007687Y005231D02* +X007532Y005231D01* +X007621Y005310D02* +X007404Y005310D01* +X007494Y005388D02* +X007276Y005388D01* +X007366Y005467D02* +X007149Y005467D01* +X007072Y005467D02* +X006566Y005467D01* +X006497Y005388D02* +X006941Y005388D01* +X006901Y005388D02* +X007045Y005388D01* +X006955Y005310D02* +X007171Y005310D01* +X007081Y005231D02* +X007297Y005231D01* +X007206Y005153D02* +X007422Y005153D01* +X007353Y005074D02* +X007332Y005074D01* +X007221Y005008D02* +X007081Y004948D01* +X006641Y005208D01* +X006741Y005288D01* +X007221Y005008D01* +X007191Y004996D02* +X007001Y004996D01* +X007109Y005074D02* +X006868Y005074D01* +X006974Y005153D02* +X006736Y005153D01* +X006699Y005231D02* +X006377Y005231D01* +X006337Y005153D02* +X006609Y005153D01* +X006670Y005231D02* +X006839Y005231D01* +X006810Y005310D02* +X006434Y005310D01* +X006519Y005074D02* +X006304Y005074D01* +X005661Y005628D02* +X005625Y005583D01* +X005591Y005535D01* +X005561Y005486D01* +X005534Y005434D01* +X005510Y005381D01* +X005490Y005327D01* +X005474Y005271D01* +X005461Y005215D01* +X005451Y005157D01* +X005446Y005100D01* +X005444Y005041D01* +X005446Y004983D01* +X005452Y004926D01* +X005461Y004868D01* +X005501Y004828D01* +X005541Y004828D01* +X005621Y004868D01* +X005550Y004917D02* +X005501Y004917D01* +X005501Y004868D02* +X005661Y005028D01* +X005741Y005308D01* +X005661Y005548D01* +X005501Y005228D01* +X005501Y004868D01* +X005501Y004996D02* +X005628Y004996D01* +X005674Y005074D02* +X005501Y005074D01* +X005501Y005153D02* +X005697Y005153D01* +X005719Y005231D02* +X005503Y005231D01* +X005542Y005310D02* +X005741Y005310D01* +X005715Y005388D02* +X005581Y005388D01* +X005620Y005467D02* +X005688Y005467D01* +X005660Y005545D02* +X005662Y005545D01* +X005661Y005628D02* +X005701Y005628D01* +X005582Y005708D02* +X005595Y005727D01* +X005610Y005744D01* +X005628Y005758D01* +X005648Y005770D01* +X005670Y005778D01* +X005692Y005784D01* +X005715Y005786D01* +X005738Y005785D01* +X005761Y005780D01* +X005782Y005772D01* +X005803Y005761D01* +X005821Y005748D01* +X005847Y005719D01* +X005869Y005687D01* +X005890Y005654D01* +X005907Y005619D01* +X005921Y005583D01* +X005933Y005546D01* +X005941Y005508D01* +X006290Y004996D02* +X006429Y004996D01* +X006389Y004917D02* +X006277Y004917D01* +X006265Y004838D02* +X006355Y004838D01* +X006327Y004760D02* +X006252Y004760D01* +X006605Y004524D02* +X006622Y004524D01* +X007021Y004148D02* +X007481Y003928D01* +X007381Y003868D01* +X007221Y003848D01* +X007061Y003948D01* +X007001Y004048D01* +X007021Y004148D01* +X007018Y004132D02* +X007056Y004132D01* +X007002Y004053D02* +X007221Y004053D01* +X007145Y003896D02* +X007427Y003896D01* +X007385Y003974D02* +X007046Y003974D01* +X007481Y004168D02* +X007561Y004208D01* +X007621Y004148D02* +X007601Y004128D01* +X007641Y004168D02* +X007701Y004088D01* +X007701Y004108D02* +X007721Y004108D01* +X007781Y004008D02* +X007801Y003988D01* +X007821Y004008D02* +X007841Y004068D01* +X007981Y003988D02* +X007921Y003928D01* +X007981Y003948D02* +X007981Y003968D01* +X008061Y003888D02* +X008081Y003888D01* +X008121Y003868D02* +X008121Y003928D01* +X008241Y003888D02* +X008241Y003828D01* +X008261Y003808D02* +X008341Y003808D01* +X008301Y003788D02* +X008301Y003768D01* +X008441Y003708D02* +X008481Y003748D01* +X008601Y003648D02* +X008721Y003628D01* +X008661Y003588D02* +X008641Y003628D01* +X008521Y003448D02* +X008401Y003368D01* +X008241Y003388D01* +X008101Y003468D01* +X008061Y003568D01* +X008101Y003648D01* +X008521Y003448D01* +X008486Y003425D02* +X008178Y003425D01* +X008087Y003503D02* +X008406Y003503D01* +X008241Y003582D02* +X008068Y003582D01* +X007976Y003660D02* +X007649Y003660D01* +X007661Y003648D02* +X007841Y003588D01* +X007941Y003608D01* +X007981Y003668D01* +X007541Y003908D01* +X007521Y003788D01* +X007661Y003648D01* +X007571Y003739D02* +X007852Y003739D01* +X007708Y003817D02* +X007526Y003817D01* +X007539Y003896D02* +X007564Y003896D01* +X007481Y004168D02* +X007481Y004248D01* +X005261Y004548D02* +X005258Y004635D01* +X005259Y004723D01* +X005265Y004810D01* +X005274Y004897D01* +X005288Y004984D01* +X005305Y005069D01* +X005327Y005154D01* +X005352Y005238D01* +X005381Y005320D01* +X005413Y005401D01* +X005450Y005481D01* +X005490Y005559D01* +X005534Y005634D01* +X005581Y005708D01* +X005701Y005628D02* +X005724Y005583D01* +X005742Y005536D01* +X005758Y005487D01* +X005769Y005438D01* +X005777Y005388D01* +X005781Y005337D01* +X005782Y005286D01* +X005778Y005236D01* +X005771Y005185D01* +X005760Y005136D01* +X005746Y005087D01* +X005727Y005040D01* +X005706Y004994D01* +X005681Y004950D01* +X005653Y004908D01* +X005621Y004868D01* +X005261Y004548D02* +X005266Y004514D01* +X005273Y004481D01* +X005284Y004448D01* +X005298Y004417D01* +X005315Y004387D01* +X005334Y004358D01* +X005357Y004332D01* +X005381Y004308D01* +X005382Y004308D02* +X005400Y004295D01* +X005421Y004284D01* +X005442Y004276D01* +X005465Y004271D01* +X005488Y004270D01* +X005511Y004272D01* +X005533Y004278D01* +X005555Y004286D01* +X005575Y004298D01* +X005593Y004312D01* +X005608Y004329D01* +X005621Y004348D01* +X004421Y004428D02* +X004412Y004486D01* +X004406Y004543D01* +X004404Y004601D01* +X004406Y004660D01* +X004411Y004717D01* +X004421Y004775D01* +X004434Y004831D01* +X004450Y004887D01* +X004470Y004941D01* +X004494Y004994D01* +X004521Y005046D01* +X004551Y005095D01* +X004585Y005143D01* +X004621Y005188D01* +X004661Y005188D01* +X004542Y005268D02* +X004555Y005287D01* +X004570Y005304D01* +X004588Y005318D01* +X004608Y005330D01* +X004630Y005338D01* +X004652Y005344D01* +X004675Y005346D01* +X004698Y005345D01* +X004721Y005340D01* +X004742Y005332D01* +X004763Y005321D01* +X004781Y005308D01* +X005941Y005509D02* +X005955Y005444D01* +X005966Y005379D01* +X005974Y005313D01* +X005977Y005247D01* +X005978Y005180D01* +X005974Y005114D01* +X005967Y005048D01* +X005957Y004983D01* +X005943Y004918D01* +X005925Y004855D01* +X005904Y004792D01* +X005879Y004730D01* +X005852Y004670D01* +X005821Y004612D01* +X005787Y004555D01* +X005749Y004500D01* +X005709Y004447D01* +X005666Y004397D01* +X005621Y004349D01* +X004541Y005268D02* +X004494Y005194D01* +X004450Y005119D01* +X004410Y005041D01* +X004373Y004961D01* +X004341Y004880D01* +X004312Y004798D01* +X004287Y004714D01* +X004265Y004629D01* +X004248Y004544D01* +X004234Y004457D01* +X004225Y004370D01* +X004219Y004283D01* +X004218Y004195D01* +X004221Y004108D01* +X004342Y003868D02* +X004360Y003855D01* +X004381Y003844D01* +X004402Y003836D01* +X004425Y003831D01* +X004448Y003830D01* +X004471Y003832D01* +X004493Y003838D01* +X004515Y003846D01* +X004535Y003858D01* +X004553Y003872D01* +X004568Y003889D01* +X004581Y003908D01* +X004341Y003868D02* +X004317Y003892D01* +X004294Y003918D01* +X004275Y003947D01* +X004258Y003977D01* +X004244Y004008D01* +X004233Y004041D01* +X004226Y004074D01* +X004221Y004108D01* +X004461Y004388D02* +X004501Y004388D01* +X004581Y004428D01* +X004557Y004524D02* +X004461Y004524D01* +X004461Y004446D02* +X004479Y004446D01* +X004461Y004428D02* +X004621Y004588D01* +X004701Y004868D01* +X004621Y005108D01* +X004461Y004788D01* +X004461Y004428D01* +X004421Y004428D02* +X004461Y004388D01* +X004461Y004603D02* +X004625Y004603D01* +X004648Y004681D02* +X004461Y004681D01* +X004461Y004760D02* +X004670Y004760D01* +X004693Y004838D02* +X004486Y004838D01* +X004526Y004917D02* +X004685Y004917D01* +X004659Y004996D02* +X004565Y004996D01* +X004604Y005074D02* +X004633Y005074D01* +X004901Y005068D02* +X004893Y005106D01* +X004881Y005143D01* +X004867Y005179D01* +X004850Y005214D01* +X004829Y005247D01* +X004807Y005279D01* +X004781Y005308D01* +X004661Y005188D02* +X004684Y005143D01* +X004702Y005096D01* +X004718Y005047D01* +X004729Y004998D01* +X004737Y004948D01* +X004741Y004897D01* +X004742Y004846D01* +X004738Y004796D01* +X004731Y004745D01* +X004720Y004696D01* +X004706Y004647D01* +X004687Y004600D01* +X004666Y004554D01* +X004641Y004510D01* +X004613Y004468D01* +X004581Y004428D01* +X004581Y003909D02* +X004626Y003957D01* +X004669Y004007D01* +X004709Y004060D01* +X004747Y004115D01* +X004781Y004172D01* +X004812Y004230D01* +X004839Y004290D01* +X004864Y004352D01* +X004885Y004415D01* +X004903Y004478D01* +X004917Y004543D01* +X004927Y004608D01* +X004934Y004674D01* +X004938Y004740D01* +X004937Y004807D01* +X004934Y004873D01* +X004926Y004939D01* +X004915Y005004D01* +X004901Y005069D01* +X003821Y005538D02* +X004061Y007188D01* +X004221Y007948D01* +X007701Y009748D01* +X008981Y008908D01* +X009008Y009032D02* +X009075Y009025D01* +X009135Y009031D01* +X009192Y009048D01* +X009245Y009076D01* +X009291Y009114D01* +X009329Y009160D01* +X009357Y009213D01* +X009374Y009270D01* +X009380Y009330D01* +X008728Y009330D01* +X008728Y009547D01* +X008293Y009547D01* +X008301Y009656D01* +X008327Y009762D01* +X008369Y009863D01* +X008426Y009956D01* +X008497Y010039D01* +X008580Y010110D01* +X008673Y010167D01* +X008774Y010209D01* +X008880Y010234D01* +X008989Y010243D01* +X009293Y010243D01* +X009554Y010199D01* +X009771Y010112D01* +X009771Y009808D01* +X009554Y009938D01* +X009293Y009982D01* +X009119Y009982D01* +X009051Y009976D01* +X008985Y009958D01* +X008923Y009929D01* +X008867Y009890D01* +X008819Y009842D01* +X008780Y009786D01* +X008751Y009724D01* +X008734Y009658D01* +X008728Y009590D01* +X009858Y009590D01* +X009858Y009503D01* +X009848Y009381D01* +X009820Y009262D01* +X009773Y009148D01* +X009709Y009043D01* +X009629Y008950D01* +X009536Y008870D01* +X009431Y008806D01* +X009317Y008759D01* +X009198Y008730D01* +X009075Y008721D01* +X009032Y008721D01* +X008916Y008730D01* +X008804Y008757D01* +X008696Y008801D01* +X008598Y008862D01* +X008509Y008937D01* +X008434Y009026D01* +X008373Y009124D01* +X008329Y009232D01* +X008302Y009344D01* +X008293Y009460D01* +X008293Y009547D01* +X008728Y009547D01* +X008728Y009373D01* +X008734Y009305D01* +X008754Y009240D01* +X008786Y009180D01* +X008830Y009127D01* +X008882Y009084D01* +X008942Y009052D01* +X009008Y009032D01* +X008890Y009080D02* +X008401Y009080D01* +X008359Y009158D02* +X008804Y009158D01* +X008756Y009237D02* +X008328Y009237D01* +X008309Y009315D02* +X008733Y009315D01* +X008728Y009394D02* +X009849Y009394D01* +X009856Y009472D02* +X008728Y009472D01* +X008293Y009472D01* +X008298Y009394D02* +X008728Y009394D01* +X008731Y009630D02* +X008299Y009630D01* +X008293Y009551D02* +X009858Y009551D01* +X010162Y009547D02* +X010119Y009808D01* +X010206Y010025D01* +X010336Y010156D01* +X010510Y010199D01* +X010858Y010199D01* +X010989Y010156D01* +X011163Y009982D01* +X011163Y010199D01* +X011467Y010199D01* +X011597Y010199D01* +X011597Y009199D01* +X011554Y009112D01* +X011467Y008982D01* +X011336Y008851D01* +X011206Y008764D01* +X011032Y008721D01* +X010684Y008721D01* +X010510Y008764D01* +X010249Y008851D01* +X010249Y009112D01* +X010380Y009069D01* +X010554Y009025D01* +X010989Y009025D01* +X011076Y009069D01* +X011163Y009156D01* +X011163Y009286D01* +X010728Y009286D01* +X010467Y009330D01* +X010293Y009416D01* +X010162Y009547D01* +X010162Y009551D02* +X010680Y009551D01* +X010684Y009547D02* +X011163Y009547D01* +X011467Y010199D01* +X011163Y009590D01* +X011163Y009764D01* +X010989Y009938D01* +X010858Y009982D01* +X010728Y009982D01* +X010597Y009895D01* +X010554Y009764D01* +X010597Y009634D01* +X010684Y009547D01* +X010602Y009630D02* +X010149Y009630D01* +X010136Y009708D02* +X010572Y009708D01* +X010561Y009787D02* +X010123Y009787D01* +X010142Y009865D02* +X010587Y009865D01* +X010671Y009944D02* +X010173Y009944D01* +X010205Y010022D02* +X011122Y010022D01* +X011163Y010022D02* +X011378Y010022D01* +X011384Y010022D02* +X011597Y010022D01* +X011597Y009944D02* +X011348Y009944D01* +X011339Y009944D02* +X010972Y009944D01* +X011062Y009865D02* +X011300Y009865D01* +X011311Y009865D02* +X011597Y009865D01* +X011597Y009787D02* +X011274Y009787D01* +X011261Y009787D02* +X011140Y009787D01* +X011163Y009708D02* +X011221Y009708D01* +X011238Y009708D02* +X011597Y009708D01* +X011597Y009630D02* +X011201Y009630D01* +X011182Y009630D02* +X011163Y009630D01* +X011164Y009551D02* +X011597Y009551D01* +X011597Y009472D02* +X010237Y009472D01* +X010338Y009394D02* +X011597Y009394D01* +X011597Y009315D02* +X010552Y009315D01* +X010347Y009080D02* +X010249Y009080D01* +X010249Y009001D02* +X011480Y009001D01* +X011532Y009080D02* +X011087Y009080D01* +X011163Y009158D02* +X011577Y009158D01* +X011597Y009237D02* +X011163Y009237D01* +X011408Y008923D02* +X010249Y008923D01* +X010271Y008844D02* +X011326Y008844D01* +X011208Y008766D02* +X010506Y008766D01* +X008941Y008749D02* +X008933Y008677D01* +X008929Y008604D01* +X008928Y008532D01* +X008932Y008459D01* +X008940Y008387D01* +X008952Y008316D01* +X008967Y008245D01* +X008986Y008175D01* +X009010Y008106D01* +X009036Y008039D01* +X009067Y007973D01* +X009101Y007909D01* +X009661Y008988D02* +X009722Y008973D01* +X009782Y008955D01* +X009841Y008933D01* +X009899Y008907D01* +X009955Y008878D01* +X010009Y008846D01* +X010061Y008811D01* +X010111Y008773D01* +X010158Y008732D01* +X010203Y008688D01* +X010245Y008641D01* +X010285Y008592D01* +X010322Y008541D01* +X010355Y008488D01* +X010385Y008433D01* +X010412Y008376D01* +X010436Y008318D01* +X010456Y008258D01* +X010472Y008197D01* +X010485Y008136D01* +X010494Y008073D01* +X010499Y008011D01* +X010501Y007948D01* +X010541Y007948D02* +X011381Y007428D01* +X011381Y004988D01* +X011341Y004268D01* +X011301Y003908D01* +X011181Y003548D01* +X011061Y003428D01* +X010901Y003388D01* +X009981Y004308D02* +X010021Y004388D01* +X010021Y004508D01* +X010037Y004524D02* +X010085Y004524D01* +X010003Y004760D02* +X009867Y004760D01* +X009909Y004838D02* +X010012Y004838D01* +X010021Y004917D02* +X009917Y004917D01* +X009925Y004996D02* +X010030Y004996D01* +X009966Y005467D02* +X009472Y005467D01* +X009445Y005467D02* +X009441Y005467D01* +X009529Y005545D02* +X009346Y005545D01* +X009541Y005748D02* +X009741Y005908D01* +X009701Y006068D01* +X009741Y006228D01* +X010221Y006708D01* +X011061Y007428D01* +X011461Y007668D01* +X011701Y007788D01* +X012101Y007828D01* +X012341Y007788D01* +X012421Y007668D01* +X012461Y007468D01* +X012421Y007268D01* +X012261Y007028D01* +X011861Y006708D01* +X011381Y006308D01* +X010861Y005948D01* +X010381Y005668D01* +X010221Y005628D01* +X009981Y005668D01* +X009821Y005748D01* +X009741Y005908D01* +X009734Y005938D02* +X010844Y005938D01* +X010960Y006017D02* +X009714Y006017D01* +X009708Y006095D02* +X011073Y006095D01* +X011187Y006174D02* +X009727Y006174D01* +X009765Y006252D02* +X011300Y006252D01* +X011408Y006331D02* +X009844Y006331D01* +X009922Y006409D02* +X011502Y006409D01* +X011597Y006488D02* +X010001Y006488D01* +X010079Y006566D02* +X011691Y006566D01* +X011785Y006645D02* +X010158Y006645D01* +X010239Y006723D02* +X011880Y006723D01* +X011978Y006802D02* +X010330Y006802D01* +X010422Y006881D02* +X012076Y006881D01* +X012175Y006959D02* +X010514Y006959D01* +X010605Y007038D02* +X012267Y007038D01* +X012320Y007116D02* +X010697Y007116D01* +X010789Y007195D02* +X012372Y007195D01* +X012422Y007273D02* +X010880Y007273D01* +X010972Y007352D02* +X012438Y007352D01* +X012454Y007430D02* +X011065Y007430D01* +X011195Y007509D02* +X012453Y007509D01* +X012437Y007587D02* +X011326Y007587D01* +X011457Y007666D02* +X012422Y007666D01* +X012370Y007745D02* +X011614Y007745D01* +X012048Y007823D02* +X012133Y007823D01* +X012076Y008764D02* +X012510Y008764D01* +X012510Y008982D01* +X012597Y008895D01* +X012728Y008808D01* +X012902Y008764D01* +X013032Y008764D01* +X013206Y008808D01* +X013336Y008895D01* +X013423Y008982D01* +X013510Y008982D01* +X013597Y008851D01* +X013728Y008764D01* +X014119Y008764D01* +X014206Y008808D01* +X014337Y008938D01* +X014380Y009069D01* +X014380Y010199D01* +X013989Y010199D01* +X013989Y009199D01* +X013902Y009112D01* +X013815Y009069D01* +X013684Y009069D01* +X013554Y009112D01* +X013467Y009286D01* +X013467Y010199D01* +X013032Y010199D01* +X013032Y009199D01* +X012945Y009112D01* +X012858Y009069D01* +X012684Y009069D01* +X012597Y009112D01* +X012510Y009286D01* +X012510Y010199D01* +X012076Y010199D01* +X012076Y008764D01* +X012076Y008766D02* +X012510Y008766D01* +X012510Y008844D02* +X012076Y008844D01* +X012076Y008923D02* +X012510Y008923D01* +X012569Y008923D02* +X013364Y008923D01* +X013261Y008844D02* +X012673Y008844D01* +X012662Y009080D02* +X012076Y009080D01* +X012076Y009158D02* +X012574Y009158D01* +X012535Y009237D02* +X012076Y009237D01* +X012076Y009315D02* +X012510Y009315D01* +X012510Y009394D02* +X012076Y009394D01* +X012076Y009472D02* +X012510Y009472D01* +X012510Y009551D02* +X012076Y009551D01* +X012076Y009630D02* +X012510Y009630D01* +X012510Y009708D02* +X012076Y009708D01* +X012076Y009787D02* +X012510Y009787D01* +X012510Y009865D02* +X012076Y009865D01* +X012076Y009944D02* +X012510Y009944D01* +X012510Y010022D02* +X012076Y010022D01* +X012076Y010101D02* +X012510Y010101D01* +X012510Y010179D02* +X012076Y010179D01* +X011597Y010179D02* +X011458Y010179D01* +X011457Y010179D02* +X011163Y010179D01* +X011163Y010101D02* +X011418Y010101D01* +X011421Y010101D02* +X011597Y010101D01* +X011043Y010101D02* +X010282Y010101D01* +X010431Y010179D02* +X010917Y010179D01* +X009833Y009315D02* +X009378Y009315D01* +X009364Y009237D02* +X009810Y009237D01* +X009777Y009158D02* +X009327Y009158D01* +X009249Y009080D02* +X009731Y009080D01* +X009673Y009001D02* +X008455Y009001D01* +X008526Y008923D02* +X009597Y008923D01* +X009493Y008844D02* +X008627Y008844D01* +X008783Y008766D02* +X009333Y008766D01* +X009101Y007908D02* +X009124Y007855D01* +X009151Y007803D01* +X009181Y007753D01* +X009215Y007706D01* +X009252Y007661D01* +X009291Y007618D01* +X009334Y007579D01* +X009379Y007542D01* +X009426Y007508D01* +X009476Y007478D01* +X009528Y007451D01* +X009581Y007428D01* +X009627Y007407D01* +X009674Y007389D01* +X009722Y007375D01* +X009771Y007365D01* +X009821Y007359D01* +X009871Y007357D01* +X009921Y007359D01* +X009971Y007365D01* +X010020Y007375D01* +X010068Y007388D01* +X010115Y007406D01* +X010160Y007427D01* +X010204Y007451D01* +X010246Y007479D01* +X010285Y007511D01* +X010321Y007545D01* +X010355Y007582D01* +X010386Y007622D01* +X010413Y007664D01* +X010437Y007708D01* +X010458Y007753D01* +X010474Y007800D01* +X010487Y007849D01* +X010497Y007898D01* +X010502Y007948D01* +X012076Y009001D02* +X014358Y009001D01* +X014380Y009080D02* +X013837Y009080D01* +X013948Y009158D02* +X014380Y009158D01* +X014380Y009237D02* +X013989Y009237D01* +X013989Y009315D02* +X014380Y009315D01* +X014380Y009394D02* +X013989Y009394D01* +X013989Y009472D02* +X014380Y009472D01* +X014380Y009551D02* +X013989Y009551D01* +X013989Y009630D02* +X014380Y009630D01* +X014380Y009708D02* +X013989Y009708D01* +X013989Y009787D02* +X014380Y009787D01* +X014380Y009865D02* +X013989Y009865D01* +X013989Y009944D02* +X014380Y009944D01* +X014380Y010022D02* +X013989Y010022D01* +X013989Y010101D02* +X014380Y010101D01* +X014380Y010179D02* +X013989Y010179D01* +X013467Y010179D02* +X013032Y010179D01* +X013032Y010101D02* +X013467Y010101D01* +X013467Y010022D02* +X013032Y010022D01* +X013032Y009944D02* +X013467Y009944D01* +X013467Y009865D02* +X013032Y009865D01* +X013032Y009787D02* +X013467Y009787D01* +X013467Y009708D02* +X013032Y009708D01* +X013032Y009630D02* +X013467Y009630D01* +X013467Y009551D02* +X013032Y009551D01* +X013032Y009472D02* +X013467Y009472D01* +X013467Y009394D02* +X013032Y009394D01* +X013032Y009315D02* +X013467Y009315D01* +X013492Y009237D02* +X013032Y009237D01* +X012991Y009158D02* +X013531Y009158D01* +X013651Y009080D02* +X012880Y009080D01* +X012896Y008766D02* +X013037Y008766D01* +X013550Y008923D02* +X014321Y008923D01* +X014242Y008844D02* +X013608Y008844D01* +X013726Y008766D02* +X014122Y008766D01* +X016026Y009568D02* +X016276Y009568D01* +X010709Y005859D02* +X009766Y005859D01* +X009805Y005781D02* +X010574Y005781D01* +X010440Y005702D02* +X009913Y005702D01* +X009897Y005545D02* +X009904Y005545D01* +X009861Y005508D02* +X010101Y005708D01* +X009537Y005781D02* +X006969Y005781D01* +X007100Y005859D02* +X009429Y005859D01* +X009245Y005938D02* +X007290Y005938D01* +X005102Y007829D02* +X005012Y007726D01* +X004926Y007620D01* +X004847Y007510D01* +X004772Y007396D01* +X004703Y007279D01* +X004639Y007158D01* +X004582Y007035D01* +X004530Y006909D01* +X004484Y006780D01* +X004445Y006650D01* +X004411Y006518D01* +X004384Y006385D01* +X004364Y006250D01* +X004349Y006115D01* +X004342Y005979D01* +X003821Y005538D02* +X003621Y004388D01* +X003621Y003988D01* +X003641Y003808D01* +X003661Y003748D01* +X003741Y003668D01* +X003901Y003528D01* +X006641Y005545D02* +X007264Y005545D01* +X007239Y005545D02* +X007155Y005545D01* +X007181Y005788D02* +X007141Y005988D01* +X007181Y006668D01* +X007261Y007268D01* +X007461Y008268D01* +X007380Y008634D02* +X006728Y008634D01* +X006728Y008286D01* +X008467Y008286D01* +X008467Y008634D01* +X007815Y008634D01* +X007815Y010199D01* +X007380Y010199D01* +X007380Y008634D01* +X007380Y008687D02* +X007815Y008687D01* +X007815Y008766D02* +X007380Y008766D01* +X007380Y008844D02* +X007815Y008844D01* +X007815Y008923D02* +X007380Y008923D01* +X007380Y009001D02* +X007815Y009001D01* +X007815Y009080D02* +X007380Y009080D01* +X007380Y009158D02* +X007815Y009158D01* +X007815Y009237D02* +X007380Y009237D01* +X007380Y009315D02* +X007815Y009315D01* +X007815Y009394D02* +X007380Y009394D01* +X007380Y009472D02* +X007815Y009472D01* +X007815Y009551D02* +X007380Y009551D01* +X007380Y009630D02* +X007815Y009630D01* +X007815Y009708D02* +X007380Y009708D01* +X007380Y009787D02* +X007815Y009787D01* +X007815Y009865D02* +X007380Y009865D01* +X007380Y009944D02* +X007815Y009944D01* +X007815Y010022D02* +X007380Y010022D01* +X007380Y010101D02* +X007815Y010101D01* +X007815Y010179D02* +X007380Y010179D01* +X006433Y009827D02* +X006458Y009732D01* +X006467Y009634D01* +X006467Y009590D01* +X006461Y009522D01* +X006443Y009457D01* +X006414Y009395D01* +X006375Y009339D01* +X006327Y009291D01* +X006271Y009252D01* +X006209Y009223D01* +X006143Y009205D01* +X006075Y009199D01* +X006135Y009193D01* +X006192Y009176D01* +X006244Y009148D01* +X006291Y009110D01* +X006328Y009064D01* +X006357Y009011D01* +X006374Y008954D01* +X006380Y008895D01* +X006380Y008634D01* +X006368Y008562D01* +X006343Y008494D01* +X006305Y008432D01* +X006256Y008379D01* +X006197Y008336D01* +X006131Y008306D01* +X006061Y008289D01* +X005988Y008286D01* +X005293Y008286D01* +X005293Y008590D01* +X005771Y008590D01* +X005810Y008595D01* +X005846Y008608D01* +X005879Y008628D01* +X005907Y008656D01* +X005928Y008689D01* +X005941Y008726D01* +X005945Y008764D01* +X005945Y008808D01* +X005941Y008850D01* +X005928Y008891D01* +X005908Y008929D01* +X005881Y008962D01* +X005848Y008989D01* +X005811Y009009D01* +X005770Y009021D01* +X005728Y009025D01* +X005293Y009025D01* +X005293Y009330D01* +X005727Y009330D01* +X005778Y009335D01* +X005827Y009349D01* +X005872Y009374D01* +X005912Y009406D01* +X005944Y009445D01* +X005969Y009491D01* +X005983Y009540D01* +X005988Y009590D01* +X005988Y009634D01* +X005983Y009685D01* +X005969Y009734D01* +X005944Y009779D01* +X005912Y009818D01* +X005872Y009851D01* +X005827Y009875D01* +X005778Y009890D01* +X005728Y009895D01* +X005293Y009895D01* +X005293Y010199D01* +X005901Y010199D01* +X006000Y010191D01* +X006095Y010165D01* +X006184Y010123D01* +X006265Y010067D01* +X006334Y009997D01* +X006391Y009916D01* +X006433Y009827D01* +X006443Y009787D02* +X005938Y009787D01* +X005976Y009708D02* +X006460Y009708D01* +X006467Y009630D02* +X005988Y009630D01* +X005984Y009551D02* +X006463Y009551D01* +X006447Y009472D02* +X005959Y009472D01* +X005897Y009394D02* +X006414Y009394D01* +X006352Y009315D02* +X005293Y009315D01* +X004858Y009315D01* +X004858Y009237D02* +X005293Y009237D01* +X006240Y009237D01* +X006225Y009158D02* +X005293Y009158D01* +X004858Y009158D01* +X004858Y009080D02* +X005293Y009080D01* +X006315Y009080D01* +X006360Y009001D02* +X005825Y009001D01* +X005911Y008923D02* +X006377Y008923D01* +X006380Y008844D02* +X005941Y008844D01* +X005945Y008766D02* +X006380Y008766D01* +X006380Y008687D02* +X005927Y008687D01* +X005848Y008609D02* +X006376Y008609D01* +X006356Y008530D02* +X005293Y008530D01* +X004858Y008530D01* +X004858Y008609D02* +X005293Y008609D01* +X005293Y008687D02* +X004858Y008687D01* +X004858Y008766D02* +X005293Y008766D01* +X005293Y008844D02* +X004858Y008844D01* +X004858Y008923D02* +X005293Y008923D01* +X005293Y009001D02* +X004858Y009001D01* +X004858Y009394D02* +X005293Y009394D01* +X005293Y009472D02* +X004858Y009472D01* +X004858Y009551D02* +X005293Y009551D01* +X005293Y009630D02* +X004858Y009630D01* +X004858Y009708D02* +X005293Y009708D01* +X005293Y009787D02* +X004858Y009787D01* +X004858Y009865D02* +X005293Y009865D01* +X005293Y009944D02* +X006372Y009944D01* +X006415Y009865D02* +X005846Y009865D01* +X006041Y010179D02* +X005293Y010179D01* +X004858Y010179D01* +X004858Y010199D02* +X005293Y010199D01* +X005293Y008286D01* +X004858Y008286D01* +X004858Y010199D01* +X004858Y010101D02* +X005293Y010101D01* +X006216Y010101D01* +X006309Y010022D02* +X005293Y010022D01* +X004858Y010022D01* +X004858Y009944D02* +X005293Y009944D01* +X004329Y009332D02* +X004373Y009264D01* +X004405Y009190D01* +X004423Y009112D01* +X004423Y008721D01* +X004412Y008642D01* +X004388Y008566D01* +X004351Y008495D01* +X004302Y008432D01* +X004243Y008379D01* +X004176Y008336D01* +X004102Y008306D01* +X004024Y008289D01* +X003945Y008286D01* +X003336Y008286D01* +X003336Y008590D01* +X003727Y008590D01* +X003787Y008596D01* +X003844Y008614D01* +X003896Y008642D01* +X003943Y008680D01* +X003980Y008726D01* +X004009Y008778D01* +X004026Y008835D01* +X004032Y008895D01* +X004032Y008938D01* +X004027Y008989D01* +X004012Y009038D01* +X003988Y009083D01* +X003955Y009123D01* +X003916Y009155D01* +X003871Y009179D01* +X003822Y009194D01* +X003771Y009199D01* +X003336Y009199D01* +X003336Y009503D01* +X003901Y009503D01* +X003982Y009508D01* +X004061Y009498D01* +X004138Y009475D01* +X004210Y009439D01* +X004274Y009390D01* +X004329Y009332D01* +X004340Y009315D02* +X003336Y009315D01* +X002901Y009315D01* +X002901Y009237D02* +X003336Y009237D01* +X004385Y009237D01* +X004412Y009158D02* +X003910Y009158D01* +X003990Y009080D02* +X004423Y009080D01* +X004423Y009001D02* +X004023Y009001D01* +X004032Y008923D02* +X004423Y008923D01* +X004423Y008844D02* +X004027Y008844D01* +X004002Y008766D02* +X004423Y008766D01* +X004419Y008687D02* +X003949Y008687D01* +X003827Y008609D02* +X004402Y008609D01* +X004369Y008530D02* +X003336Y008530D01* +X002901Y008530D01* +X002901Y008609D02* +X003336Y008609D01* +X003336Y008687D02* +X002901Y008687D01* +X002901Y008766D02* +X003336Y008766D01* +X003336Y008844D02* +X002901Y008844D01* +X002901Y008923D02* +X003336Y008923D01* +X003336Y009001D02* +X002901Y009001D01* +X002901Y009080D02* +X003336Y009080D01* +X003336Y009158D02* +X002901Y009158D01* +X002901Y009394D02* +X003336Y009394D01* +X004270Y009394D01* +X004143Y009472D02* +X003336Y009472D01* +X002901Y009472D01* +X002901Y009551D02* +X003336Y009551D01* +X003336Y009630D02* +X002901Y009630D01* +X002901Y009708D02* +X003336Y009708D01* +X003336Y009787D02* +X002901Y009787D01* +X002901Y009865D02* +X003336Y009865D01* +X003336Y009944D02* +X002901Y009944D01* +X002901Y010022D02* +X003336Y010022D01* +X003336Y010101D02* +X002901Y010101D01* +X002901Y010179D02* +X003336Y010179D01* +X003336Y010199D02* +X002901Y010199D01* +X002901Y008286D01* +X003336Y008286D01* +X003336Y010199D01* +X002597Y010199D02* +X001858Y008286D01* +X001293Y008286D01* +X000553Y010199D01* +X000988Y010199D01* +X001119Y009851D01* +X002032Y009851D01* +X002162Y010199D01* +X002597Y010199D01* +X002589Y010179D02* +X002155Y010179D01* +X002125Y010101D02* +X002559Y010101D01* +X002529Y010022D02* +X002096Y010022D01* +X002066Y009944D02* +X002498Y009944D01* +X002468Y009865D02* +X002037Y009865D01* +X002407Y009708D02* +X000743Y009708D01* +X000773Y009630D02* +X002377Y009630D01* +X002347Y009551D02* +X000804Y009551D01* +X000834Y009472D02* +X001261Y009472D01* +X001249Y009503D02* +X001553Y008721D01* +X001293Y008286D01* +X001597Y008721D01* +X001901Y009503D01* +X001249Y009503D01* +X001292Y009394D02* +X000865Y009394D01* +X000895Y009315D02* +X001322Y009315D01* +X001353Y009237D02* +X000925Y009237D01* +X000956Y009158D02* +X001383Y009158D01* +X001414Y009080D02* +X000986Y009080D01* +X001016Y009001D02* +X001444Y009001D01* +X001475Y008923D02* +X001047Y008923D01* +X001077Y008844D02* +X001505Y008844D01* +X001536Y008766D02* +X001107Y008766D01* +X001138Y008687D02* +X001533Y008687D01* +X001573Y008687D02* +X002013Y008687D01* +X002043Y008766D02* +X001614Y008766D01* +X001645Y008844D02* +X002073Y008844D01* +X002104Y008923D02* +X001675Y008923D01* +X001706Y009001D02* +X002134Y009001D01* +X002164Y009080D02* +X001736Y009080D01* +X001767Y009158D02* +X002195Y009158D01* +X002225Y009237D02* +X001798Y009237D01* +X001828Y009315D02* +X002256Y009315D01* +X002286Y009394D02* +X001859Y009394D01* +X001889Y009472D02* +X002316Y009472D01* +X002438Y009787D02* +X000713Y009787D01* +X000682Y009865D02* +X001113Y009865D01* +X001084Y009944D02* +X000652Y009944D01* +X000622Y010022D02* +X001054Y010022D01* +X001025Y010101D02* +X000591Y010101D01* +X000561Y010179D02* +X000996Y010179D01* +X001168Y008609D02* +X001486Y008609D01* +X001518Y008609D02* +X001982Y008609D01* +X001952Y008530D02* +X001463Y008530D01* +X001439Y008530D02* +X001198Y008530D01* +X001229Y008451D02* +X001392Y008451D01* +X001408Y008451D02* +X001922Y008451D01* +X001891Y008373D02* +X001353Y008373D01* +X001345Y008373D02* +X001259Y008373D01* +X001289Y008294D02* +X001298Y008294D01* +X001861Y008294D01* +X002901Y008294D02* +X003336Y008294D01* +X004048Y008294D01* +X004234Y008373D02* +X003336Y008373D01* +X002901Y008373D01* +X002901Y008451D02* +X003336Y008451D01* +X004317Y008451D01* +X004858Y008451D02* +X005293Y008451D01* +X006317Y008451D01* +X006248Y008373D02* +X005293Y008373D01* +X004858Y008373D01* +X004858Y008294D02* +X005293Y008294D01* +X006084Y008294D01* +X006728Y008294D02* +X008467Y008294D01* +X008467Y008373D02* +X006728Y008373D01* +X006728Y008451D02* +X008467Y008451D01* +X008467Y008530D02* +X006728Y008530D01* +X006728Y008609D02* +X008467Y008609D01* +X008314Y009708D02* +X008747Y009708D01* +X008780Y009787D02* +X008337Y009787D01* +X008370Y009865D02* +X008842Y009865D01* +X008954Y009944D02* +X008418Y009944D01* +X008482Y010022D02* +X009771Y010022D01* +X009771Y009944D02* +X009521Y009944D01* +X009676Y009865D02* +X009771Y009865D01* +X009771Y010101D02* +X008569Y010101D01* +X008703Y010179D02* +X009603Y010179D01* +X006741Y007388D02* +X006716Y007447D01* +X006688Y007504D01* +X006656Y007559D01* +X006621Y007612D01* +X006583Y007663D01* +X006542Y007712D01* +X006498Y007758D01* +X006451Y007801D01* +X006402Y007842D01* +X006350Y007879D01* +X006296Y007913D01* +X006240Y007944D01* +X006183Y007971D01* +X006124Y007995D01* +X006063Y008015D01* +X006002Y008032D01* +X005939Y008045D01* +X005876Y008054D01* +X005813Y008059D01* +X005749Y008060D01* +X005685Y008057D01* +X005622Y008051D01* +X005559Y008041D01* +X005497Y008027D01* +X005436Y008009D01* +X005376Y007987D01* +X005317Y007962D01* +X005260Y007934D01* +X005205Y007902D01* +X005152Y007866D01* +X005101Y007828D01* +X007578Y006017D02* +X009062Y006017D01* +X008754Y006095D02* +X007955Y006095D01* +X007949Y005702D02* +X006838Y005702D01* +X006730Y005624D02* +X007526Y005624D01* +X008541Y005228D02* +X008541Y005068D01* +X008567Y005231D02* +X008624Y005231D01* +X009541Y004748D02* +X009541Y004588D01* +X009441Y004508D01* +X009741Y004348D01* +X009441Y004508D02* +X009341Y004428D01* +D11* +X014611Y005524D02* +X014654Y005481D01* +X014828Y005654D01* +X014871Y005611D01* +X014871Y005524D01* +X014828Y005481D01* +X014654Y005481D01* +X014611Y005524D02* +X014611Y005611D01* +X014654Y005654D01* +X014828Y005654D01* +X014828Y005758D02* +X014871Y005758D01* +X014871Y005802D01* +X014828Y005802D01* +X014828Y005758D01* +X014828Y005906D02* +X014871Y005906D01* +X014871Y005949D01* +X014828Y005949D01* +X014828Y005906D01* +X014654Y006070D02* +X014828Y006244D01* +X014871Y006244D01* +X014828Y006365D02* +X014654Y006365D01* +X014611Y006408D01* +X014611Y006538D01* +X014871Y006538D01* +X014871Y006408D01* +X014828Y006365D01* +X014611Y006244D02* +X014611Y006070D01* +X014654Y006070D01* +X014654Y009151D02* +X014698Y009151D01* +X014741Y009194D01* +X014741Y009324D01* +X014611Y009324D02* +X014611Y009194D01* +X014654Y009151D01* +X014741Y009194D02* +X014784Y009151D01* +X014828Y009151D01* +X014871Y009194D01* +X014871Y009324D01* +X014611Y009324D01* +X014611Y009445D02* +X014871Y009445D01* +X014784Y009532D01* +X014871Y009619D01* +X014611Y009619D01* +X014611Y009740D02* +X014871Y009740D01* +X014741Y009740D02* +X014741Y009913D01* +X014871Y009913D02* +X014611Y009913D01* +X014611Y010320D02* +X014784Y010320D01* +X014871Y010407D01* +X014784Y010494D01* +X014611Y010494D01* +X014611Y010615D02* +X014611Y010788D01* +X014741Y010788D01* +X014698Y010702D01* +X014698Y010658D01* +X014741Y010615D01* +X014828Y010615D01* +X014871Y010658D01* +X014871Y010745D01* +X014828Y010788D01* +X012614Y010745D02* +X012614Y010571D01* +X012440Y010745D01* +X012483Y010788D01* +X012570Y010788D01* +X012614Y010745D01* +X012614Y010571D02* +X012570Y010528D01* +X012483Y010528D01* +X012440Y010571D01* +X012440Y010745D01* +X012336Y010745D02* +X012336Y010788D01* +X012293Y010788D01* +X012293Y010745D01* +X012336Y010745D01* +X012189Y010745D02* +X012189Y010788D01* +X012145Y010788D01* +X012145Y010745D01* +X012189Y010745D01* +X012024Y010571D02* +X011851Y010745D01* +X011851Y010788D01* +X011730Y010745D02* +X011686Y010788D01* +X011556Y010788D01* +X011556Y010528D01* +X011686Y010528D01* +X011730Y010571D01* +X011730Y010615D01* +X011686Y010658D01* +X011556Y010658D01* +X011686Y010658D02* +X011730Y010702D01* +X011730Y010745D01* +X011851Y010528D02* +X012024Y010528D01* +X012024Y010571D01* +X007899Y010528D02* +X007899Y010702D01* +X007813Y010788D01* +X007726Y010702D01* +X007726Y010528D01* +X007605Y010528D02* +X007431Y010528D01* +X007431Y010658D01* +X007518Y010615D01* +X007561Y010615D01* +X007605Y010658D01* +X007605Y010745D01* +X007561Y010788D01* +X007475Y010788D01* +X007431Y010745D01* +X007069Y010745D02* +X007069Y010571D01* +X007026Y010528D01* +X006895Y010528D01* +X006895Y010788D01* +X007026Y010788D01* +X007069Y010745D01* +X006774Y010788D02* +X006774Y010528D01* +X006601Y010528D02* +X006601Y010788D01* +X006480Y010745D02* +X006480Y010658D01* +X006393Y010658D01* +X006480Y010571D02* +X006436Y010528D01* +X006350Y010528D01* +X006306Y010571D01* +X006306Y010745D01* +X006350Y010788D01* +X006436Y010788D01* +X006480Y010745D01* +X006601Y010528D02* +X006774Y010788D01* +X004578Y010571D02* +X004404Y010745D01* +X004404Y010788D01* +X004300Y010788D02* +X004257Y010788D01* +X004257Y010745D01* +X004300Y010745D01* +X004300Y010788D01* +X004153Y010788D02* +X004110Y010788D01* +X004110Y010745D01* +X004153Y010745D01* +X004153Y010788D01* +X003945Y010788D02* +X003945Y010528D01* +X003815Y010658D01* +X003989Y010658D01* +X004404Y010528D02* +X004578Y010528D01* +X004578Y010571D01* +X003399Y010571D02* +X003356Y010528D01* +X003269Y010528D01* +X003226Y010571D01* +X003105Y010571D02* +X003061Y010528D01* +X002975Y010528D01* +X002931Y010571D01* +X002931Y010745D01* +X002975Y010788D01* +X003061Y010788D01* +X003105Y010745D01* +X003226Y010788D02* +X003399Y010615D01* +X003399Y010571D01* +X003399Y010788D02* +X003226Y010788D01* +X000899Y010702D02* +X000899Y010528D01* +X000726Y010528D02* +X000726Y010702D01* +X000813Y010788D01* +X000899Y010702D01* +X000605Y010658D02* +X000605Y010745D01* +X000561Y010788D01* +X000475Y010788D01* +X000431Y010745D01* +X000431Y010658D02* +X000518Y010615D01* +X000561Y010615D01* +X000605Y010658D01* +X000605Y010528D02* +X000431Y010528D01* +X000431Y010658D01* +X014611Y001995D02* +X014611Y001908D01* +X014654Y001865D01* +X014741Y001865D02* +X014741Y001952D01* +X014741Y001865D02* +X014828Y001865D01* +X014871Y001908D01* +X014871Y001995D01* +X014828Y002038D01* +X014654Y002038D01* +X014611Y001995D01* +X014611Y001744D02* +X014871Y001570D01* +X014611Y001570D01* +X014611Y001449D02* +X014611Y001319D01* +X014654Y001276D01* +X014828Y001276D01* +X014871Y001319D01* +X014871Y001449D01* +X014611Y001449D01* +X014611Y001744D02* +X014871Y001744D01* +M02* diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBS b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBS new file mode 100644 index 00000000..4d72b614 --- /dev/null +++ b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GBS @@ -0,0 +1,66 @@ +G75* +G70* +%OFA0B0*% +%FSLAX24Y24*% +%IPPOS*% +%LPD*% +%AMOC8* +5,1,8,0,0,1.08239X$1,22.5* +% +%ADD10OC8,0.0720*% +D10* +X000651Y011568D03* +X001651Y011568D03* +X002651Y011568D03* +X003651Y011568D03* +X004651Y011568D03* +X005651Y011568D03* +X006651Y011568D03* +X007651Y011568D03* +X008651Y011568D03* +X009651Y011568D03* +X010651Y011568D03* +X011651Y011568D03* +X012651Y011568D03* +X013651Y011568D03* +X014651Y011568D03* +X015651Y011568D03* +X016651Y011568D03* +X016651Y010568D03* +X015651Y010568D03* +X015651Y009568D03* +X016651Y009568D03* +X016651Y008568D03* +X015651Y008568D03* +X015651Y007568D03* +X016651Y007568D03* +X016651Y006568D03* +X015651Y006568D03* +X015651Y005568D03* +X016651Y005568D03* +X016651Y004568D03* +X015651Y004568D03* +X015651Y003568D03* +X016651Y003568D03* +X016651Y002568D03* +X015651Y002568D03* +X015651Y001568D03* +X016651Y001568D03* +X016651Y012568D03* +X015651Y012568D03* +X014651Y012568D03* +X013651Y012568D03* +X012651Y012568D03* +X011651Y012568D03* +X010651Y012568D03* +X009651Y012568D03* +X008651Y012568D03* +X007651Y012568D03* +X006651Y012568D03* +X005651Y012568D03* +X004651Y012568D03* +X003651Y012568D03* +X002651Y012568D03* +X001651Y012568D03* +X000651Y012568D03* +M02* diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTL b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTL new file mode 100644 index 00000000..2ae21b29 --- /dev/null +++ b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTL @@ -0,0 +1,3546 @@ +G75* +G70* +%OFA0B0*% +%FSLAX24Y24*% +%IPPOS*% +%LPD*% +%AMOC8* +5,1,8,0,0,1.08239X$1,22.5* +% +%ADD10C,0.0000*% +%ADD11C,0.0060*% +%ADD12R,0.0210X0.0500*% +%ADD13R,0.0500X0.0210*% +%ADD14R,0.0394X0.0433*% +%ADD15R,0.0433X0.0394*% +%ADD16R,0.2000X0.0700*% +%ADD17OC8,0.0640*% +%ADD18R,0.0138X0.0551*% +%ADD19R,0.0709X0.0748*% +%ADD20R,0.0827X0.0630*% +%ADD21C,0.0100*% +%ADD22C,0.0436*% +%ADD23C,0.0400*% +%ADD24C,0.0080*% +D10* +X000151Y001068D02* +X000151Y013068D01* +X017151Y013068D01* +X017151Y001068D01* +X000151Y001068D01* +D11* +X000956Y007323D02* +X001136Y007323D01* +X001136Y007443D02* +X000956Y007443D01* +X000956Y007323D02* +X001136Y007443D01* +X001106Y007713D02* +X001136Y007743D01* +X001136Y007803D01* +X001106Y007833D01* +X000986Y007833D01* +X000956Y007803D01* +X000956Y007743D01* +X000986Y007713D01* +X001046Y007713D01* +X001046Y007773D01* +X001136Y008083D02* +X000956Y008083D01* +X000986Y008333D02* +X000956Y008363D01* +X000956Y008423D01* +X000986Y008453D01* +X001046Y008423D02* +X001046Y008363D01* +X001016Y008333D01* +X000986Y008333D01* +X001046Y008423D02* +X001076Y008453D01* +X001106Y008453D01* +X001136Y008423D01* +X001136Y008363D01* +X001106Y008333D01* +X001136Y008703D02* +X001136Y008823D01* +X000956Y008823D01* +X000956Y008703D01* +X001046Y008763D02* +X001046Y008823D01* +X000986Y009063D02* +X001106Y009063D01* +X001136Y009093D01* +X001136Y009183D01* +X000956Y009183D01* +X000956Y009093D01* +X000986Y009063D01* +X011431Y009356D02* +X011431Y009529D01* +X011431Y009443D02* +X011691Y009443D01* +X011605Y009529D01* +X011431Y009651D02* +X011431Y009824D01* +X011431Y009737D02* +X011691Y009737D01* +X011605Y009824D01* +X011648Y009945D02* +X011475Y010119D01* +X011431Y010075D01* +X011431Y009989D01* +X011475Y009945D01* +X011648Y009945D01* +X011691Y009989D01* +X011691Y010075D01* +X011648Y010119D01* +X011475Y010119D01* +X011431Y010240D02* +X011431Y010413D01* +X011605Y010240D01* +X011648Y010240D01* +X011691Y010283D01* +X011691Y010370D01* +X011648Y010413D01* +X011931Y010370D02* +X011931Y010240D01* +X011931Y010119D02* +X012105Y010119D01* +X012105Y010075D01* +X012061Y010032D01* +X012105Y009989D01* +X012061Y009945D01* +X011931Y009945D01* +X011931Y010032D02* +X012061Y010032D01* +X012061Y009824D02* +X012105Y009781D01* +X012105Y009651D01* +X012018Y009694D02* +X011975Y009651D01* +X011931Y009694D01* +X011931Y009824D01* +X012018Y009781D02* +X012061Y009824D01* +X012018Y009781D02* +X012018Y009694D01* +X012061Y009529D02* +X012061Y009356D01* +X012105Y009235D02* +X012105Y009191D01* +X011931Y009191D01* +X011931Y009148D02* +X011931Y009235D01* +X011931Y009038D02* +X012105Y009038D01* +X012105Y008908D01* +X012061Y008865D01* +X011931Y008865D01* +X011975Y008700D02* +X011931Y008657D01* +X011975Y008700D02* +X012148Y008700D01* +X012105Y008657D02* +X012105Y008744D01* +X012105Y008547D02* +X011931Y008547D01* +X012018Y008547D02* +X012105Y008461D01* +X012105Y008417D01* +X012061Y008302D02* +X011975Y008302D01* +X011931Y008258D01* +X011931Y008172D01* +X011975Y008128D01* +X012061Y008128D01* +X012105Y008172D01* +X012105Y008258D01* +X012061Y008302D01* +X012431Y008345D02* +X012431Y008515D01* +X012488Y008572D01* +X012545Y008515D01* +X012545Y008345D01* +X012601Y008345D02* +X012431Y008345D01* +X012431Y008204D02* +X012658Y008204D01* +X012658Y008147D01* +X012601Y008090D01* +X012658Y008033D01* +X012601Y007977D01* +X012431Y007977D01* +X012431Y008090D02* +X012601Y008090D01* +X012601Y008345D02* +X012658Y008402D01* +X012658Y008515D01* +X012601Y008713D02* +X012545Y008713D01* +X012545Y008940D01* +X012601Y008940D02* +X012658Y008883D01* +X012658Y008770D01* +X012601Y008713D01* +X012431Y008770D02* +X012431Y008883D01* +X012488Y008940D01* +X012601Y008940D01* +X012771Y009082D02* +X012771Y009308D01* +X012771Y009195D02* +X012431Y009195D01* +X012235Y009191D02* +X012191Y009191D01* +X012488Y009450D02* +X012431Y009507D01* +X012431Y009677D01* +X012771Y009677D01* +X012771Y009507D01* +X012715Y009450D01* +X012658Y009450D01* +X012601Y009507D01* +X012601Y009677D01* +X012601Y009818D02* +X012545Y009875D01* +X012545Y010045D01* +X012431Y010045D02* +X012771Y010045D01* +X012771Y009875D01* +X012715Y009818D01* +X012601Y009818D01* +X012601Y009507D02* +X012545Y009450D01* +X012488Y009450D01* +X012431Y010186D02* +X012658Y010186D01* +X012771Y010300D01* +X012658Y010413D01* +X012431Y010413D01* +X012601Y010413D02* +X012601Y010186D01* +X012105Y010240D02* +X012105Y010370D01* +X012061Y010413D01* +X011975Y010413D01* +X011931Y010370D01* +D12* +X010754Y008258D03* +X010439Y008258D03* +X010124Y008258D03* +X009809Y008258D03* +X009494Y008258D03* +X009179Y008258D03* +X008864Y008258D03* +X008549Y008258D03* +X008549Y004878D03* +X008864Y004878D03* +X009179Y004878D03* +X009494Y004878D03* +X009809Y004878D03* +X010124Y004878D03* +X010439Y004878D03* +X010754Y004878D03* +D13* +X011341Y005466D03* +X011341Y005781D03* +X011341Y006096D03* +X011341Y006411D03* +X011341Y006726D03* +X011341Y007041D03* +X011341Y007356D03* +X011341Y007671D03* +X007961Y007671D03* +X007961Y007356D03* +X007961Y007041D03* +X007961Y006726D03* +X007961Y006411D03* +X007961Y006096D03* +X007961Y005781D03* +X007961Y005466D03* +D14* +X006151Y002903D03* +X006151Y002234D03* +X013151Y002234D03* +X013151Y002903D03* +D15* +X006486Y005568D03* +X005817Y005568D03* +X005817Y007568D03* +X006486Y007568D03* +X003151Y006453D03* +X003151Y005784D03* +X002151Y005784D03* +X002151Y006453D03* +D16* +X007751Y002568D03* +X011551Y002568D03* +D17* +X015651Y002568D03* +X016651Y002568D03* +X016651Y001568D03* +X015651Y001568D03* +X015651Y003568D03* +X016651Y003568D03* +X016651Y004568D03* +X015651Y004568D03* +X015651Y005568D03* +X016651Y005568D03* +X016651Y006568D03* +X015651Y006568D03* +X015651Y007568D03* +X016651Y007568D03* +X016651Y008568D03* +X015651Y008568D03* +X015651Y009568D03* +X016651Y009568D03* +X016651Y010568D03* +X015651Y010568D03* +X015651Y011568D03* +X016651Y011568D03* +X016651Y012568D03* +X015651Y012568D03* +X014651Y012568D03* +X013651Y012568D03* +X012651Y012568D03* +X011651Y012568D03* +X010651Y012568D03* +X009651Y012568D03* +X008651Y012568D03* +X007651Y012568D03* +X006651Y012568D03* +X005651Y012568D03* +X004651Y012568D03* +X003651Y012568D03* +X002651Y012568D03* +X001651Y012568D03* +X000651Y012568D03* +X000651Y011568D03* +X001651Y011568D03* +X002651Y011568D03* +X003651Y011568D03* +X004651Y011568D03* +X005651Y011568D03* +X006651Y011568D03* +X007651Y011568D03* +X008651Y011568D03* +X009651Y011568D03* +X010651Y011568D03* +X011651Y011568D03* +X012651Y011568D03* +X013651Y011568D03* +X014651Y011568D03* +D18* +X003157Y002604D03* +X002901Y002604D03* +X002645Y002604D03* +X002389Y002604D03* +X002133Y002604D03* +D19* +X001070Y001560D03* +X004220Y001560D03* +D20* +X003866Y002564D03* +X001425Y002564D03* +D21* +X001695Y003029D02* +X000949Y003029D01* +X000861Y002941D01* +X000301Y002941D01* +X000301Y003039D02* +X001685Y003039D01* +X001695Y003029D02* +X001654Y003070D01* +X001601Y003199D01* +X001601Y003338D01* +X001654Y003467D01* +X001751Y003563D01* +X001751Y004485D01* +X000301Y005936D01* +X000301Y001218D01* +X000566Y001218D01* +X000566Y001997D01* +X000654Y002084D01* +X001301Y002084D01* +X001301Y002099D01* +X000949Y002099D01* +X000861Y002187D01* +X000861Y002941D01* +X000861Y002842D02* +X000301Y002842D01* +X000301Y002744D02* +X000861Y002744D01* +X000861Y002645D02* +X000301Y002645D01* +X000301Y002546D02* +X000861Y002546D01* +X000861Y002448D02* +X000301Y002448D01* +X000301Y002349D02* +X000861Y002349D01* +X000861Y002251D02* +X000301Y002251D01* +X000301Y002152D02* +X000896Y002152D01* +X000623Y002054D02* +X000301Y002054D01* +X000301Y001955D02* +X000566Y001955D01* +X000566Y001857D02* +X000301Y001857D01* +X000301Y001758D02* +X000566Y001758D01* +X000566Y001660D02* +X000301Y001660D01* +X000301Y001561D02* +X000566Y001561D01* +X000566Y001463D02* +X000301Y001463D01* +X000301Y001364D02* +X000566Y001364D01* +X000566Y001265D02* +X000301Y001265D01* +X002133Y002604D02* +X002133Y003086D01* +X001951Y003268D01* +X001951Y004568D01* +X000451Y006068D01* +X000451Y011218D01* +X000651Y011418D01* +X000651Y011568D01* +X001151Y012068D01* +X007151Y012068D01* +X007651Y011568D01* +X008151Y012068D01* +X015951Y012068D01* +X016151Y011868D01* +X016151Y011068D01* +X015651Y010568D01* +X015251Y010304D02* +X015456Y010098D01* +X015846Y010098D01* +X016121Y010374D01* +X016121Y010755D01* +X016234Y010868D01* +X016351Y010985D01* +X016351Y011204D01* +X016456Y011098D01* +X016601Y011098D01* +X016601Y011518D01* +X016701Y011518D01* +X016701Y011098D01* +X016846Y011098D01* +X017001Y011254D01* +X017001Y010883D01* +X016846Y011038D01* +X016701Y011038D01* +X016701Y010619D01* +X016601Y010619D01* +X016601Y011038D01* +X016456Y011038D01* +X016181Y010763D01* +X016181Y010618D01* +X016601Y010618D01* +X016601Y010518D01* +X016701Y010518D01* +X016701Y010098D01* +X016846Y010098D01* +X017001Y010254D01* +X017001Y009883D01* +X016846Y010038D01* +X016701Y010038D01* +X016701Y009619D01* +X016601Y009619D01* +X016601Y010038D01* +X016456Y010038D01* +X016181Y009763D01* +X016181Y009618D01* +X016601Y009618D01* +X016601Y009518D01* +X016701Y009518D01* +X016701Y009098D01* +X016846Y009098D01* +X017001Y009254D01* +X017001Y008883D01* +X016846Y009038D01* +X016701Y009038D01* +X016701Y008619D01* +X016601Y008619D01* +X016601Y009038D01* +X016456Y009038D01* +X016181Y008763D01* +X016181Y008618D01* +X016601Y008618D01* +X016601Y008518D01* +X016701Y008518D01* +X016701Y008098D01* +X016846Y008098D01* +X017001Y008254D01* +X017001Y007883D01* +X016846Y008038D01* +X016701Y008038D01* +X016701Y007619D01* +X016601Y007619D01* +X016601Y008038D01* +X016456Y008038D01* +X016181Y007763D01* +X016181Y007618D01* +X016601Y007618D01* +X016601Y007518D01* +X016701Y007518D01* +X016701Y007098D01* +X016846Y007098D01* +X017001Y007254D01* +X017001Y006883D01* +X016846Y007038D01* +X016701Y007038D01* +X016701Y006619D01* +X016601Y006619D01* +X016601Y007038D01* +X016456Y007038D01* +X016181Y006763D01* +X016181Y006618D01* +X016601Y006618D01* +X016601Y006518D01* +X016701Y006518D01* +X016701Y006098D01* +X016846Y006098D01* +X017001Y006254D01* +X017001Y005883D01* +X016846Y006038D01* +X016701Y006038D01* +X016701Y005619D01* +X016601Y005619D01* +X016601Y006038D01* +X016456Y006038D01* +X016181Y005763D01* +X016181Y005618D01* +X016601Y005618D01* +X016601Y005518D01* +X016701Y005518D01* +X016701Y005098D01* +X016846Y005098D01* +X017001Y005254D01* +X017001Y004883D01* +X016846Y005038D01* +X016701Y005038D01* +X016701Y004619D01* +X016601Y004619D01* +X016601Y005038D01* +X016456Y005038D01* +X016181Y004763D01* +X016181Y004618D01* +X016601Y004618D01* +X016601Y004518D01* +X016701Y004518D01* +X016701Y004098D01* +X016846Y004098D01* +X017001Y004254D01* +X017001Y003883D01* +X016846Y004038D01* +X016701Y004038D01* +X016701Y003619D01* +X016601Y003619D01* +X016601Y004038D01* +X016456Y004038D01* +X016181Y003763D01* +X016181Y003618D01* +X016601Y003618D01* +X016601Y003518D01* +X016701Y003518D01* +X016701Y003098D01* +X016846Y003098D01* +X017001Y003254D01* +X017001Y002883D01* +X016846Y003038D01* +X016701Y003038D01* +X016701Y002619D01* +X016601Y002619D01* +X016601Y003038D01* +X016456Y003038D01* +X016181Y002763D01* +X016181Y002618D01* +X016601Y002618D01* +X016601Y002518D01* +X016701Y002518D01* +X016701Y002098D01* +X016846Y002098D01* +X017001Y002254D01* +X017001Y001883D01* +X016846Y002038D01* +X016701Y002038D01* +X016701Y001619D01* +X016601Y001619D01* +X016601Y002038D01* +X016456Y002038D01* +X016181Y001763D01* +X016181Y001618D01* +X016601Y001618D01* +X016601Y001518D01* +X016181Y001518D01* +X016181Y001374D01* +X016337Y001218D01* +X015966Y001218D01* +X016121Y001374D01* +X016121Y001518D01* +X015701Y001518D01* +X015701Y001618D01* +X016121Y001618D01* +X016121Y001763D01* +X015846Y002038D01* +X015701Y002038D01* +X015701Y001619D01* +X015601Y001619D01* +X015601Y002038D01* +X015456Y002038D01* +X015181Y001763D01* +X015181Y001618D01* +X015601Y001618D01* +X015601Y001518D01* +X015181Y001518D01* +X015181Y001374D01* +X015337Y001218D01* +X004724Y001218D01* +X004724Y001997D01* +X004637Y002084D01* +X004001Y002084D01* +X004001Y002099D01* +X004341Y002099D01* +X004429Y002187D01* +X004429Y002941D01* +X006102Y002941D01* +X006102Y002951D02* +X006102Y002854D01* +X005804Y002854D01* +X005804Y002667D01* +X005815Y002629D01* +X005834Y002594D01* +X005860Y002568D01* +X005804Y002512D01* +X005804Y001955D01* +X005892Y001867D01* +X006410Y001867D01* +X006498Y001955D01* +X006498Y002298D01* +X006569Y002368D01* +X006601Y002368D01* +X006601Y002156D01* +X006689Y002068D01* +X008813Y002068D01* +X008901Y002156D01* +X008901Y002936D01* +X009351Y002485D01* +X009468Y002368D01* +X010401Y002368D01* +X010401Y002156D01* +X010489Y002068D01* +X012613Y002068D01* +X012701Y002156D01* +X012701Y002368D01* +X012734Y002368D01* +X012804Y002298D01* +X012804Y001955D01* +X012892Y001867D01* +X013410Y001867D01* +X013498Y001955D01* +X013498Y002512D01* +X013442Y002568D01* +X013468Y002594D01* +X013488Y002629D01* +X013498Y002667D01* +X013498Y002854D01* +X013200Y002854D01* +X013200Y002951D01* +X013498Y002951D01* +X013498Y003139D01* +X013488Y003177D01* +X013468Y003212D01* +X013440Y003240D01* +X013406Y003259D01* +X013368Y003269D01* +X013200Y003269D01* +X013200Y002952D01* +X013103Y002952D01* +X013103Y003269D01* +X012935Y003269D01* +X012896Y003259D01* +X012862Y003240D01* +X012834Y003212D01* +X012815Y003177D01* +X012804Y003139D01* +X012804Y002951D01* +X013102Y002951D01* +X013102Y002854D01* +X012804Y002854D01* +X012804Y002768D01* +X012734Y002768D01* +X012701Y002768D01* +X012701Y002980D01* +X012613Y003068D01* +X010489Y003068D01* +X010401Y002980D01* +X010401Y002768D01* +X009634Y002768D01* +X009064Y003339D01* +X009064Y004478D01* +X009176Y004478D01* +X009176Y004876D01* +X009181Y004876D01* +X009181Y004478D01* +X009303Y004478D01* +X009322Y004483D01* +X009327Y004478D01* +X009641Y004478D01* +X009924Y004478D01* +X009924Y004313D01* +X010251Y003985D01* +X010368Y003868D01* +X011018Y003868D01* +X011184Y003868D01* +X011634Y004318D01* +X014268Y004318D01* +X014901Y003685D01* +X014901Y003201D01* +X014901Y003035D01* +X015181Y002755D01* +X015181Y002374D01* +X015456Y002098D01* +X015846Y002098D01* +X016121Y002374D01* +X016121Y002763D01* +X015846Y003038D01* +X015464Y003038D01* +X015301Y003201D01* +X015301Y003254D01* +X015456Y003098D01* +X015846Y003098D01* +X016121Y003374D01* +X016121Y003763D01* +X015846Y004038D01* +X015664Y004038D01* +X015604Y004098D01* +X015846Y004098D01* +X016121Y004374D01* +X016121Y004763D01* +X015846Y005038D01* +X015456Y005038D01* +X015360Y004942D01* +X015036Y005266D01* +X015289Y005266D01* +X015456Y005098D01* +X015846Y005098D01* +X016121Y005374D01* +X016121Y005763D01* +X015846Y006038D01* +X015456Y006038D01* +X015181Y005763D01* +X015181Y005666D01* +X015032Y005666D01* +X015464Y006098D01* +X015846Y006098D01* +X016121Y006374D01* +X016121Y006763D01* +X015846Y007038D01* +X015456Y007038D01* +X015301Y006883D01* +X015301Y006935D01* +X015464Y007098D01* +X015846Y007098D01* +X016121Y007374D01* +X016121Y007763D01* +X015846Y008038D01* +X015456Y008038D01* +X015181Y007763D01* +X015181Y007381D01* +X015001Y007201D01* +X015001Y007635D01* +X015464Y008098D01* +X015846Y008098D01* +X016121Y008374D01* +X016121Y008763D01* +X015846Y009038D01* +X015456Y009038D01* +X015181Y008763D01* +X015181Y008381D01* +X014701Y007901D01* +X014701Y008335D01* +X015464Y009098D01* +X015846Y009098D01* +X016121Y009374D01* +X016121Y009763D01* +X015846Y010038D01* +X015456Y010038D01* +X015181Y009763D01* +X015181Y009381D01* +X014401Y008601D01* +X014401Y009135D01* +X015134Y009868D01* +X015251Y009985D01* +X015251Y010304D01* +X015251Y010233D02* +X015322Y010233D01* +X015251Y010134D02* +X015421Y010134D01* +X015454Y010036D02* +X015251Y010036D01* +X015203Y009937D02* +X015355Y009937D01* +X015257Y009839D02* +X015104Y009839D01* +X015181Y009740D02* +X015006Y009740D01* +X014907Y009642D02* +X015181Y009642D01* +X015181Y009543D02* +X014809Y009543D01* +X014710Y009445D02* +X015181Y009445D01* +X015146Y009346D02* +X014612Y009346D01* +X014513Y009247D02* +X015047Y009247D01* +X014949Y009149D02* +X014415Y009149D01* +X014401Y009050D02* +X014850Y009050D01* +X014752Y008952D02* +X014401Y008952D01* +X014401Y008853D02* +X014653Y008853D01* +X014555Y008755D02* +X014401Y008755D01* +X014401Y008656D02* +X014456Y008656D01* +X014501Y008418D02* +X015651Y009568D01* +X015947Y009937D02* +X016355Y009937D01* +X016257Y009839D02* +X016045Y009839D01* +X016121Y009740D02* +X016181Y009740D01* +X016181Y009642D02* +X016121Y009642D01* +X016121Y009543D02* +X016601Y009543D01* +X016601Y009518D02* +X016181Y009518D01* +X016181Y009374D01* +X016456Y009098D01* +X016601Y009098D01* +X016601Y009518D01* +X016601Y009445D02* +X016701Y009445D01* +X016701Y009346D02* +X016601Y009346D01* +X016601Y009247D02* +X016701Y009247D01* +X016701Y009149D02* +X016601Y009149D01* +X016406Y009149D02* +X015896Y009149D01* +X015995Y009247D02* +X016307Y009247D01* +X016209Y009346D02* +X016093Y009346D01* +X016121Y009445D02* +X016181Y009445D01* +X016601Y009642D02* +X016701Y009642D01* +X016701Y009740D02* +X016601Y009740D01* +X016601Y009839D02* +X016701Y009839D01* +X016701Y009937D02* +X016601Y009937D01* +X016601Y010036D02* +X016701Y010036D01* +X016701Y010134D02* +X016601Y010134D01* +X016601Y010098D02* +X016456Y010098D01* +X016181Y010374D01* +X016181Y010518D01* +X016601Y010518D01* +X016601Y010098D01* +X016601Y010233D02* +X016701Y010233D01* +X016701Y010331D02* +X016601Y010331D01* +X016601Y010430D02* +X016701Y010430D01* +X016601Y010529D02* +X016121Y010529D01* +X016121Y010627D02* +X016181Y010627D01* +X016181Y010726D02* +X016121Y010726D01* +X016190Y010824D02* +X016242Y010824D01* +X016288Y010923D02* +X016341Y010923D01* +X016351Y011021D02* +X016439Y011021D01* +X016435Y011120D02* +X016351Y011120D01* +X016601Y011120D02* +X016701Y011120D01* +X016701Y011218D02* +X016601Y011218D01* +X016601Y011317D02* +X016701Y011317D01* +X016701Y011415D02* +X016601Y011415D01* +X016601Y011514D02* +X016701Y011514D01* +X016701Y011619D02* +X016601Y011619D01* +X016601Y012038D01* +X016456Y012038D01* +X016351Y011933D01* +X016351Y011951D01* +X016151Y012151D01* +X016034Y012268D01* +X016016Y012268D01* +X016121Y012374D01* +X016121Y012518D01* +X015701Y012518D01* +X015701Y012618D01* +X016121Y012618D01* +X016121Y012763D01* +X015966Y012918D01* +X016337Y012918D01* +X016181Y012763D01* +X016181Y012618D01* +X016601Y012618D01* +X016601Y012518D01* +X016701Y012518D01* +X016701Y012098D01* +X016846Y012098D01* +X017001Y012254D01* +X017001Y011883D01* +X016846Y012038D01* +X016701Y012038D01* +X016701Y011619D01* +X016701Y011711D02* +X016601Y011711D01* +X016601Y011810D02* +X016701Y011810D01* +X016701Y011908D02* +X016601Y011908D01* +X016601Y012007D02* +X016701Y012007D01* +X016701Y012105D02* +X016601Y012105D01* +X016601Y012098D02* +X016601Y012518D01* +X016181Y012518D01* +X016181Y012374D01* +X016456Y012098D01* +X016601Y012098D01* +X016601Y012204D02* +X016701Y012204D01* +X016701Y012302D02* +X016601Y012302D01* +X016601Y012401D02* +X016701Y012401D01* +X016701Y012499D02* +X016601Y012499D01* +X016601Y012598D02* +X015701Y012598D01* +X015601Y012598D02* +X014701Y012598D01* +X014701Y012618D02* +X015121Y012618D01* +X015121Y012763D01* +X014966Y012918D01* +X015337Y012918D01* +X015181Y012763D01* +X015181Y012618D01* +X015601Y012618D01* +X015601Y012518D01* +X015181Y012518D01* +X015181Y012374D01* +X015287Y012268D01* +X015016Y012268D01* +X015121Y012374D01* +X015121Y012518D01* +X014701Y012518D01* +X014701Y012618D01* +X014601Y012618D02* +X014601Y012518D01* +X014181Y012518D01* +X014181Y012374D01* +X014287Y012268D01* +X014016Y012268D01* +X014121Y012374D01* +X014121Y012518D01* +X013701Y012518D01* +X013701Y012618D01* +X014121Y012618D01* +X014121Y012763D01* +X013966Y012918D01* +X014337Y012918D01* +X014181Y012763D01* +X014181Y012618D01* +X014601Y012618D01* +X014601Y012598D02* +X013701Y012598D01* +X013601Y012598D02* +X012701Y012598D01* +X012701Y012618D02* +X013121Y012618D01* +X013121Y012763D01* +X012966Y012918D01* +X013337Y012918D01* +X013181Y012763D01* +X013181Y012618D01* +X013601Y012618D01* +X013601Y012518D01* +X013181Y012518D01* +X013181Y012374D01* +X013287Y012268D01* +X013016Y012268D01* +X013121Y012374D01* +X013121Y012518D01* +X012701Y012518D01* +X012701Y012618D01* +X012601Y012618D02* +X012601Y012518D01* +X012181Y012518D01* +X012181Y012374D01* +X012287Y012268D01* +X012016Y012268D01* +X012121Y012374D01* +X012121Y012518D01* +X011701Y012518D01* +X011701Y012618D01* +X012121Y012618D01* +X012121Y012763D01* +X011966Y012918D01* +X012337Y012918D01* +X012181Y012763D01* +X012181Y012618D01* +X012601Y012618D01* +X012601Y012598D02* +X011701Y012598D01* +X011601Y012598D02* +X010701Y012598D01* +X010701Y012618D02* +X011121Y012618D01* +X011121Y012763D01* +X010966Y012918D01* +X011337Y012918D01* +X011181Y012763D01* +X011181Y012618D01* +X011601Y012618D01* +X011601Y012518D01* +X011181Y012518D01* +X011181Y012374D01* +X011287Y012268D01* +X011016Y012268D01* +X011121Y012374D01* +X011121Y012518D01* +X010701Y012518D01* +X010701Y012618D01* +X010601Y012618D02* +X010601Y012518D01* +X010181Y012518D01* +X010181Y012374D01* +X010287Y012268D01* +X010016Y012268D01* +X010121Y012374D01* +X010121Y012518D01* +X009701Y012518D01* +X009701Y012618D01* +X010121Y012618D01* +X010121Y012763D01* +X009966Y012918D01* +X010337Y012918D01* +X010181Y012763D01* +X010181Y012618D01* +X010601Y012618D01* +X010601Y012598D02* +X009701Y012598D01* +X009601Y012598D02* +X008701Y012598D01* +X008701Y012618D02* +X009121Y012618D01* +X009121Y012763D01* +X008966Y012918D01* +X009337Y012918D01* +X009181Y012763D01* +X009181Y012618D01* +X009601Y012618D01* +X009601Y012518D01* +X009181Y012518D01* +X009181Y012374D01* +X009287Y012268D01* +X009016Y012268D01* +X009121Y012374D01* +X009121Y012518D01* +X008701Y012518D01* +X008701Y012618D01* +X008601Y012618D02* +X008601Y012518D01* +X008181Y012518D01* +X008181Y012374D01* +X008287Y012268D01* +X008234Y012268D01* +X008068Y012268D01* +X007838Y012038D01* +X007464Y012038D01* +X007351Y012151D01* +X007234Y012268D01* +X007016Y012268D01* +X007121Y012374D01* +X007121Y012518D01* +X006701Y012518D01* +X006701Y012618D01* +X007121Y012618D01* +X007121Y012763D01* +X006966Y012918D01* +X007337Y012918D01* +X007181Y012763D01* +X007181Y012618D01* +X007601Y012618D01* +X007601Y012518D01* +X007701Y012518D01* +X007701Y012098D01* +X007846Y012098D01* +X008121Y012374D01* +X008121Y012518D01* +X007701Y012518D01* +X007701Y012618D01* +X008121Y012618D01* +X008121Y012763D01* +X007966Y012918D01* +X008337Y012918D01* +X008181Y012763D01* +X008181Y012618D01* +X008601Y012618D01* +X008601Y012598D02* +X007701Y012598D01* +X007701Y012499D02* +X007601Y012499D01* +X007601Y012518D02* +X007601Y012098D01* +X007456Y012098D01* +X007181Y012374D01* +X007181Y012518D01* +X007601Y012518D01* +X007601Y012598D02* +X006701Y012598D01* +X006601Y012598D02* +X005701Y012598D01* +X005701Y012618D02* +X006121Y012618D01* +X006121Y012763D01* +X005966Y012918D01* +X006337Y012918D01* +X006181Y012763D01* +X006181Y012618D01* +X006601Y012618D01* +X006601Y012518D01* +X006181Y012518D01* +X006181Y012374D01* +X006287Y012268D01* +X006016Y012268D01* +X006121Y012374D01* +X006121Y012518D01* +X005701Y012518D01* +X005701Y012618D01* +X005601Y012618D02* +X005601Y012518D01* +X005181Y012518D01* +X005181Y012374D01* +X005287Y012268D01* +X005016Y012268D01* +X005121Y012374D01* +X005121Y012518D01* +X004701Y012518D01* +X004701Y012618D01* +X005121Y012618D01* +X005121Y012763D01* +X004966Y012918D01* +X005337Y012918D01* +X005181Y012763D01* +X005181Y012618D01* +X005601Y012618D01* +X005601Y012598D02* +X004701Y012598D01* +X004601Y012598D02* +X003701Y012598D01* +X003701Y012618D02* +X004121Y012618D01* +X004121Y012763D01* +X003966Y012918D01* +X004337Y012918D01* +X004181Y012763D01* +X004181Y012618D01* +X004601Y012618D01* +X004601Y012518D01* +X004181Y012518D01* +X004181Y012374D01* +X004287Y012268D01* +X004016Y012268D01* +X004121Y012374D01* +X004121Y012518D01* +X003701Y012518D01* +X003701Y012618D01* +X003601Y012618D02* +X003601Y012518D01* +X003181Y012518D01* +X003181Y012374D01* +X003287Y012268D01* +X003016Y012268D01* +X003121Y012374D01* +X003121Y012518D01* +X002701Y012518D01* +X002701Y012618D01* +X003121Y012618D01* +X003121Y012763D01* +X002966Y012918D01* +X003337Y012918D01* +X003181Y012763D01* +X003181Y012618D01* +X003601Y012618D01* +X003601Y012598D02* +X002701Y012598D01* +X002601Y012598D02* +X001701Y012598D01* +X001701Y012618D02* +X002121Y012618D01* +X002121Y012763D01* +X001966Y012918D01* +X002337Y012918D01* +X002181Y012763D01* +X002181Y012618D01* +X002601Y012618D01* +X002601Y012518D01* +X002181Y012518D01* +X002181Y012374D01* +X002287Y012268D01* +X002016Y012268D01* +X002121Y012374D01* +X002121Y012518D01* +X001701Y012518D01* +X001701Y012618D01* +X001601Y012618D02* +X001601Y012518D01* +X001181Y012518D01* +X001181Y012374D01* +X001287Y012268D01* +X001068Y012268D01* +X000951Y012151D01* +X000838Y012038D01* +X000456Y012038D01* +X000301Y011883D01* +X000301Y012254D01* +X000456Y012098D01* +X000601Y012098D01* +X000601Y012518D01* +X000701Y012518D01* +X000701Y012098D01* +X000846Y012098D01* +X001121Y012374D01* +X001121Y012518D01* +X000701Y012518D01* +X000701Y012618D01* +X001121Y012618D01* +X001121Y012763D01* +X000966Y012918D01* +X001337Y012918D01* +X001181Y012763D01* +X001181Y012618D01* +X001601Y012618D01* +X001601Y012598D02* +X000701Y012598D01* +X000701Y012499D02* +X000601Y012499D01* +X000601Y012401D02* +X000701Y012401D01* +X000701Y012302D02* +X000601Y012302D01* +X000601Y012204D02* +X000701Y012204D01* +X000701Y012105D02* +X000601Y012105D01* +X000450Y012105D02* +X000301Y012105D01* +X000301Y012007D02* +X000425Y012007D01* +X000326Y011908D02* +X000301Y011908D01* +X000301Y012204D02* +X000351Y012204D01* +X000853Y012105D02* +X000905Y012105D01* +X000951Y012151D02* +X000951Y012151D01* +X000951Y012204D02* +X001004Y012204D01* +X001050Y012302D02* +X001253Y012302D01* +X001181Y012401D02* +X001121Y012401D01* +X001121Y012499D02* +X001181Y012499D01* +X001181Y012696D02* +X001121Y012696D01* +X001089Y012795D02* +X001213Y012795D01* +X001312Y012894D02* +X000991Y012894D01* +X001991Y012894D02* +X002312Y012894D01* +X002213Y012795D02* +X002089Y012795D01* +X002121Y012696D02* +X002181Y012696D01* +X002181Y012499D02* +X002121Y012499D01* +X002121Y012401D02* +X002181Y012401D01* +X002253Y012302D02* +X002050Y012302D01* +X001287Y011868D02* +X001181Y011763D01* +X001181Y011374D01* +X001456Y011098D01* +X001838Y011098D01* +X002551Y010385D01* +X002668Y010268D01* +X004468Y010268D01* +X004951Y009785D01* +X005068Y009668D01* +X006433Y009668D01* +X006433Y009645D01* +X006489Y009510D01* +X006593Y009406D01* +X006728Y009350D01* +X006874Y009350D01* +X007010Y009406D01* +X007101Y009498D01* +X007101Y007751D01* +X007084Y007768D01* +X006918Y007768D01* +X006852Y007768D01* +X006852Y007827D01* +X006764Y007915D01* +X006207Y007915D01* +X006151Y007859D01* +X006125Y007885D01* +X006091Y007905D01* +X006053Y007915D01* +X005865Y007915D01* +X005865Y007617D01* +X005768Y007617D01* +X005768Y007915D01* +X005580Y007915D01* +X005542Y007905D01* +X005508Y007885D01* +X005480Y007857D01* +X005460Y007823D01* +X005450Y007785D01* +X005450Y007617D01* +X005768Y007617D01* +X005768Y007520D01* +X005450Y007520D01* +X005450Y007352D01* +X005460Y007314D01* +X005480Y007279D01* +X005508Y007251D01* +X005542Y007232D01* +X005580Y007221D01* +X005768Y007221D01* +X005768Y007520D01* +X005865Y007520D01* +X005865Y007221D01* +X006053Y007221D01* +X006091Y007232D01* +X006125Y007251D01* +X006151Y007277D01* +X006207Y007221D01* +X006764Y007221D01* +X006852Y007309D01* +X006852Y007368D01* +X006918Y007368D01* +X007446Y006841D01* +X007561Y006841D01* +X007561Y006728D01* +X007958Y006728D01* +X007958Y006723D01* +X007561Y006723D01* +X007561Y006618D01* +X003518Y006618D01* +X003518Y006712D01* +X003430Y006800D01* +X002872Y006800D01* +X002785Y006712D01* +X002785Y006318D01* +X002584Y006318D01* +X002518Y006385D01* +X002518Y006712D01* +X002430Y006800D01* +X001872Y006800D01* +X001785Y006712D01* +X001785Y006194D01* +X001860Y006118D01* +X001785Y006043D01* +X001785Y005525D01* +X001872Y005437D01* +X002189Y005437D01* +X002189Y003525D01* +X002151Y003563D01* +X002151Y004485D01* +X002151Y004651D01* +X000651Y006151D01* +X000651Y011098D01* +X000846Y011098D01* +X001121Y011374D01* +X001121Y011756D01* +X001234Y011868D01* +X001287Y011868D01* +X001228Y011810D02* +X001175Y011810D01* +X001181Y011711D02* +X001121Y011711D01* +X001121Y011612D02* +X001181Y011612D01* +X001181Y011514D02* +X001121Y011514D01* +X001121Y011415D02* +X001181Y011415D01* +X001238Y011317D02* +X001064Y011317D01* +X000966Y011218D02* +X001337Y011218D01* +X001435Y011120D02* +X000867Y011120D01* +X000651Y011021D02* +X001915Y011021D01* +X002014Y010923D02* +X000651Y010923D01* +X000651Y010824D02* +X002113Y010824D01* +X002211Y010726D02* +X000651Y010726D01* +X000651Y010627D02* +X001211Y010627D01* +X001217Y010633D02* +X001106Y010522D01* +X001106Y009363D01* +X001062Y009363D01* +X001031Y009363D01* +X000882Y009363D01* +X000776Y009258D01* +X000776Y009094D01* +X000776Y009019D01* +X000776Y009019D01* +X000776Y009019D01* +X000806Y008989D01* +X000806Y008989D01* +X000837Y008958D01* +X000776Y008898D01* +X000776Y008629D01* +X000842Y008563D01* +X000776Y008498D01* +X000776Y008362D01* +X000776Y008289D01* +X000776Y008289D01* +X000776Y008289D01* +X000806Y008259D01* +X000806Y008259D01* +X000842Y008223D01* +X000776Y008158D01* +X000776Y008009D01* +X000842Y007943D01* +X000776Y007878D01* +X000776Y007744D01* +X000776Y007669D01* +X000776Y007669D01* +X000776Y007669D01* +X000806Y007639D01* +X000806Y007639D01* +X000852Y007593D01* +X000776Y007518D01* +X000776Y007398D01* +X000776Y007341D01* +X000765Y007285D01* +X000776Y007269D01* +X000776Y007249D01* +X000816Y007209D01* +X000848Y007161D01* +X000867Y007157D01* +X000882Y007143D01* +X000938Y007143D01* +X000994Y007132D01* +X001011Y007143D01* +X001188Y007143D01* +X001217Y007113D01* +X001375Y007113D01* +X001421Y007160D01* +X001427Y007153D01* +X001437Y007153D01* +X001438Y007153D01* +X001487Y007103D01* +X001497Y007103D01* +X001505Y007097D01* +X001575Y007103D01* +X001587Y007103D01* +X001646Y007092D01* +X001664Y007103D01* +X001685Y007103D01* +X001727Y007146D01* +X001755Y007164D01* +X001789Y007178D01* +X001886Y007138D01* +X002031Y007198D01* +X002093Y007345D01* +X002116Y007364D01* +X002122Y007415D01* +X002142Y007463D01* +X002136Y007477D01* +X002136Y007512D01* +X002152Y007546D01* +X002140Y007578D01* +X002142Y007596D01* +X002151Y007613D01* +X002136Y007661D01* +X002136Y007712D01* +X002114Y007734D01* +X002114Y007736D01* +X002120Y007759D01* +X002092Y007809D01* +X002080Y007848D01* +X002092Y007856D01* +X002116Y008012D01* +X001954Y008234D01* +X002043Y008332D01* +X002096Y008385D01* +X002096Y008389D01* +X002099Y008393D01* +X002096Y008467D01* +X002096Y008514D01* +X002102Y008521D01* +X002096Y008591D01* +X002096Y008662D01* +X002090Y008668D01* +X002089Y008678D01* +X002060Y008702D01* +X002083Y008755D01* +X002106Y008774D01* +X002112Y008825D01* +X002113Y008828D01* +X002173Y007760D01* +X002163Y007742D01* +X002171Y007710D01* +X002162Y007679D01* +X002179Y007649D01* +X002181Y007614D01* +X002200Y007597D01* +X002202Y007590D01* +X002217Y007581D01* +X002239Y007541D01* +X002273Y007532D01* +X002298Y007509D01* +X002335Y007511D01* +X002337Y007510D01* +X002345Y007512D01* +X002345Y007512D01* +X002391Y007499D01* +X002421Y007516D01* +X002455Y007518D01* +X002481Y007547D01* +X002490Y007549D01* +X002495Y007558D01* +X003127Y007914D01* +X003137Y007903D01* +X003155Y007903D01* +X003170Y007893D01* +X003211Y007900D01* +X003252Y007891D01* +X003272Y007903D01* +X003295Y007903D01* +X003307Y007916D01* +X003325Y007919D01* +X003349Y007953D01* +X004492Y008684D01* +X004516Y008684D01* +X004533Y008701D01* +X004557Y008707D01* +X004575Y008737D01* +X004605Y008756D01* +X004610Y008779D01* +X004627Y008796D01* +X004627Y008820D01* +X004639Y008841D01* +X004631Y008875D01* +X004639Y008909D01* +X004626Y008930D01* +X004625Y008954D01* +X004608Y008971D01* +X004603Y008994D01* +X004573Y009012D01* +X004554Y009042D01* +X004530Y009047D01* +X004513Y009064D01* +X004489Y009064D01* +X003888Y009432D01* +X003890Y009440D01* +X003876Y009464D01* +X003876Y009492D01* +X003855Y009513D01* +X003846Y009542D01* +X003824Y009554D01* +X003811Y009576D01* +X003785Y009584D01* +X003765Y009603D01* +X003735Y009603D01* +X002526Y010264D01* +X002515Y010284D01* +X002488Y010291D01* +X002469Y010312D01* +X002437Y010312D01* +X002408Y010328D01* +X002386Y010321D01* +X002364Y010328D01* +X002339Y010314D01* +X002312Y010315D01* +X002288Y010293D01* +X002257Y010283D01* +X002246Y010263D01* +X002226Y010252D01* +X002218Y010225D01* +X002198Y010206D01* +X002197Y010174D01* +X002182Y010145D01* +X002188Y010123D01* +X002182Y010101D01* +X002195Y010076D01* +X002178Y009301D01* +X002159Y009295D01* +X002140Y009261D01* +X002109Y009238D01* +X002106Y009217D01* +X002092Y009201D01* +X002082Y009219D01* +X002066Y009273D01* +X002056Y009278D01* +X002056Y009292D01* +X002018Y009330D01* +X001992Y009376D01* +X001965Y009383D01* +X001945Y009403D01* +X001892Y009403D01* +X001840Y009417D01* +X001825Y009409D01* +X001816Y009415D01* +X001757Y009403D01* +X001697Y009403D01* +X001682Y009388D01* +X001662Y009384D01* +X001656Y009376D01* +X001656Y009513D01* +X002005Y009513D01* +X002116Y009625D01* +X002116Y009675D01* +X002116Y009832D01* +X002116Y009882D01* +X002100Y009898D01* +X002116Y009915D01* +X002116Y010265D01* +X002116Y010325D01* +X002116Y010482D01* +X002116Y010532D01* +X002005Y010643D01* +X001795Y010643D01* +X001637Y010643D01* +X001545Y010643D01* +X001387Y010643D01* +X001376Y010632D01* +X001375Y010633D01* +X001217Y010633D01* +X001113Y010529D02* +X000651Y010529D01* +X000651Y010430D02* +X001106Y010430D01* +X001106Y010331D02* +X000651Y010331D01* +X000651Y010233D02* +X001106Y010233D01* +X001106Y010134D02* +X000651Y010134D01* +X000651Y010036D02* +X001106Y010036D01* +X001106Y009937D02* +X000651Y009937D01* +X000651Y009839D02* +X001106Y009839D01* +X001106Y009740D02* +X000651Y009740D01* +X000651Y009642D02* +X001106Y009642D01* +X001106Y009543D02* +X000651Y009543D01* +X000651Y009445D02* +X001106Y009445D01* +X000864Y009346D02* +X000651Y009346D01* +X000651Y009247D02* +X000776Y009247D01* +X000776Y009149D02* +X000651Y009149D01* +X000651Y009050D02* +X000776Y009050D01* +X000830Y008952D02* +X000651Y008952D01* +X000651Y008853D02* +X000776Y008853D01* +X000776Y008755D02* +X000651Y008755D01* +X000651Y008656D02* +X000776Y008656D01* +X000836Y008558D02* +X000651Y008558D01* +X000651Y008459D02* +X000776Y008459D01* +X000776Y008361D02* +X000651Y008361D01* +X000651Y008262D02* +X000803Y008262D01* +X000782Y008163D02* +X000651Y008163D01* +X000651Y008065D02* +X000776Y008065D01* +X000819Y007966D02* +X000651Y007966D01* +X000651Y007868D02* +X000776Y007868D01* +X000776Y007769D02* +X000651Y007769D01* +X000651Y007671D02* +X000776Y007671D01* +X000831Y007572D02* +X000651Y007572D01* +X000651Y007474D02* +X000776Y007474D01* +X000776Y007375D02* +X000651Y007375D01* +X000651Y007277D02* +X000771Y007277D01* +X000837Y007178D02* +X000651Y007178D01* +X000651Y007079D02* +X007207Y007079D01* +X007306Y006981D02* +X000651Y006981D01* +X000651Y006882D02* +X007404Y006882D01* +X007561Y006784D02* +X003446Y006784D01* +X003518Y006685D02* +X007561Y006685D01* +X007961Y006726D02* +X008594Y006726D01* +X008601Y006718D01* +X008361Y006723D02* +X007964Y006723D01* +X007964Y006728D01* +X008361Y006728D01* +X008361Y006851D01* +X008356Y006869D01* +X008361Y006874D01* +X008361Y007208D01* +X008361Y007523D01* +X008361Y007576D01* +X009609Y006328D01* +X009609Y005444D01* +X009501Y005551D01* +X009384Y005668D01* +X008361Y005668D01* +X008361Y005948D01* +X008361Y006263D01* +X008361Y006578D01* +X008356Y006583D01* +X008361Y006601D01* +X008361Y006723D01* +X008361Y006685D02* +X009251Y006685D01* +X009153Y006784D02* +X008361Y006784D01* +X008361Y006882D02* +X009054Y006882D01* +X008956Y006981D02* +X008361Y006981D01* +X008361Y007079D02* +X008857Y007079D01* +X008759Y007178D02* +X008361Y007178D01* +X008361Y007277D02* +X008660Y007277D01* +X008562Y007375D02* +X008361Y007375D01* +X008361Y007474D02* +X008463Y007474D01* +X008364Y007572D02* +X008361Y007572D01* +X007961Y007671D02* +X007799Y007671D01* +X007601Y007868D01* +X007601Y010118D01* +X007251Y010468D01* +X005351Y010468D01* +X004751Y011068D01* +X004151Y011068D01* +X003651Y011568D01* +X003253Y012302D02* +X003050Y012302D01* +X003121Y012401D02* +X003181Y012401D01* +X003181Y012499D02* +X003121Y012499D01* +X003121Y012696D02* +X003181Y012696D01* +X003213Y012795D02* +X003089Y012795D01* +X002991Y012894D02* +X003312Y012894D01* +X003991Y012894D02* +X004312Y012894D01* +X004213Y012795D02* +X004089Y012795D01* +X004121Y012696D02* +X004181Y012696D01* +X004181Y012499D02* +X004121Y012499D01* +X004121Y012401D02* +X004181Y012401D01* +X004253Y012302D02* +X004050Y012302D01* +X004651Y011568D02* +X005451Y010768D01* +X008451Y010768D01* +X008864Y010356D01* +X008864Y008258D01* +X009179Y008258D02* +X009179Y010541D01* +X008651Y011068D01* +X006151Y011068D01* +X005651Y011568D01* +X006121Y011612D02* +X006601Y011612D01* +X006601Y011618D02* +X006601Y011518D01* +X006181Y011518D01* +X006181Y011374D01* +X006287Y011268D01* +X006234Y011268D01* +X006121Y011381D01* +X006121Y011763D01* +X006016Y011868D01* +X006287Y011868D01* +X006181Y011763D01* +X006181Y011618D01* +X006601Y011618D01* +X006701Y011618D02* +X007121Y011618D01* +X007121Y011763D01* +X007016Y011868D01* +X007068Y011868D01* +X007181Y011755D01* +X007181Y011374D01* +X007287Y011268D01* +X007016Y011268D01* +X007121Y011374D01* +X007121Y011518D01* +X006701Y011518D01* +X006701Y011618D01* +X006701Y011612D02* +X007181Y011612D01* +X007181Y011514D02* +X007121Y011514D01* +X007121Y011415D02* +X007181Y011415D01* +X007238Y011317D02* +X007064Y011317D01* +X007121Y011711D02* +X007181Y011711D01* +X007127Y011810D02* +X007075Y011810D01* +X007397Y012105D02* +X007450Y012105D01* +X007351Y012204D02* +X007299Y012204D01* +X007253Y012302D02* +X007050Y012302D01* +X007121Y012401D02* +X007181Y012401D01* +X007181Y012499D02* +X007121Y012499D01* +X007121Y012696D02* +X007181Y012696D01* +X007213Y012795D02* +X007089Y012795D01* +X006991Y012894D02* +X007312Y012894D01* +X007601Y012401D02* +X007701Y012401D01* +X007701Y012302D02* +X007601Y012302D01* +X007601Y012204D02* +X007701Y012204D01* +X007701Y012105D02* +X007601Y012105D01* +X007853Y012105D02* +X007905Y012105D01* +X007951Y012204D02* +X008004Y012204D01* +X008050Y012302D02* +X008253Y012302D01* +X008181Y012401D02* +X008121Y012401D01* +X008121Y012499D02* +X008181Y012499D01* +X008181Y012696D02* +X008121Y012696D01* +X008089Y012795D02* +X008213Y012795D01* +X008312Y012894D02* +X007991Y012894D01* +X008991Y012894D02* +X009312Y012894D01* +X009213Y012795D02* +X009089Y012795D01* +X009121Y012696D02* +X009181Y012696D01* +X009181Y012499D02* +X009121Y012499D01* +X009121Y012401D02* +X009181Y012401D01* +X009253Y012302D02* +X009050Y012302D01* +X008651Y011568D02* +X009494Y010726D01* +X009494Y008258D01* +X009809Y008258D02* +X009809Y011411D01* +X009651Y011568D01* +X010050Y012302D02* +X010253Y012302D01* +X010181Y012401D02* +X010121Y012401D01* +X010121Y012499D02* +X010181Y012499D01* +X010181Y012696D02* +X010121Y012696D01* +X010089Y012795D02* +X010213Y012795D01* +X010312Y012894D02* +X009991Y012894D01* +X010991Y012894D02* +X011312Y012894D01* +X011213Y012795D02* +X011089Y012795D01* +X011121Y012696D02* +X011181Y012696D01* +X011181Y012499D02* +X011121Y012499D01* +X011121Y012401D02* +X011181Y012401D01* +X011253Y012302D02* +X011050Y012302D01* +X010651Y011568D02* +X010551Y011468D01* +X010551Y011168D01* +X010124Y010741D01* +X010124Y008258D01* +X010439Y008258D02* +X010439Y010556D01* +X010951Y011068D01* +X011151Y011068D01* +X011651Y011568D01* +X011851Y010768D02* +X012651Y011568D01* +X012253Y012302D02* +X012050Y012302D01* +X012121Y012401D02* +X012181Y012401D01* +X012181Y012499D02* +X012121Y012499D01* +X012121Y012696D02* +X012181Y012696D01* +X012213Y012795D02* +X012089Y012795D01* +X011991Y012894D02* +X012312Y012894D01* +X012991Y012894D02* +X013312Y012894D01* +X013213Y012795D02* +X013089Y012795D01* +X013121Y012696D02* +X013181Y012696D01* +X013181Y012499D02* +X013121Y012499D01* +X013121Y012401D02* +X013181Y012401D01* +X013253Y012302D02* +X013050Y012302D01* +X013651Y011568D02* +X013601Y011518D01* +X013601Y007868D01* +X013404Y007671D01* +X011341Y007671D01* +X011341Y007356D02* +X013639Y007356D01* +X013901Y007618D01* +X013901Y010018D01* +X014651Y010768D01* +X014651Y011568D01* +X015051Y010968D02* +X015651Y011568D01* +X016296Y012007D02* +X016425Y012007D01* +X016450Y012105D02* +X016197Y012105D01* +X016099Y012204D02* +X016351Y012204D01* +X016253Y012302D02* +X016050Y012302D01* +X016121Y012401D02* +X016181Y012401D01* +X016181Y012499D02* +X016121Y012499D01* +X016121Y012696D02* +X016181Y012696D01* +X016213Y012795D02* +X016089Y012795D01* +X015991Y012894D02* +X016312Y012894D01* +X016951Y012204D02* +X017001Y012204D01* +X017001Y012105D02* +X016853Y012105D01* +X016877Y012007D02* +X017001Y012007D01* +X017001Y011908D02* +X016976Y011908D01* +X016966Y011218D02* +X017001Y011218D01* +X017001Y011120D02* +X016867Y011120D01* +X016863Y011021D02* +X017001Y011021D01* +X017001Y010923D02* +X016961Y010923D01* +X016701Y010923D02* +X016601Y010923D01* +X016601Y011021D02* +X016701Y011021D01* +X016701Y010824D02* +X016601Y010824D01* +X016601Y010726D02* +X016701Y010726D01* +X016701Y010627D02* +X016601Y010627D01* +X016181Y010430D02* +X016121Y010430D01* +X016079Y010331D02* +X016223Y010331D01* +X016322Y010233D02* +X015980Y010233D01* +X015882Y010134D02* +X016421Y010134D01* +X016454Y010036D02* +X015848Y010036D01* +X015051Y010068D02* +X015051Y010968D01* +X015051Y010068D02* +X014201Y009218D01* +X014201Y007368D01* +X013874Y007041D01* +X011341Y007041D01* +X011341Y006726D02* +X014109Y006726D01* +X014501Y007118D01* +X014501Y008418D01* +X014726Y008361D02* +X015161Y008361D01* +X015181Y008459D02* +X014825Y008459D01* +X014923Y008558D02* +X015181Y008558D01* +X015181Y008656D02* +X015022Y008656D01* +X015120Y008755D02* +X015181Y008755D01* +X015219Y008853D02* +X015271Y008853D01* +X015317Y008952D02* +X015370Y008952D01* +X015416Y009050D02* +X017001Y009050D01* +X017001Y008952D02* +X016932Y008952D01* +X016701Y008952D02* +X016601Y008952D01* +X016601Y008853D02* +X016701Y008853D01* +X016701Y008755D02* +X016601Y008755D01* +X016601Y008656D02* +X016701Y008656D01* +X016601Y008558D02* +X016121Y008558D01* +X016181Y008518D02* +X016181Y008374D01* +X016456Y008098D01* +X016601Y008098D01* +X016601Y008518D01* +X016181Y008518D01* +X016181Y008459D02* +X016121Y008459D01* +X016108Y008361D02* +X016194Y008361D01* +X016293Y008262D02* +X016009Y008262D01* +X015911Y008163D02* +X016391Y008163D01* +X016601Y008163D02* +X016701Y008163D01* +X016701Y008262D02* +X016601Y008262D01* +X016601Y008361D02* +X016701Y008361D01* +X016701Y008459D02* +X016601Y008459D01* +X016911Y008163D02* +X017001Y008163D01* +X017001Y008065D02* +X015431Y008065D01* +X015385Y007966D02* +X015332Y007966D01* +X015286Y007868D02* +X015234Y007868D01* +X015187Y007769D02* +X015135Y007769D01* +X015181Y007671D02* +X015036Y007671D01* +X015001Y007572D02* +X015181Y007572D01* +X015181Y007474D02* +X015001Y007474D01* +X015001Y007375D02* +X015175Y007375D01* +X015077Y007277D02* +X015001Y007277D01* +X015101Y007018D02* +X015651Y007568D01* +X016024Y007277D02* +X016278Y007277D01* +X016181Y007374D02* +X016456Y007098D01* +X016601Y007098D01* +X016601Y007518D01* +X016181Y007518D01* +X016181Y007374D01* +X016181Y007375D02* +X016121Y007375D01* +X016121Y007474D02* +X016181Y007474D01* +X016121Y007572D02* +X016601Y007572D01* +X016601Y007474D02* +X016701Y007474D01* +X016701Y007375D02* +X016601Y007375D01* +X016601Y007277D02* +X016701Y007277D01* +X016701Y007178D02* +X016601Y007178D01* +X016377Y007178D02* +X015926Y007178D01* +X015903Y006981D02* +X016399Y006981D01* +X016301Y006882D02* +X016002Y006882D01* +X016100Y006784D02* +X016202Y006784D01* +X016181Y006685D02* +X016121Y006685D01* +X016121Y006587D02* +X016601Y006587D01* +X016601Y006518D02* +X016181Y006518D01* +X016181Y006374D01* +X016456Y006098D01* +X016601Y006098D01* +X016601Y006518D01* +X016601Y006488D02* +X016701Y006488D01* +X016701Y006390D02* +X016601Y006390D01* +X016601Y006291D02* +X016701Y006291D01* +X016701Y006193D02* +X016601Y006193D01* +X016362Y006193D02* +X015940Y006193D01* +X016039Y006291D02* +X016264Y006291D01* +X016181Y006390D02* +X016121Y006390D01* +X016121Y006488D02* +X016181Y006488D01* +X016601Y006685D02* +X016701Y006685D01* +X016701Y006784D02* +X016601Y006784D01* +X016601Y006882D02* +X016701Y006882D01* +X016701Y006981D02* +X016601Y006981D01* +X016903Y006981D02* +X017001Y006981D01* +X017001Y007079D02* +X015445Y007079D01* +X015399Y006981D02* +X015347Y006981D01* +X015101Y007018D02* +X015101Y006618D01* +X014579Y006096D01* +X011341Y006096D01* +X011341Y006411D02* +X014344Y006411D01* +X014801Y006868D01* +X014801Y007718D01* +X015651Y008568D01* +X016031Y008853D02* +X016271Y008853D01* +X016181Y008755D02* +X016121Y008755D01* +X016121Y008656D02* +X016181Y008656D01* +X016370Y008952D02* +X015932Y008952D01* +X015062Y008262D02* +X014701Y008262D01* +X014701Y008163D02* +X014964Y008163D01* +X014865Y008065D02* +X014701Y008065D01* +X014701Y007966D02* +X014766Y007966D01* +X015918Y007966D02* +X016385Y007966D01* +X016286Y007868D02* +X016016Y007868D01* +X016115Y007769D02* +X016187Y007769D01* +X016181Y007671D02* +X016121Y007671D01* +X016601Y007671D02* +X016701Y007671D01* +X016701Y007769D02* +X016601Y007769D01* +X016601Y007868D02* +X016701Y007868D01* +X016701Y007966D02* +X016601Y007966D01* +X016918Y007966D02* +X017001Y007966D01* +X017001Y007178D02* +X016925Y007178D01* +X016940Y006193D02* +X017001Y006193D01* +X017001Y006094D02* +X015460Y006094D01* +X015414Y005996D02* +X015361Y005996D01* +X015315Y005897D02* +X015263Y005897D01* +X015217Y005798D02* +X015164Y005798D01* +X015181Y005700D02* +X015066Y005700D01* +X014864Y005781D02* +X015651Y006568D01* +X015889Y005996D02* +X016414Y005996D01* +X016315Y005897D02* +X015987Y005897D01* +X016086Y005798D02* +X016217Y005798D01* +X016181Y005700D02* +X016121Y005700D01* +X016121Y005601D02* +X016601Y005601D01* +X016601Y005518D02* +X016181Y005518D01* +X016181Y005374D01* +X016456Y005098D01* +X016601Y005098D01* +X016601Y005518D01* +X016601Y005503D02* +X016701Y005503D01* +X016701Y005404D02* +X016601Y005404D01* +X016601Y005306D02* +X016701Y005306D01* +X016701Y005207D02* +X016601Y005207D01* +X016601Y005109D02* +X016701Y005109D01* +X016701Y005010D02* +X016601Y005010D01* +X016601Y004912D02* +X016701Y004912D01* +X016701Y004813D02* +X016601Y004813D01* +X016601Y004714D02* +X016701Y004714D01* +X016601Y004616D02* +X016121Y004616D01* +X016121Y004714D02* +X016181Y004714D01* +X016231Y004813D02* +X016071Y004813D01* +X015973Y004912D02* +X016330Y004912D01* +X016428Y005010D02* +X015874Y005010D01* +X015856Y005109D02* +X016446Y005109D01* +X016348Y005207D02* +X015955Y005207D01* +X016053Y005306D02* +X016249Y005306D01* +X016181Y005404D02* +X016121Y005404D01* +X016121Y005503D02* +X016181Y005503D01* +X016601Y005700D02* +X016701Y005700D01* +X016701Y005798D02* +X016601Y005798D01* +X016601Y005897D02* +X016701Y005897D01* +X016701Y005996D02* +X016601Y005996D01* +X016889Y005996D02* +X017001Y005996D01* +X016987Y005897D02* +X017001Y005897D01* +X017001Y005207D02* +X016955Y005207D01* +X017001Y005109D02* +X016856Y005109D01* +X016874Y005010D02* +X017001Y005010D01* +X017001Y004912D02* +X016973Y004912D01* +X016701Y004517D02* +X016601Y004517D01* +X016601Y004518D02* +X016601Y004098D01* +X016456Y004098D01* +X016181Y004374D01* +X016181Y004518D01* +X016601Y004518D01* +X016601Y004419D02* +X016701Y004419D01* +X016701Y004320D02* +X016601Y004320D01* +X016601Y004222D02* +X016701Y004222D01* +X016701Y004123D02* +X016601Y004123D01* +X016601Y004025D02* +X016701Y004025D01* +X016701Y003926D02* +X016601Y003926D01* +X016601Y003828D02* +X016701Y003828D01* +X016701Y003729D02* +X016601Y003729D01* +X016601Y003630D02* +X016701Y003630D01* +X016601Y003532D02* +X016121Y003532D01* +X016181Y003518D02* +X016181Y003374D01* +X016456Y003098D01* +X016601Y003098D01* +X016601Y003518D01* +X016181Y003518D01* +X016181Y003433D02* +X016121Y003433D01* +X016082Y003335D02* +X016220Y003335D01* +X016319Y003236D02* +X015984Y003236D01* +X015885Y003138D02* +X016417Y003138D01* +X016601Y003138D02* +X016701Y003138D01* +X016701Y003236D02* +X016601Y003236D01* +X016601Y003335D02* +X016701Y003335D01* +X016701Y003433D02* +X016601Y003433D01* +X016885Y003138D02* +X017001Y003138D01* +X017001Y003236D02* +X016984Y003236D01* +X017001Y003039D02* +X015463Y003039D01* +X015417Y003138D02* +X015365Y003138D01* +X015319Y003236D02* +X015301Y003236D01* +X015101Y003118D02* +X015651Y002568D01* +X015304Y002251D02* +X013498Y002251D01* +X013498Y002349D02* +X015205Y002349D01* +X015181Y002448D02* +X013498Y002448D01* +X013464Y002546D02* +X015181Y002546D01* +X015181Y002645D02* +X013492Y002645D01* +X013498Y002744D02* +X015181Y002744D01* +X015095Y002842D02* +X013498Y002842D01* +X013498Y003039D02* +X014901Y003039D01* +X014901Y003138D02* +X013498Y003138D01* +X013443Y003236D02* +X014901Y003236D01* +X014901Y003335D02* +X009067Y003335D01* +X009064Y003433D02* +X014901Y003433D01* +X014901Y003532D02* +X009064Y003532D01* +X009064Y003630D02* +X014901Y003630D01* +X014858Y003729D02* +X009064Y003729D01* +X009064Y003828D02* +X014759Y003828D01* +X014661Y003926D02* +X011242Y003926D01* +X011340Y004025D02* +X014562Y004025D01* +X014463Y004123D02* +X011439Y004123D01* +X011537Y004222D02* +X014365Y004222D01* +X014351Y004518D02* +X015101Y003768D01* +X015101Y003118D01* +X014996Y002941D02* +X013200Y002941D01* +X013151Y002903D02* +X013151Y003518D01* +X013103Y003236D02* +X013200Y003236D01* +X013200Y003138D02* +X013103Y003138D01* +X013103Y003039D02* +X013200Y003039D01* +X013102Y002941D02* +X012701Y002941D01* +X012701Y002842D02* +X012804Y002842D01* +X012804Y003039D02* +X012642Y003039D01* +X012804Y003138D02* +X009265Y003138D01* +X009363Y003039D02* +X010460Y003039D01* +X010401Y002941D02* +X009462Y002941D01* +X009560Y002842D02* +X010401Y002842D01* +X010401Y002349D02* +X008901Y002349D01* +X008901Y002251D02* +X010401Y002251D01* +X010405Y002152D02* +X008897Y002152D01* +X008901Y002448D02* +X009389Y002448D01* +X009290Y002546D02* +X008901Y002546D01* +X008901Y002645D02* +X009192Y002645D01* +X009093Y002744D02* +X008901Y002744D01* +X008901Y002842D02* +X008995Y002842D01* +X008551Y002568D02* +X007751Y002568D01* +X006486Y002568D01* +X006151Y002234D01* +X005804Y002251D02* +X004429Y002251D01* +X004429Y002349D02* +X005804Y002349D01* +X005804Y002448D02* +X004429Y002448D01* +X004429Y002546D02* +X005838Y002546D01* +X005810Y002645D02* +X004429Y002645D01* +X004429Y002744D02* +X005804Y002744D01* +X005804Y002842D02* +X004429Y002842D01* +X004429Y002941D02* +X004341Y003029D01* +X003390Y003029D01* +X003339Y002978D01* +X003318Y002999D01* +X003284Y003019D01* +X003246Y003029D01* +X003157Y003029D01* +X003068Y003029D01* +X003040Y003022D01* +X003032Y003029D01* +X002845Y003029D01* +X002845Y003479D01* +X002853Y003472D01* +X002982Y003418D01* +X003121Y003418D01* +X003249Y003472D01* +X003348Y003570D01* +X003401Y003699D01* +X003401Y003838D01* +X003348Y003967D01* +X003249Y004065D01* +X003151Y004106D01* +X003151Y004685D01* +X003334Y004868D01* +X006434Y004868D01* +X006551Y004985D01* +X006701Y005135D01* +X006701Y005221D01* +X006764Y005221D01* +X006809Y005266D01* +X007594Y005266D01* +X007649Y005211D01* +X008273Y005211D01* +X008331Y005268D01* +X008372Y005268D01* +X008294Y005190D01* +X008294Y004566D01* +X008349Y004511D01* +X008349Y003068D01* +X006689Y003068D01* +X006601Y002980D01* +X006601Y002768D01* +X006569Y002768D01* +X006498Y002768D01* +X006498Y002854D01* +X006200Y002854D01* +X006200Y002951D01* +X006498Y002951D01* +X006498Y003139D01* +X006488Y003177D01* +X006468Y003212D01* +X006440Y003240D01* +X006406Y003259D01* +X006368Y003269D01* +X006200Y003269D01* +X006200Y002952D01* +X006103Y002952D01* +X006103Y003269D01* +X005935Y003269D01* +X005896Y003259D01* +X005862Y003240D01* +X005834Y003212D01* +X005815Y003177D01* +X005804Y003139D01* +X005804Y002951D01* +X006102Y002951D01* +X006151Y002903D02* +X006151Y003518D01* +X006103Y003236D02* +X006200Y003236D01* +X006200Y003138D02* +X006103Y003138D01* +X006103Y003039D02* +X006200Y003039D01* +X006200Y002941D02* +X006601Y002941D01* +X006601Y002842D02* +X006498Y002842D01* +X006498Y003039D02* +X006660Y003039D01* +X006498Y003138D02* +X008349Y003138D01* +X008349Y003236D02* +X006443Y003236D01* +X005859Y003236D02* +X002845Y003236D01* +X002845Y003138D02* +X005804Y003138D01* +X005804Y003039D02* +X002845Y003039D01* +X002845Y003335D02* +X008349Y003335D01* +X008349Y003433D02* +X003157Y003433D01* +X003310Y003532D02* +X008349Y003532D01* +X008349Y003630D02* +X003373Y003630D01* +X003401Y003729D02* +X008349Y003729D01* +X008349Y003828D02* +X003401Y003828D01* +X003365Y003926D02* +X008349Y003926D01* +X008349Y004025D02* +X003290Y004025D01* +X003151Y004123D02* +X008349Y004123D01* +X008349Y004222D02* +X003151Y004222D01* +X003151Y004320D02* +X008349Y004320D01* +X008349Y004419D02* +X003151Y004419D01* +X003151Y004517D02* +X008343Y004517D01* +X008294Y004616D02* +X003151Y004616D01* +X003180Y004714D02* +X008294Y004714D01* +X008294Y004813D02* +X003279Y004813D01* +X003251Y005068D02* +X002951Y004768D01* +X002951Y003868D01* +X003051Y003768D01* +X002945Y003433D02* +X002845Y003433D01* +X003157Y003029D02* +X003157Y002604D01* +X003157Y002604D01* +X003157Y003029D01* +X003157Y002941D02* +X003157Y002941D01* +X003157Y002842D02* +X003157Y002842D01* +X003157Y002744D02* +X003157Y002744D01* +X003157Y002645D02* +X003157Y002645D01* +X002645Y002604D02* +X002645Y005278D01* +X003151Y005784D01* +X003518Y005798D02* +X005454Y005798D01* +X005450Y005785D02* +X005450Y005617D01* +X005768Y005617D01* +X005768Y005915D01* +X005580Y005915D01* +X005542Y005905D01* +X005508Y005885D01* +X005480Y005857D01* +X005460Y005823D01* +X005450Y005785D01* +X005450Y005700D02* +X003518Y005700D01* +X003518Y005601D02* +X005768Y005601D01* +X005768Y005617D02* +X005768Y005520D01* +X005450Y005520D01* +X005450Y005352D01* +X005460Y005314D01* +X005480Y005279D01* +X005491Y005268D01* +X003334Y005268D01* +X003168Y005268D01* +X002868Y004968D01* +X002845Y004945D01* +X002845Y005195D01* +X003087Y005437D01* +X003430Y005437D01* +X003518Y005525D01* +X003518Y005918D01* +X007561Y005918D01* +X007561Y005666D01* +X006852Y005666D01* +X006852Y005827D01* +X006764Y005915D01* +X006207Y005915D01* +X006151Y005859D01* +X006125Y005885D01* +X006091Y005905D01* +X006053Y005915D01* +X005865Y005915D01* +X005865Y005617D01* +X005768Y005617D01* +X005817Y005568D02* +X005301Y005568D01* +X005450Y005503D02* +X003496Y005503D01* +X003107Y005207D02* +X002857Y005207D01* +X002845Y005109D02* +X003009Y005109D01* +X002910Y005010D02* +X002845Y005010D01* +X002956Y005306D02* +X005465Y005306D01* +X005450Y005404D02* +X003055Y005404D01* +X003251Y005068D02* +X006351Y005068D01* +X006501Y005218D01* +X006501Y005553D01* +X006486Y005568D01* +X006501Y005553D02* +X006588Y005466D01* +X007961Y005466D01* +X007964Y005468D01* +X009301Y005468D01* +X009494Y005276D01* +X009494Y004878D01* +X009809Y004878D02* +X009809Y006411D01* +X007901Y008318D01* +X007901Y010118D01* +X008101Y010318D01* +X007301Y009868D02* +X007001Y010168D01* +X005251Y010168D01* +X004651Y010768D01* +X003451Y010768D01* +X002651Y011568D01* +X001651Y011568D02* +X002751Y010468D01* +X004551Y010468D01* +X005151Y009868D01* +X006651Y009868D01* +X006801Y009718D01* +X006554Y009445D02* +X003887Y009445D01* +X003845Y009543D02* +X006475Y009543D01* +X006434Y009642D02* +X003665Y009642D01* +X003484Y009740D02* +X004996Y009740D01* +X004898Y009839D02* +X003304Y009839D01* +X003123Y009937D02* +X004799Y009937D01* +X004701Y010036D02* +X002943Y010036D01* +X002762Y010134D02* +X004602Y010134D01* +X004504Y010233D02* +X002582Y010233D01* +X002605Y010331D02* +X002116Y010331D01* +X002116Y010233D02* +X002220Y010233D01* +X002185Y010134D02* +X002116Y010134D01* +X002116Y010036D02* +X002194Y010036D01* +X002192Y009937D02* +X002116Y009937D01* +X002116Y009839D02* +X002190Y009839D01* +X002188Y009740D02* +X002116Y009740D01* +X002116Y009642D02* +X002186Y009642D01* +X002184Y009543D02* +X002035Y009543D01* +X002182Y009445D02* +X001656Y009445D01* +X002009Y009346D02* +X002179Y009346D01* +X002122Y009247D02* +X002073Y009247D01* +X002092Y009201D02* +X002092Y009201D01* +X002083Y008755D02* +X002117Y008755D01* +X002123Y008656D02* +X002096Y008656D01* +X002099Y008558D02* +X002128Y008558D01* +X002134Y008459D02* +X002097Y008459D01* +X002072Y008361D02* +X002139Y008361D01* +X002145Y008262D02* +X001979Y008262D01* +X002005Y008163D02* +X002150Y008163D01* +X002156Y008065D02* +X002077Y008065D01* +X002109Y007966D02* +X002161Y007966D01* +X002167Y007868D02* +X002094Y007868D01* +X002114Y007769D02* +X002172Y007769D01* +X002166Y007671D02* +X002136Y007671D01* +X002142Y007572D02* +X002222Y007572D01* +X002137Y007474D02* +X005450Y007474D01* +X005450Y007375D02* +X002117Y007375D01* +X002064Y007277D02* +X005483Y007277D01* +X005768Y007277D02* +X005865Y007277D01* +X005865Y007375D02* +X005768Y007375D01* +X005768Y007474D02* +X005865Y007474D01* +X005817Y007568D02* +X005301Y007568D01* +X005450Y007671D02* +X002695Y007671D01* +X002870Y007769D02* +X005450Y007769D01* +X005491Y007868D02* +X003044Y007868D01* +X003370Y007966D02* +X007101Y007966D01* +X007101Y007868D02* +X006812Y007868D01* +X006852Y007769D02* +X007101Y007769D01* +X007301Y007668D02* +X007301Y009868D01* +X007101Y009445D02* +X007048Y009445D01* +X007101Y009346D02* +X004029Y009346D01* +X004190Y009247D02* +X007101Y009247D01* +X007101Y009149D02* +X004350Y009149D01* +X004527Y009050D02* +X007101Y009050D01* +X007101Y008952D02* +X004626Y008952D01* +X004636Y008853D02* +X007101Y008853D01* +X007101Y008755D02* +X004603Y008755D01* +X004449Y008656D02* +X007101Y008656D01* +X007101Y008558D02* +X004295Y008558D01* +X004141Y008459D02* +X007101Y008459D01* +X007101Y008361D02* +X003987Y008361D01* +X003833Y008262D02* +X007101Y008262D01* +X007101Y008163D02* +X003679Y008163D01* +X003524Y008065D02* +X007101Y008065D01* +X007301Y007668D02* +X007614Y007356D01* +X007961Y007356D01* +X007961Y007041D02* +X007529Y007041D01* +X007001Y007568D01* +X006486Y007568D01* +X006820Y007277D02* +X007010Y007277D01* +X007109Y007178D02* +X001984Y007178D01* +X001857Y006784D02* +X000651Y006784D01* +X000651Y006685D02* +X001785Y006685D01* +X001785Y006587D02* +X000651Y006587D01* +X000651Y006488D02* +X001785Y006488D01* +X001785Y006390D02* +X000651Y006390D01* +X000651Y006291D02* +X001785Y006291D01* +X001786Y006193D02* +X000651Y006193D01* +X000708Y006094D02* +X001836Y006094D01* +X001785Y005996D02* +X000807Y005996D01* +X000905Y005897D02* +X001785Y005897D01* +X001785Y005798D02* +X001004Y005798D01* +X001102Y005700D02* +X001785Y005700D01* +X001785Y005601D02* +X001201Y005601D01* +X001300Y005503D02* +X001807Y005503D01* +X001595Y005207D02* +X002189Y005207D01* +X002189Y005109D02* +X001694Y005109D01* +X001792Y005010D02* +X002189Y005010D01* +X002189Y004912D02* +X001891Y004912D01* +X001989Y004813D02* +X002189Y004813D01* +X002189Y004714D02* +X002088Y004714D01* +X002151Y004616D02* +X002189Y004616D01* +X002189Y004517D02* +X002151Y004517D01* +X002151Y004419D02* +X002189Y004419D01* +X002189Y004320D02* +X002151Y004320D01* +X002151Y004222D02* +X002189Y004222D01* +X002189Y004123D02* +X002151Y004123D01* +X002151Y004025D02* +X002189Y004025D01* +X002189Y003926D02* +X002151Y003926D01* +X002151Y003828D02* +X002189Y003828D01* +X002189Y003729D02* +X002151Y003729D01* +X002151Y003630D02* +X002189Y003630D01* +X002183Y003532D02* +X002189Y003532D01* +X001751Y003630D02* +X000301Y003630D01* +X000301Y003532D02* +X001720Y003532D01* +X001641Y003433D02* +X000301Y003433D01* +X000301Y003335D02* +X001601Y003335D01* +X001601Y003236D02* +X000301Y003236D01* +X000301Y003138D02* +X001626Y003138D01* +X001751Y003729D02* +X000301Y003729D01* +X000301Y003828D02* +X001751Y003828D01* +X001751Y003926D02* +X000301Y003926D01* +X000301Y004025D02* +X001751Y004025D01* +X001751Y004123D02* +X000301Y004123D01* +X000301Y004222D02* +X001751Y004222D01* +X001751Y004320D02* +X000301Y004320D01* +X000301Y004419D02* +X001751Y004419D01* +X001719Y004517D02* +X000301Y004517D01* +X000301Y004616D02* +X001621Y004616D01* +X001522Y004714D02* +X000301Y004714D01* +X000301Y004813D02* +X001424Y004813D01* +X001325Y004912D02* +X000301Y004912D01* +X000301Y005010D02* +X001227Y005010D01* +X001128Y005109D02* +X000301Y005109D01* +X000301Y005207D02* +X001029Y005207D01* +X000931Y005306D02* +X000301Y005306D01* +X000301Y005404D02* +X000832Y005404D01* +X000734Y005503D02* +X000301Y005503D01* +X000301Y005601D02* +X000635Y005601D01* +X000537Y005700D02* +X000301Y005700D01* +X000301Y005798D02* +X000438Y005798D01* +X000340Y005897D02* +X000301Y005897D01* +X001398Y005404D02* +X002189Y005404D01* +X002189Y005306D02* +X001497Y005306D01* +X002151Y005784D02* +X002389Y005546D01* +X002389Y002604D01* +X004394Y002152D02* +X005804Y002152D01* +X005804Y002054D02* +X004667Y002054D01* +X004724Y001955D02* +X005804Y001955D01* +X006498Y001955D02* +X012804Y001955D01* +X012804Y002054D02* +X006498Y002054D01* +X006498Y002152D02* +X006605Y002152D01* +X006601Y002251D02* +X006498Y002251D01* +X006550Y002349D02* +X006601Y002349D01* +X004724Y001857D02* +X015275Y001857D01* +X015181Y001758D02* +X004724Y001758D01* +X004724Y001660D02* +X015181Y001660D01* +X015181Y001463D02* +X004724Y001463D01* +X004724Y001561D02* +X015601Y001561D01* +X015601Y001660D02* +X015701Y001660D01* +X015701Y001758D02* +X015601Y001758D01* +X015601Y001857D02* +X015701Y001857D01* +X015701Y001955D02* +X015601Y001955D01* +X015929Y001955D02* +X016373Y001955D01* +X016275Y001857D02* +X016027Y001857D01* +X016121Y001758D02* +X016181Y001758D01* +X016181Y001660D02* +X016121Y001660D01* +X016121Y001463D02* +X016181Y001463D01* +X016191Y001364D02* +X016111Y001364D01* +X016013Y001265D02* +X016289Y001265D01* +X016601Y001561D02* +X015701Y001561D01* +X015289Y001265D02* +X004724Y001265D01* +X004724Y001364D02* +X015191Y001364D01* +X015373Y001955D02* +X013498Y001955D01* +X013498Y002054D02* +X017001Y002054D01* +X017001Y002152D02* +X016900Y002152D01* +X016998Y002251D02* +X017001Y002251D01* +X016701Y002251D02* +X016601Y002251D01* +X016601Y002349D02* +X016701Y002349D01* +X016701Y002448D02* +X016601Y002448D01* +X016601Y002518D02* +X016601Y002098D01* +X016456Y002098D01* +X016181Y002374D01* +X016181Y002518D01* +X016601Y002518D01* +X016601Y002546D02* +X016121Y002546D01* +X016121Y002448D02* +X016181Y002448D01* +X016205Y002349D02* +X016097Y002349D01* +X015998Y002251D02* +X016304Y002251D01* +X016403Y002152D02* +X015900Y002152D01* +X015403Y002152D02* +X013498Y002152D01* +X013151Y002234D02* +X012817Y002568D01* +X011551Y002568D01* +X009551Y002568D01* +X008864Y003256D01* +X008864Y004878D01* +X008549Y004878D02* +X008549Y002571D01* +X008551Y002568D01* +X009166Y003236D02* +X012859Y003236D01* +X012753Y002349D02* +X012701Y002349D01* +X012701Y002251D02* +X012804Y002251D01* +X012804Y002152D02* +X012697Y002152D01* +X011101Y004068D02* +X010451Y004068D01* +X010124Y004396D01* +X010124Y004878D01* +X010439Y004878D02* +X010439Y004531D01* +X010601Y004368D01* +X010951Y004368D01* +X011401Y004818D01* +X014601Y004818D01* +X015651Y003768D01* +X015651Y003568D01* +X015958Y003926D02* +X016344Y003926D01* +X016246Y003828D02* +X016057Y003828D01* +X016121Y003729D02* +X016181Y003729D01* +X016181Y003630D02* +X016121Y003630D01* +X015859Y004025D02* +X016443Y004025D01* +X016432Y004123D02* +X015871Y004123D01* +X015969Y004222D02* +X016333Y004222D01* +X016235Y004320D02* +X016068Y004320D01* +X016121Y004419D02* +X016181Y004419D01* +X016181Y004517D02* +X016121Y004517D01* +X015651Y004568D02* +X015451Y004568D01* +X014901Y005118D01* +X010901Y005118D01* +X010754Y004971D01* +X010754Y004878D01* +X011341Y005466D02* +X015549Y005466D01* +X015651Y005568D01* +X015348Y005207D02* +X015095Y005207D01* +X015194Y005109D02* +X015446Y005109D01* +X015428Y005010D02* +X015292Y005010D01* +X014351Y004518D02* +X011551Y004518D01* +X011101Y004068D01* +X010311Y003926D02* +X009064Y003926D01* +X009064Y004025D02* +X010212Y004025D01* +X010113Y004123D02* +X009064Y004123D01* +X009064Y004222D02* +X010015Y004222D01* +X009924Y004320D02* +X009064Y004320D01* +X009064Y004419D02* +X009924Y004419D01* +X009351Y004218D02* +X009179Y004391D01* +X009179Y004878D01* +X009176Y004813D02* +X009181Y004813D01* +X009176Y004714D02* +X009181Y004714D01* +X009176Y004616D02* +X009181Y004616D01* +X009176Y004517D02* +X009181Y004517D01* +X008294Y004912D02* +X006477Y004912D01* +X006576Y005010D02* +X008294Y005010D01* +X008294Y005109D02* +X006674Y005109D01* +X006701Y005207D02* +X008311Y005207D01* +X007961Y005466D02* +X007961Y005781D01* +X007961Y006096D02* +X007939Y006118D01* +X002501Y006118D01* +X002167Y006453D01* +X002151Y006453D01* +X002518Y006488D02* +X002785Y006488D01* +X002785Y006390D02* +X002518Y006390D01* +X002518Y006587D02* +X002785Y006587D01* +X002785Y006685D02* +X002518Y006685D01* +X002446Y006784D02* +X002857Y006784D01* +X003151Y006453D02* +X003217Y006453D01* +X003251Y006418D01* +X007954Y006418D01* +X007961Y006411D01* +X008361Y006390D02* +X009547Y006390D01* +X009609Y006291D02* +X008361Y006291D01* +X008361Y006193D02* +X009609Y006193D01* +X009609Y006094D02* +X008361Y006094D01* +X008361Y005996D02* +X009609Y005996D01* +X009609Y005897D02* +X008361Y005897D01* +X008361Y005798D02* +X009609Y005798D01* +X009609Y005700D02* +X008361Y005700D01* +X007561Y005700D02* +X006852Y005700D01* +X006852Y005798D02* +X007561Y005798D01* +X007561Y005897D02* +X006783Y005897D01* +X006189Y005897D02* +X006105Y005897D01* +X005865Y005897D02* +X005768Y005897D01* +X005768Y005798D02* +X005865Y005798D01* +X005865Y005700D02* +X005768Y005700D01* +X005528Y005897D02* +X003518Y005897D01* +X002520Y007572D02* +X005768Y007572D01* +X005768Y007671D02* +X005865Y007671D01* +X005865Y007769D02* +X005768Y007769D01* +X005768Y007868D02* +X005865Y007868D01* +X006143Y007868D02* +X006160Y007868D01* +X006150Y007277D02* +X006152Y007277D01* +X008357Y006587D02* +X009350Y006587D01* +X009448Y006488D02* +X008361Y006488D01* +X009451Y005601D02* +X009609Y005601D01* +X009609Y005503D02* +X009550Y005503D01* +X011341Y005781D02* +X014864Y005781D01* +X016969Y004222D02* +X017001Y004222D01* +X017001Y004123D02* +X016871Y004123D01* +X016859Y004025D02* +X017001Y004025D01* +X017001Y003926D02* +X016958Y003926D01* +X016943Y002941D02* +X017001Y002941D01* +X016701Y002941D02* +X016601Y002941D01* +X016601Y002842D02* +X016701Y002842D01* +X016701Y002744D02* +X016601Y002744D01* +X016601Y002645D02* +X016701Y002645D01* +X016359Y002941D02* +X015943Y002941D01* +X016042Y002842D02* +X016260Y002842D01* +X016181Y002744D02* +X016121Y002744D01* +X016121Y002645D02* +X016181Y002645D01* +X016601Y002152D02* +X016701Y002152D01* +X016701Y001955D02* +X016601Y001955D01* +X016601Y001857D02* +X016701Y001857D01* +X016701Y001758D02* +X016601Y001758D01* +X016601Y001660D02* +X016701Y001660D01* +X016929Y001955D02* +X017001Y001955D01* +X010754Y008258D02* +X010754Y010371D01* +X011151Y010768D01* +X011851Y010768D01* +X014050Y012302D02* +X014253Y012302D01* +X014181Y012401D02* +X014121Y012401D01* +X014121Y012499D02* +X014181Y012499D01* +X014181Y012696D02* +X014121Y012696D01* +X014089Y012795D02* +X014213Y012795D01* +X014312Y012894D02* +X013991Y012894D01* +X014991Y012894D02* +X015312Y012894D01* +X015213Y012795D02* +X015089Y012795D01* +X015121Y012696D02* +X015181Y012696D01* +X015181Y012499D02* +X015121Y012499D01* +X015121Y012401D02* +X015181Y012401D01* +X015253Y012302D02* +X015050Y012302D01* +X016980Y010233D02* +X017001Y010233D01* +X017001Y010134D02* +X016882Y010134D01* +X016848Y010036D02* +X017001Y010036D01* +X017001Y009937D02* +X016947Y009937D01* +X016995Y009247D02* +X017001Y009247D01* +X017001Y009149D02* +X016896Y009149D01* +X006238Y011317D02* +X006185Y011317D01* +X006181Y011415D02* +X006121Y011415D01* +X006121Y011514D02* +X006181Y011514D01* +X006181Y011711D02* +X006121Y011711D01* +X006075Y011810D02* +X006228Y011810D01* +X006253Y012302D02* +X006050Y012302D01* +X006121Y012401D02* +X006181Y012401D01* +X006181Y012499D02* +X006121Y012499D01* +X006121Y012696D02* +X006181Y012696D01* +X006213Y012795D02* +X006089Y012795D01* +X005991Y012894D02* +X006312Y012894D01* +X005312Y012894D02* +X004991Y012894D01* +X005089Y012795D02* +X005213Y012795D01* +X005181Y012696D02* +X005121Y012696D01* +X005121Y012499D02* +X005181Y012499D01* +X005181Y012401D02* +X005121Y012401D01* +X005050Y012302D02* +X005253Y012302D01* +X002507Y010430D02* +X002116Y010430D01* +X002116Y010529D02* +X002408Y010529D01* +X002310Y010627D02* +X002021Y010627D01* +D22* +X005301Y007568D03* +X005301Y005568D03* +X006151Y003518D03* +X009351Y004218D03* +X008601Y006718D03* +X006801Y009718D03* +X008101Y010318D03* +X001501Y003668D03* +X013151Y003518D03* +D23* +X003051Y003768D03* +X001951Y003268D03* +D24* +X001606Y007293D02* +X001666Y007333D01* +X001696Y007403D01* +X001726Y007503D01* +X001756Y007633D01* +X001806Y007693D01* +X001826Y007703D01* +X001876Y007683D01* +X001916Y007633D01* +X001946Y007553D01* +X001946Y007633D01* +X001916Y007733D01* +X001876Y007803D01* +X001786Y007803D01* +X001746Y007743D01* +X001786Y007743D01* +X001866Y007753D01* +X001746Y007743D02* +X001716Y007613D01* +X001666Y007463D01* +X001616Y007413D01* +X001536Y007413D01* +X001476Y007493D01* +X001466Y007523D02* +X001476Y007423D01* +X001506Y007343D01* +X001506Y007373D01* +X001526Y007373D01* +X001626Y007363D01* +X001606Y007293D02* +X001566Y007293D01* +X001506Y007343D01* +X001466Y007523D02* +X001466Y007643D01* +X001486Y007723D01* +X001516Y007783D01* +X001916Y007963D02* +X001726Y008223D01* +X001686Y008223D01* +X001476Y008223D01* +X001476Y008323D01* +X001686Y008323D01* +X001906Y008583D01* +X001906Y008463D01* +X001716Y008256D01* +X001716Y008263D01* +X001696Y008263D01* +X001506Y008273D01* +X001686Y008223D02* +X001716Y008256D01* +X001596Y008703D02* +X001656Y008743D01* +X001686Y008813D01* +X001716Y008913D01* +X001746Y009043D01* +X001796Y009103D01* +X001816Y009113D01* +X001866Y009093D01* +X001906Y009043D01* +X001936Y008963D01* +X001934Y008944D01* +X001934Y008943D01* +X001936Y008943D01* +X001936Y009043D01* +X001906Y009143D01* +X001866Y009213D01* +X001776Y009213D01* +X001756Y009183D01* +X001756Y009153D01* +X001776Y009153D01* +X001856Y009163D01* +X001756Y009183D02* +X001736Y009153D01* +X001706Y009023D01* +X001656Y008873D01* +X001606Y008823D01* +X001526Y008823D01* +X001466Y008903D01* +X001456Y008933D02* +X001466Y008833D01* +X001496Y008753D01* +X001516Y008753D01* +X001516Y008783D01* +X001616Y008773D01* +X001596Y008703D02* +X001556Y008703D01* +X001496Y008753D01* +X001456Y008933D02* +X001456Y009053D01* +X001476Y009133D01* +X001506Y009193D01* +X001466Y009353D02* +X001466Y009703D01* +X001466Y009753D01* +X001926Y009753D01* +X001926Y009703D01* +X001466Y009703D01* +X001466Y009753D02* +X001466Y009803D01* +X001926Y009803D01* +X001926Y009753D01* +X001926Y009993D02* +X001926Y010343D01* +X001466Y010343D01* +X001466Y010403D01* +X001926Y010403D01* +X001926Y010453D01* +X001716Y010453D01* +X001716Y009993D01* +X001466Y009993D02* +X001466Y010343D01* +X001466Y010403D02* +X001466Y010453D01* +X001716Y010453D01* +X001926Y010403D02* +X001926Y010343D01* +X002386Y010123D02* +X002996Y009013D01* +X003216Y009143D02* +X003216Y008863D01* +X004436Y008873D01* +X003216Y008093D01* +X003146Y008143D01* +X002366Y007703D01* +X002286Y009133D01* +X002366Y009193D01* +X002386Y010123D01* +X003686Y009413D01* +X003686Y009333D01* +X004436Y008873D01* +X003686Y009413D02* +X002766Y008883D01* +X002976Y008743D02* +X002366Y007713D01* +X001946Y007553D02* +X001946Y007533D01* +X001946Y007553D02* +X001936Y007463D01* +X001886Y007343D01* +X001296Y007303D02* +X001296Y010443D01* +X002286Y009133D02* +X002976Y008743D01* +X003206Y008613D01* +X003216Y008863D02* +X003216Y008093D01* +X001876Y008753D02* +X001926Y008873D01* +X001934Y008944D01* +M02* diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTO b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTO new file mode 100644 index 00000000..58dee794 --- /dev/null +++ b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTO @@ -0,0 +1,244 @@ +G75* +G70* +%OFA0B0*% +%FSLAX24Y24*% +%IPPOS*% +%LPD*% +%AMOC8* +5,1,8,0,0,1.08239X$1,22.5* +% +%ADD10C,0.0060*% +%ADD11C,0.0080*% +D10* +X000475Y010598D02* +X000431Y010642D01* +X000475Y010598D02* +X000561Y010598D01* +X000605Y010642D01* +X000605Y010728D01* +X000561Y010772D01* +X000518Y010772D01* +X000431Y010728D01* +X000431Y010859D01* +X000605Y010859D01* +X000726Y010859D02* +X000726Y010685D01* +X000813Y010598D01* +X000899Y010685D01* +X000899Y010859D01* +X002931Y010815D02* +X002931Y010642D01* +X002975Y010598D01* +X003061Y010598D01* +X003105Y010642D01* +X003226Y010598D02* +X003399Y010772D01* +X003399Y010815D01* +X003356Y010859D01* +X003269Y010859D01* +X003226Y010815D01* +X003105Y010815D02* +X003061Y010859D01* +X002975Y010859D01* +X002931Y010815D01* +X003226Y010598D02* +X003399Y010598D01* +X003815Y010728D02* +X003989Y010728D01* +X003945Y010598D02* +X003945Y010859D01* +X003815Y010728D01* +X004110Y010642D02* +X004153Y010642D01* +X004153Y010598D01* +X004110Y010598D01* +X004110Y010642D01* +X004257Y010642D02* +X004300Y010642D01* +X004300Y010598D01* +X004257Y010598D01* +X004257Y010642D01* +X004404Y010642D02* +X004404Y010598D01* +X004404Y010642D02* +X004578Y010815D01* +X004578Y010859D01* +X004404Y010859D01* +X006306Y010815D02* +X006306Y010642D01* +X006350Y010598D01* +X006436Y010598D01* +X006480Y010642D01* +X006480Y010728D01* +X006393Y010728D01* +X006480Y010815D02* +X006436Y010859D01* +X006350Y010859D01* +X006306Y010815D01* +X006601Y010859D02* +X006601Y010598D01* +X006774Y010598D02* +X006774Y010859D01* +X006895Y010859D02* +X007026Y010859D01* +X007069Y010815D01* +X007069Y010642D01* +X007026Y010598D01* +X006895Y010598D01* +X006895Y010859D01* +X006774Y010598D02* +X006601Y010859D01* +X007431Y010859D02* +X007431Y010728D01* +X007518Y010772D01* +X007561Y010772D01* +X007605Y010728D01* +X007605Y010642D01* +X007561Y010598D01* +X007475Y010598D01* +X007431Y010642D01* +X007726Y010685D02* +X007813Y010598D01* +X007899Y010685D01* +X007899Y010859D01* +X007726Y010859D02* +X007726Y010685D01* +X007605Y010859D02* +X007431Y010859D01* +X011556Y010859D02* +X011556Y010598D01* +X011686Y010598D01* +X011730Y010642D01* +X011730Y010685D01* +X011686Y010728D01* +X011556Y010728D01* +X011686Y010728D02* +X011730Y010772D01* +X011730Y010815D01* +X011686Y010859D01* +X011556Y010859D01* +X011851Y010859D02* +X012024Y010859D01* +X012024Y010815D01* +X011851Y010642D01* +X011851Y010598D01* +X012145Y010598D02* +X012189Y010598D01* +X012189Y010642D01* +X012145Y010642D01* +X012145Y010598D01* +X012293Y010598D02* +X012336Y010598D01* +X012336Y010642D01* +X012293Y010642D01* +X012293Y010598D01* +X012440Y010642D02* +X012614Y010815D01* +X012614Y010642D01* +X012570Y010598D01* +X012483Y010598D01* +X012440Y010642D01* +X012440Y010815D01* +X012483Y010859D01* +X012570Y010859D01* +X012614Y010815D01* +X014681Y010745D02* +X014681Y010658D01* +X014725Y010615D01* +X014811Y010615D01* +X014855Y010658D01* +X014855Y010702D01* +X014811Y010788D01* +X014941Y010788D01* +X014941Y010615D01* +X014941Y010494D02* +X014768Y010494D01* +X014681Y010407D01* +X014768Y010320D01* +X014941Y010320D01* +X014941Y009913D02* +X014681Y009913D01* +X014811Y009913D02* +X014811Y009740D01* +X014941Y009740D02* +X014681Y009740D01* +X014681Y009619D02* +X014768Y009532D01* +X014681Y009445D01* +X014941Y009445D01* +X014941Y009324D02* +X014681Y009324D01* +X014681Y009194D01* +X014725Y009151D01* +X014768Y009151D01* +X014811Y009194D01* +X014811Y009324D01* +X014811Y009194D02* +X014855Y009151D01* +X014898Y009151D01* +X014941Y009194D01* +X014941Y009324D01* +X014941Y009619D02* +X014681Y009619D01* +X014681Y010745D02* +X014725Y010788D01* +X014681Y006538D02* +X014681Y006408D01* +X014725Y006365D01* +X014898Y006365D01* +X014941Y006408D01* +X014941Y006538D01* +X014681Y006538D01* +X014681Y006244D02* +X014725Y006244D01* +X014898Y006070D01* +X014941Y006070D01* +X014941Y006244D01* +X014725Y005949D02* +X014725Y005906D01* +X014681Y005906D01* +X014681Y005949D01* +X014725Y005949D01* +X014725Y005802D02* +X014725Y005758D01* +X014681Y005758D01* +X014681Y005802D01* +X014725Y005802D01* +X014725Y005654D02* +X014898Y005481D01* +X014725Y005481D01* +X014681Y005524D01* +X014681Y005611D01* +X014725Y005654D01* +X014898Y005654D01* +X014941Y005611D01* +X014941Y005524D01* +X014898Y005481D01* +X014898Y002038D02* +X014725Y002038D01* +X014681Y001995D01* +X014681Y001908D01* +X014725Y001865D01* +X014811Y001865D01* +X014811Y001952D01* +X014898Y002038D02* +X014941Y001995D01* +X014941Y001908D01* +X014898Y001865D01* +X014941Y001744D02* +X014681Y001744D01* +X014681Y001570D02* +X014941Y001570D01* +X014941Y001449D02* +X014681Y001449D01* +X014681Y001319D01* +X014725Y001276D01* +X014898Y001276D01* +X014941Y001319D01* +X014941Y001449D01* +X014681Y001570D02* +X014941Y001744D01* +D11* +X016026Y009568D02* +X016276Y009568D01* +M02* diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTS b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTS new file mode 100644 index 00000000..69dc6d3c --- /dev/null +++ b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.GTS @@ -0,0 +1,137 @@ +G75* +G70* +%OFA0B0*% +%FSLAX24Y24*% +%IPPOS*% +%LPD*% +%AMOC8* +5,1,8,0,0,1.08239X$1,22.5* +% +%ADD10R,0.0290X0.0580*% +%ADD11R,0.0580X0.0290*% +%ADD12R,0.0474X0.0513*% +%ADD13R,0.0513X0.0474*% +%ADD14R,0.2080X0.0780*% +%ADD15OC8,0.0720*% +%ADD16R,0.0218X0.0631*% +%ADD17R,0.0789X0.0828*% +%ADD18R,0.0907X0.0710*% +D10* +X008549Y004878D03* +X008864Y004878D03* +X009179Y004878D03* +X009494Y004878D03* +X009809Y004878D03* +X010124Y004878D03* +X010439Y004878D03* +X010754Y004878D03* +X010754Y008258D03* +X010439Y008258D03* +X010124Y008258D03* +X009809Y008258D03* +X009494Y008258D03* +X009179Y008258D03* +X008864Y008258D03* +X008549Y008258D03* +D11* +X007961Y007671D03* +X007961Y007356D03* +X007961Y007041D03* +X007961Y006726D03* +X007961Y006411D03* +X007961Y006096D03* +X007961Y005781D03* +X007961Y005466D03* +X011341Y005466D03* +X011341Y005781D03* +X011341Y006096D03* +X011341Y006411D03* +X011341Y006726D03* +X011341Y007041D03* +X011341Y007356D03* +X011341Y007671D03* +D12* +X013151Y002903D03* +X013151Y002234D03* +X006151Y002234D03* +X006151Y002903D03* +D13* +X005817Y005568D03* +X006486Y005568D03* +X006486Y007568D03* +X005817Y007568D03* +X003151Y006453D03* +X003151Y005784D03* +X002151Y005784D03* +X002151Y006453D03* +D14* +X007751Y002568D03* +X011551Y002568D03* +D15* +X015651Y002568D03* +X016651Y002568D03* +X016651Y001568D03* +X015651Y001568D03* +X015651Y003568D03* +X016651Y003568D03* +X016651Y004568D03* +X015651Y004568D03* +X015651Y005568D03* +X016651Y005568D03* +X016651Y006568D03* +X015651Y006568D03* +X015651Y007568D03* +X016651Y007568D03* +X016651Y008568D03* +X015651Y008568D03* +X015651Y009568D03* +X016651Y009568D03* +X016651Y010568D03* +X015651Y010568D03* +X015651Y011568D03* +X016651Y011568D03* +X016651Y012568D03* +X015651Y012568D03* +X014651Y012568D03* +X013651Y012568D03* +X012651Y012568D03* +X011651Y012568D03* +X010651Y012568D03* +X009651Y012568D03* +X008651Y012568D03* +X007651Y012568D03* +X006651Y012568D03* +X005651Y012568D03* +X004651Y012568D03* +X003651Y012568D03* +X002651Y012568D03* +X001651Y012568D03* +X000651Y012568D03* +X000651Y011568D03* +X001651Y011568D03* +X002651Y011568D03* +X003651Y011568D03* +X004651Y011568D03* +X005651Y011568D03* +X006651Y011568D03* +X007651Y011568D03* +X008651Y011568D03* +X009651Y011568D03* +X010651Y011568D03* +X011651Y011568D03* +X012651Y011568D03* +X013651Y011568D03* +X014651Y011568D03* +D16* +X003157Y002604D03* +X002901Y002604D03* +X002645Y002604D03* +X002389Y002604D03* +X002133Y002604D03* +D17* +X001070Y001560D03* +X004220Y001560D03* +D18* +X003866Y002564D03* +X001425Y002564D03* +M02* diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.TXT b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.TXT new file mode 100644 index 00000000..7723a9ac --- /dev/null +++ b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.TXT @@ -0,0 +1,76 @@ +% +M48 +M72 +T01C0.0200 +T02C0.0236 +T03C0.0400 +% +T01 +X1951Y3268 +X3051Y3768 +T02 +X1501Y3668 +X5301Y5568 +X5301Y7568 +X6801Y9718 +X8101Y10318 +X8601Y6718 +X9351Y4218 +X6151Y3518 +X13151Y3518 +T03 +X15651Y3568 +X16651Y3568 +X16651Y2568 +X15651Y2568 +X15651Y1568 +X16651Y1568 +X16651Y4568 +X15651Y4568 +X15651Y5568 +X16651Y5568 +X16651Y6568 +X15651Y6568 +X15651Y7568 +X16651Y7568 +X16651Y8568 +X15651Y8568 +X15651Y9568 +X16651Y9568 +X16651Y10568 +X15651Y10568 +X15651Y11568 +X16651Y11568 +X16651Y12568 +X15651Y12568 +X14651Y12568 +X13651Y12568 +X12651Y12568 +X11651Y12568 +X10651Y12568 +X9651Y12568 +X8651Y12568 +X7651Y12568 +X6651Y12568 +X5651Y12568 +X4651Y12568 +X3651Y12568 +X2651Y12568 +X1651Y12568 +X651Y12568 +X651Y11568 +X1651Y11568 +X2651Y11568 +X3651Y11568 +X4651Y11568 +X5651Y11568 +X6651Y11568 +X7651Y11568 +X8651Y11568 +X9651Y11568 +X10651Y11568 +X11651Y11568 +X12651Y11568 +X13651Y11568 +X14651Y11568 +M30 diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.brd b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.brd new file mode 100644 index 00000000..164d5e8c Binary files /dev/null and b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.brd differ diff --git a/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.sch b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.sch new file mode 100644 index 00000000..e051c246 Binary files /dev/null and b/digital/cms-intro/pcb/orders/eurocircuits_2011-05-11/cms_intro.sch differ -- cgit v1.2.3 From f58d19eb2d786cb5446d511acd6839a03d791a44 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 12 May 2011 18:54:58 +0200 Subject: digital/io-hub: add element defines --- digital/io-hub/src/robospierre/element.h | 45 ++++++++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 digital/io-hub/src/robospierre/element.h diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h new file mode 100644 index 00000000..aa7b5b4e --- /dev/null +++ b/digital/io-hub/src/robospierre/element.h @@ -0,0 +1,45 @@ +#ifndef element_h +#define element_h +/* element.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/** A pawn, not a head. */ +#define ELEMENT_PAWN 1 +/** Queen pawn, used for statistic update of table data. */ +#define ELEMENT_QUEEN 2 +/** King pawn, used for statistic update of table data. */ +#define ELEMENT_KING 4 +/** Any head, queen or king. */ +#define ELEMENT_HEAD 6 +/** Any element, pawn, queen, or king. */ +#define ELEMENT_ANY 7 + +/** Return non zero if element is a head, not a pawn. */ +#define ELEMENT_IS_HEAD(e) ((e) && !((e) & ELEMENT_PAWN)) + +/** Return non zero if element may be a head. */ +#define ELEMENT_CAN_BE_HEAD(e) ((e) & ELEMENT_HEAD) + +#endif /* element_h */ -- cgit v1.2.3 From a3fdbecac564a600c65568cac0e02fc72604567e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 12 May 2011 23:42:26 +0200 Subject: digital/{ai,io-hub}, host/simu/robots/robospierre: add doors and element move --- digital/ai/tools/test_simu_control_robospierre.py | 29 +++- digital/io-hub/src/robospierre/bot.h | 16 ++ digital/io-hub/src/robospierre/clamp.c | 169 ++++++++++++++++++++-- digital/io-hub/src/robospierre/clamp.h | 8 + digital/io-hub/src/robospierre/main.c | 17 ++- digital/io-hub/tools/io_hub/io_hub.py | 3 + host/simu/robots/robospierre/model/bag.py | 6 +- host/simu/robots/robospierre/model/clamp.py | 28 ++-- host/simu/robots/robospierre/view/clamp.py | 10 ++ 9 files changed, 258 insertions(+), 28 deletions(-) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index b1cccf4b..88fbc5f3 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -46,6 +46,12 @@ class TestSimuControl (TestSimu): indicatoron = False, variable = self.clamp_var, command = self.clamp_command) self.clamp_button.pack () + self.doors_var = IntVar () + self.doors_var.set (1) + self.doors_button = Checkbutton (self.control_frame, text = 'Doors', + indicatoron = False, + variable = self.doors_var, command = self.doors_command) + self.doors_button.pack () self.elevation_up_button = Button (self.control_frame, text = 'Elevation up', padx = 0, pady = 0, command = self.elevation_up_command) @@ -69,6 +75,13 @@ class TestSimuControl (TestSimu): text = 'Move clamp', padx = 0, pady = 0, command = self.clamp_move_command) self.clamp_move_button.pack () + self.clamp_to_scale = Scale (self.control_frame, orient = HORIZONTAL, + from_ = 0, to = 6) + self.clamp_to_scale.pack () + self.clamp_element_move_button = Button (self.control_frame, + text = 'Move element', padx = 0, pady = 0, + command = self.clamp_move_element_command) + self.clamp_element_move_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) @@ -85,9 +98,9 @@ class TestSimuControl (TestSimu): def clamp_command (self): if self.clamp_var.get (): - self.io.pwm_set_timed (0, -0x3ff, 255, 0) + self.io.pwm_set_timed (2, -0x3ff, 255, 0) else: - self.io.pwm_set_timed (0, 0x3ff, 255, 0) + self.io.pwm_set_timed (2, 0x3ff, 255, 0) def elevation_up_command (self): self.mimot.speed_pos ('a0', self.ELEVATION_STROKE / 2) @@ -104,6 +117,18 @@ class TestSimuControl (TestSimu): def clamp_move_command (self): self.io.clamp_move (self.clamp_pos_scale.get ()) + def clamp_move_element_command (self): + self.io.clamp_move_element (self.clamp_pos_scale.get (), + self.clamp_to_scale.get ()) + + def doors_command (self): + if self.doors_var.get (): + pwm = -0x3ff + else: + pwm = 0x3ff + for i in (0, 1, 3, 4): + self.io.pwm_set_timed (i, pwm, 255, 0) + def change_color (self, *dummy): pass diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 9043a62c..5e795b0f 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -73,4 +73,20 @@ #define BOT_CLAMP_ELEVATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_SPEED 0x30 +#define BOT_PWM_CLAMP 2 +#define BOT_PWM_DOOR_FRONT_BOTTOM 0 +#define BOT_PWM_DOOR_FRONT_TOP 1 +#define BOT_PWM_DOOR_BACK_BOTTOM 3 +#define BOT_PWM_DOOR_BACK_TOP 4 + +#define BOT_PWM_CLAMP_OPEN_TIME 225 +#define BOT_PWM_CLAMP_OPEN 0x3ff, 225, 0 +#define BOT_PWM_CLAMP_CLOSE_TIME 225 +#define BOT_PWM_CLAMP_CLOSE -0x3ff, 225, 0 + +#define BOT_PWM_DOOR_OPEN_TIME 225 +#define BOT_PWM_DOOR_OPEN 0x3ff, 225, 0 +#define BOT_PWM_DOOR_CLOSE_TIME 225 +#define BOT_PWM_DOOR_CLOSE -0x3ff, 225, 0 + #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 2f1b2471..5d9533fa 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -29,15 +29,28 @@ #include "fsm.h" #include "mimot.h" +#include "pwm.h" #include "bot.h" FSM_INIT FSM_STATES ( - /* Wait order. */ - CLAMP_IDLE, + /* Waiting movement order. */ + CLAMP_MOVE_IDLE, /* Moving to a final or intermediary position. */ - CLAMP_ROUTING) + CLAMP_MOVE_ROUTING, + /* Moving to source slot. */ + CLAMP_MOVE_SRC_ROUTING, + /* Closing the clamp once arrived at source. */ + CLAMP_MOVE_SRC_CLAMP_CLOSING, + /* Opening door once clamp closed. */ + CLAMP_MOVE_SRC_DOOR_OPENDING, + /* Moving to destination slot. */ + CLAMP_MOVE_DST_ROUTING, + /* Closing door once arrived at destination. */ + CLAMP_MOVE_DST_DOOR_CLOSING, + /* Opening the clamp once door closed. */ + CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( /* Order to move the clamp. */ @@ -49,15 +62,17 @@ FSM_EVENTS ( /* Rotation motor failure. */ clamp_rotation_failure) -FSM_START_WITH (CLAMP_IDLE) +FSM_START_WITH (CLAMP_MOVE_IDLE) /** Clamp context. */ struct clamp_t { - /* Current position. */ + /** Current position. */ uint8_t pos_current; - /* Requested position. */ + /** Requested position. */ uint8_t pos_request; + /** Element moving destination. */ + uint8_t moving_to; }; /** Global context. */ @@ -88,16 +103,36 @@ static const uint16_t clamp_pos[][2] = { BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, }; +/** Slot doors. */ +static const uint8_t clamp_slot_door[] = { + BOT_PWM_DOOR_FRONT_BOTTOM, + 0xff, + BOT_PWM_DOOR_FRONT_TOP, + BOT_PWM_DOOR_BACK_BOTTOM, + 0xff, + BOT_PWM_DOOR_BACK_TOP, + 0xff +}; + void clamp_move (uint8_t pos) { if (pos != ctx.pos_current) { ctx.pos_request = pos; + ctx.moving_to = CLAMP_POS_NB; FSM_HANDLE (AI, clamp_move); } } +void +clamp_move_element (uint8_t from, uint8_t to) +{ + ctx.pos_request = from; + ctx.moving_to = to; + FSM_HANDLE (AI, clamp_move); +} + /** Find next position and start motors. */ static void clamp_route (void) @@ -157,26 +192,132 @@ clamp_route (void) ctx.pos_current = pos_new; } -FSM_TRANS (CLAMP_IDLE, clamp_move, CLAMP_ROUTING) +FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, + move, CLAMP_MOVE_ROUTING, + move_element, CLAMP_MOVE_SRC_ROUTING, + move_element_here, CLAMP_MOVE_SRC_CLAMP_CLOSING) { - clamp_route (); - return FSM_NEXT (CLAMP_IDLE, clamp_move); + if (ctx.moving_to == CLAMP_POS_NB) + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move); + } + else + { + if (ctx.pos_current != ctx.pos_request) + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element); + } + else + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element_here); + } + } } -FSM_TRANS (CLAMP_ROUTING, clamp_elevation_rotation_success, - done, CLAMP_IDLE, - next, CLAMP_ROUTING) +FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, + done, CLAMP_MOVE_IDLE, + next, CLAMP_MOVE_ROUTING) { if (ctx.pos_current == ctx.pos_request) { - return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, done); } else { clamp_route (); - return FSM_NEXT (CLAMP_ROUTING, clamp_elevation_rotation_success, + return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, next); } } +FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, + done, CLAMP_MOVE_SRC_CLAMP_CLOSING, + next, CLAMP_MOVE_SRC_ROUTING) +{ + if (ctx.pos_current == ctx.pos_request) + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, + clamp_elevation_rotation_success, done); + } + else + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, + clamp_elevation_rotation_success, next); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME, + open_door, CLAMP_MOVE_SRC_DOOR_OPENDING, + move, CLAMP_MOVE_DST_ROUTING) +{ + if (clamp_slot_door[ctx.pos_current] != 0xff) + { + pwm_set_timed (clamp_slot_door[ctx.pos_current], BOT_PWM_DOOR_OPEN); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, open_door); + } + else + { + ctx.pos_request = ctx.moving_to; + clamp_route (); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, move); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING, BOT_PWM_DOOR_OPEN_TIME, + CLAMP_MOVE_DST_ROUTING) +{ + ctx.pos_request = ctx.moving_to; + clamp_route (); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_DOOR_OPENDING); +} + +FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, + done_close_door, CLAMP_MOVE_DST_DOOR_CLOSING, + done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, + next, CLAMP_MOVE_DST_ROUTING) +{ + if (ctx.pos_current == ctx.pos_request) + { + if (clamp_slot_door[ctx.pos_current] != 0xff) + { + pwm_set_timed (clamp_slot_door[ctx.pos_current], + BOT_PWM_DOOR_CLOSE); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, + done_close_door); + } + else + { + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, + done_open_clamp); + } + } + else + { + clamp_route (); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_rotation_success, next); + } +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, + CLAMP_MOVE_DST_CLAMP_OPENING) +{ + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING); +} + +FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING, BOT_PWM_CLAMP_OPEN_TIME, + CLAMP_MOVE_IDLE) +{ + return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING); +} + diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 104e3cb8..7b488f92 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -40,6 +40,10 @@ enum { CLAMP_BAY_BACK_LEAVE, /* Enter the side bay. Position on the side, above wheels. */ CLAMP_BAY_SIDE_ENTER_LEAVE, + /** Total number of position, including intermediary positions. */ + CLAMP_POS_NB, + /** Number of slots. */ + CLAMP_SLOT_NB = CLAMP_SLOT_SIDE + 1, }; /** Is slot in front bay? */ @@ -54,4 +58,8 @@ enum { void clamp_move (uint8_t pos); +/** Move element using clamp. */ +void +clamp_move_element (uint8_t from, uint8_t to); + #endif /* clamp_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 3a32a1ae..13bad26c 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -47,6 +47,8 @@ #include "clamp.h" +#include "bot.h" + #include "io.h" /** Our color. */ @@ -93,7 +95,7 @@ main_event_to_fsm (void) #define FSM_HANDLE_TIMEOUT_E(fsm) \ do { if (FSM_HANDLE_TIMEOUT (fsm)) return; } while (0) /* Update FSM timeouts. */ - //FSM_HANDLE_TIMEOUT_E (AI); + FSM_HANDLE_TIMEOUT_E (AI); /* Motor status. */ asserv_status_e mimot_motor0_status, mimot_motor1_status; mimot_motor0_status = mimot_motor0_cmd_status (); @@ -184,6 +186,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: position. */ clamp_move (args[0]); break; + case c ('c', 2): + /* Move element using clamp. + * - 1b: source. + * - 1b: destination. */ + clamp_move_element (args[0], args[1]); + break; + case c ('d', 0): + /* Open all doors. */ + pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_OPEN); + pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_OPEN); + pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_OPEN); + pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_OPEN); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index a363e5f6..1b25102c 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -47,6 +47,9 @@ class Proto: def clamp_move (self, pos): self.proto.send ('c', 'B', pos) + def clamp_move_element (self, from_, to): + self.proto.send ('c', 'BB', from_, to) + def close (self): self.reset () self.proto.wait (lambda: True) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 4d87192c..0a89d00c 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -37,10 +37,12 @@ class Bag: self.contact = [ Switch (contact) for contact in link_bag.io_hub.contact[2:] ] self.position = Position (link_bag.asserv.position) - self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[0], scheduler, + self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler, 2 * pi, 0, pi) + self.door_motors = [ MotorBasic (link_bag.io_hub.pwm[i], scheduler, + 2 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ] self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0], - link_bag.mimot.aux[1], self.clamping_motor) + link_bag.mimot.aux[1], self.clamping_motor, self.door_motors) self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index ff27a7ea..9bb8bacc 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -29,12 +29,13 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side): + def __init__ (self, x, y, z, side, door_motor): self.x = x self.y = y self.z = z self.side = side self.pawn = None + self.door_motor = door_motor class Clamp (Observable): @@ -59,21 +60,30 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor): + rotation_motor, clamping_motor, door_motors): Observable.__init__ (self) self.table = table self.robot_position = robot_position self.elevation_motor = elevation_motor self.rotation_motor = rotation_motor self.clamping_motor = clamping_motor + self.door_motors = (door_motors[0], None, door_motors[1], + door_motors[2], None, door_motors[3], None) self.slots = ( - Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0), - Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0), - Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0), - Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1), - Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1), - Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1), - Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None)) + Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, + door_motors[0]), + Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, + None), + Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, + door_motors[1]), + Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, + door_motors[2]), + Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, + None), + Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, + door_motors[3]), + Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, + None)) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py index bd5f6dab..130c3eea 100644 --- a/host/simu/robots/robospierre/view/clamp.py +++ b/host/simu/robots/robospierre/view/clamp.py @@ -133,6 +133,7 @@ class ClampSide (Drawable): self.draw_pawn ((-slot.x, slot.z), slot.pawn) # Draw clamp. if self.model.rotation is not None: + self.trans_push () self.trans_translate ((0, self.model.elevation)) m = TransMatrix () m.rotate (pi + self.model.rotation) @@ -169,4 +170,13 @@ class ClampSide (Drawable): self.draw_pawn ((lcenter, 0), self.model.load) self.draw_rectangle ((lbase, 10), (lcenter - 103, 40), **attr) self.draw_rectangle ((rbase, 10), (rtip, 40), **attr) + self.trans_pop () + # Draw doors. + for slot in self.model.slots: + if slot.door_motor is not None: + self.trans_push () + self.trans_translate ((-slot.x, slot.z + 50)) + self.trans_rotate (-0.5 * pi + slot.door_motor.angle) + self.draw_line ((0, 0), (40, 0)) + self.trans_pop () -- cgit v1.2.3 From c8d4bd0e0f79dc8e49ad0074a1e67317e903c41f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 01:28:54 +0200 Subject: digital/io-hub, host/simu: add contacts --- digital/io-hub/src/robospierre/contact_defs.h | 18 +++++++++++--- digital/io-hub/src/robospierre/simu.host.c | 2 +- digital/io-hub/src/robospierre/simu.host.h | 2 +- digital/io-hub/tools/io_hub/mex.py | 2 +- host/simu/model/switch.py | 8 ++++-- host/simu/robots/robospierre/model/bag.py | 7 +++--- host/simu/robots/robospierre/model/clamp.py | 36 ++++++++++++++++----------- 7 files changed, 49 insertions(+), 26 deletions(-) diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h index 55b34354..1e04f6bf 100644 --- a/digital/io-hub/src/robospierre/contact_defs.h +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -27,11 +27,21 @@ #define CONTACT_COLOR A, 7 #define CONTACT_JACK F, 7 -#define CONTACT_EX1 E, 0 -#define CONTACT_EX2 E, 1 +#define CONTACT_FRONT_BOTTOM A, 4 +#define CONTACT_FRONT_MIDDLE F, 4 +#define CONTACT_BACK_BOTTOM A, 5 +#define CONTACT_BACK_MIDDLE F, 5 +#define CONTACT_FRONT_TOP A, 6 +#define CONTACT_BACK_TOP F, 6 +#define CONTACT_SIDE E, 7 #define CONTACT_LIST \ - CONTACT (CONTACT_EX1) \ - CONTACT (CONTACT_EX2) + CONTACT (CONTACT_FRONT_BOTTOM) \ + CONTACT (CONTACT_FRONT_MIDDLE) \ + CONTACT (CONTACT_FRONT_TOP) \ + CONTACT (CONTACT_BACK_BOTTOM) \ + CONTACT (CONTACT_BACK_MIDDLE) \ + CONTACT (CONTACT_BACK_TOP) \ + CONTACT (CONTACT_SIDE) #endif /* contact_defs_h */ diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 8f4019aa..281a60d6 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -31,7 +31,7 @@ #include "io.h" /** AVR registers. */ -uint8_t PINE; +uint8_t PINA, PINE, PINF; /** Initialise simulation. */ void diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index 4b461e86..ed6a002d 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -27,7 +27,7 @@ #ifdef HOST -extern uint8_t PINE; +extern uint8_t PINA, PINE, PINF; #else /* !defined (HOST) */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 7c8c0012..d9568197 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -31,7 +31,7 @@ ADC_NB = 8 PWM_NB = 6 PWM_VALUE_MAX = 1024 -CONTACT_NB = 4 +CONTACT_NB = 9 CONTACT_INIT = 0xffffffff class Mex: diff --git a/host/simu/model/switch.py b/host/simu/model/switch.py index 30cc1466..6a06a954 100644 --- a/host/simu/model/switch.py +++ b/host/simu/model/switch.py @@ -26,13 +26,17 @@ from utils.observable import Observable class Switch (Observable): - def __init__ (self, link): + def __init__ (self, link, invert = False): Observable.__init__ (self) self.link = link self.state = None + self.invert = invert self.register (self.__update) def __update (self): - self.link.state = self.state + if not self.invert: + self.link.state = self.state + else: + self.link.state = not self.state self.link.notify () diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 0a89d00c..5a6fa023 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -32,8 +32,8 @@ from math import pi class Bag: def __init__ (self, scheduler, table, link_bag): - self.color_switch = Switch (link_bag.io_hub.contact[0]) - self.jack = Switch (link_bag.io_hub.contact[1]) + self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True) + self.jack = Switch (link_bag.io_hub.contact[1], invert = True) self.contact = [ Switch (contact) for contact in link_bag.io_hub.contact[2:] ] self.position = Position (link_bag.asserv.position) @@ -42,7 +42,8 @@ class Bag: self.door_motors = [ MotorBasic (link_bag.io_hub.pwm[i], scheduler, 2 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ] self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0], - link_bag.mimot.aux[1], self.clamping_motor, self.door_motors) + link_bag.mimot.aux[1], self.clamping_motor, self.door_motors, + self.contact[0:7]) self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index 9bb8bacc..e2592aa8 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -29,13 +29,19 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side, door_motor): + def __init__ (self, x, y, z, side, door_motor, contact): self.x = x self.y = y self.z = z self.side = side - self.pawn = None self.door_motor = door_motor + self.contact = contact + self.set_pawn (None) + + def set_pawn (self, pawn): + self.pawn = pawn + self.contact.state = pawn is None + self.contact.notify () class Clamp (Observable): @@ -60,7 +66,7 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor, door_motors): + rotation_motor, clamping_motor, door_motors, slot_contacts): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -71,19 +77,19 @@ class Clamp (Observable): door_motors[2], None, door_motors[3], None) self.slots = ( Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, - door_motors[0]), + door_motors[0], slot_contacts[0]), Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, - None), + None, slot_contacts[1]), Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, - door_motors[1]), + door_motors[1], slot_contacts[2]), Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, - door_motors[2]), + door_motors[2], slot_contacts[3]), Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, - None), + None, slot_contacts[4]), Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, - door_motors[3]), + door_motors[3], slot_contacts[5]), Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, - None)) + None, slot_contacts[6])) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) @@ -98,7 +104,7 @@ class Clamp (Observable): if slot.pawn is None: p = self.__get_floor_elements (slot.side) if p is not None: - slot.pawn = p + slot.set_pawn (p) p.pos = None p.notify () changed = True @@ -133,14 +139,16 @@ class Clamp (Observable): if self.clamping == 0 and self.load is None: # Load an element. slot = self.__get_clamp_slot () - if slot: - self.load, slot.pawn = slot.pawn, self.load + if slot and slot.pawn is not None: + self.load = slot.pawn + slot.set_pawn (None) elif self.clamping == self.CLAMPING_STROKE \ and self.load is not None: # Unload an element. slot = self.__get_clamp_slot () if slot and slot.pawn is None: - self.load, slot.pawn = slot.pawn, self.load + slot.set_pawn (self.load) + self.load = None self.notify () def __get_floor_elements (self, side): -- cgit v1.2.3 From 366eeaccdca0ce784415a7b1bbc7e75f7c889af7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 01:36:57 +0200 Subject: digital/io-hub: add first element handling in clamp --- digital/io-hub/src/robospierre/Makefile | 4 +- digital/io-hub/src/robospierre/clamp.c | 146 +++++++++++++++++++- digital/io-hub/src/robospierre/clamp.h | 5 + digital/io-hub/src/robospierre/logistic.c | 212 ++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/logistic.h | 68 ++++++++++ digital/io-hub/src/robospierre/main.c | 23 ++++ 6 files changed, 453 insertions(+), 5 deletions(-) create mode 100644 digital/io-hub/src/robospierre/logistic.c create mode 100644 digital/io-hub/src/robospierre/logistic.h diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 21411631..20e9dee0 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -4,8 +4,8 @@ BASE = ../../../avr PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ - clamp.c \ - fsm.host.c fsm_AI_gen.avr.c \ + clamp.c logistic.c \ + fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ twi_master.c asserv.c mimot.c \ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 5d9533fa..1ee020e9 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -25,16 +25,42 @@ #include "common.h" #include "clamp.h" -#define FSM_NAME AI -#include "fsm.h" - #include "mimot.h" #include "pwm.h" +#include "contact.h" #include "bot.h" +#include "element.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" + +#include "logistic.h" + +/* + * There is two FSM in this file. + * + * The clamp FSM handles high level clamp behaviour, new elements, drop, and + * gives orders to the clamp move FSM. + * + * The clamp move FSM only handle moving the clamp without load or moving an + * element from a slot to another one. + */ FSM_INIT FSM_STATES ( + /* Initial state, to be complete with full initialisation. */ + CLAMP_START, + /* Returning to idle position. */ + CLAMP_GOING_IDLE, + /* Waiting external events, clamp at middle level. */ + CLAMP_IDLE, + /* Taking an element at bottom slots. */ + CLAMP_TAKING_DOOR_CLOSING, + /* Moving elements around. */ + CLAMP_MOVING_ELEMENT, + /* Waiting movement order. */ CLAMP_MOVE_IDLE, /* Moving to a final or intermediary position. */ @@ -53,8 +79,14 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( + /* Here for the moment, to be moved later. */ + start, + /* New element inside bottom slot. */ + clamp_new_element, /* Order to move the clamp. */ clamp_move, + /* Clamp movement success. */ + clamp_move_success, /* Elevation and elevation motor success. */ clamp_elevation_rotation_success, /* Elevation motor failure. */ @@ -62,6 +94,7 @@ FSM_EVENTS ( /* Rotation motor failure. */ clamp_rotation_failure) +FSM_START_WITH (CLAMP_START) FSM_START_WITH (CLAMP_MOVE_IDLE) /** Clamp context. */ @@ -73,6 +106,10 @@ struct clamp_t uint8_t pos_request; /** Element moving destination. */ uint8_t moving_to; + /** Position of a new element. */ + uint8_t pos_new; + /** New element kind. */ + uint8_t new_element; }; /** Global context. */ @@ -123,16 +160,51 @@ clamp_move (uint8_t pos) ctx.moving_to = CLAMP_POS_NB; FSM_HANDLE (AI, clamp_move); } + else + fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success)); } void clamp_move_element (uint8_t from, uint8_t to) { + assert (from != to); ctx.pos_request = from; ctx.moving_to = to; FSM_HANDLE (AI, clamp_move); } +void +clamp_new_element (uint8_t pos, uint8_t element) +{ + assert (pos == CLAMP_SLOT_FRONT_BOTTOM || pos == CLAMP_SLOT_BACK_BOTTOM); + ctx.pos_new = pos; + ctx.new_element = element; + FSM_HANDLE (AI, clamp_new_element); +} + +uint8_t +clamp_handle_event (void) +{ + if (FSM_CAN_HANDLE (AI, clamp_new_element)) + { + /* XXX: temporary hack. */ + uint8_t element = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; + if (!IO_GET (CONTACT_FRONT_BOTTOM) + && !logistic_global.slots[CLAMP_SLOT_FRONT_BOTTOM]) + { + clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element); + return 1; + } + if (!IO_GET (CONTACT_BACK_BOTTOM) + && !logistic_global.slots[CLAMP_SLOT_BACK_BOTTOM]) + { + clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element); + return 1; + } + } + return 0; +} + /** Find next position and start motors. */ static void clamp_route (void) @@ -192,6 +264,72 @@ clamp_route (void) ctx.pos_current = pos_new; } +/* CLAMP FSM */ + +FSM_TRANS (CLAMP_START, start, CLAMP_GOING_IDLE) +{ + clamp_move (CLAMP_SLOT_FRONT_MIDDLE); + return FSM_NEXT (CLAMP_START, start); +} + +FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) +{ + return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); +} + +FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) +{ + pwm_set_timed (clamp_slot_door[ctx.pos_new], BOT_PWM_DOOR_CLOSE); + return FSM_NEXT (CLAMP_IDLE, clamp_new_element); +} + +FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, + move_element, CLAMP_MOVING_ELEMENT, + move_to_idle, CLAMP_GOING_IDLE, + done, CLAMP_IDLE) +{ + logistic_element_new (ctx.pos_new, ctx.new_element); + if (logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element); + } + else if (logistic_global.clamp_pos_idle != ctx.pos_current) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle); + } + else + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done); +} + +FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, + move_element, CLAMP_MOVING_ELEMENT, + move_to_idle, CLAMP_GOING_IDLE, + done, CLAMP_IDLE) +{ + logistic_element_move_done (); + if (logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + move_element); + } + else if (logistic_global.clamp_pos_idle != ctx.pos_current) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + move_to_idle); + } + else + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + done); +} + +/* CLAMP_MOVE FSM */ + FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, move, CLAMP_MOVE_ROUTING, move_element, CLAMP_MOVE_SRC_ROUTING, @@ -223,6 +361,7 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, { if (ctx.pos_current == ctx.pos_request) { + fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success)); return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, done); } @@ -318,6 +457,7 @@ FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING, BOT_PWM_CLAMP_OPEN_TIME, CLAMP_MOVE_IDLE) { + fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success)); return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_CLAMP_OPENING); } diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 7b488f92..8c5f5c4c 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -62,4 +62,9 @@ clamp_move (uint8_t pos); void clamp_move_element (uint8_t from, uint8_t to); +/** Examine sensors to generate new events, return non zero if an event was + * generated. */ +uint8_t +clamp_handle_event (void); + #endif /* clamp_h */ diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c new file mode 100644 index 00000000..534f1c46 --- /dev/null +++ b/digital/io-hub/src/robospierre/logistic.c @@ -0,0 +1,212 @@ +/* logistic.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "logistic.h" + +#include "clamp.h" +#include "defs.h" + +#include "debug.host.h" + +/** Handle elements stored inside the robot. */ + +/** Global context. */ +struct logistic_t logistic_global; +#define ctx logistic_global + +inline void +logistic_debug_dump (void) +{ +#ifdef HOST + uint8_t i; + static const char *names[][CLAMP_SLOT_NB] = { + { "f1", "f2", "f3", "b1", "b2", "b3", "s1" }, + { "F1", "F2", "F3", "B1", "B2", "B3", "S1" } + }; + static const char *names_dir[] = { "--", "<-", "->" }; + DPRINTF ("%s", names_dir[ctx.collect_direction]); + for (i = 0; i < CLAMP_SLOT_NB; i++) + { + DPRINTF (" %s", ctx.slots[i] + ? names[ELEMENT_IS_HEAD (ctx.slots[i]) ? 1 : 0][i] + : "__"); + } + if (ctx.moving_from != CLAMP_SLOT_NB) + { + DPRINTF (" %s => %s", names[0][ctx.moving_from], names[0][ctx.moving_to]); + } + DPRINTF ("\n"); +#endif +} + +/** Examine current state and take a decision. */ +static void +logistic_decision (void) +{ + uint8_t i; + /* If currently moving, do not take decision. */ + if (ctx.moving_from != CLAMP_SLOT_NB) + return; + /* Determine collect_direction. */ + uint8_t front_head = 0, back_head = 0, + front_element = 0, back_element = 0; + uint8_t collect_direction; + for (i = CLAMP_SLOT_FRONT_BOTTOM; i <= CLAMP_SLOT_FRONT_TOP; i++) + { + if (ctx.slots[i]) + { + front_element++; + if (ELEMENT_IS_HEAD (ctx.slots[i])) + front_head++; + } + } + for (i = CLAMP_SLOT_BACK_BOTTOM; i <= CLAMP_SLOT_BACK_TOP; i++) + { + if (ctx.slots[i]) + { + back_element++; + if (ELEMENT_IS_HEAD (ctx.slots[i])) + back_head++; + } + } + if (front_head < back_head) + collect_direction = DIRECTION_FORWARD; + else if (front_head > back_head) + collect_direction = DIRECTION_BACKWARD; + else if (front_head) + { + if (front_element < back_element) + collect_direction = DIRECTION_FORWARD; + else if (front_element > back_element) + collect_direction = DIRECTION_BACKWARD; + else + collect_direction = ctx.collect_direction; + } + else + collect_direction = ctx.collect_direction; + ctx.collect_direction = collect_direction; + /* Now use this direction. */ + uint8_t collect_bay, storage_bay; + uint8_t collect_bay_head, storage_bay_head; + if (collect_direction == DIRECTION_FORWARD) + { + collect_bay = CLAMP_SLOT_FRONT_BOTTOM; + storage_bay = CLAMP_SLOT_BACK_BOTTOM; + collect_bay_head = front_head; + storage_bay_head = back_head; + ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; + } + else + { + collect_bay = CLAMP_SLOT_BACK_BOTTOM; + storage_bay = CLAMP_SLOT_FRONT_BOTTOM; + collect_bay_head = back_head; + storage_bay_head = front_head; + ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; + } + /* Find a destination for an element move. */ + uint8_t moving_to = CLAMP_SLOT_NB; + uint8_t moving_from = CLAMP_SLOT_NB; + if (!ctx.slots[collect_bay + 1]) + { + /* Movements in collect bay possible. */ + if (ELEMENT_IS_HEAD (ctx.slots[collect_bay + 0])) + { + moving_to = collect_bay + 2; + moving_from = collect_bay + 0; + } + } + if (moving_to == CLAMP_SLOT_NB && !ctx.slots[storage_bay + 1]) + { + /* No movement yet and movements in storage bay possible. */ + if (ELEMENT_IS_HEAD (ctx.slots[storage_bay + 0])) + { + moving_to = storage_bay + 2; + moving_from = storage_bay + 0; + } + else if (storage_bay_head) + { + if (!ctx.slots[storage_bay + 0]) + moving_to = storage_bay + 0; + else if (!ctx.slots[storage_bay + 1]) + moving_to = storage_bay + 1; + } + } + if (moving_to == CLAMP_SLOT_NB && !ctx.slots[CLAMP_SLOT_SIDE]) + { + /* No movement yet, store in side slot. */ + moving_to = CLAMP_SLOT_SIDE; + } + /* Find a source if available. */ + if (moving_to != CLAMP_SLOT_NB && moving_from == CLAMP_SLOT_NB) + { + if (ctx.slots[collect_bay + 0]) + moving_from = collect_bay + 0; + else if (ctx.slots[CLAMP_SLOT_SIDE]) + moving_from = CLAMP_SLOT_SIDE; + } + /* Ask for movement. */ + if (moving_from != CLAMP_SLOT_NB) + { + ctx.moving_from = moving_from; + ctx.moving_to = moving_to; + } + logistic_debug_dump (); +} + +void +logistic_init (void) +{ + uint8_t i; + for (i = 0; i < CLAMP_SLOT_NB; i++) + ctx.slots[i] = 0; + ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; + ctx.collect_direction = DIRECTION_FORWARD; +} + +void +logistic_update (void) +{ +} + +void +logistic_element_new (uint8_t pos, uint8_t element) +{ + assert (pos < CLAMP_SLOT_NB); + assert (!ctx.slots[pos]); + ctx.slots[pos] = element; + logistic_decision (); +} + +void +logistic_element_move_done (void) +{ + assert (!ctx.slots[ctx.moving_to]); + ctx.slots[ctx.moving_to] = ctx.slots[ctx.moving_from]; + ctx.slots[ctx.moving_from] = 0; + ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; + logistic_decision (); +} + diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h new file mode 100644 index 00000000..75b49d53 --- /dev/null +++ b/digital/io-hub/src/robospierre/logistic.h @@ -0,0 +1,68 @@ +#ifndef logistic_h +#define logistic_h +/* logistic.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "element.h" +#include "clamp.h" + +/** Logistic context. */ +struct logistic_t +{ + /** Current robot content. + * + * Elements are fully specified (pawn, queen or king). An exception can + * occurs when a codebar is read but does not correspond to a valid word. + * + * When a movement is outgoing, the element is kept in the source slot. */ + uint8_t slots[CLAMP_SLOT_NB]; + /** Current element movement source and destination, CLAMP_SLOT_NB if no + * current movement. */ + uint8_t moving_from, moving_to; + /** Best collect direction. */ + uint8_t collect_direction; + /** Idle clamp position, depend on collect direction. */ + uint8_t clamp_pos_idle; +}; + +/** Global context. */ +extern struct logistic_t logistic_global; + +/** Initialise module. */ +void +logistic_init (void); + +/** To be called at regular interval to check for bad robot state. */ +void +logistic_update (void); + +/** To be called when a new element is entering the robot. */ +void +logistic_element_new (uint8_t pos, uint8_t element); + +/** To be called when a element movement is done. */ +void +logistic_element_move_done (void); + +#endif /* logistic_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 13bad26c..ffb05a99 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -44,8 +44,10 @@ #ifdef HOST # include #endif +#include "fsm_queue.h" #include "clamp.h" +#include "logistic.h" #include "bot.h" @@ -78,6 +80,8 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + /* AI modules. */ + logistic_init (); /* Initialization done. */ proto_send0 ('z'); } @@ -107,6 +111,23 @@ main_event_to_fsm (void) FSM_HANDLE_E (AI, clamp_elevation_failure); else if (mimot_motor1_status == failure) FSM_HANDLE_E (AI, clamp_rotation_failure); + /* Clamp specific events. */ + if (clamp_handle_event ()) + return; + /* Jack, XXX to be changed! */ + if (!contact_get_jack ()) + FSM_HANDLE_E (AI, start); + /* Events from the event queue. */ + if (fsm_queue_poll ()) + { + /* We must post the event at the end of this block because if it is + * handled, the function will return and every instruction after will + * never be executed. */ + uint8_t save_event = fsm_queue_pop_event (); + /* Post the event */ + FSM_HANDLE_VAR_E (AI, save_event); + } + } /** Main (and infinite) loop. */ @@ -132,6 +153,8 @@ main_loop (void) /* Update IO modules. */ pwm_update (); contact_update (); + /* Update AI modules. */ + logistic_update (); /* Only manage events if slaves are synchronised. */ if (twi_master_sync ()) main_event_to_fsm (); -- cgit v1.2.3 From 3fa20141eb2d87df11ded1f05ff7ddf17882c86b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 12:48:49 +0200 Subject: digital/io-hub: add hardware offset when turning clamp --- digital/io-hub/src/robospierre/bot.h | 4 ++-- digital/io-hub/src/robospierre/clamp.c | 29 ++++++++++++++++++++++++++++- digital/io-hub/src/robospierre/clamp.h | 8 +++++++- 3 files changed, 37 insertions(+), 4 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 5e795b0f..e014321e 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -43,8 +43,8 @@ # define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x3b0b / 2) # define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3b0b # define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3b0b -# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2) -# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 3) +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 3) # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 1ee020e9..227579ff 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -134,8 +134,12 @@ static const uint16_t clamp_pos[][2] = { BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP, BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, { BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + { BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, + BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, { BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP, BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, }; @@ -151,6 +155,9 @@ static const uint8_t clamp_slot_door[] = { 0xff }; +static void +clamp_route (void); + void clamp_move (uint8_t pos) { @@ -202,6 +209,18 @@ clamp_handle_event (void) return 1; } } + /* Handle special hardware offset. */ + uint16_t rotation_position = mimot_get_motor1_position (); + if ((ctx.pos_current == CLAMP_BAY_FRONT_LEAVING + && rotation_position > (BOT_CLAMP_BAY_SIDE_ROTATION_STEP - + BOT_CLAMP_BAY_FRONT_ROTATION_STEP) / 2) + || (ctx.pos_current == CLAMP_BAY_BACK_LEAVING + && rotation_position < (BOT_CLAMP_BAY_BACK_ROTATION_STEP - + BOT_CLAMP_BAY_SIDE_ROTATION_STEP) / 2)) + { + /* Go directly to next point. */ + clamp_route (); + } return 0; } @@ -232,13 +251,21 @@ clamp_route (void) pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; } else if (pos_current == CLAMP_BAY_FRONT_LEAVE) + { + pos_new = CLAMP_BAY_FRONT_LEAVING; + } + else if (pos_current == CLAMP_BAY_BACK_LEAVE) + { + pos_new = CLAMP_BAY_BACK_LEAVING; + } + else if (pos_current == CLAMP_BAY_FRONT_LEAVING) { if (pos_request == CLAMP_SLOT_SIDE) pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; else pos_new = CLAMP_SLOT_BACK_MIDDLE; } - else if (pos_current == CLAMP_BAY_BACK_LEAVE) + else if (pos_current == CLAMP_BAY_BACK_LEAVING) { if (pos_request == CLAMP_SLOT_SIDE) pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 8c5f5c4c..bb15b62e 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -36,9 +36,15 @@ enum { CLAMP_SLOT_SIDE, /** Leave the front bay, ready to enter side tunnel. */ CLAMP_BAY_FRONT_LEAVE, + /** Leaving the front bay, entered the side tunnel. When at midway point, + * go directly to the next position. */ + CLAMP_BAY_FRONT_LEAVING, /** Leave the back bay, ready to enter side tunnel. */ CLAMP_BAY_BACK_LEAVE, - /* Enter the side bay. Position on the side, above wheels. */ + /** Leaving the back bay, entered the side tunnel. When at midway point, + * go directly to the next position. */ + CLAMP_BAY_BACK_LEAVING, + /** Enter the side bay. Position on the side, above wheels. */ CLAMP_BAY_SIDE_ENTER_LEAVE, /** Total number of position, including intermediary positions. */ CLAMP_POS_NB, -- cgit v1.2.3 From 3d91eb26478dd0801294ce0ead01605687687d5e Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 14 May 2011 13:29:02 +0200 Subject: digital/ai/src/fsm: use program space read function for transition table --- digital/ai/src/fsm/fsm.host.c | 40 ++++++++++++++++++++++++++++++---------- 1 file changed, 30 insertions(+), 10 deletions(-) diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c index bf8b761d..b7b6ab4e 100644 --- a/digital/ai/src/fsm/fsm.host.c +++ b/digital/ai/src/fsm/fsm.host.c @@ -1056,11 +1056,18 @@ fsm_build_gen_avr_h (fsm_build_t *fsm, uint embedded_strings) /* Gen function table. */ fprintf (f, "typedef fsm_%s_branch_t (*fsm_%s_func_t)(void);\n", fsm->name, fsm->name); - fprintf (f, "extern const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u];\n\n", + fprintf (f, "extern const fsm_%s_func_t PROGMEM fsm_%s_trans_table[%u][%u];\n", fsm->name, fsm->name, fsm->event_nb, fsm->state_nb); + /* Gen read function for trans table. */ + fprintf (f, "fsm_%s_func_t fsm_%s_read_trans (fsm_%s_event_t event, " + "fsm_%s_state_t state);\n\n", + fsm->name, + fsm->name, + fsm->name, + fsm->name); /* Gen active states array. */ fprintf (f, "extern fsm_%s_state_t fsm_%s_active_states[%u];\n\n", @@ -1280,6 +1287,19 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) } fprintf (f, "};\n\n"); + /* Gen read function for trans table. */ + fprintf (f, "fsm_%s_func_t fsm_%s_read_trans (fsm_%s_event_t event, " + "fsm_%s_state_t state)\n{\n", + fsm->name, + fsm->name, + fsm->name, + fsm->name); + fprintf (f, "\treturn (fsm_%s_func_t) pgm_read_word " + "(&fsm_%s_trans_table[event][state]);\n", + fsm->name, + fsm->name); + fprintf (f, "}\n\n"); + /* Gen active states array. */ fprintf (f, "fsm_%s_state_t fsm_%s_active_states[%u];\n\n", fsm->name, @@ -1324,15 +1344,15 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fprintf (f, "\tint handled = 0;\n"); fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n", fsm->name); - fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n", + fprintf (f, "\t\tif (fsm_%s_read_trans (e, fsm_%s_active_states[i]))\n", fsm->name, fsm->name); fprintf (f, "\t\t{\n"); - fprintf (f, "\t\t\tfsm_%s_active_states[i] = \ - fsm_%s_trans_table[e][fsm_%s_active_states[i]]();\n", - fsm->name, - fsm->name, - fsm->name); + fprintf (f, "\t\t\tfsm_%s_active_states[i] = fsm_%s_read_trans (e, " + "fsm_%s_active_states[i])();\n", + fsm->name, + fsm->name, + fsm->name); fprintf (f, "\t\t\thandled = 1;\n"); if (fsm->timeouts != NULL) { @@ -1352,9 +1372,9 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fprintf (f, "\tuint16_t i;\n"); fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n", fsm->name); - fprintf (f, "\t\tif (fsm_%s_trans_table[e][fsm_%s_active_states[i]])\n", - fsm->name, - fsm->name); + fprintf (f, "\t\tif (fsm_%s_read_trans (e, fsm_%s_active_states[i]))\n", + fsm->name, + fsm->name); fprintf (f, "\t\t\treturn 1;\n"); fprintf (f, "\treturn 0;\n"); fprintf (f, "}\n\n"); -- cgit v1.2.3 From 5ba03e9744a8916730d9b8462cbfa6512d380ac6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 16:49:48 +0200 Subject: digital/io-hub: add drop --- digital/ai/tools/test_simu_control_robospierre.py | 21 ++++++- digital/io-hub/src/robospierre/clamp.c | 73 +++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 9 +++ digital/io-hub/src/robospierre/logistic.c | 11 ++++ digital/io-hub/src/robospierre/logistic.h | 5 ++ digital/io-hub/src/robospierre/main.c | 8 +++ digital/io-hub/tools/io_hub/io_hub.py | 10 ++++ 7 files changed, 136 insertions(+), 1 deletion(-) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 88fbc5f3..acd3ca7c 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -82,12 +82,21 @@ class TestSimuControl (TestSimu): text = 'Move element', padx = 0, pady = 0, command = self.clamp_move_element_command) self.clamp_element_move_button.pack () + self.drop_var = IntVar () + self.drop_button = Checkbutton (self.control_frame, text = 'Drop', + indicatoron = False, + variable = self.drop_var, command = self.drop_command) + self.drop_button.pack () + self.backward_var = IntVar () + self.backward_button = Checkbutton (self.control_frame, + text = 'Backward', variable = self.backward_var) + self.backward_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) def move (self, ev): pos = self.table_view.screen_coord ((ev.x, ev.y)) - self.asserv.goto (pos[0], pos[1]) + self.asserv.goto (pos[0], pos[1], self.backward_var.get ()) def orient (self, ev): x, y = self.table_view.screen_coord ((ev.x, ev.y)) @@ -129,6 +138,16 @@ class TestSimuControl (TestSimu): for i in (0, 1, 3, 4): self.io.pwm_set_timed (i, pwm, 255, 0) + def drop_command (self): + if self.drop_var.get (): + if self.backward_var.get (): + order = 'drop_backward' + else: + order = 'drop_forward' + else: + order = 'drop_clear' + self.io.drop (order) + def change_color (self, *dummy): pass diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 227579ff..6c063979 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -60,6 +60,10 @@ FSM_STATES ( CLAMP_TAKING_DOOR_CLOSING, /* Moving elements around. */ CLAMP_MOVING_ELEMENT, + /* Droping a tower. */ + CLAMP_DROPING_DOOR_OPENING, + /* Droping a tower, waiting for robot to advance. */ + CLAMP_DROPING_WAITING_ROBOT, /* Waiting movement order. */ CLAMP_MOVE_IDLE, @@ -83,6 +87,13 @@ FSM_EVENTS ( start, /* New element inside bottom slot. */ clamp_new_element, + /* Order to drop elements. */ + clamp_drop, + /* Sent once drop is done, but robot should advance to completely + * free the dropped tower. */ + clamp_drop_waiting, + /* Received when top FSM made the robot advance after a drop. */ + clamp_drop_clear, /* Order to move the clamp. */ clamp_move, /* Clamp movement success. */ @@ -110,6 +121,8 @@ struct clamp_t uint8_t pos_new; /** New element kind. */ uint8_t new_element; + /** Drop direction, drop on the other side. */ + uint8_t drop_direction; }; /** Global context. */ @@ -189,6 +202,25 @@ clamp_new_element (uint8_t pos, uint8_t element) FSM_HANDLE (AI, clamp_new_element); } +uint8_t +clamp_drop (uint8_t drop_direction) +{ + if (FSM_CAN_HANDLE (AI, clamp_drop)) + { + ctx.drop_direction = drop_direction; + FSM_HANDLE (AI, clamp_drop); + return 1; + } + else + return 0; +} + +void +clamp_drop_clear (void) +{ + FSM_HANDLE (AI, clamp_drop_clear); +} + uint8_t clamp_handle_event (void) { @@ -310,6 +342,16 @@ FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) return FSM_NEXT (CLAMP_IDLE, clamp_new_element); } +FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING) +{ + /* If going forward, drop at back. */ + uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN); + pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN); + return FSM_NEXT (CLAMP_IDLE, clamp_drop); +} + FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, move_element, CLAMP_MOVING_ELEMENT, move_to_idle, CLAMP_GOING_IDLE, @@ -355,6 +397,37 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, done); } +FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME, + CLAMP_DROPING_WAITING_ROBOT) +{ + fsm_queue_post_event (FSM_EVENT (AI, clamp_drop_waiting)); + return FSM_NEXT_TIMEOUT (CLAMP_DROPING_DOOR_OPENING); +} + +FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_element, CLAMP_MOVING_ELEMENT, + move_to_idle, CLAMP_GOING_IDLE, + done, CLAMP_IDLE) +{ + logistic_drop (ctx.drop_direction); + if (logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_element); + } + else if (logistic_global.clamp_pos_idle != ctx.pos_current) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_to_idle); + } + else + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + done); +} + /* CLAMP_MOVE FSM */ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index bb15b62e..a59e91ae 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -68,6 +68,15 @@ clamp_move (uint8_t pos); void clamp_move_element (uint8_t from, uint8_t to); +/** Drop an element tower. Return 0 if not currently possible. If + * drop_direction is forward, drop at the back. */ +uint8_t +clamp_drop (uint8_t drop_direction); + +/** Signal robot advanced, and drop is finished. */ +void +clamp_drop_clear (void); + /** Examine sensors to generate new events, return non zero if an event was * generated. */ uint8_t diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 534f1c46..945bd583 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -210,3 +210,14 @@ logistic_element_move_done (void) logistic_decision (); } +void +logistic_drop (uint8_t direction) +{ + uint8_t bay = direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + uint8_t i; + for (i = bay; i < bay + 3; i++) + ctx.slots[i] = 0; + logistic_decision (); +} + diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 75b49d53..c6aa4159 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -65,4 +65,9 @@ logistic_element_new (uint8_t pos, uint8_t element); void logistic_element_move_done (void); +/** To be called when elements have been dropped on the opposite side of + * direction. */ +void +logistic_drop (uint8_t direction); + #endif /* logistic_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index ffb05a99..938703b6 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -222,6 +222,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_OPEN); pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_OPEN); break; + case c ('d', 1): + /* Drop elements. + * - 1b: 00: drop clear, 01: drop forward, 02: drop backward. */ + if (args[0] == 0x00) + clamp_drop_clear (); + else + clamp_drop (args[0]); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index 1b25102c..d6ae8618 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -50,6 +50,16 @@ class Proto: def clamp_move_element (self, from_, to): self.proto.send ('c', 'BB', from_, to) + def drop (self, order): + if order == 'drop_clear': + self.proto.send ('d', 'B', 0x00) + elif order == 'drop_forward': + self.proto.send ('d', 'B', 0x01) + elif order == 'drop_backward': + self.proto.send ('d', 'B', 0x02) + else: + raise ValueError + def close (self): self.reset () self.proto.wait (lambda: True) -- cgit v1.2.3 From 9ee3fde114e84c53cca88dc4a3d4166ec4c675f4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 18:39:20 +0200 Subject: digital/avr/doc: add AT90USB128 fuses settings --- digital/avr/doc/fuses.txt | 60 +++++++++++++++++++++++++++-------------------- 1 file changed, 34 insertions(+), 26 deletions(-) diff --git a/digital/avr/doc/fuses.txt b/digital/avr/doc/fuses.txt index 3f353fcd..1345c0ef 100644 --- a/digital/avr/doc/fuses.txt +++ b/digital/avr/doc/fuses.txt @@ -19,36 +19,44 @@ m16 0xff 0xc1 -- m32 0xff 0xc1 -- m64 0xff 0xc1 0xff m128 0xff 0xc1 0xff +usb128 0xff 0xd1 0xf3 ====== ===== ===== ===== This correspond to ('!' means unprogrammed, i.e. '1'): -=== === === === ==== ======= -m8 m16 m32 m64 m128 Meaning -=== === === === ==== ======= -l7 l7 l7 l7 l7 !BODLEVEL (BOD trigger level) -l6 l6 l6 l6 l6 !BODEN (BOD enabled) -l5 l5 l5 l5 l5 !SUT1 (start up time) -l4 l4 l4 l4 l4 !SUT0 (start up time) -l3 l3 l3 l3 l3 !CKSEL3 (clock source select) -l2 l2 l2 l2 l2 !CKSEL2 (clock source select) -l1 l1 l1 l1 l1 !CKSEL1 (clock source select) -l0 l0 l0 l0 l0 !CKSEL0 (clock source select) ---- --- --- --- ---- ------- -h7 .. .. .. .. !RSTDISBL (no reset pin disabled) -.. h7 h7 h7 h7 !OCDEN (OCD enabled) -h6 .. .. .. .. !WDTON (no watchdog always on) -.. h6 h6 h6 h6 !JTAGEN (JTAG enabled) -h5 h5 h5 h5 h5 SPIEN (spi programming enabled) -h4 h4 h4 h4 h4 CKOPT (clock option) -h3 h3 h3 h3 h3 EESAVE (eeprom preserved on chip erase) -h2 h2 h2 h2 h2 BOOTSZ1 (boot size) -h1 h1 h1 h1 h1 BOOTSZ0 (boot size) -h0 h0 h0 h0 h0 !BOOTRST (use boot reset vector) ---- --- --- --- ---- ------- -.. .. .. e1 e1 !M103C (ATmega103 compatibility) -.. .. .. e0 e0 !WDTON (watchdog always on) -=== === === === ==== ======= +=== === === === ==== ====== ======= +m8 m16 m32 m64 m128 usb128 Meaning +=== === === === ==== ====== ======= +l7 l7 l7 l7 l7 .. !BODLEVEL (BOD trigger level) +l6 l6 l6 l6 l6 .. !BODEN (BOD enabled) +.. .. .. .. .. l7 !CKDIV8 (divide clock by 8) +.. .. .. .. .. l6 !CKOUT (clock output) +l5 l5 l5 l5 l5 l5 !SUT1 (start up time) +l4 l4 l4 l4 l4 l4 !SUT0 (start up time) +l3 l3 l3 l3 l3 l3 !CKSEL3 (clock source select) +l2 l2 l2 l2 l2 l2 !CKSEL2 (clock source select) +l1 l1 l1 l1 l1 l1 !CKSEL1 (clock source select) +l0 l0 l0 l0 l0 l0 !CKSEL0 (clock source select) +--- --- --- --- ---- ------ ------- +h7 .. .. .. .. .. !RSTDISBL (no reset pin disabled) +.. h7 h7 h7 h7 h7 !OCDEN (OCD enabled) +h6 .. .. .. .. .. !WDTON (no watchdog always on) +.. h6 h6 h6 h6 h6 !JTAGEN (JTAG enabled) +h5 h5 h5 h5 h5 h5 SPIEN (spi programming enabled) +h4 h4 h4 h4 h4 .. CKOPT (clock option) +.. .. .. .. .. h4 !WDTON (watchdog always on) +h3 h3 h3 h3 h3 h3 EESAVE (eeprom preserved on chip erase) +h2 h2 h2 h2 h2 h2 BOOTSZ1 (boot size) +h1 h1 h1 h1 h1 h1 BOOTSZ0 (boot size) +h0 h0 h0 h0 h0 h0 !BOOTRST (use boot reset vector) +--- --- --- --- ---- ------ ------- +.. .. .. e1 e1 !M103C (ATmega103 compatibility) +.. .. .. e0 e0 !WDTON (watchdog always on) +.. .. .. .. .. e3 HWBE (hardware boot enable) +.. .. .. .. .. e2 BODLEVEL2 (brown-out detector select) +.. .. .. .. .. e1 !BODLEVEL1 (brown-out detector select) +.. .. .. .. .. e0 !BODLEVEL0 (brown-out detector select) +=== === === === ==== ====== ======= - CKSEL321 = 111, CKOPT = 0 corresponds to external crystal, more than 1 MHz. - CKSEL0 = 1, SUT10 = 11 corresponds to crystal, slowly rising power, this is -- cgit v1.2.3 From 6b86c7e355fe5291b77be827a3c64d089a709b6e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 14 May 2011 21:29:37 +0200 Subject: digital/io-hub: add new element command --- digital/io-hub/src/robospierre/clamp.h | 4 ++++ digital/io-hub/src/robospierre/main.c | 3 +++ 2 files changed, 7 insertions(+) diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index a59e91ae..64575508 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -68,6 +68,10 @@ clamp_move (uint8_t pos); void clamp_move_element (uint8_t from, uint8_t to); +/** Simulate the presence of a new element. */ +void +clamp_new_element (uint8_t pos, uint8_t element); + /** Drop an element tower. Return 0 if not currently possible. If * drop_direction is forward, drop at the back. */ uint8_t diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 938703b6..3e1b48c2 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -215,6 +215,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: destination. */ clamp_move_element (args[0], args[1]); break; + case c ('n', 2): + clamp_new_element (args[0], args[1]); + break; case c ('d', 0): /* Open all doors. */ pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_OPEN); -- cgit v1.2.3 From 3444b01bc21d95e491faaf65500dcf79c77dbfb9 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 15:09:52 +0200 Subject: digital/io-hub: disable watchdog timer --- digital/io-hub/src/robospierre/main.c | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 3e1b48c2..97d5bd40 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -53,6 +53,10 @@ #include "io.h" +#ifndef HOST +# include +#endif + /** Our color. */ enum team_color_e team_color; @@ -66,6 +70,11 @@ static uint8_t main_stats_contact_, main_stats_contact_cpt_; static void main_init (void) { +#ifndef HOST + /* Disable watchdog (enabled in bootloader). */ + MCUSR &= ~(1 << WDRF); + wdt_disable (); +#endif /* Serial port */ uart0_init (); /* Enable interrupts */ -- cgit v1.2.3 From 64b3831824be67975e29b4b58cade3e1bb2e81f8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 19:14:29 +0200 Subject: host/utils: broke init_proto in class --- host/utils/init_proto.py | 76 ++++++++++++++++++++++++++++++------------------ 1 file changed, 48 insertions(+), 28 deletions(-) diff --git a/host/utils/init_proto.py b/host/utils/init_proto.py index f5c2bfa4..8125543c 100644 --- a/host/utils/init_proto.py +++ b/host/utils/init_proto.py @@ -3,34 +3,54 @@ import proto.popen_io import serial import optparse -def init_proto (default_robot, proto_class, init_module = None, init = None): +class InitProto: """Helper to create a Proto instance from command line arguments.""" - if init_module is None and init is None: - init = { } - # Parse arguments. - parser = optparse.OptionParser ( - usage = "usage: %prog [options] TTY|! PROGRAM...", - description = "TTY is a device name (example: %prog " - "/dev/ttyUSB0), PROGRAM is a host program with its arguments " - "(example: %prog -- ! ../src/board.host board_arg).") - if init_module: - parser.add_option ('-r', '--robot', help = "use specified robot", - metavar = 'NAME', default = default_robot) - (options, args) = parser.parse_args () - if init_module and options.robot is None: - parser.error ("no robot specified") - if not args: - parser.error ("not enough arguments") - # Create parameters. - if args[0] == '!': - io = proto.popen_io.PopenIO (args[1:]) - if init_module: - init = init_module.host[options.robot] - else: - if len (args) != 1: - parser.error ("too many arguments after device") - io = serial.Serial (args[0]) + + def __init__ (self, default_robot, proto_class, init_module = None, + init = None): + """Initialise helper.""" + if init_module is None and init is None: + init = { } + self.proto_class = proto_class + self.init_module = init_module + self.init = init + # Prepare parser. + self.parser = optparse.OptionParser ( + usage = "usage: %prog [options] TTY|! PROGRAM...", + description = "TTY is a device name (example: %prog " + "/dev/ttyUSB0), PROGRAM is a host program with its arguments " + "(example: %prog -- ! ../src/board.host board_arg).") if init_module: - init = init_module.target[options.robot] - return proto_class (io, **init) + self.parser.add_option ('-r', '--robot', + help = "use specified robot", + metavar = 'NAME', default = default_robot) + + def parse_args (self): + """Parse command line.""" + (self.options, self.args) = self.parser.parse_args () + if self.init_module and self.options.robot is None: + self.parser.error ("no robot specified") + + def get_proto (self): + """Return the Proto instance.""" + if not self.args: + self.parser.error ("not enough arguments") + # Create parameters. + if self.args[0] == '!': + io = proto.popen_io.PopenIO (self.args[1:]) + if self.init_module: + self.init = self.init_module.host[self.options.robot] + else: + if len (self.args) != 1: + self.parser.error ("too many arguments after device") + io = serial.Serial (self.args[0]) + if self.init_module: + self.init = self.init_module.target[self.options.robot] + return self.proto_class (io, **self.init) + +def init_proto (default_robot, proto_class, init_module = None, init = None): + """Helper to create a Proto instance from command line arguments.""" + ip = InitProto (default_robot, proto_class, init_module, init) + ip.parse_args () + return ip.get_proto () -- cgit v1.2.3 From 682308c9c774b154a16cff990fdf9f6cd5f739ae Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 19:23:33 +0200 Subject: digital/asserv/tools: improved goto test --- digital/asserv/tools/test_goto.py | 33 ++++++++++++++++++++++++--------- 1 file changed, 24 insertions(+), 9 deletions(-) diff --git a/digital/asserv/tools/test_goto.py b/digital/asserv/tools/test_goto.py index 2cbbb350..f2aa2535 100644 --- a/digital/asserv/tools/test_goto.py +++ b/digital/asserv/tools/test_goto.py @@ -3,14 +3,29 @@ import math import asserv import asserv.init -from utils.init_proto import init_proto +from utils.init_proto import InitProto -a = init_proto (None, asserv.Proto, asserv.init) -for i in xrange (10): - x = random.randrange (2000) - y = random.randrange (1100) - a.goto (x, y) - a.goto_angle (math.radians (random.randrange (360))) -a.goto (0, 0) -a.goto_angle (0) +ip = InitProto (None, asserv.Proto, asserv.init) +ip.parser.add_option ('-i', '--iterations', + help = "number of test iterations", metavar = 'NB', default = 10) +ip.parser.add_option ('-t', '--type', + help = "test type, one of random or linear", metavar = 'TYPE', + default = 'random') +ip.parse_args () +a = ip.get_proto () +try: + if ip.options.type == 'random': + for i in xrange (ip.options.iterations): + x = random.randrange (2000) + y = random.randrange (1100) + a.goto (x, y) + a.goto_angle (math.radians (random.randrange (360))) + a.goto (0, 0) + a.goto_angle (0) + elif ip.options.type == 'linear': + for i in xrange (ip.options.iterations): + a.speed_pos ('t', 1000) + a.speed_pos ('t', -1000) +except: + pass a.close () -- cgit v1.2.3 From 7fd76486e959b355a42f812c86a97713747204e4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 19:23:58 +0200 Subject: digital/asserv/tools: rough robospierre initialisation parameters --- digital/asserv/tools/asserv/init.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py index 3f0afa36..f0bfced0 100644 --- a/digital/asserv/tools/asserv/init.py +++ b/digital/asserv/tools/asserv/init.py @@ -34,8 +34,19 @@ target_marcel = dict ( l = 0x1000, w = 0x09, ) +target_robospierre = dict ( + scale = 0.031748977927129413, f = 0x1370, + c = float (0x01000000) / (1 << 24), + tkp = 1, tkd = 16, + ta = 0.75, tsm = 0x60, tss = 0x10, + akp = 2, akd = 16, + aa = 0.5, asm = 0x60, ass = 0x10, + E = 0x3ff, D = 0x1ff, + l = 0x1000, + w = 0x00, + ) target = { 'giboulee': target_marcel, 'marcel': target_marcel, - 'robospierre': target_marcel, + 'robospierre': target_robospierre, } -- cgit v1.2.3 From 73f25d3310706c5d7a559f120805c54860b16795 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 20:14:12 +0200 Subject: host/simu/robots/robospierre: add tower management --- host/simu/robots/robospierre/model/clamp.py | 60 ++++++++++++++++++++++++----- host/simu/robots/robospierre/view/clamp.py | 31 +++++++++------ host/simu/robots/robospierre/view/robot.py | 4 +- host/simu/view/table_eurobot2011.py | 32 +++++++++------ 4 files changed, 92 insertions(+), 35 deletions(-) diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index e2592aa8..976e559f 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -24,6 +24,7 @@ """Robospierre clamp.""" from utils.observable import Observable from simu.utils.trans_matrix import TransMatrix +from simu.model.round_obstacle import RoundObstacle from math import pi, cos, sin class Slot: @@ -36,12 +37,7 @@ class Slot: self.side = side self.door_motor = door_motor self.contact = contact - self.set_pawn (None) - - def set_pawn (self, pawn): - self.pawn = pawn - self.contact.state = pawn is None - self.contact.notify () + self.pawn = None class Clamp (Observable): @@ -90,6 +86,14 @@ class Clamp (Observable): door_motors[3], slot_contacts[5]), Slot (0, self.BAY_OFFSET, 2 * self.BAY_ZOFFSET, None, None, slot_contacts[6])) + self.front_slots = ( + self.slots[self.SLOT_FRONT_BOTTOM], + self.slots[self.SLOT_FRONT_MIDDLE], + self.slots[self.SLOT_FRONT_TOP]) + self.back_slots = ( + self.slots[self.SLOT_BACK_BOTTOM], + self.slots[self.SLOT_BACK_MIDDLE], + self.slots[self.SLOT_BACK_TOP]) self.load = None self.robot_position.register (self.__robot_position_notified) self.elevation_motor.register (self.__elevation_notified) @@ -104,11 +108,12 @@ class Clamp (Observable): if slot.pawn is None: p = self.__get_floor_elements (slot.side) if p is not None: - slot.set_pawn (p) + slot.pawn = p p.pos = None p.notify () changed = True if changed: + self.update_contacts () self.notify () def __elevation_notified (self): @@ -141,16 +146,53 @@ class Clamp (Observable): slot = self.__get_clamp_slot () if slot and slot.pawn is not None: self.load = slot.pawn - slot.set_pawn (None) + slot.pawn = None elif self.clamping == self.CLAMPING_STROKE \ and self.load is not None: # Unload an element. slot = self.__get_clamp_slot () if slot and slot.pawn is None: - slot.set_pawn (self.load) + slot.pawn = self.load self.load = None + # Little resources saving hack: done here, all motors are notified + # at the same time. + self.check_tower () + self.update_contacts () self.notify () + def check_tower (self): + """Check whether several elements can make a tower.""" + for slots in (self.front_slots, self.back_slots): + if slots[0].pawn is not None and slots[1].pawn is not None: + assert slots[0].pawn.kind != 'tower' + tower = RoundObstacle (100, 1) + tower.kind = 'tower' + tower.tower = [ slots[0].pawn, slots[1].pawn ] + slots[0].pawn, slots[1].pawn = tower, None + if slots[0].pawn is not None and slots[0].pawn.kind == 'tower' \ + and slots[2].pawn and slots[2].door_motor.angle: + slots[0].pawn.tower.append (slots[2].pawn) + slots[2].pawn = None + + def update_contacts (self): + """Update pawn contacts.""" + for slots in (self.front_slots, self.back_slots): + slots[0].contact.state = not (slots[0].pawn is not None) + # A tower at level 0 is seen at level 1. + slots[1].contact.state = not ( + slots[1].pawn is not None + or (slots[0].pawn is not None + and slots[0].pawn.kind == 'tower')) + # This one is really high. + slots[2].contact.state = not (slots[2].pawn is not None) + slot_side = self.slots[self.SLOT_SIDE] + slot_side.contact.state = slot_side.pawn is None + clamp_slot = self.__get_clamp_slot () + if clamp_slot is not None: + clamp_slot.contact.state = False + for slot in self.slots: + slot.contact.notify () + def __get_floor_elements (self, side): """Return an elements in front (side = 0) or in back (side = 1) of the robot, on the floor.""" diff --git a/host/simu/robots/robospierre/view/clamp.py b/host/simu/robots/robospierre/view/clamp.py index 130c3eea..07a10929 100644 --- a/host/simu/robots/robospierre/view/clamp.py +++ b/host/simu/robots/robospierre/view/clamp.py @@ -68,7 +68,7 @@ class ClampTop (Drawable): self.trans_push () self.trans_scale (1 - slot.z / 1000.0) self.trans_translate ((slot.x, slot.y)) - draw_pawn (self, slot.pawn.radius, slot.pawn.kind) + draw_pawn (self, slot.pawn) self.trans_pop () # Draw clamp. if self.model.rotation is not None: @@ -82,7 +82,7 @@ class ClampTop (Drawable): if load is not None: self.trans_push () self.trans_translate ((150, 0)) - draw_pawn (self, load.radius, load.kind) + draw_pawn (self, load) self.trans_pop () # Mobile side. self.trans_rotate (-self.model.clamping / 43) @@ -107,16 +107,23 @@ class ClampSide (Drawable): if pawn is not None: self.trans_push () self.trans_translate (pos) - self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW) - if pawn.kind == 'king': - self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), (-50, 190), - (-10, 190), (-10, 230), (10, 230), (10, 190), (50, 190), - (50, 170), (5, 170), (50, 50), fill = YELLOW, - outline = BLACK) - elif pawn.kind == 'queen': - self.draw_polygon ((-50, 50), (-10, 180), (10, 180), (50, 50), - fill = YELLOW, outline = BLACK) - self.draw_circle ((0, 180), 50, fill = YELLOW) + if pawn.kind == 'tower': + pawns = pawn.tower + else: + pawns = (pawn, ) + for p in pawns: + self.draw_rectangle ((-100, 0), (100, 50), fill = YELLOW) + if p.kind == 'king': + self.draw_polygon ((-50, 50), (-10, 170), (-50, 170), + (-50, 190), (-10, 190), (-10, 230), (10, 230), + (10, 190), (50, 190), (50, 170), (5, 170), + (50, 50), fill = YELLOW, + outline = BLACK) + elif p.kind == 'queen': + self.draw_polygon ((-50, 50), (-10, 180), (10, 180), + (50, 50), fill = YELLOW, outline = BLACK) + self.draw_circle ((0, 180), 50, fill = YELLOW) + self.trans_translate ((0, 50)) self.trans_pop () def draw (self): diff --git a/host/simu/robots/robospierre/view/robot.py b/host/simu/robots/robospierre/view/robot.py index c0f6624c..90624000 100644 --- a/host/simu/robots/robospierre/view/robot.py +++ b/host/simu/robots/robospierre/view/robot.py @@ -63,7 +63,7 @@ class Robot (simu.inter.drawable.Drawable): self.trans_push () self.trans_scale (1 - slot.z / 1000.0) self.trans_translate ((slot.x, slot.y)) - draw_pawn (self, slot.pawn.radius, slot.pawn.kind) + draw_pawn (self, slot.pawn) self.trans_pop () # Draw clamp. if self.clamp_model.rotation is not None: @@ -77,7 +77,7 @@ class Robot (simu.inter.drawable.Drawable): if load: self.trans_push () self.trans_translate ((150, 0)) - draw_pawn (self, load.radius, load.kind) + draw_pawn (self, load) self.trans_pop () # Mobile side. self.trans_rotate (- self.clamp_model.clamping / 47) diff --git a/host/simu/view/table_eurobot2011.py b/host/simu/view/table_eurobot2011.py index f0a65ce1..5725e5b8 100644 --- a/host/simu/view/table_eurobot2011.py +++ b/host/simu/view/table_eurobot2011.py @@ -31,17 +31,25 @@ GREEN = '#268126' BLACK = '#181818' YELLOW = '#cccc00' -def draw_pawn (d, radius, kind): - d.draw_circle ((0, 0), radius, fill = YELLOW) - if kind == 'king': - a = 0.1 * radius - b = 0.5 * radius - d.draw_line ((a, b), (a, a), (b, a), (b, -a), (a, -a), (a, -b), - (-a, -b), (-a, -a), (-b, -a), (-b, a), (-a, a), (-a, b), - (a, b)) - elif kind == 'queen': - d.draw_circle ((0, 0), 0.5 * radius) - d.draw_circle ((0, 0), 0.4 * radius) +def draw_pawn (d, pawn): + d.trans_push () + if pawn.kind == 'tower': + pawns = pawn.tower + else: + pawns = (pawn, ) + for p in pawns: + d.draw_circle ((0, 0), p.radius, fill = YELLOW) + if p.kind == 'king': + a = 0.1 * p.radius + b = 0.5 * p.radius + d.draw_line ((a, b), (a, a), (b, a), (b, -a), (a, -a), (a, -b), + (-a, -b), (-a, -a), (-b, -a), (-b, a), (-a, a), (-a, b), + (a, b)) + elif p.kind == 'queen': + d.draw_circle ((0, 0), 0.5 * p.radius) + d.draw_circle ((0, 0), 0.4 * p.radius) + d.trans_scale (0.95) + d.trans_pop () class Pawn (Drawable): @@ -59,7 +67,7 @@ class Pawn (Drawable): self.reset () if self.pos: self.trans_translate (self.pos) - draw_pawn (self, self.model.radius, self.model.kind) + draw_pawn (self, self.model) Drawable.draw (self) class Table (Drawable): -- cgit v1.2.3 From 201aa07cbd28c6be8c4003f126b1ae258f859c68 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 15 May 2011 22:44:20 +0200 Subject: host/simu: handle dropping tower on the table --- host/simu/model/table.py | 4 +- host/simu/model/table_eurobot2011.py | 6 +++ host/simu/robots/robospierre/model/clamp.py | 64 ++++++++++++++++++++++++----- host/simu/view/table_eurobot2011.py | 9 +++- 4 files changed, 70 insertions(+), 13 deletions(-) diff --git a/host/simu/model/table.py b/host/simu/model/table.py index 41767ac4..d79f7758 100644 --- a/host/simu/model/table.py +++ b/host/simu/model/table.py @@ -22,6 +22,7 @@ # # }}} """Table model.""" +from utils.observable import Observable class Intersect: @@ -29,9 +30,10 @@ class Intersect: self.obstacle = obstacle self.distance = distance -class Table: +class Table (Observable): def __init__ (self): + Observable.__init__ (self) self.obstacles = [ ] def intersect (self, a, b, level = None, comp = None): diff --git a/host/simu/model/table_eurobot2011.py b/host/simu/model/table_eurobot2011.py index c9c1d193..f4bb63b7 100644 --- a/host/simu/model/table_eurobot2011.py +++ b/host/simu/model/table_eurobot2011.py @@ -81,3 +81,9 @@ class Table (simu.model.table.Table): self.pawns.append (pawn) # Add everything to obstacles. self.obstacles += self.pawns + + def add_pawn (self, pawn): + self.pawns.append (pawn) + self.obstacles.append (pawn) + self.notify () + diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index 976e559f..fd3dde7c 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -101,16 +101,31 @@ class Clamp (Observable): self.clamping_motor.register (self.__clamping_notified) def __robot_position_notified (self): + # Compute robot direction. + direction = self.__get_robot_direction () # Update bottom slots. changed = False for sloti in (self.SLOT_FRONT_BOTTOM, self.SLOT_BACK_BOTTOM): slot = self.slots[sloti] - if slot.pawn is None: - p = self.__get_floor_elements (slot.side) - if p is not None: - slot.pawn = p - p.pos = None - p.notify () + if direction == slot.side or direction is None: + # If pushing, can take new elements. + if slot.pawn is None: + p = self.__get_floor_elements (slot.side) + if p is not None: + slot.pawn = p + p.pos = None + p.notify () + changed = True + else: + # Else, can drop elements. + if slot.pawn is not None and slot.door_motor.angle: + m = TransMatrix () + m.translate (self.robot_position.pos) + m.rotate (self.robot_position.angle) + xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[slot.side] + slot.pawn.pos = m.apply ((xoffset, 0)) + slot.pawn.notify () + slot.pawn = None changed = True if changed: self.update_contacts () @@ -169,6 +184,7 @@ class Clamp (Observable): tower.kind = 'tower' tower.tower = [ slots[0].pawn, slots[1].pawn ] slots[0].pawn, slots[1].pawn = tower, None + self.table.add_pawn (tower) if slots[0].pawn is not None and slots[0].pawn.kind == 'tower' \ and slots[2].pawn and slots[2].door_motor.angle: slots[0].pawn.tower.append (slots[2].pawn) @@ -199,10 +215,7 @@ class Clamp (Observable): if self.robot_position.pos is None: return None # Matrix to transform an obstacle position into robot coordinates. - m = TransMatrix () - m.rotate (-self.robot_position.angle) - m.translate ((-self.robot_position.pos[0], - -self.robot_position.pos[1])) + m = self.__get_robot_matrix () # Look up elements. xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side] xmargin = 20 @@ -230,3 +243,34 @@ class Clamp (Observable): return slot return None + def __get_robot_direction (self): + """Is robot going forward (0), backward (1), or something else + (None)?""" + if self.robot_position.pos is None: + return None + filt = 5 + if hasattr (self, 'old_robot_position'): + m = self.__get_robot_matrix () + oldrel = m.apply (self.old_robot_position) + if oldrel[0] < 0 and self.direction_counter < filt: + self.direction_counter += 1 + elif oldrel[0] > 0 and self.direction_counter > -filt: + self.direction_counter -= 1 + else: + self.direction_counter = 0 + self.old_robot_position = self.robot_position.pos + # Filter oscillations. + if self.direction_counter > 0: + return 0 + elif self.direction_counter < 0: + return 1 + else: + return None + + def __get_robot_matrix (self): + """Return robot transformation matrix.""" + m = TransMatrix () + m.rotate (-self.robot_position.angle) + m.translate ((-self.robot_position.pos[0], + -self.robot_position.pos[1])) + return m diff --git a/host/simu/view/table_eurobot2011.py b/host/simu/view/table_eurobot2011.py index 5725e5b8..b39f2834 100644 --- a/host/simu/view/table_eurobot2011.py +++ b/host/simu/view/table_eurobot2011.py @@ -76,8 +76,13 @@ class Table (Drawable): def __init__ (self, onto, model): Drawable.__init__ (self, onto) self.model = model - for e in model.pawns: - Pawn (self, e) + self.model.register (self.__notified) + self.__notified () + + def __notified (self): + for e in self.model.pawns: + if e not in self.children: + Pawn (self, e) def draw_both (self, primitive, *args, **kargs): """Draw a primitive on both sides.""" -- cgit v1.2.3 From 9d26b60d08f937ba49962ebfeaa3664f5527bf1a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 17 May 2011 01:58:19 +0200 Subject: digital/asserv: add robospierre model --- digital/ai/tools/robospierre.py | 3 +- digital/asserv/src/asserv/models.host.c | 65 ++++++++++++++++++++++-- digital/asserv/src/asserv/models.host.h | 7 +++ digital/asserv/src/asserv/simu.host.c | 76 +++++++++++++++++++++------- digital/asserv/src/asserv/simu.host.h | 3 ++ digital/asserv/src/asserv/test_motor_model.c | 6 +++ 6 files changed, 138 insertions(+), 22 deletions(-) diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 09be4bea..8d5db276 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -23,7 +23,8 @@ class Robot: self.robot_link = simu.robots.robospierre.link.bag self.robot_model = simu.robots.robospierre.model.bag self.robot_view = simu.robots.robospierre.view.bag - asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel') + asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', + 'robospierre') mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index f4d8ca5a..ae123585 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -32,6 +32,8 @@ #include #include +#define NO_CORNER { { 0, 0 }, { 0, 0 }, { 0, 0 }, { 0, 0 } } + /* RE25CLL with 1:10 gearbox model. */ static const struct motor_def_t re25cll_model = { @@ -89,6 +91,25 @@ static const struct motor_def_t amax32ghp_model = -INFINITY, +INFINITY, }; +/* Faulhaber 2657 and a 1:9.7 ratio gearbox. */ +static const struct motor_def_t faulhaber_2657_model = +{ + /* Motor characteristics. */ + 274 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 34.8 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 2.84, /* Terminal resistance (Ohm). */ + 0.380 / 1000, /* Terminal inductance (H). */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 9.7, /* Gearbox ratio. */ + 0.80, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.0, /* Load (kg.m^2). */ + /* Hardware limits. */ + 0.0, +INFINITY, +}; + /* Gloubi, Efrei 2006. */ static const struct robot_t gloubi_robot = { @@ -106,7 +127,7 @@ static const struct robot_t gloubi_robot = 13.0, // approx /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* Taz, APBTeam/Efrei 2005. */ @@ -126,7 +147,7 @@ static const struct robot_t taz_robot = 0.0, /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* TazG, Taz with RE25G motors. */ @@ -146,7 +167,7 @@ static const struct robot_t tazg_robot = 0.0, /* Whether the encoder is mounted on the main motor (false) or not (true). */ 0, - 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL + 0.0, 0.0, { NULL, NULL }, { 0, 0 }, NULL, NULL, NO_CORNER }; /* Giboulée arm model, with a RE25CLL and a 1:10 ratio gearbox. */ @@ -197,6 +218,7 @@ static const struct robot_t giboulee_robot = { 500, 0 }, /** Sensor update function. */ simu_sensor_update_giboulee, + NULL, NO_CORNER, }; /* AquaJim arm model, with a RE40G and a 1:4 + 15:80 ratio gearbox. */ @@ -267,6 +289,7 @@ static const struct robot_t aquajim_robot = { 250, 250 }, /** Sensor update function. */ simu_sensor_update_aquajim, + NULL, NO_CORNER, }; /* Marcel elevator model, with a Faulhaber 2657 and a 1:9.7 ratio gearbox. */ @@ -338,6 +361,41 @@ static const struct robot_t marcel_robot = { 256, 250 }, /** Sensor update function. */ simu_sensor_update_marcel, + NULL, NO_CORNER, +}; + +/* Robospierre, APBTeam 2011. */ +static const struct robot_t robospierre_robot = +{ + /* Main motors. */ + &faulhaber_2657_model, + /* Number of steps on the main motors encoders. */ + 2500, + /* Wheel radius (m). */ + 0.065 / 2, + /* Distance between the wheels (m). */ + 0.16, + /* Weight of the robot (kg). */ + 10.0, + /* Distance of the gravity center from the center of motors axis (m). */ + 0.0, + /* Whether the encoder is mounted on the main motor (false) or not (true). */ + 1, + /** Encoder wheel radius (m). */ + 0.063 / 2, + /** Distance between the encoders wheels (m). */ + 0.28, + /** Auxiliary motors, NULL if not present. */ + { NULL, NULL }, + /** Number of steps for each auxiliary motor encoder. */ + { 0, 0 }, + /** Sensor update function. */ + NULL, + /** Table test function, return false if given robot point is not in + * table. */ + simu_table_test_robospierre, + /** Robot corners, from front left, then clockwise. */ + { { 150, 50 }, { 150, -50 }, { -150, -50 }, { -150, 50 } }, }; /* Table of models. */ @@ -352,6 +410,7 @@ static const struct { "giboulee", &giboulee_robot }, { "aquajim", &aquajim_robot }, { "marcel", &marcel_robot }, + { "robospierre", &robospierre_robot }, { 0, 0 } }; diff --git a/digital/asserv/src/asserv/models.host.h b/digital/asserv/src/asserv/models.host.h index 403e9cd9..d8f1dcb8 100644 --- a/digital/asserv/src/asserv/models.host.h +++ b/digital/asserv/src/asserv/models.host.h @@ -27,6 +27,8 @@ #define ECHANT_PERIOD (1.0 / (14745600.0 / 256 / 256)) +#define CORNERS_NB 4 + /** Define a robot and its peripherals. * Encoder characteristics are defined at gearbox output. */ struct robot_t @@ -55,6 +57,11 @@ struct robot_t int aux_encoder_steps[AC_ASSERV_AUX_NB]; /** Sensor update function. */ void (*sensor_update) (void); + /** Table test function, return false if given robot point is not in + * table. */ + int (*table_test) (double p_x, double p_y); + /** Robot corners, from front left, then clockwise. */ + double corners[CORNERS_NB][2]; }; /** Get a pointer to a model by name, or return 0. */ diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index 10cef711..d9a97fe2 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -154,6 +154,16 @@ simu_pos_update (double dl, double dr, double footing) simu_pos_a = na; } +/** Compute a robot point absolute position. */ +static void +simu_compute_absolute_position (double p_x, double p_y, double *x, double *y) +{ + double c = cos (simu_pos_a); + double s = sin (simu_pos_a); + *x = simu_pos_x + c * p_x - s * p_y; + *y = simu_pos_y + s * p_x + c * p_y; +} + /** Update sensors for Giboulee. */ void simu_sensor_update_giboulee (void) @@ -175,10 +185,7 @@ simu_sensor_update_giboulee (void) for (i = 0; i < UTILS_COUNT (sensors); i++) { /* Compute absolute position. */ - x = simu_pos_x + cos (simu_pos_a) * sensors[i][0] - - sin (simu_pos_a) * sensors[i][1]; - y = simu_pos_y + sin (simu_pos_a) * sensors[i][0] - + cos (simu_pos_a) * sensors[i][1]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height) PINC |= sensors_bit[i]; } @@ -217,10 +224,7 @@ simu_sensor_update_aquajim (void) for (i = 0; i < UTILS_COUNT (sensors); i++) { /* Compute absolute position. */ - x = simu_pos_x + cos (simu_pos_a) * sensors[i][0] - - sin (simu_pos_a) * sensors[i][1]; - y = simu_pos_y + sin (simu_pos_a) * sensors[i][0] - + cos (simu_pos_a) * sensors[i][1]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); cx = table_width / 2 - x; cy = table_height / 2 - y; ds = cx * cx + cy * cy; @@ -268,10 +272,7 @@ simu_sensor_update_marcel (void) for (i = 0; i < UTILS_COUNT (sensors); i++) { /* Compute absolute position. */ - x = simu_pos_x + cos (simu_pos_a) * sensors[i][0] - - sin (simu_pos_a) * sensors[i][1]; - y = simu_pos_y + sin (simu_pos_a) * sensors[i][0] - + cos (simu_pos_a) * sensors[i][1]; + simu_compute_absolute_position (sensors[i][0], sensors[i][1], &x, &y); if (x >= 0.0 && x < table_width && y >= 0.0 && y < table_height && (x < stand_x_min || x >= stand_x_max || y < stand_y)) PINC |= sensors_bit[i]; @@ -281,6 +282,18 @@ simu_sensor_update_marcel (void) PINC |= IO_BV (CONTACT_AUX1_ZERO_IO); } +/* Table test for Robospierre. */ +int +simu_table_test_robospierre (double p_x, double p_y) +{ + static const double table_width = 3000.0, table_height = 2100.0; + double x, y; + simu_compute_absolute_position (p_x, p_y, &x, &y); + if (x < 0 || y < 0 || x >= table_width || y >= table_height) + return 0; + return 1; +} + /** Do a simulation step. */ static void simu_step (void) @@ -306,6 +319,39 @@ simu_step (void) if (simu_robot->aux_motor[i]) motor_model_step (&simu_aux_model[i]); } + /* Update position. */ + double old_pos_x = simu_pos_x, old_pos_y = simu_pos_y, + old_pos_a = simu_pos_a; + simu_pos_update ((simu_left_model.th - old_left_th) + / simu_left_model.m.i_G * simu_robot->wheel_r * 1000, + (simu_right_model.th - old_right_th) + / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, + simu_robot->footing * 1000); + /* Check robot is still on the table. */ + if (simu_robot->table_test) + { + static int old_out = 1; + int out = 0; + for (i = 0; i < CORNERS_NB; i++) + { + if (!simu_robot->table_test (simu_robot->corners[i][0], + simu_robot->corners[i][1])) + out = 1; + } + /* If out, cancel movement. */ + if (out && !old_out) + { + simu_pos_x = old_pos_x; + simu_pos_y = old_pos_y; + simu_pos_a = old_pos_a; + simu_left_model.th = old_left_th; + simu_right_model.th = old_right_th; + } + else + { + old_out = out; + } + } /* Modify counters. */ uint32_t counter_left_new; uint32_t counter_right_new; @@ -364,12 +410,6 @@ simu_step (void) simu_counter_aux[i] = 0; } } - /* Update position. */ - simu_pos_update ((simu_left_model.th - old_left_th) - / simu_left_model.m.i_G * simu_robot->wheel_r * 1000, - (simu_right_model.th - old_right_th) - / simu_right_model.m.i_G * simu_robot->wheel_r * 1000, - simu_robot->footing * 1000); /* Update sensors. */ if (simu_robot->sensor_update) simu_robot->sensor_update (); diff --git a/digital/asserv/src/asserv/simu.host.h b/digital/asserv/src/asserv/simu.host.h index 143d3430..93785186 100644 --- a/digital/asserv/src/asserv/simu.host.h +++ b/digital/asserv/src/asserv/simu.host.h @@ -47,4 +47,7 @@ simu_sensor_update_aquajim (void); void simu_sensor_update_marcel (void); +int +simu_table_test_robospierre (double p_x, double p_y); + #endif /* simu_host_h */ diff --git a/digital/asserv/src/asserv/test_motor_model.c b/digital/asserv/src/asserv/test_motor_model.c index 706aa31b..044bab2e 100644 --- a/digital/asserv/src/asserv/test_motor_model.c +++ b/digital/asserv/src/asserv/test_motor_model.c @@ -84,3 +84,9 @@ simu_sensor_update_marcel (void) { } +int +simu_table_test_robospierre (double p_x, double p_y) +{ + return 0; +} + -- cgit v1.2.3 From 453c275fb70db0bdd7258c3bedbd8897fa0b35cc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 19 May 2011 22:46:03 +0200 Subject: digital/asserv: slow down execution when running without mex --- digital/asserv/src/asserv/simu.host.c | 31 +++++++++++++++++++++++++++++++ digital/asserv/tools/inter_asserv.py | 2 +- 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c index d9a97fe2..7053ae60 100644 --- a/digital/asserv/src/asserv/simu.host.c +++ b/digital/asserv/src/asserv/simu.host.c @@ -36,6 +36,8 @@ #include #include #include +#include +#include #include "pwm.h" #include "aux.h" @@ -87,6 +89,9 @@ double simu_counter_left_th, simu_counter_right_th; /** Use mex. */ int simu_mex; +/** Do not sleep. */ +int simu_fast; + /** Mex message types. */ uint8_t simu_mex_position; uint8_t simu_mex_pwm; @@ -115,6 +120,7 @@ simu_init (void) simu_mex_pwm = mex_node_reservef ("%s:pwm", mex_instance); simu_mex_aux = mex_node_reservef ("%s:aux", mex_instance); } + simu_fast = simu_mex; if (argc != 1) { fprintf (stderr, "Syntax: asserv.host [-m[interval]] model\n"); @@ -490,12 +496,37 @@ timer_init (void) simu_init (); } +/** Slow down program execution. */ +void +simu_wait (int freq) +{ +#define ONE_SEC_NS 1000000000ll + struct timeval tv; + static long long int last_ns = 0; + long long int now_ns; + gettimeofday (&tv, NULL); + now_ns = tv.tv_sec * ONE_SEC_NS + tv.tv_usec * 1000; + if (last_ns == 0 || now_ns - last_ns > ONE_SEC_NS) + last_ns = now_ns; + last_ns = last_ns + ONE_SEC_NS / freq; + long long int diff_ns = last_ns - now_ns; + if (diff_ns > 0) + { + struct timespec ts; + ts.tv_sec = diff_ns / ONE_SEC_NS; + ts.tv_nsec = diff_ns % ONE_SEC_NS; + nanosleep (&ts, &ts); + } +} + /** Wait for timer overflow. */ void timer_wait (void) { if (simu_mex) mex_node_wait_date (mex_node_date () + 4); + if (!simu_fast) + simu_wait (225); simu_step (); if (simu_mex && !--simu_send_cpt) { diff --git a/digital/asserv/tools/inter_asserv.py b/digital/asserv/tools/inter_asserv.py index 82a92c32..72e31f60 100644 --- a/digital/asserv/tools/inter_asserv.py +++ b/digital/asserv/tools/inter_asserv.py @@ -43,7 +43,7 @@ class InterAsserv (Inter): self.tk.createfilehandler (self.a, READABLE, self.read) self.timeout () # Query position. - self.a.register_pos () + self.a.register_pos (interval = 225 / 20) self.a.position.register (self.pos) def createWidgets (self): -- cgit v1.2.3 From f753bc39682f980feb49b7ff1bb9e9594f986e6b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 19 May 2011 23:24:08 +0200 Subject: digital/asserv: add push the wall traj mode, closes #167 --- digital/ai/src/twi_master/asserv.c | 22 ++++++++++++++ digital/ai/src/twi_master/asserv.h | 6 ++++ digital/asserv/src/asserv/main.c | 21 +++++++++++++ digital/asserv/src/asserv/traj.c | 57 +++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 4 +++ digital/asserv/src/asserv/twi_proto.c | 18 +++++++++++ digital/asserv/tools/asserv/asserv.py | 19 ++++++++++-- 7 files changed, 145 insertions(+), 2 deletions(-) diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c index d9ae9330..839b1d23 100644 --- a/digital/ai/src/twi_master/asserv.c +++ b/digital/ai/src/twi_master/asserv.c @@ -300,6 +300,28 @@ asserv_go_to_the_wall (uint8_t backward) twi_master_send_buffer (2); } +void +asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, + uint16_t init_a) +{ + if (init_x != (uint32_t) -1) + init_x = fixed_mul_f824 (init_x, asserv_scale_inv); + if (init_y != (uint32_t) -1) + init_y = fixed_mul_f824 (init_y, asserv_scale_inv); + uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE); + buffer[0] = 'G'; + buffer[1] = backward; + buffer[2] = v32_to_v8 (init_x, 2); + buffer[3] = v32_to_v8 (init_x, 1); + buffer[4] = v32_to_v8 (init_x, 0); + buffer[5] = v32_to_v8 (init_y, 2); + buffer[6] = v32_to_v8 (init_y, 1); + buffer[7] = v32_to_v8 (init_y, 0); + buffer[8] = v16_to_v8 (init_a, 1); + buffer[9] = v16_to_v8 (init_a, 0); + twi_master_send_buffer (10); +} + void asserv_move_motor0_absolute (uint16_t position, uint8_t speed) { diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h index 2aa7a6f8..c9a52a11 100644 --- a/digital/ai/src/twi_master/asserv.h +++ b/digital/ai/src/twi_master/asserv.h @@ -198,6 +198,12 @@ asserv_goto_xya (uint32_t x, uint32_t y, int16_t a, uint8_t backward); void asserv_go_to_the_wall (uint8_t backward); +/** Push the wall and initialise position. Use -1 for coordinates to keep + * unchanged. */ +void +asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y, + uint16_t init_a); + /** * Move the motor0. * Motor0 class command. diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 2d53e2fa..677fea84 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -400,6 +400,27 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; traj_ftw_start_center (args[0], args[1], args[2]); break; + case c ('f', 12): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - d: init_x, f24.8. + * - d: init_y, f24.8. + * - w: init_a, f0.16. + * - b: sequence number. */ + { + if (args[11] == state_main.sequence) + break; + int32_t angle; + if (args[9] == 0xff && args[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, args[9], args[10], 0); + traj_ptw_start (args[0], + v8_to_v32 (args[1], args[2], args[3], args[4]), + v8_to_v32 (args[5], args[6], args[7], args[8]), + angle, args[11]); + } + break; case c ('F', 1): /* Go to the dispenser. * - b: sequence number. */ diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index 11618eca..e147ad5d 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -37,6 +37,7 @@ #include "pos.h" #include "speed.h" #include "postrack.h" +#include "pwm.h" #include "contacts.h" @@ -59,6 +60,8 @@ enum TRAJ_FTW, /* Go to the dispenser. */ TRAJ_GTD, + /* Push the wall. */ + TRAJ_PTW, /* Go to position. */ TRAJ_GOTO, /* Go to angle. */ @@ -99,6 +102,9 @@ static uint8_t traj_use_center; /** Center sensor delay. */ static uint8_t traj_center_delay; +/** Initial values for x, y and angle, or -1. */ +static int32_t traj_init_x, traj_init_y, traj_init_a; + /** Initialise computed factors. */ void traj_init (void) @@ -210,6 +216,54 @@ traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq) state_start (&state_main, MODE_TRAJ, seq); } +/** Push the wall mode. */ +static void +traj_ptw (void) +{ + /* If blocking, the wall was found. */ + if (pos_theta.blocked_counter >= pos_theta.blocked_counter_limit) + { + /* Initialise position. */ + if (traj_init_x != -1) + postrack_x = traj_init_x; + if (traj_init_y != -1) + postrack_y = traj_init_y; + if (traj_init_a != -1) + postrack_a = traj_init_a; + /* Stop motor control. */ + pos_reset (&pos_theta); + pos_reset (&pos_alpha); + state_main.variant = 0; + state_main.mode = MODE_PWM; + pwm_set (&pwm_left, 0); + pwm_set (&pwm_right, 0); + state_finish (&state_main); + traj_mode = TRAJ_DONE; + } +} + +/** Start push the wall mode. Position is initialised unless -1. */ +void +traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, + int32_t init_a, uint8_t seq) +{ + int16_t speed; + traj_mode = TRAJ_PTW; + traj_init_x = init_x; + traj_init_y = init_y; + traj_init_a = init_a; + state_start (&state_main, MODE_TRAJ, seq); + /* Use slow speed, without alpha control. */ + speed = speed_theta.slow; + speed *= 256; + if (backward) + speed = -speed; + speed_theta.use_pos = speed_alpha.use_pos = 0; + speed_theta.cons = speed; + speed_alpha.cons = 0; + state_main.variant = 2; +} + /** Go to the dispenser mode. */ static void traj_gtd (void) @@ -392,6 +446,9 @@ traj_update (void) case TRAJ_FTW: traj_ftw (); break; + case TRAJ_PTW: + traj_ptw (); + break; case TRAJ_GTD: traj_gtd (); break; diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index 706fbdaf..ec345d0c 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -48,6 +48,10 @@ traj_ftw_start (uint8_t backward, uint8_t seq); void traj_ftw_start_center (uint8_t backward, uint8_t center_delay, uint8_t seq); +void +traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y, + int32_t init_a, uint8_t seq); + void traj_gtd_start (uint8_t seq); diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index ff15c82f..fcf0a9cc 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -168,6 +168,24 @@ twi_proto_callback (u8 *buf, u8 size) * - b: 0: forward, 1: backward. */ traj_ftw_start (buf[2], 0); break; + case c ('G', 9): + /* Push the wall. + * - b: 0: forward, 1: backward. + * - 3b: init_x. + * - 3b: init_y. + * - w: init_a. */ + { + int32_t angle; + if (buf[9] == 0xff && buf[10] == 0xff) + angle = -1; + else + angle = v8_to_v32 (0, buf[9], buf[10], 0); + traj_ptw_start (buf[2], + v8_to_v32 (buf[3], buf[4], buf[5], 0xff), + v8_to_v32 (buf[6], buf[7], buf[8], 0xff), + angle, 0); + } + break; case c ('g', 2): /* Go to the wall using center sensor with delay. * - b: 0: forward, 1: backward. diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py index 997b05fb..4f386d7a 100644 --- a/digital/asserv/tools/asserv/asserv.py +++ b/digital/asserv/tools/asserv/asserv.py @@ -269,6 +269,15 @@ class Proto: self.proto.send ('f', 'BB', backward and 1 or 0, self.mseq) self.wait (self.finished, auto = True) + def ptw (self, backward = True, init_x = None, init_y = None, + init_a = None): + """Push the wall.""" + self.mseq += 1 + self.proto.send ('f', 'BllHB', backward and 1 or 0, + self._dist_f248 (init_x), self._dist_f248 (init_y), + self._angle_f16 (init_a), self.mseq) + self.wait (self.finished, auto = True) + def set_simu_pos (self, x, y, a): """Set simulated position.""" self.proto.send ('h', 'chhh', 'X', int (round (x)), int (round (y)), @@ -381,10 +390,16 @@ class Proto: return int (round (d / self.param['scale'])) def _dist_f248 (self, d): - return int (round ((1 << 8) * d / self.param['scale'])) + if d is None: + return -1 + else: + return int (round ((1 << 8) * d / self.param['scale'])) def _angle_f16 (self, a): - return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff + if a is None: + return 0xffff + else: + return int (round ((1 << 16) * a / (2 * math.pi))) & 0xffff def _angle_f824 (self, a): return int (round ((1 << 24) * a / (2 * math.pi))) -- cgit v1.2.3 From c3274377f05073c6d40265ab7849df2a72cfd54e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 20 May 2011 08:02:58 +0200 Subject: digital/asserv: fix bad model description --- digital/asserv/src/asserv/models.host.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/asserv/src/asserv/models.host.c b/digital/asserv/src/asserv/models.host.c index ae123585..3839af1e 100644 --- a/digital/asserv/src/asserv/models.host.c +++ b/digital/asserv/src/asserv/models.host.c @@ -107,7 +107,7 @@ static const struct motor_def_t faulhaber_2657_model = /* Load characteristics. */ 0.0, /* Load (kg.m^2). */ /* Hardware limits. */ - 0.0, +INFINITY, + -INFINITY, +INFINITY, }; /* Gloubi, Efrei 2006. */ @@ -395,7 +395,7 @@ static const struct robot_t robospierre_robot = * table. */ simu_table_test_robospierre, /** Robot corners, from front left, then clockwise. */ - { { 150, 50 }, { 150, -50 }, { -150, -50 }, { -150, 50 } }, + { { 150, 110 }, { 150, -110 }, { -150, -110 }, { -150, 110 } }, }; /* Table of models. */ -- cgit v1.2.3 From 2277c9a7aa41c347ba04fca8e202c77b43618802 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 20 May 2011 08:27:02 +0200 Subject: digital/{ai,io-hub}: add init FSM --- digital/ai/src/fsm/init.c | 188 +++++++++++++++++++++++++++++ digital/ai/tools/robospierre.py | 5 +- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 10 ++ digital/io-hub/src/robospierre/clamp.c | 6 +- digital/io-hub/src/robospierre/init_defs.h | 50 ++++++++ digital/io-hub/src/robospierre/main.c | 14 ++- 7 files changed, 265 insertions(+), 10 deletions(-) create mode 100644 digital/ai/src/fsm/init.c create mode 100644 digital/io-hub/src/robospierre/init_defs.h diff --git a/digital/ai/src/fsm/init.c b/digital/ai/src/fsm/init.c new file mode 100644 index 00000000..1f933ce1 --- /dev/null +++ b/digital/ai/src/fsm/init.c @@ -0,0 +1,188 @@ +/* init.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" + +#include "asserv.h" +#include "contact.h" +#include "chrono.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" + +#include "bot.h" +#include "init_defs.h" + +/* + * Initialise robot position with a calibration procedure. + */ + +FSM_STATES ( + /* Initial state, waiting for jack. */ + INIT_START, + /* After first jack insertion, waiting for removal to initialise actuators. */ + INIT_WAITING_FIRST_JACK_OUT, + /* Initialising actuators, then waiting for second jack insertion. */ + INIT_INITIALISING_ACTUATORS, + /* After second jack insertion, waiting the operator remove its + * hand. */ + INIT_WAITING_HANDS_OUT, + /* Finding the first wall. */ + INIT_FINDING_FIRST_WALL, + /* Going away from the wall. */ + INIT_GOING_AWAY_FIRST_WALL, + /* Turning to face the other wall. */ + INIT_FACING_SECOND_WALL, + /* Waiting after rotation for robot to stabilize. */ + INIT_WAITING_AFTER_FACING_SECOND_WALL, + /* Finding the second wall. */ + INIT_FINDING_SECOND_WALL, + /* Going away from the wall. */ + INIT_GOING_AWAY_SECOND_WALL, +#ifdef INIT_START_POSITION_ANGLE + /* Facing the start position. */ + INIT_FACING_START_POSITION, +#endif + /* Going to start position. */ + INIT_GOING_TO_START_POSITION, + /* Waiting for the round start (waiting for the jack). */ + INIT_WAITING_SECOND_JACK_OUT, + /* Initialisation finished, nothing else to do. */ + INIT_FINISHED) + +FSM_EVENTS ( + /* XXX: temporarily here. */ + robot_move_success, + robot_move_failure, + /* Jack is inserted. */ + jack_inserted, + /* Jack is removed. */ + jack_removed, + /* Sent to initialise actuators. */ + init_actuators, + /* Sent to start round. */ + init_start_round) + +FSM_START_WITH (INIT_START) + +FSM_TRANS (INIT_START, jack_inserted, INIT_WAITING_FIRST_JACK_OUT) +{ + return FSM_NEXT (INIT_START, jack_inserted); +} + +FSM_TRANS (INIT_WAITING_FIRST_JACK_OUT, jack_removed, + INIT_INITIALISING_ACTUATORS) +{ + fsm_queue_post_event (FSM_EVENT (AI, init_actuators)); + return FSM_NEXT (INIT_WAITING_FIRST_JACK_OUT, jack_removed); +} + +FSM_TRANS (INIT_INITIALISING_ACTUATORS, jack_inserted, INIT_WAITING_HANDS_OUT) +{ + team_color = contact_get_color (); + return FSM_NEXT (INIT_INITIALISING_ACTUATORS, jack_inserted); +} + +FSM_TRANS_TIMEOUT (INIT_WAITING_HANDS_OUT, 225, INIT_FINDING_FIRST_WALL) +{ + asserv_set_speed (BOT_SPEED_INIT); + asserv_push_the_wall (INIT_FIRST_WALL_PUSH); + return FSM_NEXT_TIMEOUT (INIT_WAITING_HANDS_OUT); +} + +FSM_TRANS (INIT_FINDING_FIRST_WALL, robot_move_success, + INIT_GOING_AWAY_FIRST_WALL) +{ + asserv_move_linearly (INIT_FIRST_WALL_AWAY); + return FSM_NEXT (INIT_FINDING_FIRST_WALL, robot_move_success); +} + +FSM_TRANS (INIT_GOING_AWAY_FIRST_WALL, robot_move_success, + INIT_FACING_SECOND_WALL) +{ + asserv_goto_angle (INIT_SECOND_WALL_ANGLE); + return FSM_NEXT (INIT_GOING_AWAY_FIRST_WALL, robot_move_success); +} + +FSM_TRANS (INIT_FACING_SECOND_WALL, robot_move_success, + INIT_WAITING_AFTER_FACING_SECOND_WALL) +{ + return FSM_NEXT (INIT_FACING_SECOND_WALL, robot_move_success); +} + +FSM_TRANS_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL, 225 / 2, + INIT_FINDING_SECOND_WALL) +{ + asserv_push_the_wall (INIT_SECOND_WALL_PUSH); + return FSM_NEXT_TIMEOUT (INIT_WAITING_AFTER_FACING_SECOND_WALL); +} + +FSM_TRANS (INIT_FINDING_SECOND_WALL, robot_move_success, + INIT_GOING_AWAY_SECOND_WALL) +{ + asserv_move_linearly (INIT_SECOND_WALL_AWAY); + return FSM_NEXT (INIT_FINDING_SECOND_WALL, robot_move_success); +} + +#ifdef INIT_START_POSITION_ANGLE +FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, + INIT_FACING_START_POSITION) +{ + asserv_goto_angle (INIT_START_POSITION_ANGLE); + return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success); +} + +FSM_TRANS (INIT_FACING_START_POSITION, robot_move_success, + INIT_GOING_TO_START_POSITION) +{ + asserv_goto_xya (INIT_START_POSITION); + return FSM_NEXT (INIT_FACING_START_POSITION, robot_move_success); +} + +#else + +FSM_TRANS (INIT_GOING_AWAY_SECOND_WALL, robot_move_success, + INIT_GOING_TO_START_POSITION) +{ + asserv_goto_xya (INIT_START_POSITION); + return FSM_NEXT (INIT_GOING_AWAY_SECOND_WALL, robot_move_success); +} + +#endif + +FSM_TRANS (INIT_GOING_TO_START_POSITION, robot_move_success, + INIT_WAITING_SECOND_JACK_OUT) +{ + asserv_set_speed (BOT_SPEED_NORMAL); + return FSM_NEXT (INIT_GOING_TO_START_POSITION, robot_move_success); +} + +FSM_TRANS (INIT_WAITING_SECOND_JACK_OUT, jack_removed, INIT_FINISHED) +{ + chrono_init (); + fsm_queue_post_event (FSM_EVENT (AI, init_start_round)); + return FSM_NEXT (INIT_WAITING_SECOND_JACK_OUT, jack_removed); +} + diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 8d5db276..e8384f6c 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -34,7 +34,8 @@ class Robot: self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time, **io_hub.init.host['robospierre']) self.robot_start_pos = { - False: (700, 2100 - 250, math.radians (-270)), - True: (3000 - 700, 2100 - 250, math.radians (-270)) + # In real life, better place the robot in green zone. + False: (300, 2100 - 200, math.radians (180)), + True: (3000 - 300, 2100 - 200, math.radians (0)) } diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 20e9dee0..d69918f5 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ - fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ + init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ twi_master.c asserv.c mimot.c \ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index e014321e..a986c015 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,6 +34,16 @@ # define BOT_SCALE 0.0415178942124 #endif +/** Distance between the front contact point and the robot center. */ +#define BOT_FRONT_CONTACT_DIST_MM 150 +/** Angle error at the front contact point. */ +#define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0 + +/** Speed used for initialisation. */ +#define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 +/** Normal cruise speed. */ +#define BOT_SPEED_NORMAL 0x40, 0x40, 0x20, 0x20 + #ifdef HOST # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 6c063979..583cadb5 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -83,8 +83,6 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( - /* Here for the moment, to be moved later. */ - start, /* New element inside bottom slot. */ clamp_new_element, /* Order to drop elements. */ @@ -325,10 +323,10 @@ clamp_route (void) /* CLAMP FSM */ -FSM_TRANS (CLAMP_START, start, CLAMP_GOING_IDLE) +FSM_TRANS (CLAMP_START, init_actuators, CLAMP_GOING_IDLE) { clamp_move (CLAMP_SLOT_FRONT_MIDDLE); - return FSM_NEXT (CLAMP_START, start); + return FSM_NEXT (CLAMP_START, init_actuators); } FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) diff --git a/digital/io-hub/src/robospierre/init_defs.h b/digital/io-hub/src/robospierre/init_defs.h new file mode 100644 index 00000000..6ce6eaae --- /dev/null +++ b/digital/io-hub/src/robospierre/init_defs.h @@ -0,0 +1,50 @@ +#ifndef init_defs_h +#define init_defs_h +/* init_defs.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#include "playground.h" +#include "bot.h" + +/** Parameters to push the first wall. */ +#define INIT_FIRST_WALL_PUSH \ + 0, PG_X (BOT_FRONT_CONTACT_DIST_MM), 200, \ + PG_A_DEG (180 + BOT_FRONT_CONTACT_ANGLE_ERROR_DEG) +/** Parameters to go away from the first wall. */ +#define INIT_FIRST_WALL_AWAY -500 +/** Parameter to face the second wall. */ +#define INIT_SECOND_WALL_ANGLE PG_A_DEG (90) +/** Parameters to push the second wall. */ +#define INIT_SECOND_WALL_PUSH \ + 0, -1, PG_Y (PG_LENGTH - BOT_FRONT_CONTACT_DIST_MM), -1 +/** Parameters to go away from the second wall. */ +#define INIT_SECOND_WALL_AWAY -(200 - BOT_FRONT_CONTACT_DIST_MM) +/** Parameter to face the start position. */ +#define INIT_START_POSITION_ANGLE PG_A_DEG (0) +/** Start position. */ +#define INIT_START_POSITION \ + PG_X (200), PG_Y (PG_LENGTH - 200), PG_A_DEG (0), ASSERV_BACKWARD + +#endif /* init_defs_h */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 97d5bd40..f1b4d8de 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -110,9 +110,15 @@ main_event_to_fsm (void) /* Update FSM timeouts. */ FSM_HANDLE_TIMEOUT_E (AI); /* Motor status. */ - asserv_status_e mimot_motor0_status, mimot_motor1_status; + asserv_status_e robot_move_status, mimot_motor0_status, + mimot_motor1_status; + robot_move_status = asserv_move_cmd_status (); mimot_motor0_status = mimot_motor0_cmd_status (); mimot_motor1_status = mimot_motor1_cmd_status (); + if (robot_move_status == success) + FSM_HANDLE_E (AI, robot_move_success); + else if (robot_move_status == failure) + FSM_HANDLE_E (AI, robot_move_failure); if (mimot_motor0_status == success && mimot_motor1_status == success) FSM_HANDLE_E (AI, clamp_elevation_rotation_success); @@ -123,9 +129,11 @@ main_event_to_fsm (void) /* Clamp specific events. */ if (clamp_handle_event ()) return; - /* Jack, XXX to be changed! */ + /* Jack. */ if (!contact_get_jack ()) - FSM_HANDLE_E (AI, start); + FSM_HANDLE_E (AI, jack_inserted); + else + FSM_HANDLE_E (AI, jack_removed); /* Events from the event queue. */ if (fsm_queue_poll ()) { -- cgit v1.2.3 From cf5b9ed3d6539e5afda57070625d8f5404faaa1b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 20 May 2011 08:30:33 +0200 Subject: host/simu/model/table_eurobot2011: each card can be drawn only once --- host/simu/model/table_eurobot2011.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/host/simu/model/table_eurobot2011.py b/host/simu/model/table_eurobot2011.py index f4bb63b7..f200028e 100644 --- a/host/simu/model/table_eurobot2011.py +++ b/host/simu/model/table_eurobot2011.py @@ -31,8 +31,11 @@ class Table (simu.model.table.Table): def __init__ (self, cards = None): simu.model.table.Table.__init__ (self) # Draw cards. - if cards is None: - cards = [ randrange (20) for i in xrange (3) ] + cards = [ ] + while len (cards) != 3: + card = randrange (20) + if card not in cards: + cards.append (card) self.cards = cards def pos (card): king_pos = card // 4 -- cgit v1.2.3 From 9bd92331b124a2123b3f9fb6bf84510c81ccce5e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 21 May 2011 13:04:28 +0200 Subject: digital/ai/src/fsm: fix AVR timeout code --- digital/ai/src/fsm/fsm.host.c | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c index b7b6ab4e..ad791272 100644 --- a/digital/ai/src/fsm/fsm.host.c +++ b/digital/ai/src/fsm/fsm.host.c @@ -1451,18 +1451,19 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fprintf (f, "\tint out = 0;\n"); fprintf (f, "\tfor (i = 0; i < fsm_%s_max_active_states; i++)\n\t{\n", fsm->name); - fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] > 0)\n", + fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] > 0)\n\t\t{\n", fsm->name); fprintf (f, "\t\t\tfsm_%s_timeout_counters[i]--;\n", fsm->name); - fprintf (f, "\t\tif (fsm_%s_timeout_counters[i] == 0)\n\t\t{\n", + fprintf (f, "\t\t\tif (fsm_%s_timeout_counters[i] == 0)\n\t\t\t{\n", fsm->name); - fprintf (f, "\t\t\tfsm_%s_handle (fsm_%s_timeout_events[fsm_%s_active_states[i]]);\n", + fprintf (f, "\t\t\t\tfsm_%s_handle (fsm_%s_timeout_events[fsm_%s_active_states[i]]);\n", fsm->name, fsm->name, fsm->name); - fprintf (f, "\t\t\tout = 1;\n"); - fprintf (f, "\t\t}\n\n"); + fprintf (f, "\t\t\t\tout = 1;\n"); + fprintf (f, "\t\t\t}\n"); + fprintf (f, "\t\t}\n"); fprintf (f, "\t}\n"); fprintf (f, "\treturn out;\n"); fprintf (f, "}\n\n"); -- cgit v1.2.3 From 8bd4ff291b5b76a636a68c6101bc1e0b7638b35c Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 21 May 2011 02:00:27 +0200 Subject: digital/ai: add function to set chrono as desired --- digital/ai/src/utils/chrono.c | 7 +++++++ digital/ai/src/utils/chrono.h | 8 ++++++++ 2 files changed, 15 insertions(+) diff --git a/digital/ai/src/utils/chrono.c b/digital/ai/src/utils/chrono.c index a1212e19..85851547 100644 --- a/digital/ai/src/utils/chrono.c +++ b/digital/ai/src/utils/chrono.c @@ -148,3 +148,10 @@ chrono_end_match (uint8_t block) ; #endif } + +void +chrono_set_timer (uint32_t elapsed_time) +{ + if (chrono_enabled_) + chrono_ov_count_ = elapsed_time / TIMER_PERIOD_MS; +} diff --git a/digital/ai/src/utils/chrono.h b/digital/ai/src/utils/chrono.h index 38bf3d61..580529cf 100644 --- a/digital/ai/src/utils/chrono.h +++ b/digital/ai/src/utils/chrono.h @@ -96,4 +96,12 @@ chrono_remaining_time (void); void chrono_end_match (uint8_t block); +/** + * Set timer at desired value. + * This function should be used for tests purpose only. + * @param elapsed_time elapsed time since beginning. + */ +void +chrono_set_timer (uint32_t elapsed_time); + #endif /* chrono_h */ -- cgit v1.2.3 From f6601c1f928cc28a88bcb8921ca927453d53a1f2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 17:26:31 +0200 Subject: digital/mimot: add offset to compensate H-bridge dead zone --- digital/mimot/src/dirty/pwm.h | 5 ++++- digital/mimot/src/dirty/pwm_ocr.avr.c | 8 ++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/digital/mimot/src/dirty/pwm.h b/digital/mimot/src/dirty/pwm.h index a751ee9e..b4f12d0d 100644 --- a/digital/mimot/src/dirty/pwm.h +++ b/digital/mimot/src/dirty/pwm.h @@ -25,8 +25,11 @@ * * }}} */ +/** Offset to compensate for H-bridge dead zone at low PWM values. */ +#define PWM_OFFSET 0x40 + /** Define the absolute maximum PWM value. */ -#define PWM_MAX 0x3ff +#define PWM_MAX (0x3ff - PWM_OFFSET) /** PWM control state. */ struct pwm_t diff --git a/digital/mimot/src/dirty/pwm_ocr.avr.c b/digital/mimot/src/dirty/pwm_ocr.avr.c index c4cd5279..a1d98c25 100644 --- a/digital/mimot/src/dirty/pwm_ocr.avr.c +++ b/digital/mimot/src/dirty/pwm_ocr.avr.c @@ -94,12 +94,12 @@ pwm_ocr_update (void) IO_SET (PWM1_BRK_IO); if (PWM_VALUE (PWM1) < 0) { - pwm1 = -PWM_VALUE (PWM1); + pwm1 = -PWM_VALUE (PWM1) + PWM_OFFSET; } else { dir_d |= _BV (PWM1_DIR); - pwm1 = PWM_VALUE (PWM1); + pwm1 = PWM_VALUE (PWM1) + PWM_OFFSET; } } # endif /* PWM1 */ @@ -115,12 +115,12 @@ pwm_ocr_update (void) IO_SET (PWM2_BRK_IO); if (PWM_VALUE (PWM2) < 0) { - pwm2 = -PWM_VALUE (PWM2); + pwm2 = -PWM_VALUE (PWM2) + PWM_OFFSET; } else { dir_d |= _BV (PWM2_DIR); - pwm2 = PWM_VALUE (PWM2); + pwm2 = PWM_VALUE (PWM2) + PWM_OFFSET; } } # endif /* PWM2 */ -- cgit v1.2.3 From 447138614e45686cff0bf55a351610c1c108168a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 18:42:06 +0200 Subject: digital/io-hub: update clamp positions and pwm values --- digital/io-hub/src/robospierre/bot.h | 75 ++++++++++++++++++++----------- digital/io-hub/src/robospierre/clamp.c | 17 +++---- host/simu/robots/robospierre/model/bag.py | 4 +- 3 files changed, 59 insertions(+), 37 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index a986c015..5f9d177a 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -53,30 +53,49 @@ # define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x3b0b / 2) # define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3b0b # define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3b0b -# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 3) -# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 3) +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) -# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 -# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x11c6 -# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x11c6 / 2) +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x11c6 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x11c6 +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x11c6 + +# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ + BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP +# define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ + BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP \ + (BOT_CLAMP_BAY_BACK_ROTATION_STEP / 2) #else /* !HOST */ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 -# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP 0x1d83 -# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x3288 -# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0 -# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP 0x1d83 -# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3288 -# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3288 -# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1d83 -# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1d83 -# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP 0x1d83 - -# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP 0 -# define BOT_CLAMP_BAY_BACK_ROTATION_STEP 0x10e2 -# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x820 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP (0x1da7 - 250) +# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x34f7 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0x0169 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x1f03 - 250) +# define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3610 +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3729 +# define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 +# define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 +# define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) + +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x10ce +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP ((0x10ce + 0x10e2) / 2) +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x10e2 + +# define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ + BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP +# define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ + BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x816 #endif /* !HOST */ @@ -89,14 +108,16 @@ #define BOT_PWM_DOOR_BACK_BOTTOM 3 #define BOT_PWM_DOOR_BACK_TOP 4 -#define BOT_PWM_CLAMP_OPEN_TIME 225 -#define BOT_PWM_CLAMP_OPEN 0x3ff, 225, 0 -#define BOT_PWM_CLAMP_CLOSE_TIME 225 -#define BOT_PWM_CLAMP_CLOSE -0x3ff, 225, 0 - -#define BOT_PWM_DOOR_OPEN_TIME 225 -#define BOT_PWM_DOOR_OPEN 0x3ff, 225, 0 -#define BOT_PWM_DOOR_CLOSE_TIME 225 -#define BOT_PWM_DOOR_CLOSE -0x3ff, 225, 0 +#define BOT_PWM_CLAMP_OPEN_TIME 75 +#define BOT_PWM_CLAMP_OPEN 0x3ff, 75, 0 +#define BOT_PWM_CLAMP_CLOSE_TIME 75 +#define BOT_PWM_CLAMP_CLOSE -0x3ff, 75, 0 + +#define BOT_PWM_DOOR_OPEN_TIME 12 +#define BOT_PWM_DOOR_OPEN 0x3ff, 37, 0x55 +#define BOT_PWM_DOOR_CLOSE_TIME 50 +#define BOT_PWM_DOOR_CLOSE(slot) \ + -0x3ff, 50, ((slot == CLAMP_SLOT_FRONT_BOTTOM \ + || slot == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180) #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 583cadb5..b7c85250 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -130,17 +130,17 @@ struct clamp_t clamp_global; /** Clamp positions. */ static const uint16_t clamp_pos[][2] = { { BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP, - BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP }, { BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP, - BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP }, { BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP, - BOT_CLAMP_BAY_FRONT_ROTATION_STEP }, + BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP }, { BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP, - BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP }, { BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP, - BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP }, { BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP, - BOT_CLAMP_BAY_BACK_ROTATION_STEP }, + BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP }, { BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP, BOT_CLAMP_BAY_SIDE_ROTATION_STEP }, { BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP, @@ -336,7 +336,8 @@ FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) { - pwm_set_timed (clamp_slot_door[ctx.pos_new], BOT_PWM_DOOR_CLOSE); + pwm_set_timed (clamp_slot_door[ctx.pos_new], + BOT_PWM_DOOR_CLOSE (ctx.pos_new)); return FSM_NEXT (CLAMP_IDLE, clamp_new_element); } @@ -524,7 +525,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, if (clamp_slot_door[ctx.pos_current] != 0xff) { pwm_set_timed (clamp_slot_door[ctx.pos_current], - BOT_PWM_DOOR_CLOSE); + BOT_PWM_DOOR_CLOSE (ctx.pos_current)); return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_close_door); diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 5a6fa023..b1c49d35 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -38,9 +38,9 @@ class Bag: for contact in link_bag.io_hub.contact[2:] ] self.position = Position (link_bag.asserv.position) self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler, - 2 * pi, 0, pi) + 8 * pi, 0, pi) self.door_motors = [ MotorBasic (link_bag.io_hub.pwm[i], scheduler, - 2 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ] + 8 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ] self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0], link_bag.mimot.aux[1], self.clamping_motor, self.door_motors, self.contact[0:7]) -- cgit v1.2.3 From 27700ff4b67d9e8351da875c6f4d58a8f1311e5d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 17:21:56 +0200 Subject: digital/io-hub: rename element to element_type where appropriated --- digital/io-hub/src/robospierre/clamp.c | 16 ++++++++-------- digital/io-hub/src/robospierre/clamp.h | 2 +- digital/io-hub/src/robospierre/logistic.c | 4 ++-- digital/io-hub/src/robospierre/logistic.h | 2 +- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index b7c85250..50fcf041 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -117,8 +117,8 @@ struct clamp_t uint8_t moving_to; /** Position of a new element. */ uint8_t pos_new; - /** New element kind. */ - uint8_t new_element; + /** New element type. */ + uint8_t new_element_type; /** Drop direction, drop on the other side. */ uint8_t drop_direction; }; @@ -192,11 +192,11 @@ clamp_move_element (uint8_t from, uint8_t to) } void -clamp_new_element (uint8_t pos, uint8_t element) +clamp_new_element (uint8_t pos, uint8_t element_type) { assert (pos == CLAMP_SLOT_FRONT_BOTTOM || pos == CLAMP_SLOT_BACK_BOTTOM); ctx.pos_new = pos; - ctx.new_element = element; + ctx.new_element_type = element_type; FSM_HANDLE (AI, clamp_new_element); } @@ -225,17 +225,17 @@ clamp_handle_event (void) if (FSM_CAN_HANDLE (AI, clamp_new_element)) { /* XXX: temporary hack. */ - uint8_t element = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; + uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; if (!IO_GET (CONTACT_FRONT_BOTTOM) && !logistic_global.slots[CLAMP_SLOT_FRONT_BOTTOM]) { - clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element); + clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element_type); return 1; } if (!IO_GET (CONTACT_BACK_BOTTOM) && !logistic_global.slots[CLAMP_SLOT_BACK_BOTTOM]) { - clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element); + clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element_type); return 1; } } @@ -356,7 +356,7 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, move_to_idle, CLAMP_GOING_IDLE, done, CLAMP_IDLE) { - logistic_element_new (ctx.pos_new, ctx.new_element); + logistic_element_new (ctx.pos_new, ctx.new_element_type); if (logistic_global.moving_from != CLAMP_SLOT_NB) { clamp_move_element (logistic_global.moving_from, diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 64575508..7471ceac 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -70,7 +70,7 @@ clamp_move_element (uint8_t from, uint8_t to); /** Simulate the presence of a new element. */ void -clamp_new_element (uint8_t pos, uint8_t element); +clamp_new_element (uint8_t pos, uint8_t element_type); /** Drop an element tower. Return 0 if not currently possible. If * drop_direction is forward, drop at the back. */ diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 945bd583..8c2a759f 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -192,11 +192,11 @@ logistic_update (void) } void -logistic_element_new (uint8_t pos, uint8_t element) +logistic_element_new (uint8_t pos, uint8_t element_type) { assert (pos < CLAMP_SLOT_NB); assert (!ctx.slots[pos]); - ctx.slots[pos] = element; + ctx.slots[pos] = element_type; logistic_decision (); } diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index c6aa4159..fd47bd1e 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -59,7 +59,7 @@ logistic_update (void); /** To be called when a new element is entering the robot. */ void -logistic_element_new (uint8_t pos, uint8_t element); +logistic_element_new (uint8_t pos, uint8_t element_type); /** To be called when a element movement is done. */ void -- cgit v1.2.3 From 4232368383074bfa9fcb98d96b6a20f0a4ef2736 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 20:42:57 +0200 Subject: digital/io-hub: handle clamp locking When the clamp is in a bay with an element at the middle level, it can no longer move. --- digital/io-hub/src/robospierre/clamp.c | 51 ++++++++++++++++++++++++++----- digital/io-hub/src/robospierre/logistic.c | 18 +++++++++++ digital/io-hub/src/robospierre/logistic.h | 4 +++ 3 files changed, 65 insertions(+), 8 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 50fcf041..70834bfe 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -64,6 +64,8 @@ FSM_STATES ( CLAMP_DROPING_DOOR_OPENING, /* Droping a tower, waiting for robot to advance. */ CLAMP_DROPING_WAITING_ROBOT, + /* Clamp locked in a bay. */ + CLAMP_LOCKED, /* Waiting movement order. */ CLAMP_MOVE_IDLE, @@ -354,6 +356,7 @@ FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING) FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, move_element, CLAMP_MOVING_ELEMENT, move_to_idle, CLAMP_GOING_IDLE, + clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { logistic_element_new (ctx.pos_new, ctx.new_element_type); @@ -365,8 +368,14 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, } else if (logistic_global.clamp_pos_idle != ctx.pos_current) { - clamp_move (logistic_global.clamp_pos_idle); - return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle); + if (logistic_path_clear (ctx.pos_current, + logistic_global.clamp_pos_idle)) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle); + } + else + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked); } else return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done); @@ -375,6 +384,7 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, move_element, CLAMP_MOVING_ELEMENT, move_to_idle, CLAMP_GOING_IDLE, + clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { logistic_element_move_done (); @@ -387,9 +397,16 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, } else if (logistic_global.clamp_pos_idle != ctx.pos_current) { - clamp_move (logistic_global.clamp_pos_idle); - return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, - move_to_idle); + if (logistic_path_clear (ctx.pos_current, + logistic_global.clamp_pos_idle)) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + move_to_idle); + } + else + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + clamp_locked); } else return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, @@ -406,6 +423,7 @@ FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME, FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, move_element, CLAMP_MOVING_ELEMENT, move_to_idle, CLAMP_GOING_IDLE, + clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { logistic_drop (ctx.drop_direction); @@ -418,15 +436,32 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, } else if (logistic_global.clamp_pos_idle != ctx.pos_current) { - clamp_move (logistic_global.clamp_pos_idle); - return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, - move_to_idle); + if (logistic_path_clear (ctx.pos_current, + logistic_global.clamp_pos_idle)) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_to_idle); + } + else + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + clamp_locked); } else return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, done); } +FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING) +{ + /* If going forward, drop at back. */ + uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN); + pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN); + return FSM_NEXT (CLAMP_LOCKED, clamp_drop); +} + /* CLAMP_MOVE FSM */ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 8c2a759f..0dbbaa6c 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -221,3 +221,21 @@ logistic_drop (uint8_t direction) logistic_decision (); } +static uint8_t +logistic_slot_clear (uint8_t slot) +{ + if (CLAMP_IS_SLOT_IN_FRONT_BAY (slot) + && ctx.slots[CLAMP_SLOT_FRONT_MIDDLE]) + return 0; + if (CLAMP_IS_SLOT_IN_BACK_BAY (slot) + && ctx.slots[CLAMP_SLOT_BACK_MIDDLE]) + return 0; + return 1; +} + +uint8_t +logistic_path_clear (uint8_t slot1, uint8_t slot2) +{ + return logistic_slot_clear (slot1) && logistic_slot_clear (slot2); +} + diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index fd47bd1e..7a742957 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -70,4 +70,8 @@ logistic_element_move_done (void); void logistic_drop (uint8_t direction); +/** Is path clear between two positions? */ +uint8_t +logistic_path_clear (uint8_t slot1, uint8_t slot2); + #endif /* logistic_h */ -- cgit v1.2.3 From 3cc3e4c14f298fd78d26fb77b1684cd3b88bed39 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 20:52:35 +0200 Subject: digital/io-hub: handle new elements when clamp locked --- digital/io-hub/src/robospierre/clamp.c | 8 ++++++++ digital/io-hub/src/robospierre/logistic.c | 3 --- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 70834bfe..0172d891 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -452,6 +452,14 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, done); } +FSM_TRANS (CLAMP_LOCKED, clamp_new_element, CLAMP_LOCKED) +{ + pwm_set_timed (clamp_slot_door[ctx.pos_new], + BOT_PWM_DOOR_CLOSE (ctx.pos_new)); + logistic_element_new (ctx.pos_new, ctx.new_element_type); + return FSM_NEXT (CLAMP_LOCKED, clamp_new_element); +} + FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING) { /* If going forward, drop at back. */ diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 0dbbaa6c..011b85a3 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -66,9 +66,6 @@ static void logistic_decision (void) { uint8_t i; - /* If currently moving, do not take decision. */ - if (ctx.moving_from != CLAMP_SLOT_NB) - return; /* Determine collect_direction. */ uint8_t front_head = 0, back_head = 0, front_element = 0, back_element = 0; -- cgit v1.2.3 From 976a1f968ddf6b1c635813aeadc76d4536fbe18f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 21:54:59 +0200 Subject: digital/io-hub: add missing comment --- digital/io-hub/src/robospierre/main.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index f1b4d8de..7d962f47 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -233,6 +233,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) clamp_move_element (args[0], args[1]); break; case c ('n', 2): + /* Simulate the presence of a new element. + * - 1b: position. + * - 1b: type. */ clamp_new_element (args[0], args[1]); break; case c ('d', 0): -- cgit v1.2.3 From 4d89714a05aeb17126d206e6003bf922a37966ab Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 22:03:17 +0200 Subject: digital/io-hub: add door/clamp command --- digital/io-hub/src/robospierre/clamp.c | 19 +++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 4 ++++ digital/io-hub/src/robospierre/main.c | 6 ++++++ 3 files changed, 29 insertions(+) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 0172d891..b32549d2 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -221,6 +221,25 @@ clamp_drop_clear (void) FSM_HANDLE (AI, clamp_drop_clear); } +void +clamp_door (uint8_t pos, uint8_t open) +{ + if (pos == 0xff) + { + if (open) + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + else + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + } + else if (clamp_slot_door[pos] != 0xff) + { + if (open) + pwm_set_timed (clamp_slot_door[pos], BOT_PWM_DOOR_OPEN); + else + pwm_set_timed (clamp_slot_door[pos], BOT_PWM_DOOR_CLOSE (pos)); + } +} + uint8_t clamp_handle_event (void) { diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 7471ceac..231c3a84 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -81,6 +81,10 @@ clamp_drop (uint8_t drop_direction); void clamp_drop_clear (void); +/** Open/close door (pos = slot) or clamp (pos = 0xff). */ +void +clamp_door (uint8_t pos, uint8_t open); + /** Examine sensors to generate new events, return non zero if an event was * generated. */ uint8_t diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 7d962f47..4e75a92e 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -253,6 +253,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) else clamp_drop (args[0]); break; + case c ('d', 2): + /* Open or close door or clamp. + * - 1b: pos, or 0xff for clamp. + * - 1b: non zero to open. */ + clamp_door (args[0], args[1]); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): -- cgit v1.2.3 From e2ea2a7d57b9a204c08488118aa054b56a1e43fe Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 May 2011 22:29:00 +0200 Subject: digital/avr/modules/devices/usdist: new US sensor module --- digital/avr/modules/devices/usdist/Makefile.module | 1 + digital/avr/modules/devices/usdist/README | 23 ++++ digital/avr/modules/devices/usdist/avrconfig.h | 40 +++++++ digital/avr/modules/devices/usdist/usdist.c | 122 ++++++++++++++++++++ digital/avr/modules/devices/usdist/usdist.h | 58 ++++++++++ digital/io/src/Makefile | 3 +- digital/io/src/avrconfig.h | 12 ++ digital/io/src/main.c | 2 +- digital/io/src/radar.c | 2 +- digital/io/src/usdist.c | 125 --------------------- digital/io/src/usdist.h | 59 ---------- 11 files changed, 260 insertions(+), 187 deletions(-) create mode 100644 digital/avr/modules/devices/usdist/Makefile.module create mode 100644 digital/avr/modules/devices/usdist/README create mode 100644 digital/avr/modules/devices/usdist/avrconfig.h create mode 100644 digital/avr/modules/devices/usdist/usdist.c create mode 100644 digital/avr/modules/devices/usdist/usdist.h delete mode 100644 digital/io/src/usdist.c delete mode 100644 digital/io/src/usdist.h diff --git a/digital/avr/modules/devices/usdist/Makefile.module b/digital/avr/modules/devices/usdist/Makefile.module new file mode 100644 index 00000000..61691300 --- /dev/null +++ b/digital/avr/modules/devices/usdist/Makefile.module @@ -0,0 +1 @@ +devices_usdist_SOURCES = usdist.c diff --git a/digital/avr/modules/devices/usdist/README b/digital/avr/modules/devices/usdist/README new file mode 100644 index 00000000..5ddb8aa6 --- /dev/null +++ b/digital/avr/modules/devices/usdist/README @@ -0,0 +1,23 @@ +usdist - Analog US distance sensor support. + +Used with Sensopart UT20-700. + +Copyright (C) 2011 Nicolas Schodet + +APBTeam: + Web: http://apbteam.org/ + Email: team AT apbteam DOT 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 +(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. diff --git a/digital/avr/modules/devices/usdist/avrconfig.h b/digital/avr/modules/devices/usdist/avrconfig.h new file mode 100644 index 00000000..2a908794 --- /dev/null +++ b/digital/avr/modules/devices/usdist/avrconfig.h @@ -0,0 +1,40 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - TWI module configuration template. */ +/* usdist - Analog US distance sensor support. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/* usdist - Analog US distance sensor. */ +/** Number of sensors. */ +#define AC_USDIST_NB 4 +/** Measuring period, in number of update call. */ +#define AC_USDIST_PERIOD 2 +/** List of space separated sensor definition, see usdist.h. */ +#define AC_USDIST_SENSORS \ + USDIST_SENSOR (0, G, 3) \ + USDIST_SENSOR (1, G, 1) \ + USDIST_SENSOR (2, C, 7) \ + USDIST_SENSOR (3, D, 4) + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/devices/usdist/usdist.c b/digital/avr/modules/devices/usdist/usdist.c new file mode 100644 index 00000000..75c7899b --- /dev/null +++ b/digital/avr/modules/devices/usdist/usdist.c @@ -0,0 +1,122 @@ +/* usdist.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "usdist.h" + +#include "modules/adc/adc.h" +#include "modules/utils/utils.h" +#include "io.h" + +/* This include is to be defined by user program. */ +#include "simu.host.h" + +uint16_t usdist_mm[AC_USDIST_NB]; + +/** Describe a sensor. */ +struct usdist_sensor_t +{ + /** ADC number. */ + uint8_t adc; + /** Sync PORT. */ + volatile uint8_t *sync_port; + /** Sync DDR. */ + volatile uint8_t *sync_ddr; + /** Sync bit mask. */ + uint8_t sync_bv; +}; + +/** Define sensors configuration. */ +#define USDIST_SENSOR(adc, p, n) \ + { adc, &IO_PORT_ (p, n), &IO_DDR_ (p, n), IO_BV_ (p, n) }, +struct usdist_sensor_t usdist_sensors[AC_USDIST_NB] = { + AC_USDIST_SENSORS +}; + +void +usdist_init (void) +{ + uint8_t i; + adc_init (); + for (i = 0; i < AC_USDIST_NB; i++) + { + usdist_mm[i] = 0xffff; + *usdist_sensors[i].sync_port &= ~usdist_sensors[i].sync_bv; + *usdist_sensors[i].sync_ddr |= usdist_sensors[i].sync_bv; + } +} + +uint8_t +usdist_update (void) +{ + /* Current measuring sensor. */ + static uint8_t current; + /* Number of cycles until the measure ends. */ + static uint8_t wait; + /* Set to one once first cycle is done. */ + static uint8_t init; + /* Time to measure? */ + if (wait == 0) + { + if (init) + { + /* Stop sensor. */ + *usdist_sensors[current].sync_port &= + ~usdist_sensors[current].sync_bv; + /* Read ADC value. */ + adc_start (usdist_sensors[current].adc); + while (!adc_checkf ()) + ; + uint16_t v = adc_read (); + /* Our sensors return a value between 1 and 5 V proportional to + * the distance between calibrated values. Ignore faulty sensors. */ + if (v <= 1024 / 5 / 4) + usdist_mm[current] = 0xffff; + else if (v <= 1024 / 5) + usdist_mm[current] = USDIST_MM_MIN; + else + usdist_mm[current] = USDIST_MM_MIN + + ((uint32_t) (v - 1024 / 5) + * (USDIST_MM_MAX - USDIST_MM_MIN) + / (4 * 1024 / 5)); + if (usdist_mm[current] >= USDIST_MM_TOO_FAR) + usdist_mm[current] = 0xffff; + /* Next. */ + current = (current + 1) % AC_USDIST_NB; + } + init = 1; + /* Prepare next measure. */ + *usdist_sensors[current].sync_port |= + usdist_sensors[current].sync_bv; + wait = AC_USDIST_PERIOD; + /* New mesure done. */ + return 1; + } + else + { + wait--; + return 0; + } +} + diff --git a/digital/avr/modules/devices/usdist/usdist.h b/digital/avr/modules/devices/usdist/usdist.h new file mode 100644 index 00000000..37683091 --- /dev/null +++ b/digital/avr/modules/devices/usdist/usdist.h @@ -0,0 +1,58 @@ +#ifndef usdist_h +#define usdist_h +/* usdist.h */ +/* usdist - Analog US distance sensor support. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/* + * Manage ultrasonic distance sensors using analog to digital converter. + * + * AC_USDIST_SENSORS should be set to a list of space separated USDIST_SENSOR + * macro calls: + * + * USDIST_SENSOR (adc_number, synchro_io_port, synchro_io_n) + */ + +/** Minimal calibrated distance. */ +#define USDIST_MM_MIN 100 + +/** Maximal calibrated distance. */ +#define USDIST_MM_MAX 700 + +/** Distance considered as too far to be true. */ +#define USDIST_MM_TOO_FAR 650 + +/** Array containing the last measures in millimeters. */ +extern uint16_t usdist_mm[AC_USDIST_NB]; + +/** Initialise module. */ +void +usdist_init (void); + +/** To be called every cycle to update sensor measures. + * - returns: non zero if sensor value has been updated. */ +uint8_t +usdist_update (void); + +#endif /* usdist_h */ diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 5cbea0ba..effe9c45 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -6,11 +6,12 @@ PROGS = io io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \ switch.avr.c chrono.c timer.avr.c servo_pos.c \ twi_master.c asserv.c mimot.c \ - simu.host.c contact.c usdist.c radar.c \ + simu.host.c contact.c radar.c \ path.c food.c events.host.c \ fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \ + devices/usdist \ trace flash spi AI_MODULES = twi_master common utils fsm # Configuration file. diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index dd3f2e87..a46b4abe 100644 --- a/digital/io/src/avrconfig.h +++ b/digital/io/src/avrconfig.h @@ -117,6 +117,18 @@ /** Heuristic callback. */ #define AC_ASTAR_HEURISTIC_CALLBACK path_astar_heuristic_callback +/* usdist - Analog US distance sensor. */ +/** Number of sensors. */ +#define AC_USDIST_NB 4 +/** Measuring period, in number of update call. */ +#define AC_USDIST_PERIOD 1 +/** List of space separated sensor definition, see usdist.h. */ +#define AC_USDIST_SENSORS \ + USDIST_SENSOR (0, G, 3) \ + USDIST_SENSOR (1, G, 1) \ + USDIST_SENSOR (2, C, 7) \ + USDIST_SENSOR (3, D, 4) + /* io - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 2 diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 22964ddd..016aad36 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -28,6 +28,7 @@ #include "modules/proto/proto.h" #include "modules/utils/utils.h" #include "modules/path/path.h" +#include "modules/devices/usdist/usdist.h" #include "modules/flash/flash.h" #include "modules/trace/trace.h" #include "events.h" @@ -51,7 +52,6 @@ #include "fsm_queue.h" #include "bot.h" #include "servo_pos.h" -#include "usdist.h" #include "radar.h" #include "chrono.h" /* chrono_end_match */ #include "pwm.h" diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c index 1ac982c7..0f61a1bf 100644 --- a/digital/io/src/radar.c +++ b/digital/io/src/radar.c @@ -27,8 +27,8 @@ #include "playground_2010.h" #include "bot.h" -#include "usdist.h" +#include "modules/devices/usdist/usdist.h" #include "modules/math/geometry/geometry.h" #include "modules/math/geometry/distance.h" #include "modules/utils/utils.h" diff --git a/digital/io/src/usdist.c b/digital/io/src/usdist.c deleted file mode 100644 index d6fd9730..00000000 --- a/digital/io/src/usdist.c +++ /dev/null @@ -1,125 +0,0 @@ -/* usdist.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT 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 - * (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. - * - * }}} */ -#include "common.h" -#include "usdist.h" -#include "timer.h" - -#include "modules/adc/adc.h" -#include "modules/utils/utils.h" -#include "io.h" - -#include "simu.host.h" - -uint16_t usdist_mm[USDIST_NB]; - -/** Describe a sensor. */ -struct usdist_sensor_t -{ - /** ADC number. */ - uint8_t adc; - /** Sync PORT. */ - volatile uint8_t *sync_port; - /** Sync DDR. */ - volatile uint8_t *sync_ddr; - /** Sync bit mask. */ - uint8_t sync_bv; -}; - -/** Define sensors configuration. */ -#define USDIST_SENSOR(adc, p, n) \ - { adc, &IO_PORT_ (p, n), &IO_DDR_ (p, n), IO_BV_ (p, n) } -struct usdist_sensor_t usdist_sensors[USDIST_NB] = { - USDIST_SENSOR (0, G, 3), - USDIST_SENSOR (1, G, 1), - USDIST_SENSOR (2, C, 7), - USDIST_SENSOR (3, D, 4), -}; - -void -usdist_init (void) -{ - uint8_t i; - adc_init (); - for (i = 0; i < USDIST_NB; i++) - { - usdist_mm[i] = 0xffff; - *usdist_sensors[i].sync_port &= ~usdist_sensors[i].sync_bv; - *usdist_sensors[i].sync_ddr |= usdist_sensors[i].sync_bv; - } -} - -uint8_t -usdist_update (void) -{ - /* Current measuring sensor. */ - static uint8_t current; - /* Number of cycles until the measure ends. */ - static uint8_t wait; - /* Set to one once first cycle is done. */ - static uint8_t init; - /* Time to measure? */ - if (wait == 0) - { - if (init) - { - /* Stop sensor. */ - *usdist_sensors[current].sync_port &= - ~usdist_sensors[current].sync_bv; - /* Read ADC value. */ - adc_start (usdist_sensors[current].adc); - while (!adc_checkf ()) - ; - uint16_t v = adc_read (); - /* Our sensors return a value between 1 and 5 V proportional to - * the distance between calibrated values. Ignore faulty sensors. */ - if (v <= 1024 / 5 / 4) - usdist_mm[current] = 0xffff; - else if (v <= 1024 / 5) - usdist_mm[current] = USDIST_MM_MIN; - else - usdist_mm[current] = USDIST_MM_MIN - + ((uint32_t) (v - 1024 / 5) - * (USDIST_MM_MAX - USDIST_MM_MIN) - / (4 * 1024 / 5)); - if (usdist_mm[current] >= USDIST_MM_TOO_FAR) - usdist_mm[current] = 0xffff; - /* Next. */ - current = (current + 1) % USDIST_NB; - } - init = 1; - /* Prepare next measure. */ - *usdist_sensors[current].sync_port |= - usdist_sensors[current].sync_bv; - wait = USDIST_PERIOD_CYCLE; - /* New mesure done. */ - return 1; - } - else - { - wait--; - return 0; - } -} - diff --git a/digital/io/src/usdist.h b/digital/io/src/usdist.h deleted file mode 100644 index 9a4a6646..00000000 --- a/digital/io/src/usdist.h +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef usdist_h -#define usdist_h -/* usdist.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT 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 - * (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. - * - * }}} */ - -/* - * Manage ultrasonic distance sensors using analog to digital converter. - */ - -/** Number of sensors. */ -#define USDIST_NB 4 - -/** Minimal calibrated distance. */ -#define USDIST_MM_MIN 100 - -/** Maximal calibrated distance. */ -#define USDIST_MM_MAX 700 - -/** Distance considered as too far to be true. */ -#define USDIST_MM_TOO_FAR 650 - -/** Measuring period in cycles. */ -#define USDIST_PERIOD_CYCLE (uint8_t) (8.0 / TIMER_PERIOD_MS) - -/** Array containing the last measures in millimeters. */ -extern uint16_t usdist_mm[USDIST_NB]; - -/** Initialise module. */ -void -usdist_init (void); - -/** To be called every cycle to update sensor measures. - * - returns: non zero if sensor value has been updated. */ -uint8_t -usdist_update (void); - -#endif /* usdist_h */ -- cgit v1.2.3 From c5b1353dea8a426d6eea043fb5aa78722f7b0bc5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 00:13:14 +0200 Subject: digital/io-hub: add US distance sensors --- digital/io-hub/src/robospierre/Makefile | 1 + digital/io-hub/src/robospierre/avrconfig.h | 12 ++++++++++++ digital/io-hub/src/robospierre/main.c | 20 ++++++++++++++++++++ digital/io-hub/src/robospierre/simu.host.c | 14 +++++++++++++- digital/io-hub/src/robospierre/simu.host.h | 2 +- digital/io-hub/tools/io_hub/mex.py | 20 ++++++++++++++++---- host/simu/model/distance_sensor_sensopart.py | 2 +- host/simu/robots/robospierre/model/bag.py | 2 -- 8 files changed, 64 insertions(+), 9 deletions(-) diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index d69918f5..bd8e920f 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -12,6 +12,7 @@ io_hub_SOURCES = main.c \ chrono.c timer.avr.c simu.host.c # Modules needed for IO. MODULES = proto uart twi utils \ + adc devices/usdist \ math/fixed math/geometry AI_MODULES = twi_master common utils fsm # Configuration file. diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h index 0fc2f11d..53e36ccf 100644 --- a/digital/io-hub/src/robospierre/avrconfig.h +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -100,6 +100,18 @@ /** Use internal pull up. */ #define AC_TWI_PULL_UP 0 +/* usdist - Analog US distance sensor. */ +/** Number of sensors. */ +#define AC_USDIST_NB 4 +/** Measuring period, in number of update call. */ +#define AC_USDIST_PERIOD 1 +/** List of space separated sensor definition, see usdist.h. */ +#define AC_USDIST_SENSORS \ + USDIST_SENSOR (0, A, 0) \ + USDIST_SENSOR (1, A, 1) \ + USDIST_SENSOR (2, A, 2) \ + USDIST_SENSOR (3, A, 3) + /* io-hub - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 10 diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 4e75a92e..8d51c469 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -28,6 +28,8 @@ #include "modules/uart/uart.h" #include "modules/proto/proto.h" +#include "modules/devices/usdist/usdist.h" + #include "timer.h" #include "chrono.h" #include "simu.host.h" @@ -66,6 +68,9 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; /** Contact stats counters. */ static uint8_t main_stats_contact_, main_stats_contact_cpt_; +/** US sensors stats counters. */ +static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; + /** Main initialisation. */ static void main_init (void) @@ -89,6 +94,7 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + usdist_init (); /* AI modules. */ logistic_init (); /* Initialization done. */ @@ -170,6 +176,10 @@ main_loop (void) /* Update IO modules. */ pwm_update (); contact_update (); + if (usdist_update ()) + { + /* TODO: update radar. */ + } /* Update AI modules. */ logistic_update (); /* Only manage events if slaves are synchronised. */ @@ -191,6 +201,12 @@ main_loop (void) proto_send1d ('P', contact_all ()); main_stats_contact_cpt_ = main_stats_contact_; } + if (main_stats_usdist_ && !--main_stats_usdist_cpt_) + { + proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2], + usdist_mm[3]); + main_stats_usdist_cpt_ = main_stats_usdist_; + } } } @@ -269,6 +285,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Contact stats. */ main_stats_contact_ = main_stats_contact_cpt_ = args[0]; break; + case c ('U', 1): + /* US sensors stats. */ + main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; + break; default: /* Unknown commands */ proto_send0 ('?'); diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 281a60d6..f7a43eea 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -28,10 +28,20 @@ #include "modules/utils/utils.h" #include "modules/host/host.h" #include "modules/host/mex.h" +#include "modules/adc/adc.h" #include "io.h" /** AVR registers. */ -uint8_t PINA, PINE, PINF; +uint8_t PORTA, DDRA, PINA, PINE, PINF; + +static void +simu_adc_handle (void *user, mex_msg_t *msg) +{ + uint8_t index; + uint16_t value; + mex_msg_pop (msg, "BH", &index, &value); + adc_values[index] = value; +} /** Initialise simulation. */ void @@ -40,6 +50,8 @@ simu_init (void) const char *mex_instance; mex_node_connect (); mex_instance = host_get_instance ("io-hub0", 0); + uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance); + mex_node_register (mtype, simu_adc_handle, 0); } /** Make a simulation step. */ diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index ed6a002d..6a636851 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -27,7 +27,7 @@ #ifdef HOST -extern uint8_t PINA, PINE, PINF; +extern uint8_t PORTA, DDRA, PINA, PINE, PINF; #else /* !defined (HOST) */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index d9568197..22e9e900 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -44,9 +44,21 @@ class Mex: """ - def __init__ (self): + def __init__ (self, node, instance, index): Observable.__init__ (self) - self.value = None + self.value = 0 + self.__node = node + self.__mtype = node.reserve (instance + ':adc') + self.__index = index + self.register (self.__notified) + + def __notified (self): + m = simu.mex.msg.Msg (self.__mtype) + v = int (1024 * self.value / 5) + v = min (v, 1023) + v = max (0, v) + m.push ('BH', self.__index, v) + self.__node.send (m) class PWM (Observable): """PWM output. @@ -110,9 +122,9 @@ class Mex: self.node.send (m) def __init__ (self, node, instance = 'io-hub0'): - self.adc = tuple (self.ADC () for i in range (0, ADC_NB)) + self.adc = tuple (self.ADC (node, instance, i) for i in range (0, ADC_NB)) self.pwm = tuple (self.PWM () for i in range (0, PWM_NB)) - self.__adc_pwm = self.PWM.Pack (node, instance, self.pwm) + self.__pwm_pack = self.PWM.Pack (node, instance, self.pwm) self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) diff --git a/host/simu/model/distance_sensor_sensopart.py b/host/simu/model/distance_sensor_sensopart.py index 16676e5d..26ed604f 100644 --- a/host/simu/model/distance_sensor_sensopart.py +++ b/host/simu/model/distance_sensor_sensopart.py @@ -59,8 +59,8 @@ class DistanceSensorSensopart (Observable): self.link = link self.scheduler = scheduler self.value = None - self.register (self.__update) self.evaluate () + self.register (self.__update) def evaluate (self): # Compute real distance. diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index b1c49d35..de8eaa22 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -54,6 +54,4 @@ class Bag: DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] - for adc in link_bag.io_hub.adc[4:]: - adc.value = 0 -- cgit v1.2.3 From 6548d83aa655176ef939817e36cd05d0e6cfaccd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 01:23:43 +0200 Subject: digital/{io,io-hub,ai}: add generic radar, add radar to robospierre --- digital/ai/src/move/radar.c | 168 +++++++++++++++++++++++++ digital/ai/src/move/radar.h | 96 ++++++++++++++ digital/io-hub/src/robospierre/Makefile | 3 +- digital/io-hub/src/robospierre/bot.h | 3 + digital/io-hub/src/robospierre/main.c | 13 +- digital/io-hub/src/robospierre/radar_defs.c | 46 +++++++ digital/io-hub/src/robospierre/radar_defs.h | 41 ++++++ digital/io-hub/src/robospierre/simu.host.c | 15 +++ digital/io-hub/src/robospierre/simu.host.h | 10 ++ digital/io-hub/tools/io_hub/mex.py | 22 ++++ digital/io/src/Makefile | 4 +- digital/io/src/radar.c | 189 ---------------------------- digital/io/src/radar.h | 78 ------------ digital/io/src/radar_defs.c | 54 ++++++++ digital/io/src/radar_defs.h | 41 ++++++ host/simu/robots/robospierre/model/bag.py | 1 + host/simu/robots/robospierre/view/bag.py | 1 + 17 files changed, 514 insertions(+), 271 deletions(-) create mode 100644 digital/ai/src/move/radar.c create mode 100644 digital/ai/src/move/radar.h create mode 100644 digital/io-hub/src/robospierre/radar_defs.c create mode 100644 digital/io-hub/src/robospierre/radar_defs.h delete mode 100644 digital/io/src/radar.c delete mode 100644 digital/io/src/radar.h create mode 100644 digital/io/src/radar_defs.c create mode 100644 digital/io/src/radar_defs.h diff --git a/digital/ai/src/move/radar.c b/digital/ai/src/move/radar.c new file mode 100644 index 00000000..a572afba --- /dev/null +++ b/digital/ai/src/move/radar.c @@ -0,0 +1,168 @@ +/* radar.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "radar.h" + +#include "bot.h" + +#include "modules/math/geometry/geometry.h" +#include "modules/math/geometry/distance.h" +#include "modules/utils/utils.h" + +/** Maximum distance for a sensor reading to be ignored if another sensor is + * nearer. */ +#define RADAR_FAR_MM 250 + +/** Define radar configuration. */ +extern struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB]; + +uint8_t +radar_valid (vect_t p); + +/** Compute the center position from several radars sensors, return 1 if + * any. */ +static uint8_t +radar_hit_center (uint8_t valid[], vect_t hit[], uint8_t sensor_nb, + vect_t *obs_pos) +{ + uint8_t i, hit_nb = 0; + vect_t hit_center = { 0, 0 }; + for (i = 0; i < sensor_nb; i++) + { + if (valid[i]) + { + vect_add (&hit_center, &hit[i]); + hit_nb++; + } + } + if (hit_nb > 1) + vect_scale_f824 (&hit_center, 0x1000000l / hit_nb); + if (hit_nb) + { + *obs_pos = hit_center; + return 1; + } + else + return 0; +} + +uint8_t +radar_update (const position_t *robot_pos, vect_t *obs_pos) +{ + uint8_t i, j; + vect_t ray; + uint8_t obs_nb = 0; + /* Compute hit points for each sensor and eliminate invalid ones. */ + vect_t hit[UTILS_COUNT (radar_sensors)]; + uint8_t valid[UTILS_COUNT (radar_sensors)]; + uint16_t dist_mm[UTILS_COUNT (radar_sensors)]; + for (i = 0; i < UTILS_COUNT (radar_sensors); i++) + { + dist_mm[i] = *radar_sensors[i].dist_mm; + if (dist_mm[i] != 0xffff) + { + hit[i] = radar_sensors[i].pos; + vect_rotate_uf016 (&hit[i], robot_pos->a); + vect_translate (&hit[i], &robot_pos->v); + vect_from_polar_uf016 (&ray, dist_mm[i], + robot_pos->a + radar_sensors[i].a); + vect_translate (&hit[i], &ray); + valid[i] = radar_valid (hit[i]); + vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_EDGE_RADIUS_MM, + robot_pos->a + radar_sensors[i].a); + vect_translate (&hit[i], &ray); + } + else + valid[i] = 0; + } + /* Ignore sensor results too far from other sensors. */ + for (i = 0; i < UTILS_COUNT (radar_sensors) - 1; i++) + { + for (j = i + 1; valid[i] && j < UTILS_COUNT (radar_sensors); j++) + { + if (valid[j]) + { + if (dist_mm[i] + RADAR_FAR_MM < dist_mm[j]) + valid[j] = 0; + else if (dist_mm[j] + RADAR_FAR_MM < dist_mm[i]) + valid[i] = 0; + } + } + } + /* Specific treatment about sensor topology. */ + obs_nb += radar_hit_center (valid + RADAR_SENSOR_FRONT_FIRST, + hit + RADAR_SENSOR_FRONT_FIRST, + RADAR_SENSOR_FRONT_NB, &obs_pos[obs_nb]); + obs_nb += radar_hit_center (valid + RADAR_SENSOR_BACK_FIRST, + hit + RADAR_SENSOR_BACK_FIRST, + RADAR_SENSOR_BACK_NB, &obs_pos[obs_nb]); + /* Done. */ + return obs_nb; +} + +uint8_t +radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, + const vect_t *obs_pos, uint8_t obs_nb) +{ + uint8_t i; + /* Stop here if no obstacle. */ + if (!obs_nb) + return 0; + vect_t vd = *dest_pos; vect_sub (&vd, robot_pos); + uint16_t d = vect_norm (&vd); + /* If destination is realy near, stop here. */ + if (d < RADAR_EPSILON_MM) + return 0; + /* If destination is near, use clearance to destination point instead of + * stop length. */ + vect_t t; + if (d < RADAR_STOP_MM) + t = *dest_pos; + else + { + vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM); + t = *robot_pos; + vect_translate (&t, &vd); + } + /* Now, look at obstacles. */ + for (i = 0; i < obs_nb; i++) + { + /* Vector from robot to obstacle. */ + vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos); + /* Ignore if in our back. */ + int32_t dp = vect_dot_product (&vd, &vo); + if (dp < 0) + continue; + /* Check distance. */ + int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]); + if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2 + + RADAR_OBSTACLE_RADIUS_MM) + continue; + /* Else, obstacle is blocking. */ + return 1; + } + return 0; +} + diff --git a/digital/ai/src/move/radar.h b/digital/ai/src/move/radar.h new file mode 100644 index 00000000..2e37d11d --- /dev/null +++ b/digital/ai/src/move/radar.h @@ -0,0 +1,96 @@ +#ifndef radar_h +#define radar_h +/* radar.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "defs.h" + +/** + * Handle any distance sensors information to extract useful data. This + * includes: + * - combining several sensors information for a more precise obstacle + * position, + * - ignoring obstacles not in the playground, + * - determining if an obstacle should make the robot stop. + */ + +/** Estimated obstacle edge radius. As the sensors detect obstacle edge, this is + * added to position obstacle center. */ +#undef RADAR_OBSTACLE_EDGE_RADIUS_MM + +/** Estimated obstacle radius. The obstacle may be larger than at the + * detected edge. */ +#undef RADAR_OBSTACLE_RADIUS_MM + +/** Stop distance. Distance under which an obstacle is considered harmful when + * moving. */ +#undef RADAR_STOP_MM + +/** Clearance distance. Distance over which an obstacle should be to the side + * when moving. + * + * OK, more explanations: when moving, a rectangle is placed in front of the + * robot, of length RADAR_STOP_MM and width 2 * (RADAR_CLEARANCE_MM + + * BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is considered + * in the way. + * + * If the destination point is near (< RADAR_STOP_MM - RADAR_CLEARANCE_MM), + * this reduce the rectangle length. + * + * If the destination is really near (< RADAR_EPSILON_MM), ignore all this. */ +#undef RADAR_CLEARANCE_MM + +/** Destination distance near enough so that obstacles could be ignored. */ +#undef RADAR_EPSILON_MM + +/* All this in another file. */ +#include "radar_defs.h" + +/** Margin to be considered inside the playground. An obstacle can not be + * exactly at the playground edge. */ +#define RADAR_MARGIN_MM 150 + +/** Describe a radar sensor. */ +struct radar_sensor_t +{ + /** Distance updated by another module. */ + uint16_t *dist_mm; + /** Position relative to the robot center. */ + vect_t pos; + /** Angle relative to the robot X axis. */ + uint16_t a; +}; + +/** Update radar view. Return the number of obstacles found. Obstacles + * positions are returned in obs_pos. */ +uint8_t +radar_update (const position_t *robot_pos, vect_t *obs_pos); + +/** Return non zero if there is a blocking obstacle near the robot while going + * to a destination point. */ +uint8_t +radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, + const vect_t *obs_pos, uint8_t obs_nb); + +#endif /* radar_h */ diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index bd8e920f..fe2b318e 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,6 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ + radar_defs.c radar.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ @@ -14,7 +15,7 @@ io_hub_SOURCES = main.c \ MODULES = proto uart twi utils \ adc devices/usdist \ math/fixed math/geometry -AI_MODULES = twi_master common utils fsm +AI_MODULES = twi_master common utils fsm move # Configuration file. CONFIGFILE = avrconfig.h AVR_MCU = at90usb1287 diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 5f9d177a..4df9b89d 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,6 +34,9 @@ # define BOT_SCALE 0.0415178942124 #endif +/** Distance from the robot axis to the side. */ +#define BOT_SIZE_SIDE 190 + /** Distance between the front contact point and the robot center. */ #define BOT_FRONT_CONTACT_DIST_MM 150 /** Angle error at the front contact point. */ diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 8d51c469..6d32c28f 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -40,6 +40,7 @@ #include "pwm.h" #include "contact.h" +#include "radar.h" #define FSM_NAME AI #include "fsm.h" @@ -62,6 +63,12 @@ /** Our color. */ enum team_color_e team_color; +/** Obstacles positions, updated using radar module. */ +vect_t main_obstacles_pos[2]; + +/** Number of obstacles in main_obstacles_pos. */ +uint8_t main_obstacles_nb; + /** Asserv stats counters. */ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; @@ -178,7 +185,11 @@ main_loop (void) contact_update (); if (usdist_update ()) { - /* TODO: update radar. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos); + //move_obstacles_update (); + simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); } /* Update AI modules. */ logistic_update (); diff --git a/digital/io-hub/src/robospierre/radar_defs.c b/digital/io-hub/src/robospierre/radar_defs.c new file mode 100644 index 00000000..85f015e1 --- /dev/null +++ b/digital/io-hub/src/robospierre/radar_defs.c @@ -0,0 +1,46 @@ +/* radar_defs.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "radar.h" + +#include "modules/devices/usdist/usdist.h" +#include "playground.h" + +/** Define radar configuration. */ +struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = { + { &usdist_mm[0], { 20, 20 }, G_ANGLE_UF016_DEG (10) }, + { &usdist_mm[1], { 20, -20 }, G_ANGLE_UF016_DEG (-10) }, + { &usdist_mm[2], { -20, -20 }, G_ANGLE_UF016_DEG (180 + 10) }, + { &usdist_mm[3], { -20, 20 }, G_ANGLE_UF016_DEG (180 - 10) }, +}; + +/** Define exclusion area (considered as invalid point). */ +uint8_t +radar_valid (vect_t p) +{ + return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM + && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM; +} + diff --git a/digital/io-hub/src/robospierre/radar_defs.h b/digital/io-hub/src/robospierre/radar_defs.h new file mode 100644 index 00000000..5f6a2a73 --- /dev/null +++ b/digital/io-hub/src/robospierre/radar_defs.h @@ -0,0 +1,41 @@ +#ifndef radar_defs_h +#define radar_defs_h +/* radar_defs.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 +#define RADAR_OBSTACLE_RADIUS_MM 150 +#define RADAR_STOP_MM 350 +#define RADAR_CLEARANCE_MM 100 +#define RADAR_EPSILON_MM 70 + +#define RADAR_SENSOR_NB 4 + +#define RADAR_SENSOR_FRONT_FIRST 0 +#define RADAR_SENSOR_FRONT_NB 2 +#define RADAR_SENSOR_BACK_FIRST 2 +#define RADAR_SENSOR_BACK_NB 2 + +#endif /* radar_defs_h */ diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index f7a43eea..7ca7364b 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -34,6 +34,9 @@ /** AVR registers. */ uint8_t PORTA, DDRA, PINA, PINE, PINF; +/** Message types. */ +uint8_t simu_mex_pos_report; + static void simu_adc_handle (void *user, mex_msg_t *msg) { @@ -52,6 +55,7 @@ simu_init (void) mex_instance = host_get_instance ("io-hub0", 0); uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance); mex_node_register (mtype, simu_adc_handle, 0); + simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance); } /** Make a simulation step. */ @@ -74,3 +78,14 @@ timer_wait (void) return 0; } +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id) +{ + mex_msg_t *m; + m = mex_msg_new (simu_mex_pos_report); + mex_msg_push (m, "b", id); + for (; pos_nb; pos++, pos_nb--) + mex_msg_push (m, "hh", pos->x, pos->y); + mex_node_send (m); +} + diff --git a/digital/io-hub/src/robospierre/simu.host.h b/digital/io-hub/src/robospierre/simu.host.h index 6a636851..6224e345 100644 --- a/digital/io-hub/src/robospierre/simu.host.h +++ b/digital/io-hub/src/robospierre/simu.host.h @@ -24,13 +24,23 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "defs.h" #ifdef HOST extern uint8_t PORTA, DDRA, PINA, PINE, PINF; +/** Send general purpose positions to indicate computation results. + * - pos: array of positions to report. + * - pos_nb: number of elements in the array. + * - id: identifier so that several unrelated positions could be reported. */ +void +simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id); + #else /* !defined (HOST) */ +#define simu_send_pos_report(pos, pos_nb, id) ((void) 0) + #endif /* !defined (HOST) */ #endif /* simu_host_h */ diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 22e9e900..44c41010 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,27 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class PosReport (Observable): + """General purpose position report. + + - pos: dict of sequence of (x, y) coordinates (millimeters). The dict + is indexed by position identifier. + + """ + + def __init__ (self, node, instance): + Observable.__init__ (self) + self.pos = { } + node.register (instance + ':pos-report', self.__handle) + + def __handle (self, msg): + p = [ ] + id, = msg.pop ('b') + while len (msg) >= 4: + p.append (msg.pop ('hh')) + self.pos[id] = p + self.notify () + def __init__ (self, node, instance = 'io-hub0'): self.adc = tuple (self.ADC (node, instance, i) for i in range (0, ADC_NB)) self.pwm = tuple (self.PWM () for i in range (0, PWM_NB)) @@ -128,4 +149,5 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.pos_report = self.PosReport (node, instance) diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index effe9c45..463e148e 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -6,14 +6,14 @@ PROGS = io io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \ switch.avr.c chrono.c timer.avr.c servo_pos.c \ twi_master.c asserv.c mimot.c \ - simu.host.c contact.c radar.c \ + simu.host.c contact.c radar.c radar_defs.c \ path.c food.c events.host.c \ fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \ devices/usdist \ trace flash spi -AI_MODULES = twi_master common utils fsm +AI_MODULES = twi_master common utils fsm move # Configuration file. CONFIGFILE = avrconfig.h # IO board use an ATMega128. diff --git a/digital/io/src/radar.c b/digital/io/src/radar.c deleted file mode 100644 index 0f61a1bf..00000000 --- a/digital/io/src/radar.c +++ /dev/null @@ -1,189 +0,0 @@ -/* radar.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT 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 - * (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. - * - * }}} */ -#include "common.h" -#include "radar.h" - -#include "playground_2010.h" -#include "bot.h" - -#include "modules/devices/usdist/usdist.h" -#include "modules/math/geometry/geometry.h" -#include "modules/math/geometry/distance.h" -#include "modules/utils/utils.h" - -/** Margin to be considered inside the playground. An obstacle can not be - * exactly at the playground edge. */ -#define RADAR_MARGIN_MM 150 - -/** Maximum distance for a sensor reading to be ignored if another sensor is - * nearer. */ -#define RADAR_FAR_MM 250 - -/** Describe a radar sensor. */ -struct radar_sensor_t -{ - /** Distance updated by another module. */ - uint16_t *dist_mm; - /** Position relative to the robot center. */ - vect_t pos; - /** Angle relative to the robot X axis. */ - uint16_t a; -}; - -/** Define radar configuration. */ -struct radar_sensor_t radar_sensors[] = { -#define RADAR_SENSOR_FRONT 0 - { &usdist_mm[0], { 30 - 20, 0 }, G_ANGLE_UF016_DEG (0) }, -#define RADAR_SENSOR_LEFT 1 - { &usdist_mm[1], { 20 - 20, 20 }, G_ANGLE_UF016_DEG (30) }, -#define RADAR_SENSOR_RIGHT 2 - { &usdist_mm[2], { 20 - 20, -20 }, G_ANGLE_UF016_DEG (-30) }, -#define RADAR_SENSOR_BACK 3 - { &usdist_mm[3], { -30 - 20, 0 }, G_ANGLE_UF016_DEG (180) }, -}; - -/** Define exclusion area (considered as invalid point). */ -static uint8_t -radar_valid (vect_t p) -{ - return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM - && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM - /* Ignore points on slope, no margin for the slope start. */ - && (p.x < PG_WIDTH / 2 - PG_SLOPE_WIDTH / 2 - || p.x >= PG_WIDTH / 2 + PG_SLOPE_WIDTH / 2 - || p.y < PG_LENGTH - PG_SLOPE_LENGTH - RADAR_MARGIN_MM / 2); -} - -uint8_t -radar_update (const position_t *robot_pos, vect_t *obs_pos) -{ - uint8_t i, j; - vect_t ray; - uint8_t obs_nb = 0; - uint8_t front_nb; - vect_t front_center; - /* Compute hit points for each sensor and eliminate invalid ones. */ - vect_t hit[UTILS_COUNT (radar_sensors)]; - uint8_t valid[UTILS_COUNT (radar_sensors)]; - uint16_t dist_mm[UTILS_COUNT (radar_sensors)]; - for (i = 0; i < UTILS_COUNT (radar_sensors); i++) - { - dist_mm[i] = *radar_sensors[i].dist_mm; - if (dist_mm[i] != 0xffff) - { - hit[i] = radar_sensors[i].pos; - vect_rotate_uf016 (&hit[i], robot_pos->a); - vect_translate (&hit[i], &robot_pos->v); - vect_from_polar_uf016 (&ray, dist_mm[i], - robot_pos->a + radar_sensors[i].a); - vect_translate (&hit[i], &ray); - valid[i] = radar_valid (hit[i]); - vect_from_polar_uf016 (&ray, RADAR_OBSTACLE_EDGE_RADIUS_MM, - robot_pos->a + radar_sensors[i].a); - vect_translate (&hit[i], &ray); - } - else - valid[i] = 0; - } - /* Ignore sensor results too far from other sensors. */ - for (i = 0; i < UTILS_COUNT (radar_sensors) - 1; i++) - { - for (j = i + 1; valid[i] && j < UTILS_COUNT (radar_sensors); j++) - { - if (valid[j]) - { - if (dist_mm[i] + RADAR_FAR_MM < dist_mm[j]) - valid[j] = 0; - else if (dist_mm[j] + RADAR_FAR_MM < dist_mm[i]) - valid[i] = 0; - } - } - } - /* Specific treatment about sensor topology. */ - if (valid[RADAR_SENSOR_BACK]) - obs_pos[obs_nb++] = hit[RADAR_SENSOR_BACK]; - front_nb = 0; - front_center.x = 0; front_center.y = 0; - for (i = RADAR_SENSOR_FRONT; i < RADAR_SENSOR_BACK; i++) - { - if (valid[i]) - { - vect_add (&front_center, &hit[i]); - front_nb++; - } - } - if (front_nb) - { - vect_scale_f824 (&front_center, 0x1000000l / front_nb); - obs_pos[obs_nb++] = front_center; - } - /* Done. */ - return obs_nb; -} - -uint8_t -radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, - const vect_t *obs_pos, uint8_t obs_nb) -{ - uint8_t i; - /* Stop here if no obstacle. */ - if (!obs_nb) - return 0; - vect_t vd = *dest_pos; vect_sub (&vd, robot_pos); - uint16_t d = vect_norm (&vd); - /* If destination is realy near, stop here. */ - if (d < RADAR_EPSILON_MM) - return 0; - /* If destination is near, use clearance to destination point instead of - * stop length. */ - vect_t t; - if (d < RADAR_STOP_MM) - t = *dest_pos; - else - { - vect_scale_f824 (&vd, (1ll << 24) / d * RADAR_STOP_MM); - t = *robot_pos; - vect_translate (&t, &vd); - } - /* Now, look at obstacles. */ - for (i = 0; i < obs_nb; i++) - { - /* Vector from robot to obstacle. */ - vect_t vo = obs_pos[i]; vect_sub (&vo, robot_pos); - /* Ignore if in our back. */ - int32_t dp = vect_dot_product (&vd, &vo); - if (dp < 0) - continue; - /* Check distance. */ - int16_t od = distance_segment_point (robot_pos, &t, &obs_pos[i]); - if (od > BOT_SIZE_SIDE + RADAR_CLEARANCE_MM / 2 - + RADAR_OBSTACLE_RADIUS_MM) - continue; - /* Else, obstacle is blocking. */ - return 1; - } - return 0; -} - diff --git a/digital/io/src/radar.h b/digital/io/src/radar.h deleted file mode 100644 index b4611ec7..00000000 --- a/digital/io/src/radar.h +++ /dev/null @@ -1,78 +0,0 @@ -#ifndef radar_h -#define radar_h -/* radar.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2010 Nicolas Schodet - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT 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 - * (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. - * - * }}} */ -#include "defs.h" - -/** - * Handle any distance sensors information to extract useful data. This - * includes: - * - combining several sensors information for a more precise obstacle - * position, - * - ignoring obstacles not in the playground, - * - determining if an obstacle should make the robot stop. - */ - -/** Estimated obstacle edge radius. As the sensors detect obstacle edge, this is - * added to position obstacle center. */ -#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 - -/** Estimated obstacle radius. The obstacle may be larger than at the - * detected edge. */ -#define RADAR_OBSTACLE_RADIUS_MM 150 - -/** Stop distance. Distance under which an obstacle is considered harmful when - * moving. */ -#define RADAR_STOP_MM 350 - -/** Clearance distance. Distance over which an obstacle should be to the side - * when moving. - * - * OK, more explanations: when moving, a rectangle is placed in front of the - * robot, of length RADAR_STOP_MM and width 2 * (RADAR_CLEARANCE_MM + - * BOT_SIZE_SIDE). If an obstacle is inside this rectangle, it is considered - * in the way. - * - * If the destination point is near (< RADAR_STOP_MM - RADAR_CLEARANCE_MM), - * this reduce the rectangle length. - * - * If the destination is really near (< RADAR_EPSILON_MM), ignore all this. */ -#define RADAR_CLEARANCE_MM 100 - -/** Destination distance near enough so that obstacles could be ignored. */ -#define RADAR_EPSILON_MM 70 - -/** Update radar view. Return the number of obstacles found. Obstacles - * positions are returned in obs_pos. */ -uint8_t -radar_update (const position_t *robot_pos, vect_t *obs_pos); - -/** Return non zero if there is a blocking obstacle near the robot while going - * to a destination point. */ -uint8_t -radar_blocking (const vect_t *robot_pos, const vect_t *dest_pos, - const vect_t *obs_pos, uint8_t obs_nb); - -#endif /* radar_h */ diff --git a/digital/io/src/radar_defs.c b/digital/io/src/radar_defs.c new file mode 100644 index 00000000..2787ed97 --- /dev/null +++ b/digital/io/src/radar_defs.c @@ -0,0 +1,54 @@ +/* radar_defs.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "radar.h" + +#include "modules/devices/usdist/usdist.h" +#include "playground_2010.h" + +/** Define radar configuration. */ +struct radar_sensor_t radar_sensors[RADAR_SENSOR_NB] = { +#define RADAR_SENSOR_FRONT 0 + { &usdist_mm[0], { 30 - 20, 0 }, G_ANGLE_UF016_DEG (0) }, +#define RADAR_SENSOR_LEFT 1 + { &usdist_mm[1], { 20 - 20, 20 }, G_ANGLE_UF016_DEG (30) }, +#define RADAR_SENSOR_RIGHT 2 + { &usdist_mm[2], { 20 - 20, -20 }, G_ANGLE_UF016_DEG (-30) }, +#define RADAR_SENSOR_BACK 3 + { &usdist_mm[3], { -30 - 20, 0 }, G_ANGLE_UF016_DEG (180) }, +}; + +/** Define exclusion area (considered as invalid point). */ +uint8_t +radar_valid (vect_t p) +{ + return p.x >= RADAR_MARGIN_MM && p.x < PG_WIDTH - RADAR_MARGIN_MM + && p.y >= RADAR_MARGIN_MM && p.y < PG_LENGTH - RADAR_MARGIN_MM + /* Ignore points on slope, no margin for the slope start. */ + && (p.x < PG_WIDTH / 2 - PG_SLOPE_WIDTH / 2 + || p.x >= PG_WIDTH / 2 + PG_SLOPE_WIDTH / 2 + || p.y < PG_LENGTH - PG_SLOPE_LENGTH - RADAR_MARGIN_MM / 2); +} + diff --git a/digital/io/src/radar_defs.h b/digital/io/src/radar_defs.h new file mode 100644 index 00000000..51bc5943 --- /dev/null +++ b/digital/io/src/radar_defs.h @@ -0,0 +1,41 @@ +#ifndef radar_defs_h +#define radar_defs_h +/* radar_defs.h */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2010 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#define RADAR_OBSTACLE_EDGE_RADIUS_MM 40 +#define RADAR_OBSTACLE_RADIUS_MM 150 +#define RADAR_STOP_MM 350 +#define RADAR_CLEARANCE_MM 100 +#define RADAR_EPSILON_MM 70 + +#define RADAR_SENSOR_NB 4 + +#define RADAR_SENSOR_FRONT_FIRST 0 +#define RADAR_SENSOR_FRONT_NB 3 +#define RADAR_SENSOR_BACK_FIRST 3 +#define RADAR_SENSOR_BACK_NB 1 + +#endif /* radar_defs_h */ diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index de8eaa22..260960f8 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -54,4 +54,5 @@ class Bag: DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] + self.pos_report = link_bag.io_hub.pos_report diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py index 0b5a85ad..73aa9fe3 100644 --- a/host/simu/robots/robospierre/view/bag.py +++ b/host/simu/robots/robospierre/view/bag.py @@ -43,4 +43,5 @@ class Bag: ClampSide.height), model_bag.clamp)) self.distance_sensor = [DistanceSensorUS (self.robot, ds) for ds in model_bag.distance_sensor] + self.pos_report = PosReport (table, model_bag.pos_report) -- cgit v1.2.3 From bd04b586866d0770905cf5e89c97face316719d2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 20:39:11 +0200 Subject: digital/mimot: default to unlimited motor --- digital/mimot/src/dirty/models.host.c | 2 +- host/simu/robots/marcel/model/loader.py | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c index efea35bc..a835298f 100644 --- a/digital/mimot/src/dirty/models.host.c +++ b/digital/mimot/src/dirty/models.host.c @@ -49,7 +49,7 @@ static const struct motor_def_t marcel_clamp_f2342_model = 0.100 * 0.005 * 0.005,/* Load (kg.m^2). */ /* This is a pifometric estimation. */ /* Hardware limits. */ - 0.0, +INFINITY, + -INFINITY, +INFINITY, }; /* Marcel, APBTeam 2010. */ diff --git a/host/simu/robots/marcel/model/loader.py b/host/simu/robots/marcel/model/loader.py index ae1caa7d..7f767b5a 100644 --- a/host/simu/robots/marcel/model/loader.py +++ b/host/simu/robots/marcel/model/loader.py @@ -91,6 +91,7 @@ class Loader (Observable): limit = (((self.CLAMP_WIDTH - tickness) * 0.5 + 5) / self.CLAMP_PULLEY_RADIUS) for l in self.clamp_link: + l.limits.min = 0 l.limits.max = limit l.limits.notify () self.tickness = tickness -- cgit v1.2.3 From 2f37578582ba9831122d55e189fe4cce41aa758a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 20:40:28 +0200 Subject: digital/ai/tools: use new door and clamp command --- digital/ai/tools/test_simu_control_robospierre.py | 13 +++---------- digital/io-hub/tools/io_hub/io_hub.py | 6 ++++++ 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index acd3ca7c..77f017c2 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -106,10 +106,7 @@ class TestSimuControl (TestSimu): self.asserv.goto_angle (a) def clamp_command (self): - if self.clamp_var.get (): - self.io.pwm_set_timed (2, -0x3ff, 255, 0) - else: - self.io.pwm_set_timed (2, 0x3ff, 255, 0) + self.io.clamp_openclose (not self.clamp_var.get ()) def elevation_up_command (self): self.mimot.speed_pos ('a0', self.ELEVATION_STROKE / 2) @@ -131,12 +128,8 @@ class TestSimuControl (TestSimu): self.clamp_to_scale.get ()) def doors_command (self): - if self.doors_var.get (): - pwm = -0x3ff - else: - pwm = 0x3ff - for i in (0, 1, 3, 4): - self.io.pwm_set_timed (i, pwm, 255, 0) + for i in (0, 2, 3, 5): + self.io.door (i, not self.doors_var.get ()) def drop_command (self): if self.drop_var.get (): diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index d6ae8618..aafc2d1d 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -60,6 +60,12 @@ class Proto: else: raise ValueError + def door (self, pos, open_): + self.proto.send ('d', 'BB', pos, (0, 1)[open_]) + + def clamp_openclose (self, open_): + self.proto.send ('d', 'BB', 0xff, (0, 1)[open_]) + def close (self): self.reset () self.proto.wait (lambda: True) -- cgit v1.2.3 From 9cbeca7897dda3dc1e35677d2573e7b3528a58e1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 20:40:50 +0200 Subject: digital/io-hub: handle clamp open/close offset --- digital/io-hub/src/robospierre/bot.h | 5 ++++ digital/io-hub/src/robospierre/clamp.c | 50 ++++++++++++++++++++++++++-------- digital/io-hub/src/robospierre/clamp.h | 4 +++ digital/io-hub/src/robospierre/main.c | 1 + 4 files changed, 49 insertions(+), 11 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 4df9b89d..d68b0c6b 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -74,6 +74,8 @@ # define BOT_CLAMP_BAY_SIDE_ROTATION_STEP \ (BOT_CLAMP_BAY_BACK_ROTATION_STEP / 2) +#define BOT_CLAMP_CLOSED_ROTATION_OFFSET 0 + #else /* !HOST */ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 @@ -100,10 +102,13 @@ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x816 +#define BOT_CLAMP_CLOSED_ROTATION_OFFSET -61 + #endif /* !HOST */ #define BOT_CLAMP_ELEVATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_SPEED 0x30 +#define BOT_CLAMP_ROTATION_OFFSET_SPEED 2 #define BOT_PWM_CLAMP 2 #define BOT_PWM_DOOR_FRONT_BOTTOM 0 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index b32549d2..9dfb01f9 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -123,6 +123,10 @@ struct clamp_t uint8_t new_element_type; /** Drop direction, drop on the other side. */ uint8_t drop_direction; + /** True if clamp is open. */ + uint8_t open; + /** True if clamp position is controled. */ + uint8_t controled; }; /** Global context. */ @@ -168,9 +172,18 @@ static const uint8_t clamp_slot_door[] = { 0xff }; +static void +clamp_openclose (uint8_t open); + static void clamp_route (void); +void +clamp_init (void) +{ + ctx.open = 1; +} + void clamp_move (uint8_t pos) { @@ -225,12 +238,7 @@ void clamp_door (uint8_t pos, uint8_t open) { if (pos == 0xff) - { - if (open) - pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); - else - pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); - } + clamp_openclose (open); else if (clamp_slot_door[pos] != 0xff) { if (open) @@ -275,6 +283,23 @@ clamp_handle_event (void) return 0; } +/** Open or close clamp and adjust rotation. */ +static void +clamp_openclose (uint8_t open) +{ + if (open) + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + else + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + if (ctx.controled && CLAMP_IS_SLOT_IN_FRONT_BAY (ctx.pos_current)) + { + int16_t offset = open ? 0 : BOT_CLAMP_CLOSED_ROTATION_OFFSET; + mimot_move_motor1_absolute (clamp_pos[ctx.pos_current][1] + offset, + BOT_CLAMP_ROTATION_OFFSET_SPEED); + } + ctx.open = open; +} + /** Find next position and start motors. */ static void clamp_route (void) @@ -336,8 +361,11 @@ clamp_route (void) /* Run motors. */ mimot_move_motor0_absolute (clamp_pos[pos_new][0], BOT_CLAMP_ELEVATION_SPEED); - mimot_move_motor1_absolute (clamp_pos[pos_new][1], + int16_t offset = !ctx.open && CLAMP_IS_SLOT_IN_FRONT_BAY (pos_new) + ? BOT_CLAMP_CLOSED_ROTATION_OFFSET : 0; + mimot_move_motor1_absolute (clamp_pos[pos_new][1] + offset, BOT_CLAMP_ROTATION_SPEED); + ctx.controled = 1; /* Remember new position. */ ctx.pos_current = pos_new; } @@ -510,7 +538,7 @@ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, } else { - pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + clamp_openclose (0); return FSM_NEXT (CLAMP_MOVE_IDLE, clamp_move, move_element_here); } } @@ -540,7 +568,7 @@ FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, { if (ctx.pos_current == ctx.pos_request) { - pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); + clamp_openclose (0); return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, done); } @@ -594,7 +622,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, } else { - pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + clamp_openclose (1); return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_open_clamp); @@ -611,7 +639,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, CLAMP_MOVE_DST_CLAMP_OPENING) { - pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); + clamp_openclose (1); return FSM_NEXT_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING); } diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 231c3a84..09233cce 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -60,6 +60,10 @@ enum { #define CLAMP_IS_SLOT_IN_BACK_BAY(slot) \ ((slot) >= CLAMP_SLOT_BACK_BOTTOM && (slot) <= CLAMP_SLOT_BACK_TOP) +/** Initialise clamp module. */ +void +clamp_init (void); + /** Move clamp to given position. */ void clamp_move (uint8_t pos); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 6d32c28f..36a1c157 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -103,6 +103,7 @@ main_init (void) contact_init (); usdist_init (); /* AI modules. */ + clamp_init (); logistic_init (); /* Initialization done. */ proto_send0 ('z'); -- cgit v1.2.3 From 9377154ec66848759806d0fe924ded961c9f392b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 23 May 2011 22:56:02 +0200 Subject: digital/io-hub: add stub path module --- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/avrconfig.h | 8 +++ digital/io-hub/src/robospierre/main.c | 3 ++ digital/io-hub/src/robospierre/path.c | 83 ++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/path.h | 78 ++++++++++++++++++++++++++++ 5 files changed, 173 insertions(+), 1 deletion(-) create mode 100644 digital/io-hub/src/robospierre/path.c create mode 100644 digital/io-hub/src/robospierre/path.h diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index fe2b318e..dd4bfe5c 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ - radar_defs.c radar.c \ + radar_defs.c radar.c path.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h index 53e36ccf..c3f107c5 100644 --- a/digital/io-hub/src/robospierre/avrconfig.h +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -112,6 +112,14 @@ USDIST_SENSOR (2, A, 2) \ USDIST_SENSOR (3, A, 3) +/* path - Path finding module. */ +/** Report path found for debug. */ +#define AC_PATH_REPORT defined (HOST) +/** Report function name. */ +#define AC_PATH_REPORT_CALLBACK simu_send_path +/** Number of possible obstacles. */ +#define AC_PATH_OBSTACLES_NB 2 + /* io-hub - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 10 diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 36a1c157..b1abfd8a 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -51,6 +51,7 @@ #include "clamp.h" #include "logistic.h" +#include "path.h" #include "bot.h" @@ -105,6 +106,7 @@ main_init (void) /* AI modules. */ clamp_init (); logistic_init (); + path_init (); /* Initialization done. */ proto_send0 ('z'); } @@ -194,6 +196,7 @@ main_loop (void) } /* Update AI modules. */ logistic_update (); + path_decay (); /* Only manage events if slaves are synchronised. */ if (twi_master_sync ()) main_event_to_fsm (); diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c new file mode 100644 index 00000000..b3c91ac8 --- /dev/null +++ b/digital/io-hub/src/robospierre/path.c @@ -0,0 +1,83 @@ +/* path.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "path.h" + +/** Context. */ +struct path_t +{ + /** Position of end points. */ + vect_t endpoints[2]; + /** Whether the last update was a success. */ + uint8_t found; +}; +static struct path_t path; + +void +path_init (void) +{ +} + +void +path_endpoints (vect_t s, vect_t d) +{ + path.endpoints[0] = d; + path.endpoints[1] = s; +} + +void +path_escape (uint8_t factor) +{ +} + +void +path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor, + uint16_t valid) +{ +} + +void +path_decay (void) +{ +} + +void +path_update (void) +{ + path.found = 1; +} + +uint8_t +path_get_next (vect_t *p) +{ + if (path.found) + { + *p = path.endpoints[0]; + return 1; + } + else + return 0; +} + diff --git a/digital/io-hub/src/robospierre/path.h b/digital/io-hub/src/robospierre/path.h new file mode 100644 index 00000000..22a8ead8 --- /dev/null +++ b/digital/io-hub/src/robospierre/path.h @@ -0,0 +1,78 @@ +#ifndef path_h +#define path_h +/* path.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "defs.h" + +/** This implement a interface similar to the path module, but adapted for the + * special grid of Eurobot 2011. See real path modules for interface comments. */ + +/** Infinite validity for an obstacle. */ +#define PATH_OBSTACLE_VALID_ALWAYS 0xffff + +/** Obstacle. */ +struct path_obstacle_t +{ + /** Center. */ + vect_t c; + /** Radius. */ + uint16_t r; + /** Validity counter, when this is zero, the obstacle is ignored. */ + uint16_t valid; +}; + +void +path_init (void); + +void +path_endpoints (vect_t s, vect_t d); + +void +path_escape (uint8_t factor); + +void +path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor, + uint16_t valid); + +void +path_decay (void); + +void +path_update (void); + +uint8_t +path_get_next (vect_t *p); + +#if AC_PATH_REPORT + +/** Report computed path. */ +void +AC_PATH_REPORT_CALLBACK (vect_t *points, uint8_t len, + struct path_obstacle_t *obstacles, + uint8_t obstacles_nb); + +#endif + +#endif /* path_h */ -- cgit v1.2.3 From 579d7d9ad8caf4299c5c672b724d056d9e9d9797 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 24 May 2011 01:33:18 +0200 Subject: digital/io-hub: add move FSM --- digital/ai/tools/test_simu_control_robospierre.py | 9 +- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 4 + digital/io-hub/src/robospierre/main.c | 17 +- digital/io-hub/src/robospierre/main.h | 31 ++ digital/io-hub/src/robospierre/move.c | 496 ++++++++++++++++++++++ digital/io-hub/src/robospierre/move.h | 59 +++ digital/io-hub/tools/io_hub/io_hub.py | 3 + 8 files changed, 617 insertions(+), 4 deletions(-) create mode 100644 digital/io-hub/src/robospierre/main.h create mode 100644 digital/io-hub/src/robospierre/move.c create mode 100644 digital/io-hub/src/robospierre/move.h diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 77f017c2..0d52219e 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -91,12 +91,19 @@ class TestSimuControl (TestSimu): self.backward_button = Checkbutton (self.control_frame, text = 'Backward', variable = self.backward_var) self.backward_button.pack () + self.goto_var = IntVar () + self.goto_button = Checkbutton (self.control_frame, + text = 'Goto FSM', variable = self.goto_var) + self.goto_button.pack () self.table_view.bind ('<1>', self.move) self.table_view.bind ('<3>', self.orient) def move (self, ev): pos = self.table_view.screen_coord ((ev.x, ev.y)) - self.asserv.goto (pos[0], pos[1], self.backward_var.get ()) + if self.goto_var.get (): + self.io.goto (pos[0], pos[1], self.backward_var.get ()) + else: + self.asserv.goto (pos[0], pos[1], self.backward_var.get ()) def orient (self, ev): x, y = self.table_view.screen_coord ((ev.x, ev.y)) diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index dd4bfe5c..05b3d92c 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub # Sources to compile. io_hub_SOURCES = main.c \ clamp.c logistic.c \ - radar_defs.c radar.c path.c \ + radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index d68b0c6b..05c2b436 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -34,6 +34,10 @@ # define BOT_SCALE 0.0415178942124 #endif +/** Distance from the robot axis to the front. */ +#define BOT_SIZE_FRONT 150 +/** Distance from the robot axis to the back. */ +#define BOT_SIZE_BACK 150 /** Distance from the robot axis to the side. */ #define BOT_SIZE_SIDE 190 diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index b1abfd8a..b1e85b72 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -52,6 +52,7 @@ #include "clamp.h" #include "logistic.h" #include "path.h" +#include "move.h" #include "bot.h" @@ -160,7 +161,9 @@ main_event_to_fsm (void) /* Post the event */ FSM_HANDLE_VAR_E (AI, save_event); } - + /* Check obstables. */ + if (move_check_obstacles ()) + return; } /** Main (and infinite) loop. */ @@ -191,7 +194,7 @@ main_loop (void) position_t robot_pos; asserv_get_position (&robot_pos); main_obstacles_nb = radar_update (&robot_pos, main_obstacles_pos); - //move_obstacles_update (); + move_obstacles_update (); simu_send_pos_report (main_obstacles_pos, main_obstacles_nb, 0); } /* Update AI modules. */ @@ -252,6 +255,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) v8_to_v16 (args[3], args[4]), v8_to_v16 (args[5], args[6])); break; + case c ('m', 5): + /* Go to position. + * - 2w: x, y. + * - 1b: backward. */ + { + vect_t position = { v8_to_v16 (args[0], args[1]), + v8_to_v16 (args[2], args[3]) }; + move_start_noangle (position, args[4], 0); + } + break; case c ('c', 1): /* Move clamp. * - 1b: position. */ diff --git a/digital/io-hub/src/robospierre/main.h b/digital/io-hub/src/robospierre/main.h new file mode 100644 index 00000000..529aa0b2 --- /dev/null +++ b/digital/io-hub/src/robospierre/main.h @@ -0,0 +1,31 @@ +#ifndef main_h +#define main_h +/* main.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +extern vect_t main_obstacles_pos[2]; +extern uint8_t main_obstacles_nb; + +#endif /* main_h */ diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c new file mode 100644 index 00000000..18759c14 --- /dev/null +++ b/digital/io-hub/src/robospierre/move.c @@ -0,0 +1,496 @@ +/* move.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "move.h" + +#include "main.h" +#include "asserv.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" + +#include "radar.h" +#include "path.h" + +#include "modules/utils/utils.h" + +#include + +/** Move context. */ +struct move_t +{ + /** Final position. */ + position_t final; + /** Use angle consign for final point. */ + uint8_t with_angle; + /** Next step. */ + vect_t step; + /** Next step angle. */ + uint16_t step_angle; + /** Next step with_angle. */ + uint8_t step_with_angle; + /** Next step backward. */ + uint8_t step_backward; + /** Non zero means this is a tricky move, slow down, and minimize + * turns. */ + uint8_t slow; + /** Backward direction allowed flag. */ + uint8_t backward_movement_allowed; + /** Try again counter. */ + uint8_t try_again_counter; + /** Dirty fix to know this is the final move. */ + uint8_t final_move; + /** Distance to remove from path. */ + int16_t shorten; +}; + +/* Global context. */ +struct move_t move_data; + +void +move_start (position_t position, uint8_t backward) +{ + /* Set parameters. */ + move_data.final = position; + move_data.with_angle = 1; + move_data.backward_movement_allowed = backward; + move_data.final_move = 0; + move_data.shorten = 0; + /* Reset try counter. */ + move_data.try_again_counter = 3; + /* Start the FSM. */ + FSM_HANDLE (AI, move_start); +} + +void +move_start_noangle (vect_t position, uint8_t backward, int16_t shorten) +{ + /* Set parameters. */ + move_data.final.v = position; + move_data.with_angle = 0; + move_data.backward_movement_allowed = backward; + move_data.final_move = 0; + move_data.shorten = shorten; + /* Reset try counter. */ + move_data.try_again_counter = 3; + /* Start the FSM. */ + FSM_HANDLE (AI, move_start); +} + +void +move_obstacles_update (void) +{ + uint8_t i; + for (i = 0; i < main_obstacles_nb; i++) + path_obstacle (i, main_obstacles_pos[i], MOVE_OBSTACLE_RADIUS, 0, + MOVE_OBSTACLE_VALIDITY); +} + +uint8_t +move_check_obstacles (void) +{ + if (FSM_CAN_HANDLE (AI, obstacle_in_front)) + { + position_t robot_pos; + asserv_get_position (&robot_pos); + if (radar_blocking (&robot_pos.v, &move_data.step, main_obstacles_pos, + main_obstacles_nb)) + if (FSM_HANDLE (AI, obstacle_in_front)) + return 1; + } + return 0; +} + +FSM_STATES ( + /* Waiting for the start order. */ + MOVE_IDLE, + /* Rotating towards next point. */ + MOVE_ROTATING, + /* Moving to a position (intermediate or final). */ + MOVE_MOVING, + /* Moving backward to go away from what is blocking the bot. */ + MOVE_MOVING_BACKWARD_TO_TURN_FREELY, + /* Waiting for obstacle to disappear. */ + MOVE_WAIT_FOR_CLEAR_PATH) + +FSM_EVENTS ( + /* Initialize the FSM and start the movement directly. */ + move_start, + /* Movement success. */ + move_success, + /* Movement failure. */ + move_failure, + /* The bot has seen something (front is the same when going backward). */ + obstacle_in_front) + +FSM_START_WITH (MOVE_IDLE) + +/** Go to current step, low level function. */ +static void +move_go (void) +{ + vect_t dst = move_data.step; + /* Modify final point if requested. */ + if (move_data.final_move && move_data.shorten) + { + /* Compute a vector from destination to robot with lenght + * 'shorten'. */ + position_t robot_position; + asserv_get_position (&robot_position); + vect_t v = robot_position.v; + vect_sub (&v, &move_data.step); + int16_t d = vect_norm (&v); + if (d > move_data.shorten) + { + vect_scale_f824 (&v, 0x1000000 / d * move_data.shorten); + vect_translate (&dst, &v); + } + } + if (move_data.step_with_angle) + asserv_goto_xya (dst.x, dst.y, move_data.step_angle, + move_data.step_backward); + else + asserv_goto (dst.x, dst.y, move_data.step_backward); +} + +/** Go or rotate toward position, returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle, + uint8_t backward) +{ + position_t robot_position; + /* Remember step. */ + move_data.step = dst; + move_data.step_angle = angle; + move_data.step_with_angle = with_angle; + move_data.step_backward = backward; + /* Compute angle to destination. */ + asserv_get_position (&robot_position); + vect_t v = dst; vect_sub (&v, &robot_position.v); + uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); + if (backward & ASSERV_BACKWARD) + dst_angle += 0x8000; + if ((backward & ASSERV_REVERT_OK) + && (dst_angle ^ robot_position.a) & 0x8000) + dst_angle += 0x8000; + int16_t diff = dst_angle - robot_position.a; + /* Move or rotate. */ + if (UTILS_ABS (diff) < 0x1000) + { + move_go (); + return 1; + } + else + { + asserv_goto_angle (dst_angle); + return 2; + } +} + +/** Go to next position computed by path module, to be called by + * move_path_init and move_path_next. Returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_go_to_next (vect_t dst) +{ + uint8_t r; + /* If it is not the last position. */ + if (dst.x != move_data.final.v.x || dst.y != move_data.final.v.y) + { + /* Not final position. */ + move_data.final_move = 0; + /* Goto without angle. */ + r = move_go_or_rotate (dst, 0, 0, move_data.backward_movement_allowed + | (move_data.slow ? ASSERV_REVERT_OK : 0)); + } + else + { + /* Final position. */ + move_data.final_move = 1; + /* Goto with angle if requested. */ + r = move_go_or_rotate (dst, move_data.final.a, move_data.with_angle, + move_data.backward_movement_allowed); + } + /* Next time, do not use slow. */ + move_data.slow = 0; + return r; +} + +/** Update and go to first position, return non zero if a path is found, 1 for + * linear move, 2 for angular move. */ +static uint8_t +move_path_init (void) +{ + uint8_t found; + vect_t dst; + /* Get the current position */ + position_t current_pos; + asserv_get_position (¤t_pos); + /* Give the current position of the bot to the path module */ + path_endpoints (current_pos.v, move_data.final.v); + /* Update the path module */ + move_data.slow = 0; + path_update (); + found = path_get_next (&dst); + /* If not found, try to escape. */ + if (!found) + { + move_data.slow = 1; + path_escape (8); + path_update (); + found = path_get_next (&dst); + } + /* If found, go. */ + if (found) + { + return move_go_to_next (dst); + } + else + { + /* Error, not final move. */ + move_data.final_move = 0; + return 0; + } +} + +/** Go to next position in path. Returns 1 for linear move, 2 for angular + * move. */ +static uint8_t +move_path_next (void) +{ + vect_t dst; + path_get_next (&dst); + return move_go_to_next (dst); +} + +FSM_TRANS (MOVE_IDLE, move_start, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_IDLE, move_start, path_found_rotate); + else + return FSM_NEXT (MOVE_IDLE, move_start, path_found); + } + else + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_IDLE, move_start, no_path_found); + } +} + +FSM_TRANS (MOVE_ROTATING, + robot_move_success, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT (MOVE_ROTATING, robot_move_success); +} + +FSM_TRANS (MOVE_ROTATING, + robot_move_failure, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT (MOVE_ROTATING, robot_move_failure); +} + +FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250, + MOVE_MOVING) +{ + move_go (); + return FSM_NEXT_TIMEOUT (MOVE_ROTATING); +} + +FSM_TRANS (MOVE_MOVING, robot_move_success, + done, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + if (move_data.final_move) + { + fsm_queue_post_event (FSM_EVENT (AI, move_success)); + return FSM_NEXT (MOVE_MOVING, robot_move_success, done); + } + else + { + uint8_t next = move_path_next (); + if (next == 2) + return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING, robot_move_success, path_found); + } + //return FSM_NEXT (MOVE_MOVING, robot_move_success, no_path_found); +} + +static void +move_moving_backward_to_turn_freely (void) +{ + move_data.final_move = 0; + /* Assume there is an obstacle in front of the robot. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + vect_t obstacle_pos; + int16_t dist = asserv_get_last_moving_direction () == DIRECTION_FORWARD + ? BOT_SIZE_FRONT + MOVE_REAL_OBSTACLE_RADIUS + : -(BOT_SIZE_BACK + MOVE_REAL_OBSTACLE_RADIUS); + vect_from_polar_uf016 (&obstacle_pos, dist, robot_pos.a); + vect_translate (&obstacle_pos, &robot_pos.v); + path_obstacle (0, obstacle_pos, MOVE_OBSTACLE_RADIUS, 0, + MOVE_OBSTACLE_VALIDITY); + /* Move backward to turn freely. */ + asserv_move_linearly (asserv_get_last_moving_direction () + == DIRECTION_FORWARD ? -300 : 300); +} + +FSM_TRANS (MOVE_MOVING, + robot_move_failure, + MOVE_MOVING_BACKWARD_TO_TURN_FREELY) +{ + move_moving_backward_to_turn_freely (); + return FSM_NEXT (MOVE_MOVING, robot_move_failure); +} + +FSM_TRANS_TIMEOUT (MOVE_MOVING, 2500, + MOVE_MOVING_BACKWARD_TO_TURN_FREELY) +{ + move_moving_backward_to_turn_freely (); + return FSM_NEXT_TIMEOUT (MOVE_MOVING); +} + +FSM_TRANS (MOVE_MOVING, obstacle_in_front, + tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + tryout, MOVE_IDLE) +{ + move_data.final_move = 0; + asserv_stop_motor (); + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryout); + } + else + return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain); +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, + tryout, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found, MOVE_IDLE) +{ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, tryout); + } + else + { + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, path_found); + } + else + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, no_path_found); + } + } +} + +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, + tryout, MOVE_IDLE, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + no_path_found_tryout, MOVE_IDLE) +{ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, tryout); + } + else + { + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found_rotate); + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, path_found); + } + else + { + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryout); + } + else + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, no_path_found_tryagain); + } + } +} + +FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250, + path_found_rotate, MOVE_ROTATING, + path_found, MOVE_MOVING, + no_path_found_tryagain, MOVE_WAIT_FOR_CLEAR_PATH, + no_path_found_tryout, MOVE_IDLE) +{ + /* Try to move. */ + uint8_t next = move_path_init (); + if (next) + { + if (next == 2) + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found_rotate); + else + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, path_found); + } + else + { + /* Error, no new position, should we try again? */ + if (--move_data.try_again_counter == 0) + { + fsm_queue_post_event (FSM_EVENT (AI, move_failure)); + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryout); + } + else + return FSM_NEXT_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, no_path_found_tryagain); + } +} + diff --git a/digital/io-hub/src/robospierre/move.h b/digital/io-hub/src/robospierre/move.h new file mode 100644 index 00000000..877002fd --- /dev/null +++ b/digital/io-hub/src/robospierre/move.h @@ -0,0 +1,59 @@ +#ifndef move_h +#define move_h +/* move.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "defs.h" +#include "bot.h" + +/** Real radius of an obstacle. */ +#define MOVE_REAL_OBSTACLE_RADIUS 150 + +/** Obstacle radius for the path module. + * It corresponds to the real radius of the obstacle plus the distance you + * want to add to avoid it. */ +#define MOVE_OBSTACLE_RADIUS (MOVE_REAL_OBSTACLE_RADIUS \ + + RADAR_CLEARANCE_MM \ + + BOT_SIZE_SIDE) + +/** Obstacle validity time (in term of number of cycles). */ +#define MOVE_OBSTACLE_VALIDITY (3 * 225) + +/** Go to a position, see asserv.h for backward argument. */ +void +move_start (position_t position, uint8_t backward); + +/** Go to a position, with no angle consign. */ +void +move_start_noangle (vect_t position, uint8_t backward, int16_t shorten); + +/** To be called when obstacles positions are computed. */ +void +move_obstacles_update (void); + +/** Check for blocking obstacles, return non zero if an event is handled. */ +uint8_t +move_check_obstacles (void); + +#endif /* move_h */ diff --git a/digital/io-hub/tools/io_hub/io_hub.py b/digital/io-hub/tools/io_hub/io_hub.py index aafc2d1d..06d237e9 100644 --- a/digital/io-hub/tools/io_hub/io_hub.py +++ b/digital/io-hub/tools/io_hub/io_hub.py @@ -44,6 +44,9 @@ class Proto: def pwm_set_timed (self, index, value, time, rest_value): self.proto.send ('w', 'BhHh', index, value, time, rest_value) + def goto (self, x, y, backward): + self.proto.send ('m', 'hhB', x, y, backward) + def clamp_move (self, pos): self.proto.send ('c', 'B', pos) -- cgit v1.2.3 From d2e4e1f473154775de48d19a31a97e9604e30905 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 28 May 2011 14:27:14 +0200 Subject: digital/io-hub: add element score computation --- digital/io-hub/src/robospierre/Makefile | 5 +- digital/io-hub/src/robospierre/element.c | 685 +++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/element.h | 127 +++++- 3 files changed, 810 insertions(+), 7 deletions(-) create mode 100644 digital/io-hub/src/robospierre/element.c diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 05b3d92c..e4b87124 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -2,20 +2,23 @@ BASE = ../../../avr # Name of the program to build. PROGS = io_hub +HOST_PROGS = test_element # Sources to compile. io_hub_SOURCES = main.c \ - clamp.c logistic.c \ + clamp.c logistic.c element.c \ radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ contact.avr.c contact.host.c \ twi_master.c asserv.c mimot.c \ chrono.c timer.avr.c simu.host.c +test_element_SOURCES = test_element.c logistic.c element.c # Modules needed for IO. MODULES = proto uart twi utils \ adc devices/usdist \ math/fixed math/geometry AI_MODULES = twi_master common utils fsm move +test_element_MODULES = host math/fixed math/geometry # Configuration file. CONFIGFILE = avrconfig.h AVR_MCU = at90usb1287 diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c new file mode 100644 index 00000000..341206b0 --- /dev/null +++ b/digital/io-hub/src/robospierre/element.c @@ -0,0 +1,685 @@ +/* element.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2011 Jérôme Jutteau + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "element.h" + +#include "modules/utils/utils.h" +#include "modules/math/geometry/distance.h" +#include "modules/math/geometry/geometry.h" + +#include "chrono.h" +#include "logistic.h" + +/** Elements on table. */ +struct element_t element_table[] = +{ + /* + 20 elements on intersections. + To be symmetric, alternate % 2. + See ELEMENT_INTERSEC_START and ELEMENT_INTERSEC_END + ELEMENT_INTERSEC_END don't takes central pawn. + */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* top left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, /* top right */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* middle left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, /* middle right */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* 2nd line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* 3th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* 4th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* 5th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + + /* Central pawn. (see ELEMENT_CENTRAL_PAWN) */ + {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER}, + + /* + 10 elements on green zones. + To be symmetric, alternate % 2. + See ELEMENT_GREEN_START and ELEMENT_GREEN_END + */ + {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT}, /* top left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT}, /* top right */ + {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT}, /* 2nd line left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT}, /* 2nd line right */ + {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT}, /* ... */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT}, + {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT}, + {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT}, + + /* + 32 elements in the middle of a square. + Altern colors in order to retrieve position % 2. + See ELEMENT_UNLOAD_START and ELEMENT_UNLOAD_END + */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, /* Top left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, /* 2nd line left red */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, /* 3th line left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, /* 4th line left red */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, /* 5th line left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS}, /* middle bonus left, red. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS}, /* middle bonus right, blue. */ + + /* + 4 elements in safe zones. + see ELEMENT_UNLOAD_SAFE_START and ELEMENT_UNLOAD_SAFE_END + */ + {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT}, /* left red */ + {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT}, /* left blue */ + {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT}, /* right red */ + {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT} /* right blue */ +}; + +inline element_t +element_get (uint8_t element_id) +{ + assert (element_id < UTILS_COUNT (element_table)); + return element_table[element_id]; +} + +inline void +element_set (uint8_t element_id, element_t element) +{ + assert (element_id < UTILS_COUNT (element_table)); + element_table[element_id] = element; +} + +void +element_init () +{ + /* Set NONE in middle of our squares and keep ELEMENT_NONE | ELEMENT_ANY + in others square middle. */ + uint8_t i; + element_t e; + for (i = ELEMENT_UNLOAD_START; i <= ELEMENT_UNLOAD_END; i++) + { + e = element_get (i); + if ((team_color == TEAM_COLOR_LEFT && + (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && + (e.attr & ELEMENT_RIGHT))) + { + e.type = ELEMENT_NONE; + element_set (i, e); + } + } +} + +int32_t +element_unload_score (position_t robot_pos, uint8_t element_id) +{ + int32_t score = 0; + assert (element_id < UTILS_COUNT (element_table)); + element_t e = element_get (element_id); + + /* If it is not an unload zone, quit. */ + if (!((e.attr & ELEMENT_CENTER) || (e.attr & ELEMENT_INTERSEC))) + return -1; + + /* Unload color. */ + if (!( + (e.attr & ELEMENT_CENTER) && + ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) + ||(team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + )) + return -1; + + /* If there is already something here, do not score. */ + if (e.type != ELEMENT_NONE) + return -1; + + /* Bonus score. */ + if ((e.attr & ELEMENT_BONUS) && + (chrono_remaining_time () * 100 / CHRONO_MATCH_DURATION_MS > 40)) + score += 1000; + + /* Unload distance. */ + /* TODO: minimal distance may not be the best choice. */ + vect_t v = e.pos; + int32_t dr = distance_point_point (&v, &robot_pos.v); + score += 4242 - dr; + + /* Alignment with the robot. */ + if (dr > 100) + { + vect_t vr = v; + vect_sub (&vr, &robot_pos.v); + vect_t u; + uint16_t a; + if (logistic_global.collect_direction == DIRECTION_FORWARD) + a = robot_pos.a; + else + a = robot_pos.a + G_ANGLE_UF016_DEG (180); + vect_from_polar_uf016 (&u, 100, a); + int32_t dp = vect_dot_product (&u, &vr); + int32_t align = dp / dr; + score += align; + } + + return score; +} + +uint8_t +element_unload_best (position_t robot_pos) +{ + uint8_t i; + uint8_t best = 0xff; + int32_t score, best_score = 0; + for (i = ELEMENT_UNLOAD_START; + i <= ELEMENT_UNLOAD_END; + i++) + { + score = element_score (robot_pos , i); + if (best == 0xff || best_score < score) + { + best = i; + best_score = score; + } + } + return best; +} + +int32_t +element_score (position_t robot_pos, uint8_t element_id) +{ + int32_t score = 0; + assert (element_id < UTILS_COUNT (element_table)); + element_t e = element_get (element_id); + + if (e.attr & ELEMENT_SAFE) + return -1; + + if ((e.attr & ELEMENT_CENTER) && + ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))) + return -1; + + if (e.type == ELEMENT_NONE && + ((e.attr & ELEMENT_INTERSEC) ||(e.attr & ELEMENT_GREEN))) + return -1; + + if (e.type & ELEMENT_PAWN) + score += ELEMENT_PAWN_SCORE; + if (e.type & ELEMENT_QUEEN) + score += ELEMENT_QUEEN_SCORE; + if (e.type & ELEMENT_KING) + score += ELEMENT_KING_SCORE; + if (e.type & ELEMENT_ANY) + score += ELEMENT_ANY_SCORE; + else if (e.type & ELEMENT_NONE) + score /= 2; + + /* Big score for our green zone. */ + if (e.type != ELEMENT_NONE && (e.attr & ELEMENT_GREEN)) + { + /* In our zone. */ + if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + score *= 10; + /* In the other zone, boost score if our green zone is empty. */ + else + { + element_t el; + int cpt = 0; + u8 i; + /* Is our green zone empty ? */ + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END; i++) + { + el = element_get (i); + if (((team_color == TEAM_COLOR_LEFT && (el.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (el.attr & ELEMENT_RIGHT))) + && el.type == ELEMENT_NONE) + cpt++; + } + if (cpt == 5) + score *= 10; + } + } + + //TODO Set right modifier at this time + score *= 100; + + /* Central pawn. */ + if (element_id == ELEMENT_CENTRAL_PAWN) + score += 1000; + + /* We are sure of this element. */ + if (!(e.type & ELEMENT_NONE) && + (e.attr == ELEMENT_INTERSEC || e.attr == ELEMENT_CENTER) && + element_id != ELEMENT_CENTRAL_PAWN) + score += score / 4; + + /* Distance from the robot. */ + vect_t v = e.pos; + int32_t dr = distance_point_point (&v, &robot_pos.v); + score += (4242 - dr); + + /* Alignment with the robot. */ + if (dr > 100) + { + vect_t vr = v; + vect_sub (&vr, &robot_pos.v); + vect_t u; + uint16_t a; + if (logistic_global.collect_direction == DIRECTION_FORWARD) + a = robot_pos.a; + else + a = robot_pos.a + G_ANGLE_UF016_DEG (180); + vect_from_polar_uf016 (&u, 100, a); + int32_t dp = vect_dot_product (&u, &vr); + int32_t align = dp / dr; + score += align; + } + + /* Adjust score with time probability. */ + score += element_proba (element_id); + return score; +} + +uint32_t +element_proba (uint8_t element_id) +{ + assert (element_id < UTILS_COUNT (element_table)); + element_t e = element_get (element_id); + uint32_t p_t = chrono_remaining_time () * 500 / CHRONO_MATCH_DURATION_MS; + uint8_t p_pos; + int32_t out = 0; + + /* Intersections. */ + if (e.attr & ELEMENT_INTERSEC) + { + /* Element on our side ? */ + if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + { + p_pos = 20 - element_id / 2; + out = p_pos * p_t; + } + /* Element on the other side ? */ + else + { + p_pos = element_id / 2 + 1; + out = p_pos * p_t; + } + } + /* Green zone. */ + else if (e.attr & ELEMENT_GREEN) + { + /* In our side. */ + if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + { + p_pos = 5 - ((element_id - ELEMENT_GREEN_START) / 2); + out = p_t * p_pos; + + } + /* Other side. */ + else + { + p_pos = (element_id - ELEMENT_GREEN_START) / 2 + 1; + out = p_t * p_pos; + } + + } + /* Central pawn */ + else if (e.type == ELEMENT_CENTRAL_PAWN) + out = p_t; + /* Centre of squares. */ + else if (e.attr & ELEMENT_CENTER) + { + /* In our side. */ + if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + out = 0; + /* Other side. */ + else + { + p_pos = (element_id - ELEMENT_UNLOAD_START) / 2 + 1; + out = (1000 - p_t) * p_pos; + if (e.attr & ELEMENT_BONUS) + out += 1000; + } + + } + else + out = 0; + return out; +} + +uint8_t +element_best (position_t robot_pos) +{ + uint8_t i; + uint8_t best = 0xff; + int32_t score = 0, best_score = 0; + for (i = 0; i < UTILS_COUNT (element_table); i++) + { + score = element_score (robot_pos ,i); + if (best == 0xff || best_score < score) + { + best = i; + best_score = score; + } + } + return best; +} + +void +element_not_here (uint8_t element_id) +{ + assert (element_id < UTILS_COUNT (element_table)); + element_t e = element_get (element_id); + e.type = ELEMENT_NONE; + element_set (element_id, e); + + /* Invalidate the same element to the other side. */ + if (e.attr & ELEMENT_INTERSEC) + { + uint8_t other_side_id = element_opposed (element_id); + element_t other_side = element_get (other_side_id); + other_side.type = ELEMENT_NONE; + element_set (other_side_id, other_side); + } + + /* If the element is on an intersection, try to guess last elements. */ + element_intersec_symetric (element_id, ELEMENT_NONE); +} + +inline void +element_intersec_symetric (uint8_t element_id, uint8_t element_type) +{ + static uint8_t element_columns[2][5] = + { + { + ELEMENT_NONE | ELEMENT_PAWN, + ELEMENT_NONE | ELEMENT_PAWN, + ELEMENT_NONE | ELEMENT_PAWN, + ELEMENT_NONE | ELEMENT_PAWN, + ELEMENT_NONE | ELEMENT_PAWN + }, + { + ELEMENT_NONE | ELEMENT_PAWN, + ELEMENT_NONE | ELEMENT_PAWN, + ELEMENT_NONE | ELEMENT_PAWN, + ELEMENT_NONE | ELEMENT_PAWN, + ELEMENT_NONE | ELEMENT_PAWN + } + }; + element_t e = element_get (element_id); + + /* if the element is on an intersection, try to guess last elements. */ + if ((e.attr & ELEMENT_INTERSEC) && !(e.attr & ELEMENT_CENTER)) + { + uint8_t cpt_none = 0; + uint8_t cpt_pawn = 0; + uint8_t col; + uint8_t i; + uint8_t type = 0; + + if (element_id % 4 == 0 || element_id % 4 == 1) + col = 0; + else + col = 1; + + /* Nothing to see. */ + if (element_columns[col][4] != (ELEMENT_NONE | ELEMENT_PAWN)) + return; + + /* Count. */ + for (i = 0; i < 5; i++) + { + if (element_columns[col][i] == ELEMENT_PAWN ||element_type == ELEMENT_PAWN) + cpt_pawn++; + if (element_columns[col][i] == ELEMENT_NONE ||element_type == ELEMENT_NONE) + cpt_none++; + if (element_columns[col][i] == (ELEMENT_NONE | ELEMENT_PAWN)) + { + element_columns[col][i] = element_type; + break; + } + } + + /* With which element are we going to fill the rest ? */ + if (cpt_pawn == 2) + type = ELEMENT_NONE; + if (cpt_none == 3) + type = ELEMENT_PAWN; + + if (type) + for (i = 0; i < 5; i++) + if (element_columns[col][i] == (ELEMENT_NONE | ELEMENT_PAWN)) + element_columns[col][i] = type; + + /* Complete if possible. */ + if (type != 0) + for (i = ELEMENT_INTERSEC_START; i < ELEMENT_INTERSEC_END; i++) + if (i % 4 == element_id % 4) + { + element_t el = element_get (i); + element_t sym = element_get (element_opposed (i)); + if (el.type == (ELEMENT_NONE | ELEMENT_PAWN)) + { + el.type = type; + element_set (i, el); + /* Set opposed. */ + if (sym.type == (ELEMENT_NONE | ELEMENT_PAWN)) + { + sym.type = type; + element_set (element_opposed (i), sym); + } + } + } + } +} + +void +element_taken (uint8_t element_id, uint8_t element_type) +{ + assert (element_id < UTILS_COUNT (element_table)); + static uint8_t pawn_c = 3, queen_c = 1, king_c = 1, any_nb = 5; + uint8_t other_side_id, other_side_id_element; + + if (element_type != ELEMENT_PAWN && element_type != ELEMENT_QUEEN && element_type != ELEMENT_KING) + return; + + /* Set element. */ + element_t e = element_get (element_id); + e.type = ELEMENT_NONE; + element_set (element_id, e); + + /* Deduce symmetric position. */ + if ((e.attr & ELEMENT_INTERSEC) || (e.attr & ELEMENT_GREEN)) + { + other_side_id_element = element_opposed (element_id); + element_t other_side = element_get (other_side_id_element); + if (other_side.type != ELEMENT_NONE) + { + other_side.type = element_type; + element_set (other_side_id_element, other_side); + } + } + + /* If the element is on an intersection, try to guess last elements. */ + element_intersec_symetric (element_id, element_type); + + /* If the element is in the green zone, try to guess last elements. */ + if ((e.attr & ELEMENT_GREEN) && any_nb > 0) + { + uint8_t i; + if (element_type == ELEMENT_PAWN) + pawn_c--; + else if (element_type == ELEMENT_QUEEN) + queen_c--; + else if (element_type == ELEMENT_KING) + king_c--; + any_nb--; + + /* If there is something to guess. */ + if (any_nb <= 3) + { + /* All others are pawns. */ + if (!queen_c && !king_c) + { + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END; i++) + { + element_t el = element_get (i); + if (el.type == ELEMENT_ANY) + { + el.type = ELEMENT_PAWN; + element_set (i, el); + any_nb--; + /* Set opposed side. */ + other_side_id = element_opposed (element_id); + element_t other_side = element_get (other_side_id); + if (other_side.type != ELEMENT_NONE && + other_side_id != other_side_id_element) + { + other_side.type = ELEMENT_PAWN; + element_set (other_side_id, other_side); + } + } + } + } + /* All others are unidentified heads */ + else if (pawn_c == 0 && any_nb == 2) + { + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END; i++) + { + element_t el = element_get (i); + if (el.type == ELEMENT_ANY) + { + el.type = ELEMENT_HEAD; + element_set (i, el); + /* Set opposed side. */ + other_side_id = element_opposed (element_id); + element_t other_side = element_get (other_side_id); + if (other_side.type != ELEMENT_NONE && + other_side_id != other_side_id_element) + { + other_side.type = ELEMENT_HEAD; + element_set (other_side_id, other_side); + } + } + } + } + /* Last element. */ + if (any_nb == 1) + { + uint8_t last_type; + if (pawn_c == 1) last_type = ELEMENT_PAWN; + else if (queen_c == 1) last_type = ELEMENT_QUEEN; + else last_type = ELEMENT_KING; + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END; i++) + { + element_t el = element_get (i); + if (el.type == ELEMENT_ANY || el.type == ELEMENT_HEAD) + { + el.type = last_type; + element_set (i, el); + any_nb--; + /* Set opposed side. */ + other_side_id = element_opposed (i); + element_t other_side = element_get (other_side_id); + if (other_side.type != ELEMENT_NONE && + other_side_id != other_side_id_element) + { + other_side.type = last_type; + element_set (other_side_id, other_side); + } + break; + } + } + } + } + } +} + +void +element_down (uint8_t element_id, uint8_t element_type) +{ + element_t e = element_get (element_id); + e.type = element_type; + element_set (element_id, e); +} + +uint8_t +element_give_position (position_t pos) +{ + uint8_t e = 0xff; + + return e; +} + +uint8_t +element_opposed (uint8_t element_id) +{ + uint8_t op = 0xff; + element_t e = element_get (element_id); + if ((e.attr & ELEMENT_GREEN) ||(e.attr & ELEMENT_INTERSEC)) + { + if (e.attr & ELEMENT_LEFT) + op = element_id + 1; + else + op = element_id - 1; + } + return op; +} diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index aa7b5b4e..95cf2b2c 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -3,7 +3,7 @@ /* element.h */ /* robospierre - Eurobot 2011 AI. {{{ * - * Copyright (C) 2011 Nicolas Schodet + * Copyright (C) 2011 Nicolas Schodet, Jérôme Jutteau * * APBTeam: * Web: http://apbteam.org/ @@ -24,17 +24,25 @@ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * * }}} */ +#include "defs.h" +/** None. */ +#define ELEMENT_NONE 1 /** A pawn, not a head. */ -#define ELEMENT_PAWN 1 +#define ELEMENT_PAWN 2 /** Queen pawn, used for statistic update of table data. */ -#define ELEMENT_QUEEN 2 +#define ELEMENT_QUEEN 4 /** King pawn, used for statistic update of table data. */ -#define ELEMENT_KING 4 +#define ELEMENT_KING 8 /** Any head, queen or king. */ -#define ELEMENT_HEAD 6 +#define ELEMENT_HEAD (ELEMENT_QUEEN |ELEMENT_KING) /** Any element, pawn, queen, or king. */ -#define ELEMENT_ANY 7 +#define ELEMENT_ANY (ELEMENT_HEAD |ELEMENT_PAWN) +/** Tower types. */ +#define ELEMENT_TOWER_1_QUEEN 8 +#define ELEMENT_TOWER_2_QUEEN 16 +#define ELEMENT_TOWER_1_KING 32 +#define ELEMENT_TOWER_2_KING 64 /** Return non zero if element is a head, not a pawn. */ #define ELEMENT_IS_HEAD(e) ((e) && !((e) & ELEMENT_PAWN)) @@ -42,4 +50,111 @@ /** Return non zero if element may be a head. */ #define ELEMENT_CAN_BE_HEAD(e) ((e) & ELEMENT_HEAD) +#define ELEMENT_PAWN_SCORE 10 +#define ELEMENT_ANY_SCORE 15 +#define ELEMENT_QUEEN_SCORE 20 +#define ELEMENT_KING_SCORE 30 + +/** Emplacement attributes bits. */ +#define ELEMENT_BONUS 1 +/** + * Two meanings: + * - When it is on an intersection or in the green zone, this mean that it is positioned on the + * left size of the table. + * - When it is located on an color, this corresponds to the red color. + */ +#define ELEMENT_LEFT 2 +/** + * Two meanings: + * - When it is on an intersection or in the green zone, this mean that it is positioned on the + * left right of the table. + * - When it is located on an color, this corresponds to the blue color. + */ +#define ELEMENT_RIGHT 4 +#define ELEMENT_SAFE 8 +#define ELEMENT_GREEN 16 +#define ELEMENT_INTERSEC 64 +/** Center of a square or central element. */ +#define ELEMENT_CENTER 128 + +struct element_t +{ + /** May have several types if unknown (pawn, king, queen). */ + uint8_t type; + /** Element position. */ + vect_t pos; + /** Emplacement attributes. */ + uint8_t attr; +}; +typedef struct element_t element_t; + +/* Elements range, see element table content. */ +#define ELEMENT_INTERSEC_START 0 +#define ELEMENT_INTERSEC_END 19 +#define ELEMENT_CENTRAL_PAWN 20 +#define ELEMENT_GREEN_START 21 +#define ELEMENT_GREEN_END 30 +#define ELEMENT_UNLOAD_START 31 +#define ELEMENT_UNLOAD_END 62 +#define ELEMENT_UNLOAD_SAFE_START 63 +#define ELEMENT_UNLOAD_SAFE_END 66 + +/** Elements on table. */ +extern struct element_t element_table[]; + +/** Initialize elements. */ +void +element_init (void); + +/** Gives the score of an element considering it as an unload zone. */ +int32_t +element_unload_score (position_t robot_pos, uint8_t element_id); + +/** Gives best unload. */ +uint8_t +element_unload_best (position_t robot_pos); + +/** Gives score of an element. */ +int32_t +element_score (position_t robot_pos, uint8_t element_id); + +/** Return a probability of an element to be here. + * This depends of the remaining time. + * Returns a probability from 0 (element may really not be here) + * to maximal value (element may be here). + */ +uint32_t +element_proba (uint8_t element_id); + +/** Return the best element to pick. */ +uint8_t +element_best (position_t robot_pos); + +/** Function to call when we see an element is not here. */ +void +element_not_here (uint8_t element_id); + +/** Call this function when an element is taken. */ +void +element_taken (uint8_t element_id, uint8_t element_type); + +/** Call this function when the robot put down an element. */ +void +element_down (uint8_t element_id, uint8_t element_type); + +/** Gives the nearest element from a position. */ +uint8_t +element_give_position (position_t pos); + +/** Give the opposed element. */ +uint8_t +element_opposed (uint8_t element_id); + +/** Return 1 if the element is in our side, 0 otherwise. */ +int +element_our_side (uint8_t element_id); + +inline void +element_intersec_symetric (uint8_t element_id, uint8_t element_type); + #endif /* element_h */ -- cgit v1.2.3 From 4b2e09a630c2242197d39920173b27d1968e4f85 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 28 May 2011 17:52:26 +0200 Subject: digital/io-hub: add function to get nearest element id and add test_element --- digital/io-hub/src/robospierre/element.c | 21 +- digital/io-hub/src/robospierre/element.h | 3 + digital/io-hub/src/robospierre/test_element.c | 336 ++++++++++++++++++++++++++ 3 files changed, 356 insertions(+), 4 deletions(-) create mode 100644 digital/io-hub/src/robospierre/test_element.c diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 341206b0..28c72e4d 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -662,11 +662,24 @@ element_down (uint8_t element_id, uint8_t element_type) } uint8_t -element_give_position (position_t pos) +element_nearest_element_id (position_t robot_pos) { - uint8_t e = 0xff; - - return e; + uint8_t i; + uint8_t id = 0; + int32_t distance = 4242; + element_t e; + for (i = 0; i < UTILS_COUNT (element_table); i++) + { + e = element_get (i); + vect_t v = e.pos; + int32_t dr = distance_point_point (&v, &robot_pos.v); + if (dr < distance) + { + id = i; + distance = dr; + } + } + return id; } uint8_t diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 95cf2b2c..fbe3bee5 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -157,4 +157,7 @@ element_our_side (uint8_t element_id); inline void element_intersec_symetric (uint8_t element_id, uint8_t element_type); +uint8_t +element_nearest_element_id (position_t robot_pos); + #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c new file mode 100644 index 00000000..30975751 --- /dev/null +++ b/digital/io-hub/src/robospierre/test_element.c @@ -0,0 +1,336 @@ +/* test_element.c */ +/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ + * + * Copyright (C) 2011 Jérôme Jutteau + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "element.h" + +#include "chrono.h" + +#include + +#define TEST_PRINT_TYPE_SCORE_PICK 0 +#define TEST_PRINT_TYPE_SCORE_UNLOAD 1 +#define TEST_PRINT_TYPE_ELEMENT 2 +int test_print_type_ = TEST_PRINT_TYPE_SCORE_PICK; +position_t test_robot_pos_ = {{0, 2100}, 1}; + +enum team_color_e team_color; + +/** Stubbed chrono. */ +uint32_t test_chrono_ms; + +void +chrono_init (void) +{ + test_chrono_ms = CHRONO_MATCH_DURATION_MS; +} + +uint32_t +chrono_remaining_time (void) +{ + return test_chrono_ms; +} + +uint8_t +test_element_convert (const char *l, uint8_t number) +{ + switch (l[0]) + { + case 'A': + switch (number) + { + case 1: return (31); + case 3: return (32); + case 5: return (33); + case 7: return (34); + case 9: return (35); + case 11: return (36); + } + case 'B': + switch (number) + { + case 2: return (0); + case 4: return (2); + case 8: return (3); + case 10: return (1); + } + case 'C': + switch (number) + { + case 1: return (37); + case 3: return (38); + case 5: return (39); + case 7: return (40); + case 9: return (41); + case 11: return (42); + } + case 'D': + switch (number) + { + case 0: return (21); + case 2: return (4); + case 4: return (6); + case 8: return (7); + case 10: return (5); + case 12: return (22); + } + case 'E': + switch (number) + { + case 1: return (43); + case 3: return (44); + case 5: return (45); + case 7: return (46); + case 9: return (47); + case 11: return (48); + } + case 'F': + switch (number) + { + case 0: return (23); + case 12: return (24); + } + case 'G': + switch (number) + { + case 2: return (8); + case 4: return (10); + case 6: return (20); + case 8: return (11); + case 10: return (9); + } + case 'H': + switch (number) + { + case 0: return (25); + case 1: return (49); + case 3: return (50); + case 5: return (51); + case 7: return (52); + case 9: return (53); + case 11: return (54); + case 12: return (26); + } + case 'I': + switch (number) /*12*/ + { + case 2: return (12); + case 4: return (14); + case 8: return (15); + case 10: return (13); + } + case 'J': + switch (number) + { + case 0: return (27); + case 12: return (28); + } + case 'K': + switch (number) + { + case 1: return (55); + case 3: return (56); + case 5: return (57); + case 7: return (58); + case 9: return (59); + case 11: return (60); + } + case 'L': + switch (number) + { + case 2: return (16); + case 4: return (18); + case 8: return (19); + case 10: return (17); + } + case 'M': + switch (number) + { + case 0: return (29); + case 1: return (63); + case 3: return (64); + case 5: return (61); + case 7: return (62); + case 9: return (65); + case 11: return (66); + case 12: return (30); + } + } + return 0; +} + +long int +test_element_get_score (uint8_t element_id) +{ + if (test_print_type_ == TEST_PRINT_TYPE_SCORE_PICK) + return (long int) element_score (test_robot_pos_, element_id); + else if (test_print_type_ == TEST_PRINT_TYPE_SCORE_UNLOAD) + return (long int) element_unload_score (test_robot_pos_, element_id); + else + return (long int) element_table[element_id].type; +} + +#define p(l, n) (test_element_print (#l, n)) +long int +test_element_print (const char *l, uint8_t number) +{ + return test_element_get_score (test_element_convert (l, number)); +} + +void +test_element_print_table () +{ + printf ("parameters:\n"); + printf ("- robot_pos = (%u, %u)\n", test_robot_pos_.v.x, test_robot_pos_.v.y); + printf ("- side = %u\n", team_color); + printf (" _____0_________1_____2____3____4____5____6____7____8____9___10___11_________12_____ \n"); + printf (" | | | | | | | | |\n"); + printf (" | | | | | | | | |\n"); + printf ("A| Red | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | Blue |\n", p (A, 1), p (A, 3), p (A, 5), p(A, 7), p (A, 9), p (A, 11)); + printf (" | | | | | | | | |\n"); + printf ("B| |--------%5ld-----%5ld------+--------%5ld-----%5ld------| |\n", p (B, 2), p (B, 4), p (B, 8), p (B, 10)); + printf (" |-----------+ | | | | | +-----------|\n"); + printf (" | | | [B] | | | [R] | | |\n"); + printf ("C| | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | |\n", p (C, 1), p (C, 3), p (C, 5), p (C, 7), p(C, 9), p (C, 11)); + printf (" | | | | | | | | |\n"); + printf ("D| %5ld |--------%5ld-----%5ld------+--------%5ld-----%5ld------| %5ld |\n", p (D, 0), p (D, 2), p (D, 4), p (D, 8), p (D, 10), p (D, 12)); + printf (" | | | | | | | | |\n"); + printf (" | | | | | | | | |\n"); + printf ("E| | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | |\n", p (E, 1), p (E, 3), p (E, 5), p (E, 7), p(E, 9), p(E, 11)); + printf ("F| %5ld | | | | | | | %5ld |\n", p (F, 0), p(F, 12)); + printf ("G| |--------%5ld-----%5ld-----%5ld-----%5ld-----%5ld------| |\n", p (G, 2), p (G, 4), p(G, 6), p (G, 8), p (G, 10)); + printf (" | | | | | | | | |\n"); + printf (" | | | [B] | | | [R] | | |\n"); + printf ("H| %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld |\n", p(H, 0), p (H, 1), p (H, 3), p (H, 5), p (H, 7), p(H, 9), p(H, 11), p(H, 12)); + printf (" | | | | | | | | |\n"); + printf ("I| |--------%5ld-----%5ld------+--------%5ld-----%5ld------| |\n", p (I, 2), p (I, 4), p (I, 8), p (I, 10)); + printf (" | | | | | | | | |\n"); + printf ("J| %5ld | | | | | | | %5ld |\n", p (J, 0), p(J, 12)); + printf ("K| | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | |\n", p (K, 1), p (K, 3), p (K, 5), p (K, 7), p(K, 9), p(K, 11)); + printf (" | | | | | | | | |\n"); + printf ("L| |--------%5ld-----%5ld------+--------%5ld-----%5ld------| |\n", p (L, 2), p (L, 4), p (L, 8), p (L, 10)); + printf (" | | | | | | | | |\n"); + printf ("M| %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld | %5ld |\n", p(M, 0), p (M, 1), p (M, 3), p (M, 5), p (M, 7), p(M, 9), p(M, 11), p(M, 12)); + printf (" | |###################| [R] | [B] |###################| |\n"); + printf (" | |###################| | |###################| |\n"); + printf (" +-----------|###################+---------+---------+###################+-----------+\n"); +} + +int main () +{ + int exit = 0; + char cmd; + int x; + char y[10], z[10]; + int type = 0; + chrono_init (); + + while (!exit) + { + element_init (); + printf ("\ncommands:\n"); + printf ("s: print scores, "); + printf ("p: print unload scores, "); + printf ("e: print elements, "); + printf ("t: set match time, "); + printf ("c: set robot side, "); + printf ("r: set robot position, "); + printf ("i: pick up an element, "); + printf ("n: indicate there is no element, "); + printf ("m: id of nearest element, "); + printf ("q: quit\n"); + printf ("your choice: "); + fflush (stdin); + scanf ("%c", &cmd); + switch (cmd) + { + case 's': + test_print_type_ = TEST_PRINT_TYPE_SCORE_PICK; + test_element_print_table (); + break; + case 'p': + test_print_type_ = TEST_PRINT_TYPE_SCORE_UNLOAD; + test_element_print_table (); + break; + case 'e': + test_print_type_ = TEST_PRINT_TYPE_ELEMENT; + test_element_print_table (); + break; + case 't': + printf ("set match time: "); + scanf ("%i", &test_chrono_ms); + break; + case 'c': + team_color ^= TEAM_COLOR_RIGHT; + printf ("Side is now %u\n", team_color); + break; + case 'r': + printf ("set robot position:\n"); + printf ("number: "); + scanf ("%i", &x); + printf ("letter: "); + scanf ("%s", y); + test_robot_pos_.v = element_table[test_element_convert (y, x)].pos; + break; + case 'i': + printf ("pick up an element:\n"); + printf ("number: "); + scanf ("%i", &x); + printf ("letter: "); + scanf ("%s", y); + printf ("type: (P=Pawn Q=Queen K=King) "); + fflush (stdin); + scanf ("%s", z); + if (z[0] == 'P') + type = ELEMENT_PAWN; + else if (z[0] == 'Q') + type = ELEMENT_QUEEN; + else if (z[0] == 'K') + type = ELEMENT_KING; + else + printf ("Unknown !\n"); + element_taken (test_element_convert (y, x), type); + break; + case 'n': + printf ("indicate there no element\n"); + printf ("number: "); + scanf ("%i", &x); + printf ("letter: "); + scanf ("%s", y); + element_not_here (test_element_convert (y, x)); + break; + case 'm': + printf ("nearest element id from robot: %u\n", + element_nearest_element_id (test_robot_pos_)); + break; + case 'q': + exit = 1; + break; + } + fflush (stdin); + scanf ("%c", &cmd); + } + return 0; +} + -- cgit v1.2.3 From 4515445795af425fa91920a28ec3f3108f0fa83e Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 28 May 2011 18:36:26 +0200 Subject: digital/io-hub: add function to get position from element id This is useful to indicate an angle when we want to approach an element the green zone. --- digital/io-hub/src/robospierre/element.c | 23 +++++++++++++++++++++++ digital/io-hub/src/robospierre/element.h | 9 +++++++++ digital/io-hub/src/robospierre/test_element.c | 7 +++++++ 3 files changed, 39 insertions(+) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 28c72e4d..ebc5874a 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -696,3 +696,26 @@ element_opposed (uint8_t element_id) } return op; } + +position_t +element_get_pos (uint8_t element_id) +{ + element_t e = element_get (element_id); + position_t pos; + pos.v = e.pos; + pos.a = 0; + if (e.attr == (ELEMENT_GREEN |ELEMENT_RIGHT)) + { + /* Set angle to 90° clockwise. */ + pos.a = 0x4000; + /* Remove 400 mm. */ + pos.v.x -= 400; + } + if (e.attr == (ELEMENT_GREEN |ELEMENT_LEFT)) + { + /* Set angle to 270° clockwise. */ + pos.a = 0xc000; + pos.v.x += 400; + } + return pos; +} diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index fbe3bee5..4655c3c1 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -160,4 +160,13 @@ element_intersec_symetric (uint8_t element_id, uint8_t element_type); uint8_t element_nearest_element_id (position_t robot_pos); +/** + Give the position where the robot need to be placed to get an element. + The position correspond to the emplacement of the element with an angle of + zero. If the element is in the green zone, the returned position include + the angle the robot need to set before moving to the element. + */ +position_t +element_get_pos (uint8_t element_id); + #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index 30975751..2de92361 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -259,6 +259,7 @@ int main () printf ("i: pick up an element, "); printf ("n: indicate there is no element, "); printf ("m: id of nearest element, "); + printf ("g: get position and angle of desired element,"); printf ("q: quit\n"); printf ("your choice: "); fflush (stdin); @@ -324,6 +325,12 @@ int main () printf ("nearest element id from robot: %u\n", element_nearest_element_id (test_robot_pos_)); break; + case 'g': + printf ("element id: "); + scanf ("%i", &x); + position_t pos = element_get_pos (x); + printf ("x: %u y: %u a: %u\n", pos.v.x, pos.v.y, pos.a); + break; case 'q': exit = 1; break; -- cgit v1.2.3 From fe08673321eff8634e42c4225ec69c0af535dfaa Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Sat, 28 May 2011 19:49:36 +0200 Subject: digital/io: fixed how to deduce elements on intersections --- digital/io-hub/src/robospierre/element.c | 59 ++++++++++++++++++-------------- 1 file changed, 33 insertions(+), 26 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index ebc5874a..0022f09a 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -475,7 +475,7 @@ element_intersec_symetric (uint8_t element_id, uint8_t element_type) uint8_t i; uint8_t type = 0; - if (element_id % 4 == 0 || element_id % 4 == 1) + if (element_id % 4 <= 1) col = 0; else col = 1; @@ -487,9 +487,9 @@ element_intersec_symetric (uint8_t element_id, uint8_t element_type) /* Count. */ for (i = 0; i < 5; i++) { - if (element_columns[col][i] == ELEMENT_PAWN ||element_type == ELEMENT_PAWN) + if (element_columns[col][i] == ELEMENT_PAWN || element_type == ELEMENT_PAWN) cpt_pawn++; - if (element_columns[col][i] == ELEMENT_NONE ||element_type == ELEMENT_NONE) + if (element_columns[col][i] == ELEMENT_NONE || element_type == ELEMENT_NONE) cpt_none++; if (element_columns[col][i] == (ELEMENT_NONE | ELEMENT_PAWN)) { @@ -501,33 +501,40 @@ element_intersec_symetric (uint8_t element_id, uint8_t element_type) /* With which element are we going to fill the rest ? */ if (cpt_pawn == 2) type = ELEMENT_NONE; - if (cpt_none == 3) + else if (cpt_none == 3) type = ELEMENT_PAWN; + else + return; - if (type) - for (i = 0; i < 5; i++) - if (element_columns[col][i] == (ELEMENT_NONE | ELEMENT_PAWN)) - element_columns[col][i] = type; + for (i = 0; i < 5; i++) + if (element_columns[col][i] == (ELEMENT_NONE | ELEMENT_PAWN)) + element_columns[col][i] = type; /* Complete if possible. */ - if (type != 0) - for (i = ELEMENT_INTERSEC_START; i < ELEMENT_INTERSEC_END; i++) - if (i % 4 == element_id % 4) + for (i = ELEMENT_INTERSEC_START; i < ELEMENT_INTERSEC_END; i++) + { + uint8_t col_i; + if (i % 4 <= 1) + col_i = 0; + else + col_i = 1; + if (col_i == col) + { + element_t el = element_get (i); + element_t sym = element_get (element_opposed (i)); + if (el.type == (ELEMENT_NONE | ELEMENT_PAWN)) { - element_t el = element_get (i); - element_t sym = element_get (element_opposed (i)); - if (el.type == (ELEMENT_NONE | ELEMENT_PAWN)) + el.type = type; + element_set (i, el); + /* Set opposed. */ + if (sym.type == (ELEMENT_NONE | ELEMENT_PAWN)) { - el.type = type; - element_set (i, el); - /* Set opposed. */ - if (sym.type == (ELEMENT_NONE | ELEMENT_PAWN)) - { - sym.type = type; - element_set (element_opposed (i), sym); - } + sym.type = type; + element_set (element_opposed (i), sym); } } + } + } } } @@ -669,16 +676,16 @@ element_nearest_element_id (position_t robot_pos) int32_t distance = 4242; element_t e; for (i = 0; i < UTILS_COUNT (element_table); i++) - { + { e = element_get (i); - vect_t v = e.pos; + vect_t v = e.pos; int32_t dr = distance_point_point (&v, &robot_pos.v); if (dr < distance) { id = i; distance = dr; } - } + } return id; } @@ -715,7 +722,7 @@ element_get_pos (uint8_t element_id) { /* Set angle to 270° clockwise. */ pos.a = 0xc000; - pos.v.x += 400; + pos.v.x += 400; } return pos; } -- cgit v1.2.3 From 4f55b190e1ea314d8ce9e56ecb42bacd26ed80ed Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 25 May 2011 01:13:10 +0200 Subject: digital/{ai,io-hub}: move events --- digital/ai/src/fsm/init.c | 3 --- digital/io-hub/src/robospierre/move.c | 4 ++++ 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/digital/ai/src/fsm/init.c b/digital/ai/src/fsm/init.c index 1f933ce1..51ba31ab 100644 --- a/digital/ai/src/fsm/init.c +++ b/digital/ai/src/fsm/init.c @@ -73,9 +73,6 @@ FSM_STATES ( INIT_FINISHED) FSM_EVENTS ( - /* XXX: temporarily here. */ - robot_move_success, - robot_move_failure, /* Jack is inserted. */ jack_inserted, /* Jack is removed. */ diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index 18759c14..7ce9b3c8 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -137,6 +137,10 @@ FSM_STATES ( MOVE_WAIT_FOR_CLEAR_PATH) FSM_EVENTS ( + /* Report from asserv after a successful move command. */ + robot_move_success, + /* Report from asserv after a failed move command. */ + robot_move_failure, /* Initialize the FSM and start the movement directly. */ move_start, /* Movement success. */ -- cgit v1.2.3 From 22d384a1c0fc9464c25db92550210c4d633c97e1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 00:21:28 +0200 Subject: digital/ai/src/fsm: fix timeout assignment --- digital/ai/src/fsm/fsm.host.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c index ad791272..ed93221d 100644 --- a/digital/ai/src/fsm/fsm.host.c +++ b/digital/ai/src/fsm/fsm.host.c @@ -1356,7 +1356,9 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fprintf (f, "\t\t\thandled = 1;\n"); if (fsm->timeouts != NULL) { - fprintf (f, "\t\t\tfsm_%s_timeout_counters[i] = fsm_%s_timeout_values[e];\n", + fprintf (f, "\t\t\tfsm_%s_timeout_counters[i] = " + "fsm_%s_timeout_values[fsm_%s_active_states[i]];\n", + fsm->name, fsm->name, fsm->name); } -- cgit v1.2.3 From a186eaba13baab4cebeaeb8f2701ed00197764f1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 01:00:23 +0200 Subject: digital/io-hub: fix hardware offset when going to front --- digital/io-hub/src/robospierre/clamp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 9dfb01f9..0c01e468 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -271,10 +271,10 @@ clamp_handle_event (void) /* Handle special hardware offset. */ uint16_t rotation_position = mimot_get_motor1_position (); if ((ctx.pos_current == CLAMP_BAY_FRONT_LEAVING - && rotation_position > (BOT_CLAMP_BAY_SIDE_ROTATION_STEP - + && rotation_position > (BOT_CLAMP_BAY_SIDE_ROTATION_STEP + BOT_CLAMP_BAY_FRONT_ROTATION_STEP) / 2) || (ctx.pos_current == CLAMP_BAY_BACK_LEAVING - && rotation_position < (BOT_CLAMP_BAY_BACK_ROTATION_STEP - + && rotation_position < (BOT_CLAMP_BAY_BACK_ROTATION_STEP + BOT_CLAMP_BAY_SIDE_ROTATION_STEP) / 2)) { /* Go directly to next point. */ -- cgit v1.2.3 From b5d74afb5302b41d0ebc44d0c77db348016154ad Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 07:26:51 +0200 Subject: digital/io-hub: open clamp more slowly There is an hard point when opening the door, this is hopefully a temporary workaround. --- digital/io-hub/src/robospierre/bot.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 05c2b436..1071bf7e 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -120,8 +120,8 @@ #define BOT_PWM_DOOR_BACK_BOTTOM 3 #define BOT_PWM_DOOR_BACK_TOP 4 -#define BOT_PWM_CLAMP_OPEN_TIME 75 -#define BOT_PWM_CLAMP_OPEN 0x3ff, 75, 0 +#define BOT_PWM_CLAMP_OPEN_TIME 125 +#define BOT_PWM_CLAMP_OPEN 0x1ff, 125, 0 #define BOT_PWM_CLAMP_CLOSE_TIME 75 #define BOT_PWM_CLAMP_CLOSE -0x3ff, 75, 0 -- cgit v1.2.3 From 9b1ce3a050d9169f3bf77764063de0cff800190e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 07:58:36 +0200 Subject: digital/{ai,io-hub,mimot}, host/simu: change robospierre rotation motor --- digital/ai/tools/robospierre.py | 2 +- digital/ai/tools/test_simu_control_robospierre.py | 2 +- digital/io-hub/src/robospierre/bot.h | 18 ++++++------- digital/mimot/src/dirty/models.host.c | 32 +++++++++++++++++++++++ digital/mimot/tools/mimot/init.py | 2 +- host/simu/robots/robospierre/model/clamp.py | 2 +- 6 files changed, 45 insertions(+), 13 deletions(-) diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index e8384f6c..6a44883e 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -25,7 +25,7 @@ class Robot: self.robot_view = simu.robots.robospierre.view.bag asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'robospierre') - mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') + mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'robospierre') io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, **asserv.init.host['robospierre']) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 0d52219e..1795e17f 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -30,7 +30,7 @@ class TestSimuControl (TestSimu): ELEVATION_STROKE = 0x3b0b - ROTATION_STROKE = 0x11c6 + ROTATION_STROKE = 0x233e def __init__ (self, robot_class): TestSimu.__init__ (self, robot_class) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 1071bf7e..65f6adf6 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -67,9 +67,9 @@ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 -# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x11c6 -# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x11c6 -# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x11c6 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x233e # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP @@ -96,23 +96,23 @@ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 -# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x10ce -# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP ((0x10ce + 0x10e2) / 2) -# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x10e2 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x233e # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP -# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x816 +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x10de #define BOT_CLAMP_CLOSED_ROTATION_OFFSET -61 #endif /* !HOST */ #define BOT_CLAMP_ELEVATION_SPEED 0x60 -#define BOT_CLAMP_ROTATION_SPEED 0x30 -#define BOT_CLAMP_ROTATION_OFFSET_SPEED 2 +#define BOT_CLAMP_ROTATION_SPEED 0x60 +#define BOT_CLAMP_ROTATION_OFFSET_SPEED 1 #define BOT_PWM_CLAMP 2 #define BOT_PWM_DOOR_FRONT_BOTTOM 0 diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c index a835298f..b1141748 100644 --- a/digital/mimot/src/dirty/models.host.c +++ b/digital/mimot/src/dirty/models.host.c @@ -52,6 +52,26 @@ static const struct motor_def_t marcel_clamp_f2342_model = -INFINITY, +INFINITY, }; +/* Robospierre rotation motor, AMAX32GHP with 1:16 gearbox model. */ +static const struct motor_def_t robospierre_rotation_amax32ghp_model = +{ + /* Motor characteristics. */ + 269 * (2*M_PI) / 60,/* Speed constant ((rad/s)/V). */ + 25.44 / 1000, /* Torque constant (N.m/A). */ + 0, /* Bearing friction (N.m/(rad/s)). */ + 3.99, /* Terminal resistance (Ohm). */ + 0.24 / 1000, /* Terminal inductance (H). */ + 24.0, /* Maximum voltage (V). */ + /* Gearbox characteristics. */ + 16, /* Gearbox ratio. */ + 0.75, /* Gearbox efficiency. */ + /* Load characteristics. */ + 0.200 * 0.010 * 0.010, /* Load (kg.m^2). */ + /* This is a pifometric estimation. */ + /* Hardware limits. */ + -INFINITY, +INFINITY, +}; + /* Marcel, APBTeam 2010. */ static const struct robot_t marcel_robot = { @@ -63,6 +83,17 @@ static const struct robot_t marcel_robot = simu_sensor_update_marcel, }; +/* Robospierre, APBTeam 2011. */ +static const struct robot_t robospierre_robot = +{ + /** Auxiliary motors, NULL if not present. */ + { &marcel_clamp_f2342_model, &robospierre_rotation_amax32ghp_model }, + /** Number of steps for each auxiliary motor encoder. */ + { 256, 250 }, + /** Sensor update function. */ + NULL, +}; + /* Table of models. */ static const struct { @@ -70,6 +101,7 @@ static const struct const struct robot_t *robot; } models[] = { { "marcel", &marcel_robot }, + { "robospierre", &robospierre_robot }, { 0, 0 } }; diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index a05f8c44..d302e060 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -17,7 +17,7 @@ target_robospierre = dict ( a1a = 0.5, a1sm = 0x30, a1ss = 0x08, a1be = 256, a1bs = 0x18, a1bc = 5, E = 0x3ff, D = 0x1ff, - w = 0x01, + w = 0x03, ) target = { 'marcel': target_marcel, diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index fd3dde7c..c36d0d36 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -45,7 +45,7 @@ class Clamp (Observable): ELEVATION_MOTOR_STROKE = 120.0 * 5.0 / 6.0 ROTATION_STROKE = pi - ROTATION_MOTOR_STROKE = pi * 115.0 / 12.0 + ROTATION_MOTOR_STROKE = 2 * pi * 36.088 / 16 CLAMPING_STROKE = 10 CLAMPING_MOTOR_STROKE = pi -- cgit v1.2.3 From 9d61a51ee604fb1f9863d00ff4348c939ce2eeb5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 07:59:28 +0200 Subject: digital/io-hub, digital/mimot: change robospierre elevation motor parameters --- digital/io-hub/src/robospierre/bot.h | 2 +- digital/mimot/tools/mimot/init.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 65f6adf6..6079b9d8 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -88,7 +88,7 @@ # define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0x0169 # define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x1f03 - 250) # define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3610 -# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3729 +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3535 # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index d302e060..6a4889d4 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -10,7 +10,7 @@ target_marcel = dict ( w = 0x03, ) target_robospierre = dict ( - a0kp = 4, + a0kp = 8, a0kd = 1, a0a = 8, a0sm = 0x60, a0ss = 0x10, a0be = 256, a0bs = 0x18, a0bc = 5, a1kp = 4, -- cgit v1.2.3 From fd8ce8373abf81f5d6781b577a0f50906f2f4ff8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 27 May 2011 08:15:52 +0200 Subject: digital/io-hub: add offset to back bay too --- digital/io-hub/src/robospierre/bot.h | 12 ++++++++++-- digital/io-hub/src/robospierre/clamp.c | 10 ++++++---- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 6079b9d8..02b74da9 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -78,7 +78,8 @@ # define BOT_CLAMP_BAY_SIDE_ROTATION_STEP \ (BOT_CLAMP_BAY_BACK_ROTATION_STEP / 2) -#define BOT_CLAMP_CLOSED_ROTATION_OFFSET 0 +#define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET 0 +#define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET 0 #else /* !HOST */ @@ -106,10 +107,17 @@ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x10de -#define BOT_CLAMP_CLOSED_ROTATION_OFFSET -61 +#define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET -129 +#define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET -60 #endif /* !HOST */ +#define BOT_CLAMP_CLOSED_ROTATION_OFFSET(pos) \ + (CLAMP_IS_SLOT_IN_FRONT_BAY (pos) \ + ? BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET \ + : (CLAMP_IS_SLOT_IN_BACK_BAY (pos) \ + ? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET : 0)) + #define BOT_CLAMP_ELEVATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_OFFSET_SPEED 1 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 0c01e468..71cb2dfb 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -291,9 +291,11 @@ clamp_openclose (uint8_t open) pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); else pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); - if (ctx.controled && CLAMP_IS_SLOT_IN_FRONT_BAY (ctx.pos_current)) + if (ctx.controled && (CLAMP_IS_SLOT_IN_FRONT_BAY (ctx.pos_current) + || CLAMP_IS_SLOT_IN_BACK_BAY (ctx.pos_current))) { - int16_t offset = open ? 0 : BOT_CLAMP_CLOSED_ROTATION_OFFSET; + int16_t offset = open ? 0 + : BOT_CLAMP_CLOSED_ROTATION_OFFSET (ctx.pos_current); mimot_move_motor1_absolute (clamp_pos[ctx.pos_current][1] + offset, BOT_CLAMP_ROTATION_OFFSET_SPEED); } @@ -361,8 +363,8 @@ clamp_route (void) /* Run motors. */ mimot_move_motor0_absolute (clamp_pos[pos_new][0], BOT_CLAMP_ELEVATION_SPEED); - int16_t offset = !ctx.open && CLAMP_IS_SLOT_IN_FRONT_BAY (pos_new) - ? BOT_CLAMP_CLOSED_ROTATION_OFFSET : 0; + int16_t offset = ctx.open ? 0 + : BOT_CLAMP_CLOSED_ROTATION_OFFSET (pos_new); mimot_move_motor1_absolute (clamp_pos[pos_new][1] + offset, BOT_CLAMP_ROTATION_SPEED); ctx.controled = 1; -- cgit v1.2.3 From 4882fbefaeca9cc5292cec9a2f93d34f04e67b5c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 28 May 2011 00:31:13 +0200 Subject: digital/io-hub: add clamp initialisation --- digital/ai/src/twi_master/mimot.c | 42 +++++++++++++ digital/ai/src/twi_master/mimot.h | 16 +++++ digital/io-hub/src/robospierre/bot.h | 14 +++++ digital/io-hub/src/robospierre/clamp.c | 92 ++++++++++++++++++++++++++++- digital/io-hub/src/robospierre/main.c | 4 ++ digital/mimot/src/dirty/twi_proto.c | 21 +++++++ host/simu/robots/robospierre/model/clamp.py | 2 + 7 files changed, 188 insertions(+), 3 deletions(-) diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index af6ff5f9..c3b82823 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -117,6 +117,30 @@ mimot_reset (void) twi_master_send_buffer (1); } +void +mimot_set_motor0_position (uint16_t position) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'p'; + buffer[1] = 'Y'; + buffer[2] = 0; + buffer[3] = v16_to_v8 (position, 1); + buffer[4] = v16_to_v8 (position, 0); + twi_master_send_buffer (5); +} + +void +mimot_set_motor1_position (uint16_t position) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'p'; + buffer[1] = 'Y'; + buffer[2] = 1; + buffer[3] = v16_to_v8 (position, 1); + buffer[4] = v16_to_v8 (position, 0); + twi_master_send_buffer (5); +} + void mimot_move_motor0_absolute (uint16_t position, uint8_t speed) { @@ -181,3 +205,21 @@ mimot_motor1_clamp (int8_t speed, int16_t pwm) twi_master_send_buffer (5); } +void +mimot_motor0_free (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'w'; + buffer[1] = 0; + twi_master_send_buffer (2); +} + +void +mimot_motor1_free (void) +{ + uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); + buffer[0] = 'w'; + buffer[1] = 1; + twi_master_send_buffer (2); +} + diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index 60811c10..97575b0d 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -68,6 +68,14 @@ mimot_get_motor1_position (void); void mimot_reset (void); +/** Set motor0 position in steps. */ +void +mimot_set_motor0_position (uint16_t position); + +/** Set motor1 position in steps. */ +void +mimot_set_motor1_position (uint16_t position); + /** Move motor0 to absolute position in steps. */ void mimot_move_motor0_absolute (uint16_t position, uint8_t speed); @@ -92,4 +100,12 @@ mimot_motor0_clamp (int8_t speed, int16_t pwm); void mimot_motor1_clamp (int8_t speed, int16_t pwm); +/** Free motor0. */ +void +mimot_motor0_free (void); + +/** Free motor1. */ +void +mimot_motor1_free (void); + #endif /* mimot_h */ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 02b74da9..7a8065dd 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -63,6 +63,8 @@ # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) +# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP 0x18ca +# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP 0x18ca # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 @@ -93,6 +95,9 @@ # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) +// TODO: to be measured. +# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP (0x1da7 / 2) +# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP (0x1f03 / 2) # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 @@ -112,12 +117,17 @@ #endif /* !HOST */ +#define BOT_CLAMP_INIT_ELEVATION_STEP \ + BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP + #define BOT_CLAMP_CLOSED_ROTATION_OFFSET(pos) \ (CLAMP_IS_SLOT_IN_FRONT_BAY (pos) \ ? BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET \ : (CLAMP_IS_SLOT_IN_BACK_BAY (pos) \ ? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET : 0)) +#define BOT_CLAMP_INIT_ELEVATION_SPEED 0x08 +#define BOT_CLAMP_INIT_ROTATION_SPEED -0x04 #define BOT_CLAMP_ELEVATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_OFFSET_SPEED 1 @@ -140,4 +150,8 @@ -0x3ff, 50, ((slot == CLAMP_SLOT_FRONT_BOTTOM \ || slot == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180) +#define BOT_PWM_CLAMP_INIT 0x1ff, 125, 0 +#define BOT_PWM_DOOR_INIT 0x1ff, 74, 0x55 +#define BOT_PWM_CLAMP_DOOR_INIT 125 + #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 71cb2dfb..65a90a7a 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -35,6 +35,8 @@ #include "fsm.h" #include "fsm_queue.h" +#include "modules/proto/proto.h" + #include "logistic.h" /* @@ -50,8 +52,18 @@ FSM_INIT FSM_STATES ( - /* Initial state, to be complete with full initialisation. */ + /* Initial state. */ CLAMP_START, + /* Initialisation sequence: opening everything. */ + CLAMP_INIT_OPENING, + /* Initialisation sequence: going up until a middle pawn sensor + * see the clamp. */ + CLAMP_INIT_UPING_UNTIL_SEEN, + /* Initialisation sequence: going to the middle level. */ + CLAMP_INIT_GOING_MIDDLE, + /* Initialisation sequence: finding front right edge. */ + CLAMP_INIT_FINDING_EDGE, + /* Returning to idle position. */ CLAMP_GOING_IDLE, /* Waiting external events, clamp at middle level. */ @@ -85,6 +97,8 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( + /* During initialisation sequence, clamp seen by a sensor. */ + clamp_init_seen, /* New element inside bottom slot. */ clamp_new_element, /* Order to drop elements. */ @@ -100,8 +114,12 @@ FSM_EVENTS ( clamp_move_success, /* Elevation and elevation motor success. */ clamp_elevation_rotation_success, + /* Elevation motor success. */ + clamp_elevation_success, /* Elevation motor failure. */ clamp_elevation_failure, + /* Rotation motor success. */ + clamp_rotation_success, /* Rotation motor failure. */ clamp_rotation_failure) @@ -127,6 +145,10 @@ struct clamp_t uint8_t open; /** True if clamp position is controled. */ uint8_t controled; + /** Position of clamp when seen by sensor. */ + uint16_t init_seen_step; + /** Position to initialise at seen position. */ + uint16_t init_seen_init_step; }; /** Global context. */ @@ -280,6 +302,24 @@ clamp_handle_event (void) /* Go directly to next point. */ clamp_route (); } + /* Handle initialisation. */ + if (FSM_CAN_HANDLE (AI, clamp_init_seen)) + { + if (!IO_GET (CONTACT_FRONT_MIDDLE)) + { + ctx.init_seen_step = mimot_get_motor0_position (); + ctx.init_seen_init_step = BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP; + FSM_HANDLE (AI, clamp_init_seen); + return 1; + } + if (!IO_GET (CONTACT_BACK_MIDDLE)) + { + ctx.init_seen_step = mimot_get_motor0_position (); + ctx.init_seen_init_step = BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP; + FSM_HANDLE (AI, clamp_init_seen); + return 1; + } + } return 0; } @@ -374,12 +414,58 @@ clamp_route (void) /* CLAMP FSM */ -FSM_TRANS (CLAMP_START, init_actuators, CLAMP_GOING_IDLE) +FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING) { - clamp_move (CLAMP_SLOT_FRONT_MIDDLE); + pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT); + pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_INIT); return FSM_NEXT (CLAMP_START, init_actuators); } +FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT, + CLAMP_INIT_UPING_UNTIL_SEEN) +{ + mimot_move_motor0_absolute (mimot_get_motor0_position () + + BOT_CLAMP_INIT_ELEVATION_STEP, + BOT_CLAMP_INIT_ELEVATION_SPEED); + return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPENING); +} + +FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success, CLAMP_START) +{ + /* Dead end. */ + mimot_motor0_free (); + return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success); +} + +FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen, + CLAMP_INIT_GOING_MIDDLE) +{ + mimot_motor0_free (); + mimot_set_motor0_position (ctx.init_seen_init_step + + mimot_get_motor0_position () + - ctx.init_seen_step); + proto_send1w ('C', ctx.init_seen_step); + mimot_move_motor0_absolute (BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, + BOT_CLAMP_ELEVATION_SPEED); + return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen); +} + +FSM_TRANS (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success, + CLAMP_INIT_FINDING_EDGE) +{ + mimot_motor1_zero_position (BOT_CLAMP_INIT_ROTATION_SPEED); + return FSM_NEXT (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success); +} + +FSM_TRANS (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success, CLAMP_GOING_IDLE) +{ + clamp_move (CLAMP_SLOT_FRONT_MIDDLE); + return FSM_NEXT (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success); +} + FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) { return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index b1e85b72..7c773761 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -139,8 +139,12 @@ main_event_to_fsm (void) if (mimot_motor0_status == success && mimot_motor1_status == success) FSM_HANDLE_E (AI, clamp_elevation_rotation_success); + if (mimot_motor0_status == success) + FSM_HANDLE_E (AI, clamp_elevation_success); else if (mimot_motor0_status == failure) FSM_HANDLE_E (AI, clamp_elevation_failure); + if (mimot_motor1_status == success) + FSM_HANDLE_E (AI, clamp_rotation_success); else if (mimot_motor1_status == failure) FSM_HANDLE_E (AI, clamp_rotation_failure); /* Clamp specific events. */ diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c index 3048c594..bda255ef 100644 --- a/digital/mimot/src/dirty/twi_proto.c +++ b/digital/mimot/src/dirty/twi_proto.c @@ -150,6 +150,18 @@ twi_proto_callback (u8 *buf, u8 size) else buf[0] = 0; break; + case c ('w', 1): + /* Free motor. + * - b: aux index. */ + if (buf[2] < AC_ASSERV_AUX_NB) + { + pos_reset (&pos_aux[buf[2]]); + state_aux[buf[2]].mode = MODE_PWM; + pwm_set (&pwm_aux[buf[2]], 0); + } + else + buf[0] = 0; + break; case c ('p', x): /* Set parameters. */ if (twi_proto_params (&buf[2], size - 2) != 0) @@ -173,6 +185,15 @@ twi_proto_params (u8 *buf, u8 size) size--; switch (*buf++) { + case 'Y': + /* Set current aux position. + * - b: aux index. + * - w: position. */ + if (buf[0] >= AC_ASSERV_AUX_NB || size < 3) + return 1; + aux[buf[0]].pos = v8_to_v16 (buf[1], buf[2]); + eat = 3; + break; default: return 1; } diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index c36d0d36..a3476a46 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -69,6 +69,8 @@ class Clamp (Observable): self.elevation_motor = elevation_motor self.rotation_motor = rotation_motor self.clamping_motor = clamping_motor + self.rotation_motor.limits.min = 0 + self.rotation_motor.limits.notify () self.door_motors = (door_motors[0], None, door_motors[1], door_motors[2], None, door_motors[3], None) self.slots = ( -- cgit v1.2.3 From 4c89145944e6f3bdedf5a85011e9b3087e51414b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 28 May 2011 00:48:25 +0200 Subject: digital/avr/make: add size report for AVR --- digital/avr/make/Makefile.avr | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/digital/avr/make/Makefile.avr b/digital/avr/make/Makefile.avr index bffca25a..1fe5ff31 100644 --- a/digital/avr/make/Makefile.avr +++ b/digital/avr/make/Makefile.avr @@ -11,6 +11,7 @@ AVR_LDLIBS := $(LDLIBS) $(AVR_LIBS) AVR_CC := avr-gcc AVR_OBJCOPY := avr-objcopy AVR_OBJDUMP := avr-objdump +AVR_SIZE := avr-size --mcu=$(AVR_MCU) -C AVR_COMPILE.c = $(AVR_CC) $(AVR_CFLAGS) $(AVR_CPPFLAGS) -c ifdef L AVR_COMPILE.c += -Wa,-adhlns=$(@:%.avr.o=%.c.avr.lst) @@ -20,7 +21,7 @@ AVR_LINK.o := $(AVR_CC) $(AVR_CFLAGS) $(AVR_LDFLAGS) # Main rules. -avr: elf lst hex +avr: elf lst hex size simu: simuelf @@ -90,6 +91,7 @@ text: hex hex: $(AVR_PROGS:%=%.hex) bin: $(AVR_PROGS:%=%.bin) srec: $(AVR_PROGS:%=%.srec) +size: $(AVR_PROGS:%=%.size) %.hex: %.avr.elf $(AVR_OBJCOPY) -j .text -j .data -O ihex $< $@ @@ -100,6 +102,9 @@ srec: $(AVR_PROGS:%=%.srec) %.bin: %.avr.elf $(AVR_OBJCOPY) -j .text -j .data -O binary $< $@ +%.size: %.avr.elf + $(AVR_SIZE) $< | grep Full + # Rules for building the .eeprom rom images. EEPROMS := $(AVR_PROGS:%=%_eeprom.hex) $(AVR_PROGS:%=%_eeprom.bin) \ -- cgit v1.2.3 From 7ddda7dc89ec24984c6569e54004654eb3aa9b79 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 28 May 2011 12:34:37 +0200 Subject: digital/mimot: new blocking parameters --- digital/mimot/tools/mimot/init.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index 6a4889d4..5cdab647 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -12,10 +12,10 @@ target_marcel = dict ( target_robospierre = dict ( a0kp = 8, a0kd = 1, a0a = 8, a0sm = 0x60, a0ss = 0x10, - a0be = 256, a0bs = 0x18, a0bc = 5, + a0be = 32, a0bs = 0x08, a0bc = 125, a1kp = 4, a1a = 0.5, a1sm = 0x30, a1ss = 0x08, - a1be = 256, a1bs = 0x18, a1bc = 5, + a1be = 64, a1bs = 0x08, a1bc = 5, E = 0x3ff, D = 0x1ff, w = 0x03, ) -- cgit v1.2.3 From 498d62b1e3f505379b102949dc13fc45b919570d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 28 May 2011 12:43:17 +0200 Subject: digital/asserv: new blocking parameters --- digital/asserv/tools/asserv/init.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py index f0bfced0..db33364d 100644 --- a/digital/asserv/tools/asserv/init.py +++ b/digital/asserv/tools/asserv/init.py @@ -39,8 +39,10 @@ target_robospierre = dict ( c = float (0x01000000) / (1 << 24), tkp = 1, tkd = 16, ta = 0.75, tsm = 0x60, tss = 0x10, + tbe = 256, tbs = 0x08, tbc = 40, akp = 2, akd = 16, aa = 0.5, asm = 0x60, ass = 0x10, + abe = 128, abs = 0x08, abc = 40, E = 0x3ff, D = 0x1ff, l = 0x1000, w = 0x00, -- cgit v1.2.3 From 4042e67d5aed51502538b96c959d88fd3cc1779a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 29 May 2011 09:48:09 +0200 Subject: digital/io-hub: new clamp parameter, with offset on all positions --- digital/io-hub/src/robospierre/bot.h | 15 +++++++++------ digital/io-hub/src/robospierre/clamp.c | 3 +-- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 7a8065dd..1ba432a8 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -82,6 +82,7 @@ #define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET 0 #define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET 0 +#define BOT_CLAMP_CLOSED_SIDE_ROTATION_OFFSET 0 #else /* !HOST */ @@ -91,7 +92,7 @@ # define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0x0169 # define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x1f03 - 250) # define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3610 -# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3535 +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3596 # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) @@ -102,18 +103,19 @@ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 -# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x233e -# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x233e -# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x233e +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x23d7 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x23d7 +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x23d7 # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP -# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP 0x10de +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x1127 + 120) #define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET -129 #define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET -60 +#define BOT_CLAMP_CLOSED_SIDE_ROTATION_OFFSET -120 #endif /* !HOST */ @@ -124,7 +126,8 @@ (CLAMP_IS_SLOT_IN_FRONT_BAY (pos) \ ? BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET \ : (CLAMP_IS_SLOT_IN_BACK_BAY (pos) \ - ? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET : 0)) + ? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET \ + : BOT_CLAMP_CLOSED_SIDE_ROTATION_OFFSET)) #define BOT_CLAMP_INIT_ELEVATION_SPEED 0x08 #define BOT_CLAMP_INIT_ROTATION_SPEED -0x04 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 65a90a7a..f799e7d7 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -331,8 +331,7 @@ clamp_openclose (uint8_t open) pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_OPEN); else pwm_set_timed (BOT_PWM_CLAMP, BOT_PWM_CLAMP_CLOSE); - if (ctx.controled && (CLAMP_IS_SLOT_IN_FRONT_BAY (ctx.pos_current) - || CLAMP_IS_SLOT_IN_BACK_BAY (ctx.pos_current))) + if (ctx.controled) { int16_t offset = open ? 0 : BOT_CLAMP_CLOSED_ROTATION_OFFSET (ctx.pos_current); -- cgit v1.2.3 From 21deab476ce421f04d367959110afbd85656bdde Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 29 May 2011 09:51:30 +0200 Subject: digital/io-hub: softer clamp & door control --- digital/io-hub/src/robospierre/bot.h | 28 ++++++++++++++++------------ digital/io-hub/src/robospierre/clamp.c | 13 +++++++------ digital/io-hub/src/robospierre/main.c | 12 ++++++++---- 3 files changed, 31 insertions(+), 22 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 1ba432a8..bd7cc181 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -141,20 +141,24 @@ #define BOT_PWM_DOOR_BACK_BOTTOM 3 #define BOT_PWM_DOOR_BACK_TOP 4 -#define BOT_PWM_CLAMP_OPEN_TIME 125 -#define BOT_PWM_CLAMP_OPEN 0x1ff, 125, 0 -#define BOT_PWM_CLAMP_CLOSE_TIME 75 -#define BOT_PWM_CLAMP_CLOSE -0x3ff, 75, 0 +#define BOT_PWM_CLAMP_OPEN_TIME 150 +#define BOT_PWM_CLAMP_OPEN 0x1ff, 150, 0 +#define BOT_PWM_CLAMP_CLOSE_TIME 150 +#define BOT_PWM_CLAMP_CLOSE -0x1ff, 150, 0 #define BOT_PWM_DOOR_OPEN_TIME 12 -#define BOT_PWM_DOOR_OPEN 0x3ff, 37, 0x55 -#define BOT_PWM_DOOR_CLOSE_TIME 50 +#define BOT_PWM_DOOR_OPEN(slot) \ + 0x1ff, (((slot) == CLAMP_SLOT_FRONT_BOTTOM \ + || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? 80 : 62), 0x55 +#define BOT_PWM_DOOR_CLOSE_TIME 100 #define BOT_PWM_DOOR_CLOSE(slot) \ - -0x3ff, 50, ((slot == CLAMP_SLOT_FRONT_BOTTOM \ - || slot == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180) - -#define BOT_PWM_CLAMP_INIT 0x1ff, 125, 0 -#define BOT_PWM_DOOR_INIT 0x1ff, 74, 0x55 -#define BOT_PWM_CLAMP_DOOR_INIT 125 + -0x1ff, (((slot) == CLAMP_SLOT_FRONT_BOTTOM \ + || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? 80 : 62), \ + (((slot) == CLAMP_SLOT_FRONT_BOTTOM \ + || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180) + +#define BOT_PWM_CLAMP_INIT 0x1ff, 150, 0 +#define BOT_PWM_DOOR_INIT 0x1ff, 80, 0x55 +#define BOT_PWM_CLAMP_DOOR_INIT 150 #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index f799e7d7..4c0fc9a7 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -264,7 +264,7 @@ clamp_door (uint8_t pos, uint8_t open) else if (clamp_slot_door[pos] != 0xff) { if (open) - pwm_set_timed (clamp_slot_door[pos], BOT_PWM_DOOR_OPEN); + pwm_set_timed (clamp_slot_door[pos], BOT_PWM_DOOR_OPEN (pos)); else pwm_set_timed (clamp_slot_door[pos], BOT_PWM_DOOR_CLOSE (pos)); } @@ -482,8 +482,8 @@ FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING) /* If going forward, drop at back. */ uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; - pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN); - pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN); + pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN (bay + 0)); + pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN (bay + 2)); return FSM_NEXT (CLAMP_IDLE, clamp_drop); } @@ -599,8 +599,8 @@ FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING) /* If going forward, drop at back. */ uint8_t bay = ctx.drop_direction == DIRECTION_FORWARD ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; - pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN); - pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN); + pwm_set_timed (clamp_slot_door[bay + 0], BOT_PWM_DOOR_OPEN (bay + 0)); + pwm_set_timed (clamp_slot_door[bay + 2], BOT_PWM_DOOR_OPEN (bay + 2)); return FSM_NEXT (CLAMP_LOCKED, clamp_drop); } @@ -673,7 +673,8 @@ FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME, { if (clamp_slot_door[ctx.pos_current] != 0xff) { - pwm_set_timed (clamp_slot_door[ctx.pos_current], BOT_PWM_DOOR_OPEN); + pwm_set_timed (clamp_slot_door[ctx.pos_current], + BOT_PWM_DOOR_OPEN (ctx.pos_current)); return FSM_NEXT_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, open_door); } else diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 7c773761..f6ef1013 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -288,10 +288,14 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) break; case c ('d', 0): /* Open all doors. */ - pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_OPEN); - pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_OPEN); - pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_OPEN); - pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_OPEN); + pwm_set_timed (BOT_PWM_DOOR_FRONT_BOTTOM, + BOT_PWM_DOOR_OPEN (CLAMP_SLOT_FRONT_BOTTOM)); + pwm_set_timed (BOT_PWM_DOOR_FRONT_TOP, + BOT_PWM_DOOR_OPEN (CLAMP_SLOT_FRONT_TOP)); + pwm_set_timed (BOT_PWM_DOOR_BACK_BOTTOM, + BOT_PWM_DOOR_OPEN (CLAMP_SLOT_BACK_BOTTOM)); + pwm_set_timed (BOT_PWM_DOOR_BACK_TOP, + BOT_PWM_DOOR_OPEN (CLAMP_SLOT_BACK_TOP)); break; case c ('d', 1): /* Drop elements. -- cgit v1.2.3 From 7d4be3876d30ddad9c0a505a67246d0fd95f438e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 28 May 2011 16:29:14 +0200 Subject: digital/io-hub: add jack command --- digital/io-hub/src/robospierre/main.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index f6ef1013..60fe28c0 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -243,6 +243,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Reset */ utils_reset (); break; + case c ('j', 0): + /* Simulate jack insertion. */ + fsm_queue_post_event (FSM_EVENT (AI, jack_inserted)); + break; case c ('w', 3): /* Set PWM. * - 1b: index. -- cgit v1.2.3 From 13ad47d0242573e0212c328d38538d17e7e0fb2b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 29 May 2011 09:38:26 +0200 Subject: digital/asserv: callibrate asserv parameters --- digital/asserv/tools/asserv/init.py | 4 ++-- digital/io-hub/src/robospierre/bot.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py index db33364d..5bbffe7d 100644 --- a/digital/asserv/tools/asserv/init.py +++ b/digital/asserv/tools/asserv/init.py @@ -35,8 +35,8 @@ target_marcel = dict ( w = 0x09, ) target_robospierre = dict ( - scale = 0.031748977927129413, f = 0x1370, - c = float (0x01000000) / (1 << 24), + scale = 0.0317975134344, f = 0x134e, + c = float (0xffa897) / (1 << 24), tkp = 1, tkd = 16, ta = 0.75, tsm = 0x60, tss = 0x10, tbe = 256, tbs = 0x08, tbc = 40, diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index bd7cc181..890780b3 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -31,7 +31,7 @@ #ifdef HOST # define BOT_SCALE 0.0395840674352314 #else -# define BOT_SCALE 0.0415178942124 +# define BOT_SCALE 0.0317975134344 #endif /** Distance from the robot axis to the front. */ -- cgit v1.2.3 From 2700442e8ecb331844687e349f5b20162a008b27 Mon Sep 17 00:00:00 2001 From: Maxime Hadjinlian Date: Sun, 29 May 2011 14:20:56 +0200 Subject: digital/avr/module: add module servo from digital/io/src --- digital/avr/modules/devices/servo/Makefile.module | 1 + digital/avr/modules/devices/servo/README | 23 +++ digital/avr/modules/devices/servo/avrconfig.h | 33 ++++ digital/avr/modules/devices/servo/servo.avr.c | 193 ++++++++++++++++++++ digital/avr/modules/devices/servo/servo.h | 96 ++++++++++ digital/avr/modules/devices/servo/servo_pos.c | 60 +++++++ digital/avr/modules/devices/servo/servo_pos.h | 73 ++++++++ digital/avr/modules/devices/servo/test/Makefile | 12 ++ digital/avr/modules/devices/servo/test/avrconfig.h | 91 ++++++++++ .../avr/modules/devices/servo/test/test_servo.c | 86 +++++++++ digital/io/src/Makefile | 6 +- digital/io/src/avrconfig.h | 5 + digital/io/src/eeprom.avr.c | 4 +- digital/io/src/main.c | 4 +- digital/io/src/servo.avr.c | 200 --------------------- digital/io/src/servo.h | 96 ---------- digital/io/src/servo_pos.c | 60 ------- digital/io/src/servo_pos.h | 73 -------- digital/io/src/simu.host.c | 26 +-- 19 files changed, 693 insertions(+), 449 deletions(-) create mode 100644 digital/avr/modules/devices/servo/Makefile.module create mode 100644 digital/avr/modules/devices/servo/README create mode 100644 digital/avr/modules/devices/servo/avrconfig.h create mode 100644 digital/avr/modules/devices/servo/servo.avr.c create mode 100644 digital/avr/modules/devices/servo/servo.h create mode 100644 digital/avr/modules/devices/servo/servo_pos.c create mode 100644 digital/avr/modules/devices/servo/servo_pos.h create mode 100644 digital/avr/modules/devices/servo/test/Makefile create mode 100644 digital/avr/modules/devices/servo/test/avrconfig.h create mode 100644 digital/avr/modules/devices/servo/test/test_servo.c delete mode 100644 digital/io/src/servo.avr.c delete mode 100644 digital/io/src/servo.h delete mode 100644 digital/io/src/servo_pos.c delete mode 100644 digital/io/src/servo_pos.h diff --git a/digital/avr/modules/devices/servo/Makefile.module b/digital/avr/modules/devices/servo/Makefile.module new file mode 100644 index 00000000..bf82ffac --- /dev/null +++ b/digital/avr/modules/devices/servo/Makefile.module @@ -0,0 +1 @@ +devices_servo_SOURCES = servo_pos.c servo.avr.c diff --git a/digital/avr/modules/devices/servo/README b/digital/avr/modules/devices/servo/README new file mode 100644 index 00000000..4981de17 --- /dev/null +++ b/digital/avr/modules/devices/servo/README @@ -0,0 +1,23 @@ +avr.devices.servo - Servo AVR module. + +Servo module for AVR. See modules README for more details about AVR modules. + +Copyright (C) 2011 Maxime Hadjinlian + +APBTeam: + Web: http://apbteam.org/ + Email: team AT apbteam DOT 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 +(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. diff --git a/digital/avr/modules/devices/servo/avrconfig.h b/digital/avr/modules/devices/servo/avrconfig.h new file mode 100644 index 00000000..d192598a --- /dev/null +++ b/digital/avr/modules/devices/servo/avrconfig.h @@ -0,0 +1,33 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2011 Maxime Hadjinlian + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/* servo - Servo module. */ +/** All servos are currently connected to the same port. */ +#define AC_SERVO_PORT PORTA +#define AC_SERVO_DDR DDRA + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/devices/servo/servo.avr.c b/digital/avr/modules/devices/servo/servo.avr.c new file mode 100644 index 00000000..fc5adc78 --- /dev/null +++ b/digital/avr/modules/devices/servo/servo.avr.c @@ -0,0 +1,193 @@ +/* servo.avr.c */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2008 Dufour Jérémy + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "servo.h" + +#include "modules/utils/utils.h" /* regv, set_bit */ +#include "modules/utils/byte.h" /* v16_to_v8 */ +#include "io.h" /* General defines of registers */ + +/** + * @defgroup ServoConfig Servo module configuration variables and defines. + * @{ + */ + +/** + * TOP of the timer/counter. + */ +#define SERVO_TCNT_TOP 0xFF + +/** + * Number of TIC the timer/counter need to do to make a whole cycle of servo + * update. + * It does not depend on the servo motors we manage but on the time we want a + * whole cycle to last. + * The formula used is: + * time_of_a_cycle * AVR_frequency / timer_counter_prescaler + * We want a time of 20ms (20/1000). + * See @a servo_init to know the prescaler value of the timer/counter. + */ +static const uint16_t servo_tic_cycle_ = AC_FREQ / 256 * 20 / 1000; + +/** @} */ + +/** + * @defgroup ServoPrivate Servo module private variables and functions + * declarations + * @{ + */ + +/** + * Identifier of the servo we are currently updating. + * Note: -1 is a special value used by the servo module system to update the + * low state of all the servos. + */ +volatile int8_t servo_updating_id_; + +/** + * A table for the time spent by each servo in high state. + */ +volatile uint8_t servo_position_[SERVO_NUMBER]; + +/** + * Overflow of timer/counter 2 handler. + */ +SIGNAL (SIG_OVERFLOW2); + +/** @} */ + +/* Initialize servo module. */ +void +servo_init (void) +{ + /* Set-up all the pins of the servo to out direction */ + AC_SERVO_DDR = 0xff; + /* All pins are at low state by default */ + + /* Set-up the timer/counter 2: + - prescaler 256 => 4.44 ms TOP */ + TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20, + 0, 0, 0, 0, 0, 1, 0, 0); + + /* The state machine start with the first servo */ + servo_updating_id_ = 0; + + /* Enable overflow interrupt */ + set_bit (TIMSK, TOIE2); + + /* By default, servo init disable all servo. */ + uint8_t i; + for (i = 0; i < SERVO_NUMBER; i++) + servo_set_position (i, 0); +} + +/* Set the duration of the input signal at the high state of a servo. */ +void +servo_set_position (uint8_t servo, uint8_t position) +{ + uint8_t filtered = position; + if (filtered != 0) + UTILS_BOUND (filtered, SERVO_HIGH_TIME_MIN, SERVO_HIGH_TIME_MAX); + /* Sanity check */ + if (servo < SERVO_NUMBER) + /* Set new desired position (high value time) */ + servo_position_[servo] = filtered; +} + +/* Get the duration of the servo's input signal at high state. */ +uint8_t +servo_get_position (uint8_t servo) +{ + /* Sanity check */ + if (servo < SERVO_NUMBER) + return servo_position_[servo]; + return 0; +} + +/* Overflow of timer/counter 2 handler. */ +SIGNAL (SIG_OVERFLOW2) +{ + /* Overflow count (used when we wait in the lower state). + -1 is used for the first count where we wait less than a complete + overflow */ + static int8_t servo_overflow_count = -1; + /* Time spent by each servo motor at high state during a whole cycle */ + static uint16_t servo_position_cycle = servo_tic_cycle_; + + /* State machine actions */ + if (servo_updating_id_ >= 0) + { + /* Servos motor high state mode */ + + /* Set to low state the previous servo motor pin if needed (not for + * the first one) */ + if (servo_updating_id_ != 0) + AC_SERVO_PORT &= ~_BV (servo_updating_id_ - 1); + /* Set to high state the current servo motor pin, unless is zero */ + if (servo_position_[servo_updating_id_]) + set_bit (AC_SERVO_PORT, servo_updating_id_); + /* Plan next timer overflow to the TOP minus the current configuration + * of the servo motor */ + TCNT2 = SERVO_TCNT_TOP - servo_position_[servo_updating_id_]; + /* Update the time spent at high state by all servo motors for this + * cycle */ + servo_position_cycle += servo_position_[servo_updating_id_]; + /* Update the identifier of the current servo motor (and manage when + * we are at the last one) */ + if (++servo_updating_id_ == SERVO_NUMBER) + servo_updating_id_ = -1; + } + else + { + /* Sleeping time mode */ + + /* Is it the first we are in this mode? */ + if (servo_overflow_count == -1) + { + /* Set to low state the previous servo motor pin */ + AC_SERVO_PORT &= ~_BV (SERVO_NUMBER - 1); + /* Number of full overflow (from 0 to SERVO_TCNT_TOP) we need to + * wait (division by SERVO_TCNT_TOP or >> 8) */ + servo_overflow_count = servo_position_cycle >> 8; + /* Restart the counter from remaining TIC that are left and can + * not be used to make a full overflow */ + TCNT2 = SERVO_TCNT_TOP - v16_to_v8 (servo_position_cycle, 0); + } + else + { + /* We just have an overflow, are we at the last one needed? The -1 + * is normal: we do not count the first overflow of the sleeping + * mode because it is not a full one */ + if (--servo_overflow_count == -1) + { + /* Restart with first servo motor */ + servo_updating_id_ = 0; + /* Re-initialize the counter of time spent by each servo motor + * at high state */ + servo_position_cycle = servo_tic_cycle_; + } + } + } +} diff --git a/digital/avr/modules/devices/servo/servo.h b/digital/avr/modules/devices/servo/servo.h new file mode 100644 index 00000000..59d35907 --- /dev/null +++ b/digital/avr/modules/devices/servo/servo.h @@ -0,0 +1,96 @@ +#ifndef servo_h +#define servo_h +/* servo.h */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2008 Dufour Jérémy + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" // uint8_t + +/** + * @file Module to control servo motors. + * This module contains low-level functions to control the servo motors. If + * you want to control the traps to store the balls at a specific place, have + * a look at the trap module. + * It uses the timer/counter 2 (8-bit). + * + * Servo motors can be controlled by the time the input signal spend at its + * high state. For example, if the signal sent to the servo motor only spend + * 1ms at the high state, it will have a 0° angle position. If the signal + * stays for 1.5ms at high state, it will have a 90° angle position. + * To manage all servo motors in a "one time shot", we need to use the + * timer/counter 2 of the ATmega128 and its overflow. + * We setup the timer/counter to the value of its overflow minus the time the + * input signal of the servo need to spend at high state ; we put the input + * signal of this servo motor to the high value. When the timer overflows, we + * put it back to the low state. We go to the next servo motors and do the + * same algorithm. When the all servos motor have been taken care of, we + * set-up the timer to overflow a certain number of times to wait before + * restarting the whole cycle. + * + * All servos are connected to AC_SERVO_PORT defined in avrconfig.h + */ + +/** + * Number of servos motor managed by this module. + * If you change it, you _must_ update the key of the eeprom module! + */ +#define SERVO_NUMBER 8 + +/** + * Minimum high time for servos. + */ +#define SERVO_HIGH_TIME_MIN 0x24 + +/** + * Maximum high time for servos. + */ +#define SERVO_HIGH_TIME_MAX 0x88 + +/** + * Initialize servo module. + * This functions put the pins of the servos motor in the right direction, + * initialize the timer/counter 2 and some internals stuff. + */ +void +servo_init (void); + +/** + * Set the duration of the input signal of a servo at high state + * and thus its position. + * @param servo the servo to change the position. + * @param position the duration while the input signal will be at the + * high state. The servo will then move to a position. + * A zero will let the servo floating. + */ +void +servo_set_position (uint8_t servo, uint8_t position); + +/** + * Get the duration of the servo's input signal at high state. + * @param servo the servo to get the position of. + * @return the current position of the servo. + */ +uint8_t +servo_get_position (uint8_t servo); + +#endif /* servo_h */ diff --git a/digital/avr/modules/devices/servo/servo_pos.c b/digital/avr/modules/devices/servo/servo_pos.c new file mode 100644 index 00000000..6308ceaf --- /dev/null +++ b/digital/avr/modules/devices/servo/servo_pos.c @@ -0,0 +1,60 @@ +/* servo_pos.c */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2009 Dufour Jérémy + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#include "servo_pos.h" + +uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; + +void +servo_pos_init (void) +{ + /* Initialize under layer, servo module. */ + servo_init (); +} + +void +servo_pos_set_high_time (uint8_t servo_id, + uint8_t high_times[SERVO_POS_NUMBER]) +{ + uint8_t i; + /* If servo exists. */ + if (servo_id < SERVO_NUMBER) + { + /* For each position. */ + for (i = 0; i < SERVO_POS_NUMBER; i++) + servo_pos_high_time[servo_id][i] = high_times[i]; + } +} + +/** + * Move a servo to a specific position. + * @param servo_id the id of the servo to move. + * @param position the position identifier where to move the servo. + */ +void +servo_pos_move_to (uint8_t servo_id, uint8_t position) +{ + servo_set_position (servo_id, servo_pos_high_time[servo_id][position]); +} diff --git a/digital/avr/modules/devices/servo/servo_pos.h b/digital/avr/modules/devices/servo/servo_pos.h new file mode 100644 index 00000000..e173ab6d --- /dev/null +++ b/digital/avr/modules/devices/servo/servo_pos.h @@ -0,0 +1,73 @@ +#ifndef servo_pos_h +#define servo_pos_h +/* servo.pos.h */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2009 Dufour Jérémy + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/** + * Module to control two positions of the servo motor. + * It's an higher interface that uses the servo module but remember the + * positions (for example, two positions, open and close) that they can take. + */ + +#include "servo.h" + +/** + * Number of positions the servo can take. + * @warning if you change this define, you must update the key value in the + * EEPROM module. Otherwise, everything will fail! + */ +#define SERVO_POS_NUMBER 2 + +/** + * Table to store the correspondence between the positions and the high time. + */ +extern uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; + +/** + * Initialize the servo with positions module. + * @note it also initialize the servo module for you. + */ +void +servo_pos_init (void); + +/** + * Set the high times of a servo for positions. + * @param servo_id the ID of the servo. + * @param high_times the different high times for the positions of the + * servo. + */ +void +servo_pos_set_high_time (uint8_t servo_id, + uint8_t high_times[SERVO_POS_NUMBER]); + +/** + * Move a servo to a specific position. + * @param servo_id the id of the servo to move. + * @param position the position identifier where to move the servo. + */ +void +servo_pos_move_to (uint8_t servo_id, uint8_t position); + +#endif /* servo_pos_h */ diff --git a/digital/avr/modules/devices/servo/test/Makefile b/digital/avr/modules/devices/servo/test/Makefile new file mode 100644 index 00000000..a0ad639f --- /dev/null +++ b/digital/avr/modules/devices/servo/test/Makefile @@ -0,0 +1,12 @@ +BASE = ../../../.. +AVR_PROGS = test_servo +test_servo_SOURCES = test_servo.c +MODULES = proto devices/servo uart utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 + +include $(BASE)/make/Makefile.gen diff --git a/digital/avr/modules/devices/servo/test/avrconfig.h b/digital/avr/modules/devices/servo/test/avrconfig.h new file mode 100644 index 00000000..6ee81392 --- /dev/null +++ b/digital/avr/modules/devices/servo/test/avrconfig.h @@ -0,0 +1,91 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h */ +/* avr.devices.servo - Servo AVR module. {{{ + * + * Copyright (C) 2011 Maxime Hadjinlian + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/* global */ +/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, + * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#define AC_FREQ 14745600 + +/* servo - Servo module. */ +/** All servos are currently connected to the same port. */ +#define AC_SERVO_PORT PORTA +#define AC_SERVO_DDR DDRA + +/* uart - UART module. */ +/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */ +#define AC_UART0_PORT 1 +/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, + * 115200, 230400, 250000, 500000, 1000000. */ +#define AC_UART0_BAUDRATE 38400 +/** Send mode: + * - POLLING: no interrupts. + * - RING: interrupts, ring buffer. */ +#define AC_UART0_SEND_MODE RING +/** Recv mode, same as send mode. */ +#define AC_UART0_RECV_MODE RING +/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_UART0_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_UART0_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_UART0_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_SEND_BUFFER_SIZE 32 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_RECV_BUFFER_SIZE 32 +/** If the send buffer is full when putc: + * - DROP: drop the new byte. + * - WAIT: wait until there is room in the send buffer. */ +#define AC_UART0_SEND_BUFFER_FULL DROP +/** In HOST compilation: + * - STDIO: use stdin/out. + * - PTS: use pseudo terminal. */ +#define AC_UART0_HOST_DRIVER STDIO +/** Same thing for secondary port. */ +#define AC_UART1_PORT -1 +#define AC_UART1_BAUDRATE 115200 +#define AC_UART1_SEND_MODE RING +#define AC_UART1_RECV_MODE RING +#define AC_UART1_CHAR_SIZE 8 +#define AC_UART1_PARITY EVEN +#define AC_UART1_STOP_BITS 1 +#define AC_UART1_SEND_BUFFER_SIZE 32 +#define AC_UART1_RECV_BUFFER_SIZE 32 +#define AC_UART1_SEND_BUFFER_FULL WAIT +#define AC_UART1_HOST_DRIVER PTS + +/* proto - Protocol module. */ +/** Maximum argument size. */ +#define AC_PROTO_ARGS_MAX_SIZE 8 +/** Callback function name. */ +#define AC_PROTO_CALLBACK proto_callback +/** Putchar function name. */ +#define AC_PROTO_PUTC uart0_putc +/** Support for quote parameter. */ +#define AC_PROTO_QUOTE 1 + +#endif /* avrconfig_h */ diff --git a/digital/avr/modules/devices/servo/test/test_servo.c b/digital/avr/modules/devices/servo/test/test_servo.c new file mode 100644 index 00000000..924fae1c --- /dev/null +++ b/digital/avr/modules/devices/servo/test/test_servo.c @@ -0,0 +1,86 @@ +/* test_servo.c. */ +/* avr.servo - Servo AVR module. {{{ + * + * Copyright (C) 2011 Maxime Hadjinlian + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#include "common.h" +#include "modules/uart/uart.h" +#include "modules/proto/proto.h" +#include "modules/devices/servo/servo.h" +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "io.h" + +/* proto_callback is called by proto + * when a command is complete and valid. + * proto act as an intelligent buffer for us. +*/ +void +proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ + /* This macro combine command and size in one integer. */ +#define c(cmd, size) (cmd << 8 | size) + switch (c (cmd, size)) + { + case c ('z', 0): + /* This should be generaly implemented. */ + utils_reset (); + break; + case c ('s', 3): + /* We are looking to receive a command in the form !sxxyyyy + * where xx is the servo id and yyyy is the command itself. + * For the size, please read digital/avr/modules/proto/proto.txt + */ + servo_set_position (args[0], v8_to_v16 (args[1], args[2])); + break; + default: + /* This is to handle default commands, return an error. */ + proto_send0 ('?'); + return; + } + /* When no error acknoledge. */ + proto_send (cmd, size, args); +#undef c +} + +int +main (int argc, char **argv) +{ + avr_init (argc, argv); + sei (); + uart0_init (); + servo_init (); + + /* This command should be generaly sent on reset. */ + proto_send0 ('z'); + /* This is to accept commands. */ + while (1) + { + uint8_t c = uart0_getc (); + proto_accept (c); + /* once the command is complete according to + * proto's state machine, proto_callback will + * be called with the full command. + */ + } +} diff --git a/digital/io/src/Makefile b/digital/io/src/Makefile index 463e148e..4ebc5a66 100644 --- a/digital/io/src/Makefile +++ b/digital/io/src/Makefile @@ -3,15 +3,15 @@ BASE = ../../avr # Name of the program to build. PROGS = io # Sources to compile. -io_SOURCES = main.c fsm_queue.c servo.avr.c eeprom.avr.c pwm.c \ - switch.avr.c chrono.c timer.avr.c servo_pos.c \ +io_SOURCES = main.c fsm_queue.c eeprom.avr.c pwm.c \ + switch.avr.c chrono.c timer.avr.c \ twi_master.c asserv.c mimot.c \ simu.host.c contact.c radar.c radar_defs.c \ path.c food.c events.host.c \ fsm.host.c init.c move.c top.c hola.c loader.c fsm_AI_gen.avr.c # Modules needed for IO. MODULES = proto uart twi utils adc math/fixed math/geometry path/astar \ - devices/usdist \ + devices/usdist devices/servo \ trace flash spi AI_MODULES = twi_master common utils fsm move # Configuration file. diff --git a/digital/io/src/avrconfig.h b/digital/io/src/avrconfig.h index a46b4abe..cb9061fd 100644 --- a/digital/io/src/avrconfig.h +++ b/digital/io/src/avrconfig.h @@ -129,6 +129,11 @@ USDIST_SENSOR (2, C, 7) \ USDIST_SENSOR (3, D, 4) +/* servo - Servo module */ +/** All servos are currently connected to the same port. */ +#define AC_SERVO_PORT PORTA +#define AC_SERVO_DDR DDRA + /* io - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 2 diff --git a/digital/io/src/eeprom.avr.c b/digital/io/src/eeprom.avr.c index a32b9606..febba55c 100644 --- a/digital/io/src/eeprom.avr.c +++ b/digital/io/src/eeprom.avr.c @@ -26,8 +26,8 @@ #include "common.h" #include "eeprom.h" -#include "servo.h" /* SERVO_NUMBER */ -#include "servo_pos.h" +#include "modules/devices/servo/servo.h" +#include "modules/devices/servo/servo_pos.h" #include /* eeprom_{read,write}_byte */ diff --git a/digital/io/src/main.c b/digital/io/src/main.c index 016aad36..5aa35fb7 100644 --- a/digital/io/src/main.c +++ b/digital/io/src/main.c @@ -29,6 +29,7 @@ #include "modules/utils/utils.h" #include "modules/path/path.h" #include "modules/devices/usdist/usdist.h" +#include "modules/devices/servo/servo_pos.h" #include "modules/flash/flash.h" #include "modules/trace/trace.h" #include "events.h" @@ -51,7 +52,6 @@ #include "fsm.h" #include "fsm_queue.h" #include "bot.h" -#include "servo_pos.h" #include "radar.h" #include "chrono.h" /* chrono_end_match */ #include "pwm.h" @@ -408,7 +408,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: servo id number; * - 1b: pwm high time value (position). */ - servo_set_high_time (args[0], args[1]); + servo_set_position (args[0], args[1]); break; case c ('S', 0): diff --git a/digital/io/src/servo.avr.c b/digital/io/src/servo.avr.c deleted file mode 100644 index de50c819..00000000 --- a/digital/io/src/servo.avr.c +++ /dev/null @@ -1,200 +0,0 @@ -/* servo.avr.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Dufour Jérémy - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT 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 - * (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. - * - * }}} */ - -#include "common.h" -#include "servo.h" - -#include "modules/utils/utils.h" /* regv, set_bit */ -#include "modules/utils/byte.h" /* v16_to_v8 */ -#include "io.h" /* General defines of registers */ - -/** - * @defgroup ServoConfig Servo module configuration variables and defines. - * @{ - */ - -/** - * All servos are connected to the PORTA. - */ -#define SERVO_PORT PORTA -#define SERVO_DDR DDRA - -/** - * TOP of the timer/counter. - */ -#define SERVO_TCNT_TOP 0xFF - -/** - * Number of TIC the timer/counter need to do to make a whole cycle of servo - * update. - * It does not depend on the servo motors we manage but on the time we want a - * whole cycle to last. - * The formula used is: - * time_of_a_cycle * AVR_frequency / timer_counter_prescaler - * We want a time of 20ms (20/1000). - * See @a servo_init to know the prescaler value of the timer/counter. - */ -static const uint16_t servo_tic_cycle_ = AC_FREQ / 256 * 20 / 1000; - -/** @} */ - -/** - * @defgroup ServoPrivate Servo module private variables and functions - * declarations - * @{ - */ - -/** - * Identifier of the servo we are currently updating. - * Note: -1 is a special value used by the servo module system to update the - * low state of all the servos. - */ -volatile int8_t servo_updating_id_; - -/** - * A table for the time spent by each servo in high state. - */ -volatile uint8_t servo_high_time_[SERVO_NUMBER]; - -/** - * Overflow of timer/counter 2 handler. - */ -SIGNAL (SIG_OVERFLOW2); - -/** @} */ - -/* Initialize servo module. */ -void -servo_init (void) -{ - /* Set-up all the pins of the servo to out direction */ - SERVO_DDR = 0xff; - /* All pins are at low state by default */ - - /* Set-up the timer/counter 2: - - prescaler 256 => 4.44 ms TOP */ - TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20, - 0, 0, 0, 0, 0, 1, 0, 0); - - /* The state machine start with the first servo */ - servo_updating_id_ = 0; - - /* Enable overflow interrupt */ - set_bit (TIMSK, TOIE2); - - /* By default, servo init disable all servo. */ - uint8_t i; - for (i = 0; i < SERVO_NUMBER; i++) - servo_set_high_time (i, 0); -} - -/* Set the high time of the input signal of a servo (and its position). */ -void -servo_set_high_time (uint8_t servo, uint8_t high_time) -{ - uint8_t filtered = high_time; - if (filtered != 0) - UTILS_BOUND (filtered, SERVO_HIGH_TIME_MIN, SERVO_HIGH_TIME_MAX); - /* Sanity check */ - if (servo < SERVO_NUMBER) - /* Set new desired position (high value time) */ - servo_high_time_[servo] = filtered; -} - -/* Get the high time of the servo. */ -uint8_t -servo_get_high_time (uint8_t servo) -{ - /* Sanity check */ - if (servo < SERVO_NUMBER) - return servo_high_time_[servo]; - return 0; -} - -/* Overflow of timer/counter 2 handler. */ -SIGNAL (SIG_OVERFLOW2) -{ - /* Overflow count (used when we wait in the lower state). - -1 is used for the first count where we wait less than a complete - overflow */ - static int8_t servo_overflow_count = -1; - /* Time spent by each servo motor at high state during a whole cycle */ - static uint16_t servo_high_time_cycle = servo_tic_cycle_; - - /* State machine actions */ - if (servo_updating_id_ >= 0) - { - /* Servos motor high state mode */ - - /* Set to low state the previous servo motor pin if needed (not for - * the first one) */ - if (servo_updating_id_ != 0) - SERVO_PORT &= ~_BV (servo_updating_id_ - 1); - /* Set to high state the current servo motor pin, unless is zero */ - if (servo_high_time_[servo_updating_id_]) - set_bit (SERVO_PORT, servo_updating_id_); - /* Plan next timer overflow to the TOP minus the current configuration - * of the servo motor */ - TCNT2 = SERVO_TCNT_TOP - servo_high_time_[servo_updating_id_]; - /* Update the time spent at high state by all servo motors for this - * cycle */ - servo_high_time_cycle += servo_high_time_[servo_updating_id_]; - /* Update the identifier of the current servo motor (and manage when - * we are at the last one) */ - if (++servo_updating_id_ == SERVO_NUMBER) - servo_updating_id_ = -1; - } - else - { - /* Sleeping time mode */ - - /* Is it the first we are in this mode? */ - if (servo_overflow_count == -1) - { - /* Set to low state the previous servo motor pin */ - SERVO_PORT &= ~_BV (SERVO_NUMBER - 1); - /* Number of full overflow (from 0 to SERVO_TCNT_TOP) we need to - * wait (division by SERVO_TCNT_TOP or >> 8) */ - servo_overflow_count = servo_high_time_cycle >> 8; - /* Restart the counter from remaining TIC that are left and can - * not be used to make a full overflow */ - TCNT2 = SERVO_TCNT_TOP - v16_to_v8 (servo_high_time_cycle, 0); - } - else - { - /* We just have an overflow, are we at the last one needed? The -1 - * is normal: we do not count the first overflow of the sleeping - * mode because it is not a full one */ - if (--servo_overflow_count == -1) - { - /* Restart with first servo motor */ - servo_updating_id_ = 0; - /* Re-initialize the counter of time spent by each servo motor - * at high state */ - servo_high_time_cycle = servo_tic_cycle_; - } - } - } -} diff --git a/digital/io/src/servo.h b/digital/io/src/servo.h deleted file mode 100644 index b9f7e3ea..00000000 --- a/digital/io/src/servo.h +++ /dev/null @@ -1,96 +0,0 @@ -#ifndef servo_h -#define servo_h -/* servo.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2008 Dufour Jérémy - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT 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 - * (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. - * - * }}} */ - -#include "common.h" // uint8_t - -/** - * @file Module to control servo motors. - * This module contains low-level functions to control the servo motors. If - * you want to control the traps to store the balls at a specific place, have - * a look at the trap module. - * It uses the timer/counter 2 (8-bit). - * - * Servo motors can be controlled by the time the input signal spend at its - * high state. For example, if the signal sent to the servo motor only spend - * 1ms at the high state, it will have a 0° angle position. If the signal - * stays for 1.5ms at high state, it will have a 90° angle position. - * To manage all servo motors in a "one time shot", we need to use the - * timer/counter 2 of the ATmega128 and its overflow. - * We setup the timer/counter to the value of its overflow minus the time the - * input signal of the servo need to spend at high state ; we put the input - * signal of this servo motor to the high value. When the timer overflows, we - * put it back to the low state. We go to the next servo motors and do the - * same algorithm. When the all servos motor have been taken care of, we - * set-up the timer to overflow a certain number of times to wait before - * restarting the whole cycle. - * - * All servos are connected to the PORTA of the ATmega. - */ - -/** - * Number of servos motor managed by this module. - * If you change it, you _must_ update the key of the eeprom module! - */ -#define SERVO_NUMBER 8 - -/** - * Minimum high time for servos. - */ -#define SERVO_HIGH_TIME_MIN 0x24 - -/** - * Maximum high time for servos. - */ -#define SERVO_HIGH_TIME_MAX 0x88 - -/** - * Initialize servo module. - * This functions put the pins of the servos motor in the right direction, - * initialize the timer/counter 2 and some internals stuff. - */ -void -servo_init (void); - -/** - * Set the high time of the input signal of a servo (and its position). - * @param servo the servo to change the position. - * @param high_time the high time we want the input signal to spend at the - * high state to set the servo motor to a position. A zero will let the servo - * floating. - */ -void -servo_set_high_time (uint8_t servo, uint8_t high_time); - -/** - * Get the high time of the servo. - * @param servo the servo to get the position of. - * @return the current position of the servo. - */ -uint8_t -servo_get_high_time (uint8_t servo); - -#endif /* servo_h */ diff --git a/digital/io/src/servo_pos.c b/digital/io/src/servo_pos.c deleted file mode 100644 index d04077fd..00000000 --- a/digital/io/src/servo_pos.c +++ /dev/null @@ -1,60 +0,0 @@ -/* servo_pos.c */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Dufour Jérémy - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT 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 - * (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. - * - * }}} */ - -#include "servo_pos.h" - -uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; - -void -servo_pos_init (void) -{ - /* Initialize under layer, servo module. */ - servo_init (); -} - -void -servo_pos_set_high_time (uint8_t servo_id, - uint8_t high_times[SERVO_POS_NUMBER]) -{ - uint8_t i; - /* If servo exists. */ - if (servo_id < SERVO_NUMBER) - { - /* For each position. */ - for (i = 0; i < SERVO_POS_NUMBER; i++) - servo_pos_high_time[servo_id][i] = high_times[i]; - } -} - -/** - * Move a servo to a specific position. - * @param servo_id the id of the servo to move. - * @param position the position identifier where to move the servo. - */ -void -servo_pos_move_to (uint8_t servo_id, uint8_t position) -{ - servo_set_high_time (servo_id, servo_pos_high_time[servo_id][position]); -} diff --git a/digital/io/src/servo_pos.h b/digital/io/src/servo_pos.h deleted file mode 100644 index c8e56d2b..00000000 --- a/digital/io/src/servo_pos.h +++ /dev/null @@ -1,73 +0,0 @@ -#ifndef servo_pos_h -#define servo_pos_h -/* servo_pos.h */ -/* io - Input & Output with Artificial Intelligence (ai) support on AVR. {{{ - * - * Copyright (C) 2009 Dufour Jérémy - * - * APBTeam: - * Web: http://apbteam.org/ - * Email: team AT apbteam DOT 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 - * (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. - * - * }}} */ - -/** - * Module to control two positions of the servo motor. - * It's an higher interface that uses the servo module but remember the - * positions (for example, two positions, open and close) that they can take. - */ - -#include "servo.h" - -/** - * Number of positions the servo can take. - * @warning if you change this define, you must update the key value in the - * EEPROM module. Otherwise, everything will fail! - */ -#define SERVO_POS_NUMBER 2 - -/** - * Table to store the correspondence between the positions and the high time. - */ -extern uint8_t servo_pos_high_time[SERVO_NUMBER][SERVO_POS_NUMBER]; - -/** - * Initialize the servo with positions module. - * @note it also initialize the servo module for you. - */ -void -servo_pos_init (void); - -/** - * Set the high times of a servo for positions. - * @param servo_id the ID of the servo. - * @param high_times the different high times for the positions of the - * servo. - */ -void -servo_pos_set_high_time (uint8_t servo_id, - uint8_t high_times[SERVO_POS_NUMBER]); - -/** - * Move a servo to a specific position. - * @param servo_id the id of the servo to move. - * @param position the position identifier where to move the servo. - */ -void -servo_pos_move_to (uint8_t servo_id, uint8_t position); - -#endif /* servo_pos_h */ diff --git a/digital/io/src/simu.host.c b/digital/io/src/simu.host.c index f1b7e0d2..a35b9d12 100644 --- a/digital/io/src/simu.host.c +++ b/digital/io/src/simu.host.c @@ -25,7 +25,7 @@ #include "common.h" #include "simu.host.h" -#include "servo.h" +#include "modules/devices/servo/servo.h" #include "pwm.h" #include "modules/utils/utils.h" @@ -36,10 +36,10 @@ #include "io.h" /** Requested servo position. */ -uint8_t servo_high_time_[SERVO_NUMBER]; +uint8_t servo_position_[SERVO_NUMBER]; /** Current servo position. */ -uint8_t servo_high_time_current_[SERVO_NUMBER]; +uint8_t servo_position_current_[SERVO_NUMBER]; /** Servo speed is about 120 ms for 60 degrees. This means about 360 ms for * the full swing. */ @@ -112,13 +112,13 @@ simu_step (void) /* Update servos. */ for (i = 0; i < SERVO_NUMBER; i++) { - if (UTILS_ABS (servo_high_time_current_[i] - servo_high_time_[i]) < + if (UTILS_ABS (servo_position_current_[i] - servo_position_[i]) < SERVO_SPEED) - servo_high_time_current_[i] = servo_high_time_[i]; - else if (servo_high_time_current_[i] < servo_high_time_[i]) - servo_high_time_current_[i] += SERVO_SPEED; + servo_position_current_[i] = servo_position_[i]; + else if (servo_position_current_[i] < servo_position_[i]) + servo_position_current_[i] += SERVO_SPEED; else - servo_high_time_current_[i] -= SERVO_SPEED; + servo_position_current_[i] -= SERVO_SPEED; } /* Send servos. */ if (simu_servo_update && !--simu_servo_update_cpt) @@ -126,7 +126,7 @@ simu_step (void) simu_servo_update_cpt = simu_servo_update; m = mex_msg_new (simu_mex_servo); for (i = 0; i < SERVO_NUMBER; i++) - mex_msg_push (m, "B", servo_high_time_current_[i]); + mex_msg_push (m, "B", servo_position_current_[i]); mex_node_send (m); } /* Update switches. */ @@ -180,15 +180,15 @@ servo_init (void) } void -servo_set_high_time (uint8_t servo, uint8_t high_time) +servo_set_position (uint8_t servo, uint8_t position) { - servo_high_time_[servo] = high_time; + servo_position_[servo] = position; } uint8_t -servo_get_high_time (uint8_t servo) +servo_get_position (uint8_t servo) { - return servo_high_time_[servo]; + return servo_position_[servo]; } void -- cgit v1.2.3 From 5c8d716b96d32609d18af5219c541201209e586d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 29 May 2011 19:19:28 +0200 Subject: digital/io-hub: handle element detection on table --- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/bot.h | 7 ++ digital/io-hub/src/robospierre/clamp.c | 12 +-- digital/io-hub/src/robospierre/pawn_sensor.c | 113 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 33 ++++++++ host/simu/robots/robospierre/model/clamp.py | 2 +- 6 files changed, 161 insertions(+), 8 deletions(-) create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.c create mode 100644 digital/io-hub/src/robospierre/pawn_sensor.h diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index e4b87124..a9bb2283 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -5,7 +5,7 @@ PROGS = io_hub HOST_PROGS = test_element # Sources to compile. io_hub_SOURCES = main.c \ - clamp.c logistic.c element.c \ + clamp.c logistic.c element.c pawn_sensor.c \ radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 890780b3..7e773032 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -46,6 +46,13 @@ /** Angle error at the front contact point. */ #define BOT_FRONT_CONTACT_ANGLE_ERROR_DEG 0 +/** Distance from robot center to front pawn detection threshold. */ +#define BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM 190 +/** Distance from robot center to back pawn detection threshold. */ +#define BOT_PAWN_BACK_DETECTION_THRESHOLD_MM -190 +/** Distance from robot center to an element near enough to be taken. */ +#define BOT_PAWN_TAKING_DISTANCE_MM 150 + /** Speed used for initialisation. */ #define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 /** Normal cruise speed. */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 4c0fc9a7..bae3591d 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -38,6 +38,7 @@ #include "modules/proto/proto.h" #include "logistic.h" +#include "pawn_sensor.h" /* * There is two FSM in this file. @@ -275,16 +276,15 @@ clamp_handle_event (void) { if (FSM_CAN_HANDLE (AI, clamp_new_element)) { - /* XXX: temporary hack. */ - uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; - if (!IO_GET (CONTACT_FRONT_BOTTOM) - && !logistic_global.slots[CLAMP_SLOT_FRONT_BOTTOM]) + uint8_t element_type; + element_type = pawn_sensor_get (DIRECTION_FORWARD); + if (element_type) { clamp_new_element (CLAMP_SLOT_FRONT_BOTTOM, element_type); return 1; } - if (!IO_GET (CONTACT_BACK_BOTTOM) - && !logistic_global.slots[CLAMP_SLOT_BACK_BOTTOM]) + element_type = pawn_sensor_get (DIRECTION_BACKWARD); + if (element_type) { clamp_new_element (CLAMP_SLOT_BACK_BOTTOM, element_type); return 1; diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c new file mode 100644 index 00000000..65f0afe4 --- /dev/null +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -0,0 +1,113 @@ +/* pawn_sensor.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "pawn_sensor.h" + +#include "asserv.h" +#include "contact.h" +#include "logistic.h" +#include "element.h" +#include "clamp.h" +#include "bot.h" + +#include "modules/math/geometry/distance.h" + +/* Handle pawn sensors. When a pawn is detected, it can not be taken + * directly, but only once it is inside the robot. */ + +/** Pawn sensor context. */ +struct pawn_sensor_t +{ + /** Is there something in front of the sensor? */ + uint8_t active; + /** If active, supposed position of element. */ + vect_t active_position; +}; + +/** Global contexts. */ +struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; + +static uint8_t +pawn_sensor_get_type (uint8_t direction) +{ + uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; + return element_type; +} + +uint8_t +pawn_sensor_get (uint8_t direction) +{ + struct pawn_sensor_t *ctx; + uint8_t contact_value, slot; + int16_t dist; + /* Check direction. */ + if (direction == DIRECTION_FORWARD) + { + ctx = &pawn_sensor_front; + contact_value = !IO_GET (CONTACT_FRONT_BOTTOM); + slot = CLAMP_SLOT_FRONT_BOTTOM; + dist = BOT_PAWN_FRONT_DETECTION_THRESHOLD_MM; + } + else + { + ctx = &pawn_sensor_back; + contact_value = !IO_GET (CONTACT_BACK_BOTTOM); + slot = CLAMP_SLOT_BACK_BOTTOM; + dist = BOT_PAWN_BACK_DETECTION_THRESHOLD_MM; + } + /* Handle contact. */ + if (contact_value) + { + if (!logistic_global.slots[slot] + && logistic_global.moving_to != slot) + { + position_t robot_position; + asserv_get_position (&robot_position); + if (ctx->active) + { + int32_t d = distance_point_point (&ctx->active_position, + &robot_position.v); + if (d < BOT_PAWN_TAKING_DISTANCE_MM) + { + ctx->active = 0; + return pawn_sensor_get_type (direction); + } + } + else + { + ctx->active = 1; + vect_from_polar_uf016 (&ctx->active_position, dist, + robot_position.a); + vect_translate (&ctx->active_position, &robot_position.v); + } + } + } + else + { + ctx->active = 0; + } + return 0; +} + diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h new file mode 100644 index 00000000..14ce2d62 --- /dev/null +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -0,0 +1,33 @@ +#ifndef pawn_sensor_h +#define pawn_sensor_h +/* pawn_sensor.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/** Update sensor state and return the element type if an element is ready to + * be taken. */ +uint8_t +pawn_sensor_get (uint8_t direction); + +#endif /* pawn_sensor_h */ diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index a3476a46..0af4ccf3 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -220,7 +220,7 @@ class Clamp (Observable): m = self.__get_robot_matrix () # Look up elements. xoffset = (self.BAY_OFFSET, -self.BAY_OFFSET)[side] - xmargin = 20 + xmargin = 40 ymargin = 50 for o in self.table.obstacles: if o.level == 1 and o.pos is not None: -- cgit v1.2.3 From 00f78226167fcb06d8b0c4bddf97018c4c2175ef Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 00:02:46 +0200 Subject: digital/{ai,io-hub}: fix wrong team color definition --- digital/ai/src/common/defs.h | 4 ++-- digital/io-hub/src/common/contact.avr.c | 2 +- digital/io-hub/src/common/contact.host.c | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/digital/ai/src/common/defs.h b/digital/ai/src/common/defs.h index beec5608..12c47584 100644 --- a/digital/ai/src/common/defs.h +++ b/digital/ai/src/common/defs.h @@ -53,8 +53,8 @@ typedef struct position_t position_t; /** Team color, determine the start zone side. */ enum team_color_e { - TEAM_COLOR_LEFT = 0, - TEAM_COLOR_RIGHT = 1 + TEAM_COLOR_RIGHT = 0, + TEAM_COLOR_LEFT = 1 }; /** Current team color, to be read at start up. */ diff --git a/digital/io-hub/src/common/contact.avr.c b/digital/io-hub/src/common/contact.avr.c index 9f6869b4..d6692566 100644 --- a/digital/io-hub/src/common/contact.avr.c +++ b/digital/io-hub/src/common/contact.avr.c @@ -73,7 +73,7 @@ contact_update (void) enum team_color_e contact_get_color (void) { - return !IO_GET (CONTACT_COLOR) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; + return IO_GET (CONTACT_COLOR) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; } uint8_t diff --git a/digital/io-hub/src/common/contact.host.c b/digital/io-hub/src/common/contact.host.c index 5e2309c4..cc302f46 100644 --- a/digital/io-hub/src/common/contact.host.c +++ b/digital/io-hub/src/common/contact.host.c @@ -49,7 +49,7 @@ contact_handle (void *user, mex_msg_t *msg) uint32_t contacts; mex_msg_pop (msg, "L", &contacts); ctx.all = contacts; - ctx.color_state = !(contacts & 1) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; + ctx.color_state = (contacts & 1) ? TEAM_COLOR_LEFT : TEAM_COLOR_RIGHT; ctx.jack_state = (contacts & 2) ? 1 : 0; contacts >>= 2; #define CONTACT(io) do { \ -- cgit v1.2.3 From bbf3187bdefa35d9ca1c9dc27d3145083e8fb96f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 01:06:06 +0200 Subject: digital/io-hub: move: post events instead of handling them This caused problem because the move FSM can receive events after the top FSM (this depends on the order of the FSM), and therefore, it can receive the robot_move_success event that was for the top FSM. --- digital/io-hub/src/robospierre/move.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index 7ce9b3c8..e89e6d6b 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -82,7 +82,7 @@ move_start (position_t position, uint8_t backward) /* Reset try counter. */ move_data.try_again_counter = 3; /* Start the FSM. */ - FSM_HANDLE (AI, move_start); + fsm_queue_post_event (FSM_EVENT (AI, move_start)); } void @@ -97,7 +97,7 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten) /* Reset try counter. */ move_data.try_again_counter = 3; /* Start the FSM. */ - FSM_HANDLE (AI, move_start); + fsm_queue_post_event (FSM_EVENT (AI, move_start)); } void -- cgit v1.2.3 From 5cff7f4d119ef237e95f26a970f771939dd54260 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 01:08:52 +0200 Subject: digital/io-hub: faster simulated initialisation --- digital/io-hub/src/robospierre/bot.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 7e773032..661ba584 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -54,7 +54,11 @@ #define BOT_PAWN_TAKING_DISTANCE_MM 150 /** Speed used for initialisation. */ -#define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 +#ifdef HOST +# define BOT_SPEED_INIT 0x20, 0x20, 0x20, 0x20 +#else +# define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 +#endif /** Normal cruise speed. */ #define BOT_SPEED_NORMAL 0x40, 0x40, 0x20, 0x20 -- cgit v1.2.3 From e0e813dc81eb0d5346ed76724438c94e4ac1e54b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 01:10:13 +0200 Subject: digital/io-hub: change element_get_pos --- digital/io-hub/src/robospierre/bot.h | 10 ++++++++++ digital/io-hub/src/robospierre/element.c | 17 +++++++++-------- digital/io-hub/src/robospierre/element.h | 2 +- 3 files changed, 20 insertions(+), 9 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 661ba584..1c068def 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -41,6 +41,9 @@ /** Distance from the robot axis to the side. */ #define BOT_SIZE_SIDE 190 +/** Radius of an element. */ +#define BOT_ELEMENT_RADIUS 100 + /** Distance between the front contact point and the robot center. */ #define BOT_FRONT_CONTACT_DIST_MM 150 /** Angle error at the front contact point. */ @@ -53,6 +56,13 @@ /** Distance from robot center to an element near enough to be taken. */ #define BOT_PAWN_TAKING_DISTANCE_MM 150 +/** Distance from border to position in front of a green element. */ +#define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600 +/** Distance to go to capture a green element. */ +#define BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM \ + (BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM - BOT_ELEMENT_RADIUS \ + - BOT_SIZE_FRONT - 20) + /** Speed used for initialisation. */ #ifdef HOST # define BOT_SPEED_INIT 0x20, 0x20, 0x20, 0x20 diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 0022f09a..bc76a2b0 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -31,6 +31,8 @@ #include "chrono.h" #include "logistic.h" +#include "bot.h" +#include "playground.h" /** Elements on table. */ struct element_t element_table[] = @@ -710,19 +712,18 @@ element_get_pos (uint8_t element_id) element_t e = element_get (element_id); position_t pos; pos.v = e.pos; - pos.a = 0; + pos.a = 0xffff; if (e.attr == (ELEMENT_GREEN |ELEMENT_RIGHT)) { - /* Set angle to 90° clockwise. */ - pos.a = 0x4000; - /* Remove 400 mm. */ - pos.v.x -= 400; + /* To the right. */ + pos.a = 0x0000; + pos.v.x = PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM; } if (e.attr == (ELEMENT_GREEN |ELEMENT_LEFT)) { - /* Set angle to 270° clockwise. */ - pos.a = 0xc000; - pos.v.x += 400; + /* To the left. */ + pos.a = 0x8000; + pos.v.x = BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM; } return pos; } diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 4655c3c1..7ef80e48 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -163,7 +163,7 @@ element_nearest_element_id (position_t robot_pos); /** Give the position where the robot need to be placed to get an element. The position correspond to the emplacement of the element with an angle of - zero. If the element is in the green zone, the returned position include + 0xffff. If the element is in the green zone, the returned position include the angle the robot need to set before moving to the element. */ position_t -- cgit v1.2.3 From e4b0fd53b70e7b46522117601940574a67fb2d58 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 01:11:35 +0200 Subject: digital/io-hub: start top FSM --- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/clamp.c | 2 - digital/io-hub/src/robospierre/top.c | 148 ++++++++++++++++++++++++++++++++ 3 files changed, 149 insertions(+), 3 deletions(-) create mode 100644 digital/io-hub/src/robospierre/top.c diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index a9bb2283..43970369 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -4,7 +4,7 @@ BASE = ../../../avr PROGS = io_hub HOST_PROGS = test_element # Sources to compile. -io_hub_SOURCES = main.c \ +io_hub_SOURCES = main.c top.c \ clamp.c logistic.c element.c pawn_sensor.c \ radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index bae3591d..db15639c 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -50,8 +50,6 @@ * element from a slot to another one. */ -FSM_INIT - FSM_STATES ( /* Initial state. */ CLAMP_START, diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c new file mode 100644 index 00000000..5f27d7bc --- /dev/null +++ b/digital/io-hub/src/robospierre/top.c @@ -0,0 +1,148 @@ +/* top.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" + +#include "playground.h" +#include "asserv.h" + +#define FSM_NAME AI +#include "fsm.h" + +#include "logistic.h" +#include "move.h" + +/* + * Here is the top FSM. This FSM is suppose to give life to the robot with an + * impression of intelligence... Well... + */ + +FSM_INIT + +FSM_STATES ( + /* Initial state. */ + TOP_START, + /* Going out of start area. */ + TOP_GOING_OUT1, + /* Going out, first pawn emplacement. */ + TOP_GOING_OUT2, + + TOP_GOING_TO_ELEMENT, + TOP_GOING_TO_GREEN_POS, + TOP_GOING_TO_GREEN_ELEMENT, + TOP_GOING_FROM_GREEN_ELEMENT) + +FSM_START_WITH (TOP_START) + +/** Top context. */ +struct top_t +{ + /** Target element. */ + uint8_t target_element_id; +}; + +/** Global context. */ +struct top_t top_global; +#define ctx top_global + +FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1) +{ + element_init (); + asserv_goto (PG_X (400 + 100), PG_Y (PG_LENGTH - 200), 0); + return FSM_NEXT (TOP_START, init_start_round); +} + +FSM_TRANS (TOP_GOING_OUT1, robot_move_success, TOP_GOING_OUT2) +{ + asserv_goto (PG_X (1500 - 2 * 350), PG_Y (PG_LENGTH - 350), 0); + return FSM_NEXT (TOP_GOING_OUT1, robot_move_success); +} + +uint8_t +top_go_element (void) +{ + position_t robot_pos; + asserv_get_position (&robot_pos); + ctx.target_element_id = element_best (robot_pos); + position_t element_pos = element_get_pos (ctx.target_element_id); + if (element_pos.a != 0xffff) + { + /* Green zone element. */ + move_start (element_pos, 0); + return 2; + } + else + { + /* Regular element. */ + move_start_noangle (element_pos.v, 0, 0); + return 1; + } +} + +FSM_TRANS (TOP_GOING_OUT2, robot_move_success, + element, TOP_GOING_TO_ELEMENT, + green_element, TOP_GOING_TO_GREEN_POS) +{ + if (top_go_element () == 1) + return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element); + else + return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, green_element); +} + +FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, + element, TOP_GOING_TO_ELEMENT, + green_element, TOP_GOING_TO_GREEN_POS) +{ + if (top_go_element () == 1) + return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element); + else + return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, green_element); +} + +FSM_TRANS (TOP_GOING_TO_GREEN_POS, move_success, TOP_GOING_TO_GREEN_ELEMENT) +{ + asserv_move_linearly (BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM); + return FSM_NEXT (TOP_GOING_TO_GREEN_POS, move_success); +} + +FSM_TRANS (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success, + TOP_GOING_FROM_GREEN_ELEMENT) +{ + element_taken (ctx.target_element_id, ELEMENT_PAWN); + asserv_move_linearly (-BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM); + return FSM_NEXT (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success); +} + +FSM_TRANS (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success, + element, TOP_GOING_TO_ELEMENT, + green_element, TOP_GOING_TO_GREEN_POS) +{ + if (top_go_element () == 1) + return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success, + element); + else + return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success, + green_element); +} + -- cgit v1.2.3 From 0e1b966d3a66518bae9b44d888b3daea918f10f7 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 30 May 2011 03:14:40 +0200 Subject: digital/io-hub: change algorithm for logistic --- digital/io-hub/src/robospierre/logistic.c | 222 ++++++++++++++++++------------ digital/io-hub/src/robospierre/logistic.h | 6 + 2 files changed, 143 insertions(+), 85 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 011b85a3..1bbd1ddf 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -61,114 +61,163 @@ logistic_debug_dump (void) #endif } +static void +logistic_tower_possible () +{ + uint8_t i, head = 0, pawn = 0; + for (i = CLAMP_SLOT_FRONT_BOTTOM; i <= CLAMP_SLOT_NB; i++) + { + if (ELEMENT_IS_HEAD (ctx.slots[i])) + head++; + else if (ctx.slots[i]) + pawn++; + if (head >= 1 && pawn >= 2) + { + ctx.tower_possible = 1; + return; + } + } + ctx.tower_possible = 0; + return; +} + + + /** Examine current state and take a decision. */ static void logistic_decision (void) { - uint8_t i; - /* Determine collect_direction. */ - uint8_t front_head = 0, back_head = 0, - front_element = 0, back_element = 0; - uint8_t collect_direction; - for (i = CLAMP_SLOT_FRONT_BOTTOM; i <= CLAMP_SLOT_FRONT_TOP; i++) + /* Reset moving from / to. */ + ctx.moving_from = CLAMP_SLOT_NB; + ctx.moving_to = CLAMP_SLOT_NB; + /* Update if a tower is possible. */ + logistic_tower_possible (); + /* A tower is here, put it in the top ! */ + if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]) && + !ctx.slots[CLAMP_SLOT_FRONT_TOP]) { - if (ctx.slots[i]) + ctx.moving_from = CLAMP_SLOT_FRONT_BOTTOM; + ctx.moving_to = CLAMP_SLOT_FRONT_TOP; + if (!ctx.slots[CLAMP_SLOT_BACK_TOP]) { - front_element++; - if (ELEMENT_IS_HEAD (ctx.slots[i])) - front_head++; + ctx.collect_direction = DIRECTION_BACKWARD; + ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; } + return; } - for (i = CLAMP_SLOT_BACK_BOTTOM; i <= CLAMP_SLOT_BACK_TOP; i++) + if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) && + !ctx.slots[CLAMP_SLOT_BACK_TOP]) { - if (ctx.slots[i]) + ctx.moving_from = CLAMP_SLOT_BACK_BOTTOM; + ctx.moving_to = CLAMP_SLOT_BACK_TOP; + if (!ctx.slots[CLAMP_SLOT_FRONT_TOP]) { - back_element++; - if (ELEMENT_IS_HEAD (ctx.slots[i])) - back_head++; + ctx.collect_direction = DIRECTION_FORWARD; + ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; } + return; } - if (front_head < back_head) - collect_direction = DIRECTION_FORWARD; - else if (front_head > back_head) - collect_direction = DIRECTION_BACKWARD; - else if (front_head) + + /* We can build a tower and we are authorized to do so. */ + if (ctx.tower_possible && ctx.tower_authorized) { - if (front_element < back_element) - collect_direction = DIRECTION_FORWARD; - else if (front_element > back_element) - collect_direction = DIRECTION_BACKWARD; + /* Where to build the tower. */ + uint8_t build_dir; + if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_TOP]) && + ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_FRONT_TOP])) + { + if (ctx.slots[CLAMP_SLOT_BACK_BOTTOM] && + ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]) + build_dir = ctx.collect_direction; + else if (ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) + build_dir = DIRECTION_BACKWARD; + else + build_dir = DIRECTION_FORWARD; + } + else if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_TOP])) + build_dir = DIRECTION_BACKWARD; else - collect_direction = ctx.collect_direction; - } - else - collect_direction = ctx.collect_direction; - ctx.collect_direction = collect_direction; - /* Now use this direction. */ - uint8_t collect_bay, storage_bay; - uint8_t collect_bay_head, storage_bay_head; - if (collect_direction == DIRECTION_FORWARD) - { - collect_bay = CLAMP_SLOT_FRONT_BOTTOM; - storage_bay = CLAMP_SLOT_BACK_BOTTOM; - collect_bay_head = front_head; - storage_bay_head = back_head; - ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; - } - else - { - collect_bay = CLAMP_SLOT_BACK_BOTTOM; - storage_bay = CLAMP_SLOT_FRONT_BOTTOM; - collect_bay_head = back_head; - storage_bay_head = front_head; - ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; - } - /* Find a destination for an element move. */ - uint8_t moving_to = CLAMP_SLOT_NB; - uint8_t moving_from = CLAMP_SLOT_NB; - if (!ctx.slots[collect_bay + 1]) - { - /* Movements in collect bay possible. */ - if (ELEMENT_IS_HEAD (ctx.slots[collect_bay + 0])) + build_dir = DIRECTION_FORWARD; + ctx.collect_direction = build_dir; + /* Fill with pawns. */ + uint8_t build_bay, collect_bay; + if (build_dir == DIRECTION_FORWARD) { - moving_to = collect_bay + 2; - moving_from = collect_bay + 0; + build_bay = CLAMP_SLOT_FRONT_BOTTOM; + collect_bay = CLAMP_SLOT_BACK_BOTTOM; } - } - if (moving_to == CLAMP_SLOT_NB && !ctx.slots[storage_bay + 1]) - { - /* No movement yet and movements in storage bay possible. */ - if (ELEMENT_IS_HEAD (ctx.slots[storage_bay + 0])) + else { - moving_to = storage_bay + 2; - moving_from = storage_bay + 0; + build_bay = CLAMP_SLOT_BACK_BOTTOM; + collect_bay = CLAMP_SLOT_FRONT_BOTTOM; } - else if (storage_bay_head) + if (!ctx.slots[build_bay]) + ctx.moving_to = build_bay; + else if (!ctx.slots[build_bay + 1]) + ctx.moving_to = build_bay + 1; + else { - if (!ctx.slots[storage_bay + 0]) - moving_to = storage_bay + 0; - else if (!ctx.slots[storage_bay + 1]) - moving_to = storage_bay + 1; + /* Build is finished. */ + ctx.tower_ready_side = build_bay; + return; } - } - if (moving_to == CLAMP_SLOT_NB && !ctx.slots[CLAMP_SLOT_SIDE]) - { - /* No movement yet, store in side slot. */ - moving_to = CLAMP_SLOT_SIDE; - } - /* Find a source if available. */ - if (moving_to != CLAMP_SLOT_NB && moving_from == CLAMP_SLOT_NB) - { - if (ctx.slots[collect_bay + 0]) - moving_from = collect_bay + 0; + + if (ctx.slots[collect_bay + 2] && + !ELEMENT_IS_HEAD (ctx.slots[collect_bay + 2])) + ctx.moving_from = collect_bay + 2; + else if (ctx.slots[collect_bay] && + !ELEMENT_IS_HEAD (ctx.slots[collect_bay])) + ctx.moving_from = collect_bay; else if (ctx.slots[CLAMP_SLOT_SIDE]) - moving_from = CLAMP_SLOT_SIDE; + ctx.moving_from = CLAMP_SLOT_SIDE; } - /* Ask for movement. */ - if (moving_from != CLAMP_SLOT_NB) + /* Time to adjust element inside de robot, not build. */ + else { - ctx.moving_from = moving_from; - ctx.moving_to = moving_to; + uint8_t get_bay, put_bay; + if (ctx.collect_direction == DIRECTION_FORWARD) + { + get_bay = CLAMP_SLOT_FRONT_BOTTOM; + put_bay = CLAMP_SLOT_BACK_BOTTOM; + } + else + { + get_bay = CLAMP_SLOT_BACK_BOTTOM; + put_bay = CLAMP_SLOT_FRONT_BOTTOM; + } + /* Adjust some pawns. */ + /* Search source. */ + if (ctx.slots[get_bay] == ELEMENT_PAWN) + ctx.moving_from = get_bay; + else if (ctx.slots[get_bay + 2] == ELEMENT_PAWN) + ctx.moving_from = get_bay + 2; + else if (ctx.slots[put_bay + 2] == ELEMENT_PAWN) + ctx.moving_from = put_bay + 2; + else if (ctx.slots[put_bay] == ELEMENT_PAWN) + ctx.moving_from = put_bay; + + /* Search destination. */ + if (ctx.moving_from != CLAMP_SLOT_NB) + { + if (!ctx.slots[CLAMP_SLOT_SIDE]) + ctx.moving_to = CLAMP_SLOT_SIDE; + else if (!ctx.slots[put_bay]) + ctx.moving_to = put_bay; + else if (!ctx.slots[put_bay + 2] && ctx.moving_from != put_bay) + ctx.moving_to = put_bay + 2; + else + ctx.moving_from = CLAMP_SLOT_NB; + } + if (ctx.moving_from == ctx.moving_to) + { + ctx.moving_from = CLAMP_SLOT_NB; + ctx.moving_to = CLAMP_SLOT_NB; + } + + if (ctx.collect_direction == DIRECTION_FORWARD) + ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; + else + ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; } logistic_debug_dump (); } @@ -181,6 +230,9 @@ logistic_init (void) ctx.slots[i] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; ctx.collect_direction = DIRECTION_FORWARD; + ctx.tower_possible = 0; + ctx.tower_authorized = 1; + ctx.tower_ready_side = DIRECTION_NONE; } void diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 7a742957..93101064 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -44,6 +44,12 @@ struct logistic_t uint8_t collect_direction; /** Idle clamp position, depend on collect direction. */ uint8_t clamp_pos_idle; + /** A tower is possible. */ + uint8_t tower_possible; + /** 0 if we can't build a tower, 1 if we can? */ + uint8_t tower_authorized; + /** Inform if a tower is ready and which side. */ + uint8_t tower_ready_side; }; /** Global context. */ -- cgit v1.2.3 From 5e4f6921f117d650025fee172af7bf4e68b69129 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 11:45:09 +0200 Subject: digital/io-hub, host/simu: add Strat contact --- digital/io-hub/src/robospierre/contact_defs.h | 8 +++++--- digital/io-hub/src/robospierre/pawn_sensor.c | 2 +- digital/io-hub/tools/io_hub/mex.py | 2 +- host/simu/robots/robospierre/model/bag.py | 3 ++- host/simu/robots/robospierre/view/bag.py | 2 ++ 5 files changed, 11 insertions(+), 6 deletions(-) diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h index 1e04f6bf..8c60342a 100644 --- a/digital/io-hub/src/robospierre/contact_defs.h +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -25,8 +25,9 @@ * * }}} */ -#define CONTACT_COLOR A, 7 -#define CONTACT_JACK F, 7 +#define CONTACT_COLOR E, 5 +#define CONTACT_JACK E, 6 +#define CONTACT_STRAT E, 5 #define CONTACT_FRONT_BOTTOM A, 4 #define CONTACT_FRONT_MIDDLE F, 4 #define CONTACT_BACK_BOTTOM A, 5 @@ -42,6 +43,7 @@ CONTACT (CONTACT_BACK_BOTTOM) \ CONTACT (CONTACT_BACK_MIDDLE) \ CONTACT (CONTACT_BACK_TOP) \ - CONTACT (CONTACT_SIDE) + CONTACT (CONTACT_SIDE) \ + CONTACT (CONTACT_STRAT) #endif /* contact_defs_h */ diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index 65f0afe4..3a59e908 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -52,7 +52,7 @@ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; static uint8_t pawn_sensor_get_type (uint8_t direction) { - uint8_t element_type = contact_get_color () ? ELEMENT_PAWN : ELEMENT_KING; + uint8_t element_type = IO_GET (CONTACT_STRAT) ? ELEMENT_PAWN : ELEMENT_KING; return element_type; } diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 44c41010..66060ea1 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -31,7 +31,7 @@ ADC_NB = 8 PWM_NB = 6 PWM_VALUE_MAX = 1024 -CONTACT_NB = 9 +CONTACT_NB = 10 CONTACT_INIT = 0xffffffff class Mex: diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 260960f8..b98dafd9 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -34,8 +34,9 @@ class Bag: def __init__ (self, scheduler, table, link_bag): self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True) self.jack = Switch (link_bag.io_hub.contact[1], invert = True) + self.strat_switch = Switch (link_bag.io_hub.contact[9], invert = True) self.contact = [ Switch (contact) - for contact in link_bag.io_hub.contact[2:] ] + for contact in link_bag.io_hub.contact[2:9] ] self.position = Position (link_bag.asserv.position) self.clamping_motor = MotorBasic (link_bag.io_hub.pwm[2], scheduler, 8 * pi, 0, pi) diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py index 73aa9fe3..b718f6fe 100644 --- a/host/simu/robots/robospierre/view/bag.py +++ b/host/simu/robots/robospierre/view/bag.py @@ -35,6 +35,8 @@ class Bag: self.jack = Switch (sensor_frame, model_bag.jack, 'Jack') self.color_switch = Switch (sensor_frame, model_bag.color_switch, 'Color') + self.strat_switch = Switch (sensor_frame, model_bag.strat_switch, + 'Strat') self.robot = Robot (table, model_bag.position, model_bag.clamp) self.clamp = ( ClampTop (actuator_view.add_view (ClampTop.width, -- cgit v1.2.3 From 907228a5034d71ced8098a1a0973939df42eb9db Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 13:50:42 +0200 Subject: digital/io-hub: improve top FSM --- digital/io-hub/src/robospierre/top.c | 106 ++++++++++++++++++++++++++++------- 1 file changed, 86 insertions(+), 20 deletions(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 5f27d7bc..78bb06aa 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -48,9 +48,11 @@ FSM_STATES ( /* Going out, first pawn emplacement. */ TOP_GOING_OUT2, + TOP_GOING_TO_DROP, TOP_GOING_TO_ELEMENT, TOP_GOING_TO_GREEN_POS, TOP_GOING_TO_GREEN_ELEMENT, + TOP_TAKING_GREEN_ELEMENT, TOP_GOING_FROM_GREEN_ELEMENT) FSM_START_WITH (TOP_START) @@ -60,6 +62,8 @@ struct top_t { /** Target element. */ uint8_t target_element_id; + /** Green element move distance. */ + int16_t green_move_distance_mm; }; /** Global context. */ @@ -79,70 +83,132 @@ FSM_TRANS (TOP_GOING_OUT1, robot_move_success, TOP_GOING_OUT2) return FSM_NEXT (TOP_GOING_OUT1, robot_move_success); } -uint8_t +static uint8_t top_go_element (void) { position_t robot_pos; asserv_get_position (&robot_pos); ctx.target_element_id = element_best (robot_pos); position_t element_pos = element_get_pos (ctx.target_element_id); + uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD + ? 0 : ASSERV_BACKWARD; if (element_pos.a != 0xffff) { /* Green zone element. */ - move_start (element_pos, 0); + ctx.green_move_distance_mm = BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM; + if (backward) + { + element_pos.a += 0x8000; + ctx.green_move_distance_mm = -ctx.green_move_distance_mm; + } + move_start (element_pos, backward); return 2; } else { /* Regular element. */ - move_start_noangle (element_pos.v, 0, 0); + move_start_noangle (element_pos.v, backward, 0); return 1; } } +static uint8_t +top_go_drop (void) +{ + position_t robot_pos; + asserv_get_position (&robot_pos); + uint8_t drop_pos_id = 37; + position_t drop_pos = element_get_pos (drop_pos_id); + uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD + ? 0 : ASSERV_BACKWARD; + move_start_noangle (drop_pos.v, backward, 0); + return 0; +} + +static uint8_t +top_decision (void) +{ + if (logistic_global.tower_possible) + return top_go_drop (); + else + return top_go_element (); +} + FSM_TRANS (TOP_GOING_OUT2, robot_move_success, + drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT, green_element, TOP_GOING_TO_GREEN_POS) { - if (top_go_element () == 1) - return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element); - else - return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, green_element); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, drop); + case 1: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element); + case 2: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, + green_element); + } +} + +FSM_TRANS (TOP_GOING_TO_DROP, move_success, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT, + green_element, TOP_GOING_TO_GREEN_POS) +{ + clamp_drop (logistic_global.collect_direction); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, drop); + case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, element); + case 2: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, + green_element); + } } FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, + drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT, green_element, TOP_GOING_TO_GREEN_POS) { - if (top_go_element () == 1) - return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element); - else - return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, green_element); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop); + case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element); + case 2: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, + green_element); + } } FSM_TRANS (TOP_GOING_TO_GREEN_POS, move_success, TOP_GOING_TO_GREEN_ELEMENT) { - asserv_move_linearly (BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM); + asserv_move_linearly (ctx.green_move_distance_mm); return FSM_NEXT (TOP_GOING_TO_GREEN_POS, move_success); } FSM_TRANS (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success, - TOP_GOING_FROM_GREEN_ELEMENT) + TOP_TAKING_GREEN_ELEMENT) { element_taken (ctx.target_element_id, ELEMENT_PAWN); - asserv_move_linearly (-BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM); return FSM_NEXT (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success); } +FSM_TRANS_TIMEOUT (TOP_TAKING_GREEN_ELEMENT, 250, TOP_GOING_FROM_GREEN_ELEMENT) +{ + asserv_move_linearly (-ctx.green_move_distance_mm); + return FSM_NEXT_TIMEOUT (TOP_TAKING_GREEN_ELEMENT); +} + FSM_TRANS (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success, + drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT, green_element, TOP_GOING_TO_GREEN_POS) { - if (top_go_element () == 1) - return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success, - element); - else - return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success, - green_element); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, + robot_move_success, drop); + case 1: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, + robot_move_success, element); + case 2: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, + robot_move_success, green_element); + } } -- cgit v1.2.3 From 826af7f2a4a4d6a372bc15fb764d8e50c43decf1 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 30 May 2011 13:13:40 +0200 Subject: digital/io-hub: collect direction is opposed to tower build direction. --- digital/io-hub/src/robospierre/logistic.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 1bbd1ddf..af87693e 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -81,8 +81,6 @@ logistic_tower_possible () return; } - - /** Examine current state and take a decision. */ static void logistic_decision (void) @@ -138,7 +136,11 @@ logistic_decision (void) build_dir = DIRECTION_BACKWARD; else build_dir = DIRECTION_FORWARD; - ctx.collect_direction = build_dir; + /* Adapt collect direction. */ + if (build_dir == DIRECTION_FORWARD) + ctx.collect_direction = DIRECTION_BACKWARD; + else + ctx.collect_direction = DIRECTION_FORWARD; /* Fill with pawns. */ uint8_t build_bay, collect_bay; if (build_dir == DIRECTION_FORWARD) -- cgit v1.2.3 From 35d59d44b1b2fa98de77192cefffd0018acd1048 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 30 May 2011 13:08:51 +0200 Subject: digital/io-hub: add special cases and when clamp is broken --- digital/io-hub/src/robospierre/logistic.c | 253 ++++++++++++++++++++++++++---- digital/io-hub/src/robospierre/logistic.h | 17 +- 2 files changed, 233 insertions(+), 37 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index af87693e..1e44d675 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -58,6 +58,10 @@ logistic_debug_dump (void) DPRINTF (" %s => %s", names[0][ctx.moving_from], names[0][ctx.moving_to]); } DPRINTF ("\n"); + DPRINTF ("construct_possible: %u\n", ctx.construct_possible); + DPRINTF ("prepare: %u\n", ctx.prepare); + DPRINTF ("ready: %u\n", ctx.ready); + DPRINTF ("need_prepare: %u\n", ctx.need_prepare); #endif } @@ -65,6 +69,23 @@ static void logistic_tower_possible () { uint8_t i, head = 0, pawn = 0; + ctx.ready = 0; + /* If clamp broken. */ + if (ctx.prepare == 3 && ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]) + { + ctx.construct_possible = 2; + ctx.ready = 1; + ctx.collect_direction = DIRECTION_BACKWARD; + return; + } + else if (ctx.prepare == 3 && ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) + { + ctx.construct_possible = 2; + ctx.ready = 1; + ctx.collect_direction = DIRECTION_FORWARD; + return; + } + for (i = CLAMP_SLOT_FRONT_BOTTOM; i <= CLAMP_SLOT_NB; i++) { if (ELEMENT_IS_HEAD (ctx.slots[i])) @@ -73,51 +94,213 @@ logistic_tower_possible () pawn++; if (head >= 1 && pawn >= 2) { - ctx.tower_possible = 1; + ctx.construct_possible = 1; return; } } - ctx.tower_possible = 0; + if (pawn || head) + ctx.construct_possible = 2; + else + ctx.construct_possible = 0; return; } +/* Check if we can't take element anymore and we need to be authorized to + * unload. */ +static void +logistic_check_need_prepare () +{ + uint8_t i, head = 0, pawn = 0; + for (i = CLAMP_SLOT_FRONT_BOTTOM; i < CLAMP_SLOT_NB; i++) + { + if (ELEMENT_IS_HEAD (ctx.slots[i])) + head++; + else if (ctx.slots[i]) + pawn++; + } + if ((head == 1 && pawn == 3) || + head == 2 || + pawn > 4) + ctx.need_prepare = 1; + else + ctx.need_prepare = 0; +} + +/** Build something not a tower. */ +static void +logistic_put_element () +{ + uint8_t dir_bay, nodir_bay; + if (ctx.collect_direction == DIRECTION_FORWARD) + { + dir_bay = CLAMP_SLOT_FRONT_BOTTOM; + nodir_bay = CLAMP_SLOT_BACK_BOTTOM; + } + else + { + dir_bay = CLAMP_SLOT_BACK_BOTTOM; + nodir_bay = CLAMP_SLOT_FRONT_BOTTOM; + } + + /* If we may build a little tower ? */ + /* Put the tower on the pawn. */ + if (ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 2]) && + ctx.slots[nodir_bay] && + !ctx.slots[nodir_bay + 1]) + { + ctx.moving_from = nodir_bay + 2; + ctx.moving_to = nodir_bay + 1; + return; + } + /* Little tower is ready. */ + else if (ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 1]) && + ctx.slots[nodir_bay]) + ctx.ready = 1; + else if (ELEMENT_IS_HEAD (ctx.slots[nodir_bay])) + ctx.ready = 1; + /* Find a pawn for a lonely head. */ + else if (ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 2])) + { + if (!ctx.slots[nodir_bay] && ctx.slots[CLAMP_SLOT_SIDE]) + { + ctx.moving_from = CLAMP_SLOT_SIDE; + ctx.moving_to = nodir_bay; + } + else if (!ctx.slots[nodir_bay] && !ELEMENT_IS_HEAD (ctx.slots[dir_bay + 2]) && + ctx.slots[dir_bay + 2]) + { + ctx.moving_from = dir_bay + 2; + ctx.moving_to = nodir_bay; + } + else if (!ctx.slots[nodir_bay] && + !ELEMENT_IS_HEAD (ctx.slots[dir_bay + 1]) && + ctx.slots[dir_bay + 1]) + { + ctx.moving_from = dir_bay + 1; + ctx.moving_to = nodir_bay; + } + /* No pawn ! */ + else if (!ctx.slots[nodir_bay]) + { + ctx.moving_from = nodir_bay + 2; + ctx.moving_to = nodir_bay; + } + } + /* If we may build a little tower ? */ + /* Put the tower on the pawn. */ + else if (ELEMENT_IS_HEAD (ctx.slots[dir_bay + 2]) && + ctx.slots[dir_bay] && + !ctx.slots[dir_bay + 1]) + { + ctx.moving_from = dir_bay + 2; + ctx.moving_to = dir_bay + 1; + return; + } + /* Little tower is ready. */ + else if (ELEMENT_IS_HEAD (ctx.slots[dir_bay + 1]) && + ctx.slots[dir_bay]) + ctx.ready = 1; + else if (ELEMENT_IS_HEAD (ctx.slots[dir_bay])) + ctx.ready = 1; + /* Find a pawn for a lonely head. */ + else if (ELEMENT_IS_HEAD (ctx.slots[dir_bay + 2])) + { + if (!ctx.slots[dir_bay] && ctx.slots[CLAMP_SLOT_SIDE]) + { + ctx.moving_from = CLAMP_SLOT_SIDE; + ctx.moving_to = dir_bay; + } + else if (!ctx.slots[dir_bay] && + !ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 2]) && + ctx.slots[nodir_bay + 2]) + { + ctx.moving_from = nodir_bay + 2; + ctx.moving_to = dir_bay; + } + else if (!ctx.slots[dir_bay] && + !ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 1]) && + ctx.slots[nodir_bay + 1]) + { + ctx.moving_from = nodir_bay + 1; + ctx.moving_to = dir_bay; + } + /* No pawn ! */ + else if (!ctx.slots[dir_bay]) + { + ctx.moving_from = dir_bay + 2; + ctx.moving_to = dir_bay; + } + } + /* No head, find any pawn. */ + else if (!ctx.slots[nodir_bay]) + { + ctx.moving_to = nodir_bay; + if (ctx.slots[nodir_bay + 2]) + ctx.moving_from = nodir_bay + 2; + else if (ctx.slots[CLAMP_SLOT_SIDE]) + ctx.moving_from = CLAMP_SLOT_SIDE; + else if (ctx.slots[dir_bay + 2]) + ctx.moving_from = dir_bay + 2; + else if (ctx.slots[dir_bay]) + ctx.moving_from = dir_bay; + } + else if (ctx.slots[nodir_bay]) + { + ctx.ready = 1; + } +} + /** Examine current state and take a decision. */ static void logistic_decision (void) { - /* Reset moving from / to. */ + /* Reset. */ ctx.moving_from = CLAMP_SLOT_NB; ctx.moving_to = CLAMP_SLOT_NB; - /* Update if a tower is possible. */ + ctx.construct_possible = 0; + ctx.ready = 0; + ctx.need_prepare = 0; + + /* Update if a something is possible. */ logistic_tower_possible (); - /* A tower is here, put it in the top ! */ - if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]) && - !ctx.slots[CLAMP_SLOT_FRONT_TOP]) + + /* Clamp is broken, can't move anything. */ + if (ctx.prepare == 3) + return; + + /* Check if we really need to prepare something. */ + logistic_check_need_prepare (); + + /* A head is here, put it in the top ! */ + if (ctx.prepare != 2) { - ctx.moving_from = CLAMP_SLOT_FRONT_BOTTOM; - ctx.moving_to = CLAMP_SLOT_FRONT_TOP; - if (!ctx.slots[CLAMP_SLOT_BACK_TOP]) + if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]) && + !ctx.slots[CLAMP_SLOT_FRONT_TOP]) { - ctx.collect_direction = DIRECTION_BACKWARD; - ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; + ctx.moving_from = CLAMP_SLOT_FRONT_BOTTOM; + ctx.moving_to = CLAMP_SLOT_FRONT_TOP; + if (!ctx.slots[CLAMP_SLOT_BACK_TOP]) + { + ctx.collect_direction = DIRECTION_BACKWARD; + ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; + } + return; } - return; - } - if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) && - !ctx.slots[CLAMP_SLOT_BACK_TOP]) - { - ctx.moving_from = CLAMP_SLOT_BACK_BOTTOM; - ctx.moving_to = CLAMP_SLOT_BACK_TOP; - if (!ctx.slots[CLAMP_SLOT_FRONT_TOP]) + if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) && + !ctx.slots[CLAMP_SLOT_BACK_TOP]) { - ctx.collect_direction = DIRECTION_FORWARD; - ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; + ctx.moving_from = CLAMP_SLOT_BACK_BOTTOM; + ctx.moving_to = CLAMP_SLOT_BACK_TOP; + if (!ctx.slots[CLAMP_SLOT_FRONT_TOP]) + { + ctx.collect_direction = DIRECTION_FORWARD; + ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; + } + return; } - return; } - /* We can build a tower and we are authorized to do so. */ - if (ctx.tower_possible && ctx.tower_authorized) + if (ctx.construct_possible == 1 && ctx.prepare) { /* Where to build the tower. */ uint8_t build_dir; @@ -153,6 +336,7 @@ logistic_decision (void) build_bay = CLAMP_SLOT_BACK_BOTTOM; collect_bay = CLAMP_SLOT_FRONT_BOTTOM; } + /* Destination. */ if (!ctx.slots[build_bay]) ctx.moving_to = build_bay; else if (!ctx.slots[build_bay + 1]) @@ -160,18 +344,24 @@ logistic_decision (void) else { /* Build is finished. */ - ctx.tower_ready_side = build_bay; + ctx.ready = 1; return; } - + /* Source. */ if (ctx.slots[collect_bay + 2] && - !ELEMENT_IS_HEAD (ctx.slots[collect_bay + 2])) + !ELEMENT_IS_HEAD (ctx.slots[collect_bay + 2])) ctx.moving_from = collect_bay + 2; else if (ctx.slots[collect_bay] && !ELEMENT_IS_HEAD (ctx.slots[collect_bay])) ctx.moving_from = collect_bay; else if (ctx.slots[CLAMP_SLOT_SIDE]) ctx.moving_from = CLAMP_SLOT_SIDE; + return; + } + /* We have to prepare an element to put out. */ + else if ((ctx.prepare == 2 || ctx.need_prepare) && ctx.construct_possible) + { + logistic_put_element (); } /* Time to adjust element inside de robot, not build. */ else @@ -232,9 +422,10 @@ logistic_init (void) ctx.slots[i] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; ctx.collect_direction = DIRECTION_FORWARD; - ctx.tower_possible = 0; - ctx.tower_authorized = 1; - ctx.tower_ready_side = DIRECTION_NONE; + ctx.construct_possible = 0; + ctx.prepare = 0; + ctx.ready = 0; + ctx.need_prepare = 0; } void diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 93101064..05b640db 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -44,12 +44,17 @@ struct logistic_t uint8_t collect_direction; /** Idle clamp position, depend on collect direction. */ uint8_t clamp_pos_idle; - /** A tower is possible. */ - uint8_t tower_possible; - /** 0 if we can't build a tower, 1 if we can? */ - uint8_t tower_authorized; - /** Inform if a tower is ready and which side. */ - uint8_t tower_ready_side; + /** Inform TOP if no construct is possible (0), if a tower is possible (1), if an + * other element can be put even if the clamp is broken (2). */ + uint8_t construct_possible; + /** TOP set 0 if we do not want to build something, 1 if we can build a tower, 2 to prepare + anything possible, 3 the clamp is broken, prepare anything with *_BOTTOM. */ + uint8_t prepare; + /** Inform TOP if a construct is ready (1) or not (0). */ + uint8_t ready; + /* Inform TOP that we can't take any more elements and needs to put + * construction somewhere. */ + uint8_t need_prepare; }; /** Global context. */ -- cgit v1.2.3 From 69bae8c81600790e5593facf92be774458d7b7c7 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 30 May 2011 18:15:49 +0200 Subject: digital/io-hub: manage founded towers --- digital/io-hub/src/robospierre/element.h | 5 +++++ digital/io-hub/src/robospierre/logistic.c | 34 ++++++++++++++++++++++++++++++- 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 7ef80e48..01e8f22e 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -44,6 +44,11 @@ #define ELEMENT_TOWER_1_KING 32 #define ELEMENT_TOWER_2_KING 64 +#define ELEMENT_TOWER (ELEMENT_TOWER_1_QUEEN | \ + ELEMENT_TOWER_2_QUEEN | \ + ELEMENT_TOWER_1_KING | \ + ELEMENT_TOWER_2_KING) + /** Return non zero if element is a head, not a pawn. */ #define ELEMENT_IS_HEAD(e) ((e) && !((e) & ELEMENT_PAWN)) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 1e44d675..35e40efa 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -250,6 +250,33 @@ logistic_put_element () } } +/** If we picked up a tower. */ +static void +logistic_new_tower () +{ + uint8_t dir_bay, nodir_bay; + if (ctx.collect_direction == DIRECTION_FORWARD) + { + dir_bay = CLAMP_SLOT_FRONT_BOTTOM; + nodir_bay = CLAMP_SLOT_BACK_BOTTOM; + } + else + { + dir_bay = CLAMP_SLOT_BACK_BOTTOM; + nodir_bay = CLAMP_SLOT_FRONT_BOTTOM; + } + if (ctx.slots[dir_bay] == ELEMENT_TOWER) + { + ctx.ready = 1; + if (ctx.collect_direction == DIRECTION_FORWARD) + ctx.collect_direction = DIRECTION_BACKWARD; + else + ctx.collect_direction = DIRECTION_FORWARD; + } + if (ctx.slots[nodir_bay] == ELEMENT_TOWER) + ctx.ready = 1; +} + /** Examine current state and take a decision. */ static void logistic_decision (void) @@ -268,6 +295,11 @@ logistic_decision (void) if (ctx.prepare == 3) return; + /* We founded a tower ! */ + logistic_new_tower (); + if (ctx.ready) + return; + /* Check if we really need to prepare something. */ logistic_check_need_prepare (); @@ -359,7 +391,7 @@ logistic_decision (void) return; } /* We have to prepare an element to put out. */ - else if ((ctx.prepare == 2 || ctx.need_prepare) && ctx.construct_possible) + else if (ctx.prepare == 2 && ctx.construct_possible) { logistic_put_element (); } -- cgit v1.2.3 From 3eeefad855e75c09a62579b3d2cbf51894d137ca Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Mon, 30 May 2011 18:39:12 +0200 Subject: digital/io-hub: added some logistic dump and little fix in top --- digital/io-hub/src/robospierre/logistic.c | 12 +++++++++++- digital/io-hub/src/robospierre/top.c | 3 ++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 35e40efa..a9712c23 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -293,12 +293,18 @@ logistic_decision (void) /* Clamp is broken, can't move anything. */ if (ctx.prepare == 3) + { + logistic_debug_dump (); return; + } /* We founded a tower ! */ logistic_new_tower (); if (ctx.ready) + { + logistic_debug_dump (); return; + } /* Check if we really need to prepare something. */ logistic_check_need_prepare (); @@ -316,6 +322,7 @@ logistic_decision (void) ctx.collect_direction = DIRECTION_BACKWARD; ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; } + logistic_debug_dump (); return; } if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) && @@ -328,6 +335,7 @@ logistic_decision (void) ctx.collect_direction = DIRECTION_FORWARD; ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; } + logistic_debug_dump (); return; } } @@ -377,6 +385,7 @@ logistic_decision (void) { /* Build is finished. */ ctx.ready = 1; + logistic_debug_dump (); return; } /* Source. */ @@ -388,6 +397,7 @@ logistic_decision (void) ctx.moving_from = collect_bay; else if (ctx.slots[CLAMP_SLOT_SIDE]) ctx.moving_from = CLAMP_SLOT_SIDE; + logistic_debug_dump (); return; } /* We have to prepare an element to put out. */ @@ -455,7 +465,7 @@ logistic_init (void) ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; ctx.collect_direction = DIRECTION_FORWARD; ctx.construct_possible = 0; - ctx.prepare = 0; + ctx.prepare = 1; ctx.ready = 0; ctx.need_prepare = 0; } diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 78bb06aa..5c10a0f3 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -128,7 +128,8 @@ top_go_drop (void) static uint8_t top_decision (void) { - if (logistic_global.tower_possible) + /* If we can make a tower. */ + if (logistic_global.construct_possible == 1) return top_go_drop (); else return top_go_element (); -- cgit v1.2.3 From b8d4b6293bbc4ee12e0102db7858bb07d0ac57b8 Mon Sep 17 00:00:00 2001 From: Maxime Hadjinlian Date: Sat, 28 May 2011 20:06:13 +0200 Subject: digital/io-serial/src/codebar: initial codebar code --- digital/avr/modules/twi/twi_hard.avr.c | 1 + digital/io-serial/src/codebar/Makefile | 16 +++ digital/io-serial/src/codebar/avrconfig.h | 110 +++++++++++++++++++ digital/io-serial/src/codebar/codebar.c | 171 ++++++++++++++++++++++++++++++ digital/io-serial/src/codebar/codebar.h | 39 +++++++ 5 files changed, 337 insertions(+) create mode 100644 digital/io-serial/src/codebar/Makefile create mode 100644 digital/io-serial/src/codebar/avrconfig.h create mode 100644 digital/io-serial/src/codebar/codebar.c create mode 100644 digital/io-serial/src/codebar/codebar.h diff --git a/digital/avr/modules/twi/twi_hard.avr.c b/digital/avr/modules/twi/twi_hard.avr.c index 8bd18d34..71483ecb 100644 --- a/digital/avr/modules/twi/twi_hard.avr.c +++ b/digital/avr/modules/twi/twi_hard.avr.c @@ -56,6 +56,7 @@ # if defined (__AVR_ATmega32__) # elif defined (__AVR_ATmega64__) # elif defined (__AVR_ATmega128__) +# elif defined (__AVR_ATmega164P__) # elif defined (__AVR_AT90USB646__) # elif defined (__AVR_AT90USB647__) # elif defined (__AVR_AT90USB1286__) diff --git a/digital/io-serial/src/codebar/Makefile b/digital/io-serial/src/codebar/Makefile new file mode 100644 index 00000000..a62147a3 --- /dev/null +++ b/digital/io-serial/src/codebar/Makefile @@ -0,0 +1,16 @@ +BASE = ../../../avr +AVR_PROGS = codebar_reader +codebar_reader_SOURCES = codebar.c timer.avr.c +MODULES = proto twi uart utils +AI_MODULES = utils +CONFIGFILE = avrconfig.h +AVR_MCU = atmega164p +# -O2 : speed +# -Os : size +OPTIMIZE = -Os + +vpath %.c $(AI_MODULES:%=../../../ai/src/%) +vpath %.h $(AI_MODULES:%=../../../ai/src/%) +INCLUDES += $(AI_MODULES:%=-I../../../ai/src/%) + +include $(BASE)/make/Makefile.gen diff --git a/digital/io-serial/src/codebar/avrconfig.h b/digital/io-serial/src/codebar/avrconfig.h new file mode 100644 index 00000000..6acd0e81 --- /dev/null +++ b/digital/io-serial/src/codebar/avrconfig.h @@ -0,0 +1,110 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - Codebar configuration template. */ +/* io-serial.codebar - Codebar AVR module. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * Robot APB Team/Efrei 2011. + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * 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. + * + * }}} */ + +/* global */ +/** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, + * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#define AC_FREQ 14745600 + +/* uart - UART module. */ +/** Select hardware uart for primary uart: 0, 1 or -1 to disable. */ +#define AC_UART0_PORT 0 +/** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, + * 115200, 230400, 250000, 500000, 1000000. */ +#define AC_UART0_BAUDRATE 115200 +/** Send mode: + * - POLLING: no interrupts. + * - RING: interrupts, ring buffer. */ +#define AC_UART0_SEND_MODE RING +/** Recv mode, same as send mode. */ +#define AC_UART0_RECV_MODE RING +/** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */ +#define AC_UART0_CHAR_SIZE 8 +/** Parity : ODD, EVEN, NONE. */ +#define AC_UART0_PARITY EVEN +/** Stop bits : 1, 2. */ +#define AC_UART0_STOP_BITS 1 +/** Send buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_SEND_BUFFER_SIZE 32 +/** Recv buffer size, should be power of 2 for RING mode. */ +#define AC_UART0_RECV_BUFFER_SIZE 32 +/** If the send buffer is full when putc: + * - DROP: drop the new byte. + * - WAIT: wait until there is room in the send buffer. */ +#define AC_UART0_SEND_BUFFER_FULL WAIT +/** In HOST compilation: + * - STDIO: use stdin/out. + * - PTS: use pseudo terminal. */ +#define AC_UART0_HOST_DRIVER PTS +/** Same thing for secondary port. */ +#define AC_UART1_PORT 1 +#define AC_UART1_BAUDRATE 115200 +#define AC_UART1_SEND_MODE RING +#define AC_UART1_RECV_MODE RING +#define AC_UART1_CHAR_SIZE 8 +#define AC_UART1_PARITY EVEN +#define AC_UART1_STOP_BITS 1 +#define AC_UART1_SEND_BUFFER_SIZE 32 +#define AC_UART1_RECV_BUFFER_SIZE 32 +#define AC_UART1_SEND_BUFFER_FULL WAIT +#define AC_UART1_HOST_DRIVER PTS + +/* twi - TWI module. */ +/** Driver to implement TWI: HARD, SOFT, or USI. */ +#define AC_TWI_DRIVER HARD +/** Do not use interrupts. */ +#define AC_TWI_NO_INTERRUPT 0 +/** TWI frequency, should really be 100 kHz. */ +#define AC_TWI_FREQ 100000 +/** Enable slave part. */ +#define AC_TWI_SLAVE_ENABLE 1 +/** Enable master part. */ +#define AC_TWI_MASTER_ENABLE 0 +/** Use polled slave mode: received data is stored in a buffer which can be + * polled using twi_slave_poll. */ +#define AC_TWI_SLAVE_POLLED 1 +/** Slave reception callback to be defined by the user when not in polled + * mode. */ +#undef AC_TWI_SLAVE_RECV +/** Use internal pull up. */ +#define AC_TWI_PULL_UP 0 +/** Slave reception buffer size. */ +#define AC_TWI_SLAVE_RECV_BUFFER_SIZE 16 +/** Slave transmission buffer size. */ +#define AC_TWI_SLAVE_SEND_BUFFER_SIZE 16 + +/* proto - Protocol module. */ +/** Maximum argument size. */ +#define AC_PROTO_ARGS_MAX_SIZE 12 +/** Callback function name. */ +#define AC_PROTO_CALLBACK proto_callback +/** Putchar function name. */ +#define AC_PROTO_PUTC uart0_putc +/** Support for quote parameter. */ +#define AC_PROTO_QUOTE 1 + +#endif /* avrconfig_h */ diff --git a/digital/io-serial/src/codebar/codebar.c b/digital/io-serial/src/codebar/codebar.c new file mode 100644 index 00000000..056508c3 --- /dev/null +++ b/digital/io-serial/src/codebar/codebar.c @@ -0,0 +1,171 @@ +/* codebar.c */ +/* codebar - Codebar Reader. {{{ + * + * Copyright (C) 2011 + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#include +#include +#include + +#include "common.h" +#include "codebar.h" +#include "modules/twi/twi.h" +#include "modules/proto/proto.h" +#include "modules/uart/uart.h" +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "modules/utils/crc.h" +#include "timer.h" + +/* from robospierre/element.h file */ +#define ELEMENT_UNKOWN 0 +#define ELEMENT_PAWN 1 +#define ELEMENT_QUEEN 2 +#define ELEMENT_KING 4 + +#define KING "KING" +#define QUEEN "QUEEN" +#define PAWN "PAWN" + +#define STRING_MAX 10 + +struct status_t +{ + uint16_t age1; + uint8_t piece1; + uint16_t age2; + uint8_t piece2; +}; + +void proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) +{ + /* nothing here*/ +} + +uint8_t string_to_element(char* data) +{ + if (strlen(data) == 5) + return ELEMENT_QUEEN; + if (strcmp(data, KING) == 0) + return ELEMENT_KING; + if (strcmp(data, PAWN) == 0) + return ELEMENT_PAWN; + return ELEMENT_UNKOWN; +} + +char* read_string(int uart_port) +{ + static int cnt_char_u0 = 0; + static char buffer_u0[STRING_MAX] = {'\0'}; + static int cnt_char_u1 = 0; + static char buffer_u1[STRING_MAX] = {'\0'}; + + char c; + + if (uart_port == 0 && uart0_poll ()) + { + while ( uart0_poll() && (c = uart0_getc()) != '\r') + { + buffer_u0[cnt_char_u0] = c; + cnt_char_u0++; + } + buffer_u0[cnt_char_u0+1] = '\0'; + cnt_char_u0 = 0; + return buffer_u0; + } + else if (uart_port == 1 && uart1_poll()) + { + while ( uart1_poll() && (c = uart1_getc()) != '\r') + { + buffer_u1[cnt_char_u1] = c; + cnt_char_u1++; + } + buffer_u1[cnt_char_u1+1] = '\0'; + cnt_char_u1 = 0; + return buffer_u1; + } + else + { + return NULL; + } +} + +int +main (int argc, char **argv) +{ + char* buffer; + struct status_t status; + + status.age1 = 0; + status.piece1 = ELEMENT_UNKOWN; + status.age2 = 0; + status.piece2 = ELEMENT_UNKOWN; + + timer_init (); + avr_init (argc, argv); + sei (); + uart0_init (); + uart1_init (); + /* We have successfully boot. */ + proto_send0 ('z'); + /* Initialize TWI. */ + twi_init (0x04); + /* I am a slave. */ + proto_send0 ('S'); + + while (1) + { + /* Wait until next cycle. */ + timer_wait (); + if (status.age1 < (uint16_t) -1) + status.age1 ++; + if (status.age2 < (uint16_t) -1) + status.age2 ++; + + if ((buffer = read_string(0)) != NULL) + { + status.piece1 = string_to_element(buffer); + } + + if ((buffer = read_string(1)) != NULL) + { + status.piece2 = string_to_element(buffer); + } + + uint8_t status_with_crc[8]; + uint8_t *status_twi = &status_with_crc[1]; + status_twi[0] = v16_to_v8 (status.age1, 0); + status_twi[1] = v16_to_v8 (status.age1, 1); + status_twi[2] = status.piece1; + status_twi[3] = v16_to_v8 (status.age2, 0); + status_twi[4] = v16_to_v8 (status.age2, 1); + status_twi[5] = status.piece2; + status_twi[6] = 42; + status_twi[7] = 32; + /* Compute CRC. */ + status_with_crc[0] = crc_compute (&status_with_crc[1], sizeof (status_with_crc) - 1); + twi_slave_update (status_with_crc, sizeof (status_with_crc)); + + } + return 0; +} diff --git a/digital/io-serial/src/codebar/codebar.h b/digital/io-serial/src/codebar/codebar.h new file mode 100644 index 00000000..5b84f898 --- /dev/null +++ b/digital/io-serial/src/codebar/codebar.h @@ -0,0 +1,39 @@ +#ifndef codebar_h +#define codebar_h +/* codebar.h */ +/* {{{ + * + * Copyright (C) 2011 + * + * Robot APB Team/Efrei 2011 + * Web: http://assos.efrei.fr/robot/ + * Email: robot AT efrei DOT fr + * + * 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. + * + * }}} */ + +/* Read a string from UART0 and return NULL or the string read. */ +char* read_string(int uart_port); + +/* Take a string and check it againt the list of valid element + * as defined in io-hub/src/robospierre/element.h and return the + * u8 value of this element. + */ +uint8_t string_to_element(char* data); + +void proto_callback (uint8_t cmd, uint8_t size, uint8_t *args); + +#endif /* codebar_h */ -- cgit v1.2.3 From 498657366716d99aee951aca3a83ba842aa86239 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 17:28:12 +0200 Subject: digital/io-serial/src/codebar: code update, tested --- digital/avr/modules/uart/uart.avr.c | 1 + digital/io-serial/src/codebar/Makefile | 2 +- digital/io-serial/src/codebar/avrconfig.h | 18 +--- digital/io-serial/src/codebar/codebar.c | 173 ++++++++++++++---------------- digital/io-serial/src/codebar/codebar.h | 39 ------- 5 files changed, 84 insertions(+), 149 deletions(-) delete mode 100644 digital/io-serial/src/codebar/codebar.h diff --git a/digital/avr/modules/uart/uart.avr.c b/digital/avr/modules/uart/uart.avr.c index 689866ae..0b0ab107 100644 --- a/digital/avr/modules/uart/uart.avr.c +++ b/digital/avr/modules/uart/uart.avr.c @@ -35,6 +35,7 @@ #if defined (__AVR_ATmega8__) #elif defined (__AVR_ATmega8535__) #elif defined (__AVR_ATmega16__) +#elif defined (__AVR_ATmega164P__) #elif defined (__AVR_ATmega32__) #elif defined (__AVR_ATmega64__) #elif defined (__AVR_ATmega128__) diff --git a/digital/io-serial/src/codebar/Makefile b/digital/io-serial/src/codebar/Makefile index a62147a3..f607da5f 100644 --- a/digital/io-serial/src/codebar/Makefile +++ b/digital/io-serial/src/codebar/Makefile @@ -1,7 +1,7 @@ BASE = ../../../avr AVR_PROGS = codebar_reader codebar_reader_SOURCES = codebar.c timer.avr.c -MODULES = proto twi uart utils +MODULES = twi uart utils AI_MODULES = utils CONFIGFILE = avrconfig.h AVR_MCU = atmega164p diff --git a/digital/io-serial/src/codebar/avrconfig.h b/digital/io-serial/src/codebar/avrconfig.h index 6acd0e81..b7624a5c 100644 --- a/digital/io-serial/src/codebar/avrconfig.h +++ b/digital/io-serial/src/codebar/avrconfig.h @@ -35,7 +35,7 @@ #define AC_UART0_PORT 0 /** Baudrate: 2400, 4800, 9600, 14400, 19200, 28800, 38400, 57600, 76800, * 115200, 230400, 250000, 500000, 1000000. */ -#define AC_UART0_BAUDRATE 115200 +#define AC_UART0_BAUDRATE 9600 /** Send mode: * - POLLING: no interrupts. * - RING: interrupts, ring buffer. */ @@ -45,7 +45,7 @@ /** Character size: 5, 6, 7, 8, 9 (only 8 implemented). */ #define AC_UART0_CHAR_SIZE 8 /** Parity : ODD, EVEN, NONE. */ -#define AC_UART0_PARITY EVEN +#define AC_UART0_PARITY NONE /** Stop bits : 1, 2. */ #define AC_UART0_STOP_BITS 1 /** Send buffer size, should be power of 2 for RING mode. */ @@ -62,11 +62,11 @@ #define AC_UART0_HOST_DRIVER PTS /** Same thing for secondary port. */ #define AC_UART1_PORT 1 -#define AC_UART1_BAUDRATE 115200 +#define AC_UART1_BAUDRATE 9600 #define AC_UART1_SEND_MODE RING #define AC_UART1_RECV_MODE RING #define AC_UART1_CHAR_SIZE 8 -#define AC_UART1_PARITY EVEN +#define AC_UART1_PARITY NONE #define AC_UART1_STOP_BITS 1 #define AC_UART1_SEND_BUFFER_SIZE 32 #define AC_UART1_RECV_BUFFER_SIZE 32 @@ -97,14 +97,4 @@ /** Slave transmission buffer size. */ #define AC_TWI_SLAVE_SEND_BUFFER_SIZE 16 -/* proto - Protocol module. */ -/** Maximum argument size. */ -#define AC_PROTO_ARGS_MAX_SIZE 12 -/** Callback function name. */ -#define AC_PROTO_CALLBACK proto_callback -/** Putchar function name. */ -#define AC_PROTO_PUTC uart0_putc -/** Support for quote parameter. */ -#define AC_PROTO_QUOTE 1 - #endif /* avrconfig_h */ diff --git a/digital/io-serial/src/codebar/codebar.c b/digital/io-serial/src/codebar/codebar.c index 056508c3..8d845784 100644 --- a/digital/io-serial/src/codebar/codebar.c +++ b/digital/io-serial/src/codebar/codebar.c @@ -23,12 +23,11 @@ * * }}} */ -#include +#include #include #include #include "common.h" -#include "codebar.h" #include "modules/twi/twi.h" #include "modules/proto/proto.h" #include "modules/uart/uart.h" @@ -39,13 +38,8 @@ /* from robospierre/element.h file */ #define ELEMENT_UNKOWN 0 -#define ELEMENT_PAWN 1 -#define ELEMENT_QUEEN 2 -#define ELEMENT_KING 4 - -#define KING "KING" -#define QUEEN "QUEEN" -#define PAWN "PAWN" +#define ELEMENT_QUEEN 4 +#define ELEMENT_KING 8 #define STRING_MAX 10 @@ -57,64 +51,56 @@ struct status_t uint8_t piece2; }; -void proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) -{ - /* nothing here*/ -} +char buffer_u0[STRING_MAX], buffer_u1[STRING_MAX]; -uint8_t string_to_element(char* data) +uint8_t +string_to_element (char *data, uint8_t data_len) { - if (strlen(data) == 5) - return ELEMENT_QUEEN; - if (strcmp(data, KING) == 0) - return ELEMENT_KING; - if (strcmp(data, PAWN) == 0) - return ELEMENT_PAWN; + uint8_t i; + for (i = 0; i < data_len - 5; i++) + { + if (memcmp (data + i, "QUEEN", 5) == 0) + { + data[i] = 0; + return ELEMENT_QUEEN; + } + } + for (i = 0; i < data_len - 4; i++) + { + if (memcmp (data + i, "KING", 4) == 0) + { + data[i] = 0; + return ELEMENT_KING; + } + } return ELEMENT_UNKOWN; } -char* read_string(int uart_port) -{ - static int cnt_char_u0 = 0; - static char buffer_u0[STRING_MAX] = {'\0'}; - static int cnt_char_u1 = 0; - static char buffer_u1[STRING_MAX] = {'\0'}; - - char c; - - if (uart_port == 0 && uart0_poll ()) - { - while ( uart0_poll() && (c = uart0_getc()) != '\r') - { - buffer_u0[cnt_char_u0] = c; - cnt_char_u0++; - } - buffer_u0[cnt_char_u0+1] = '\0'; - cnt_char_u0 = 0; - return buffer_u0; - } - else if (uart_port == 1 && uart1_poll()) - { - while ( uart1_poll() && (c = uart1_getc()) != '\r') - { - buffer_u1[cnt_char_u1] = c; - cnt_char_u1++; - } - buffer_u1[cnt_char_u1+1] = '\0'; - cnt_char_u1 = 0; - return buffer_u1; - } - else - { - return NULL; - } +void +read_strings (void) +{ + uint8_t i; + while (uart0_poll ()) + { + /* Insert char at end of string. */ + for (i = 1; i < STRING_MAX; i++) + buffer_u0[i - 1] = buffer_u0[i]; + buffer_u0[STRING_MAX - 1] = uart0_getc (); + } + while (uart1_poll ()) + { + /* Insert char at end of string. */ + for (i = 1; i < STRING_MAX; i++) + buffer_u1[i - 1] = buffer_u1[i]; + buffer_u1[STRING_MAX - 1] = uart1_getc (); + } } int main (int argc, char **argv) { - char* buffer; struct status_t status; + uint8_t element_type; status.age1 = 0; status.piece1 = ELEMENT_UNKOWN; @@ -126,46 +112,43 @@ main (int argc, char **argv) sei (); uart0_init (); uart1_init (); - /* We have successfully boot. */ - proto_send0 ('z'); /* Initialize TWI. */ - twi_init (0x04); - /* I am a slave. */ - proto_send0 ('S'); - - while (1) - { - /* Wait until next cycle. */ - timer_wait (); - if (status.age1 < (uint16_t) -1) - status.age1 ++; - if (status.age2 < (uint16_t) -1) - status.age2 ++; - - if ((buffer = read_string(0)) != NULL) - { - status.piece1 = string_to_element(buffer); - } - - if ((buffer = read_string(1)) != NULL) - { - status.piece2 = string_to_element(buffer); - } + twi_init (0x20); - uint8_t status_with_crc[8]; - uint8_t *status_twi = &status_with_crc[1]; - status_twi[0] = v16_to_v8 (status.age1, 0); - status_twi[1] = v16_to_v8 (status.age1, 1); - status_twi[2] = status.piece1; - status_twi[3] = v16_to_v8 (status.age2, 0); - status_twi[4] = v16_to_v8 (status.age2, 1); - status_twi[5] = status.piece2; - status_twi[6] = 42; - status_twi[7] = 32; - /* Compute CRC. */ - status_with_crc[0] = crc_compute (&status_with_crc[1], sizeof (status_with_crc) - 1); - twi_slave_update (status_with_crc, sizeof (status_with_crc)); - - } + while (1) + { + /* Wait until next cycle. */ + timer_wait (); + if (status.age1 < (uint16_t) -1) + status.age1 ++; + if (status.age2 < (uint16_t) -1) + status.age2 ++; + + read_strings (); + element_type = string_to_element (buffer_u0, STRING_MAX); + if (element_type) + { + status.piece1 = element_type; + status.age1 = 0; + } + element_type = string_to_element (buffer_u1, STRING_MAX); + if (element_type) + { + status.piece2 = element_type; + status.age2 = 0; + } + + uint8_t status_with_crc[7]; + uint8_t *status_twi = &status_with_crc[1]; + status_twi[0] = v16_to_v8 (status.age1, 1); + status_twi[1] = v16_to_v8 (status.age1, 0); + status_twi[2] = status.piece1; + status_twi[3] = v16_to_v8 (status.age2, 1); + status_twi[4] = v16_to_v8 (status.age2, 0); + status_twi[5] = status.piece2; + /* Compute CRC. */ + status_with_crc[0] = crc_compute (&status_with_crc[1], sizeof (status_with_crc) - 1); + twi_slave_update (status_with_crc, sizeof (status_with_crc)); + } return 0; } diff --git a/digital/io-serial/src/codebar/codebar.h b/digital/io-serial/src/codebar/codebar.h deleted file mode 100644 index 5b84f898..00000000 --- a/digital/io-serial/src/codebar/codebar.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef codebar_h -#define codebar_h -/* codebar.h */ -/* {{{ - * - * Copyright (C) 2011 - * - * Robot APB Team/Efrei 2011 - * Web: http://assos.efrei.fr/robot/ - * Email: robot AT efrei DOT fr - * - * 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. - * - * }}} */ - -/* Read a string from UART0 and return NULL or the string read. */ -char* read_string(int uart_port); - -/* Take a string and check it againt the list of valid element - * as defined in io-hub/src/robospierre/element.h and return the - * u8 value of this element. - */ -uint8_t string_to_element(char* data); - -void proto_callback (uint8_t cmd, uint8_t size, uint8_t *args); - -#endif /* codebar_h */ -- cgit v1.2.3 From da7e2b5bd1f777a7a23553802524522376cce5e8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 18:49:15 +0200 Subject: digital/io-hub: change clamp parameters --- digital/io-hub/src/robospierre/bot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 1c068def..4333ba8a 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -109,7 +109,7 @@ # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ELEVATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP (0x1da7 - 250) -# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x34f7 +# define BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP 0x35e2 # define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0x0169 # define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x1f03 - 250) # define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3610 -- cgit v1.2.3 From 683d37717c83cd8ec8b752c194cb8c0bbc19af6d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 19:48:03 +0200 Subject: digital/io-hub: add !w command --- digital/io-hub/src/robospierre/main.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 60fe28c0..79c0fe2a 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -315,6 +315,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) * - 1b: non zero to open. */ clamp_door (args[0], args[1]); break; + case c ('w', 0): + /* Disable all motor control. */ + mimot_motor0_free (); + mimot_motor1_free (); + asserv_free_motor (); + break; /* Stats commands. * - b: interval between stats. */ case c ('A', 1): -- cgit v1.2.3 From 850cc292ac7ea847f6be2afcdc5bae89418cabda Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 30 May 2011 23:42:52 +0200 Subject: digital/io-hub: add path finding --- digital/io-hub/src/robospierre/Makefile | 2 +- digital/io-hub/src/robospierre/avrconfig.h | 6 + digital/io-hub/src/robospierre/path.c | 398 ++++++++++++++++++++++++++++- digital/io-hub/src/robospierre/simu.host.c | 16 ++ digital/io-hub/tools/io_hub/mex.py | 19 ++ host/simu/robots/robospierre/model/bag.py | 1 + host/simu/robots/robospierre/view/bag.py | 1 + 7 files changed, 440 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 43970369..714541dc 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -16,7 +16,7 @@ test_element_SOURCES = test_element.c logistic.c element.c # Modules needed for IO. MODULES = proto uart twi utils \ adc devices/usdist \ - math/fixed math/geometry + math/fixed math/geometry path/astar AI_MODULES = twi_master common utils fsm move test_element_MODULES = host math/fixed math/geometry # Configuration file. diff --git a/digital/io-hub/src/robospierre/avrconfig.h b/digital/io-hub/src/robospierre/avrconfig.h index c3f107c5..581fa136 100644 --- a/digital/io-hub/src/robospierre/avrconfig.h +++ b/digital/io-hub/src/robospierre/avrconfig.h @@ -120,6 +120,12 @@ /** Number of possible obstacles. */ #define AC_PATH_OBSTACLES_NB 2 +/* astar - A* path finding module. */ +/** Neighbor callback. */ +#define AC_ASTAR_NEIGHBOR_CALLBACK path_astar_neighbor_callback +/** Heuristic callback. */ +#define AC_ASTAR_HEURISTIC_CALLBACK path_astar_heuristic_callback + /* io-hub - io/ai board. */ /** TWI address of the io board. */ #define AC_IO_TWI_ADDRESS 10 diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index b3c91ac8..1122c684 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -23,18 +23,235 @@ * * }}} */ #include "common.h" +#include "defs.h" #include "path.h" +#include "bot.h" +#include "playground.h" + +#include "modules/path/astar/astar.h" +#include "modules/utils/utils.h" +#include "modules/math/geometry/distance.h" + +#define PATH_DEBUG 0 + +#if PATH_DEBUG +#include "debug.host.h" +#endif + +/** + * This year, due to the large number of obstacles, a grid like structure is + * used for path finding on the playground. The A* algorithm is used to find + * path along nodes. + * + * The grid is composed of 11 columns of 4 node each. They are numbered by + * column. Even columns are aligned with center of squares, while odd columns + * are at squares intersections. Therefore, odd columns have a offset of + * 352/2 mm, and that is the reason why code should handle odd and even + * columns differently. + * + * All those tricks are used to reduce the number of nodes. + */ + +/** Number of possible obstacles. */ +#define PATH_OBSTACLES_NB AC_PATH_OBSTACLES_NB + +/** Number of nodes in a column. */ +#define PATH_COLUMN_NODES_NB 4 + +/** Number of columns. */ +#define PATH_COLUMNS_NB 11 + +/** Number of nodes in the grid. */ +#define PATH_GRID_NODES_NB (PATH_COLUMNS_NB * PATH_COLUMN_NODES_NB) + +/** Number of nodes in search graph, last two nodes are destination and source + * nodes. */ +#define PATH_NODES_NB (PATH_GRID_NODES_NB + 2) + +/** Index of destination node. */ +#define PATH_DST_NODE_INDEX PATH_GRID_NODES_NB + +/** Index of source node. */ +#define PATH_SRC_NODE_INDEX (PATH_DST_NODE_INDEX + 1) + +/** Information on a node. */ +struct path_node_t +{ + /** Whether this node can be used. */ + uint8_t usable; +}; /** Context. */ struct path_t { + /** List of obstacles. */ + struct path_obstacle_t obstacles[PATH_OBSTACLES_NB]; + /** Escape factor, 0 if none. */ + uint8_t escape_factor; + /** List of nodes used for A*. */ + struct astar_node_t astar_nodes[PATH_NODES_NB]; + /** Cache of whether a node is blocked. */ + uint8_t valid[PATH_GRID_NODES_NB]; /** Position of end points. */ vect_t endpoints[2]; /** Whether the last update was a success. */ uint8_t found; + /** Which node to look at for next step. */ + uint8_t get; }; static struct path_t path; +/** Static information on nodes. */ +static const struct path_node_t path_nodes[PATH_GRID_NODES_NB] = { + /* {{{ */ + { 1 }, /* 0 column 0. */ + { 1 }, /* 1 */ + { 1 }, /* 2 */ + { 1 }, /* 3 */ + { 1 }, /* 4 column 1. */ + { 1 }, /* 5 */ + { 1 }, /* 6 */ + { 1 }, /* 7 */ + { 1 }, /* 8 column 2. */ + { 1 }, /* 9 */ + { 1 }, /* 10 */ + { 1 }, /* 11 */ + { 1 }, /* 12 column 3. */ + { 1 }, /* 13 */ + { 1 }, /* 14 */ + { 1 }, /* 15 */ + { 1 }, /* 16 column 4. */ + { 1 }, /* 17 */ + { 1 }, /* 18 */ + { 1 }, /* 19 */ + { 1 }, /* 20 column 5. */ + { 1 }, /* 21 */ + { 1 }, /* 22 */ + { 1 }, /* 23 */ + { 1 }, /* 24 column 6. */ + { 1 }, /* 25 */ + { 1 }, /* 26 */ + { 1 }, /* 27 */ + { 1 }, /* 28 column 7. */ + { 1 }, /* 29 */ + { 1 }, /* 30 */ + { 1 }, /* 31 */ + { 1 }, /* 32 column 8. */ + { 1 }, /* 33 */ + { 1 }, /* 34 */ + { 1 }, /* 35 */ + { 1 }, /* 36 column 9. */ + { 1 }, /* 37 */ + { 1 }, /* 38 */ + { 1 }, /* 39 */ + { 1 }, /* 40 column 10. */ + { 1 }, /* 41 */ + { 1 }, /* 42 */ + { 1 }, /* 43 */ + /* }}} */ +}; + +/** Compute position of a node. */ +static void +path_pos (uint8_t node, vect_t *pos) +{ + assert (node < PATH_NODES_NB); + if (node < PATH_GRID_NODES_NB) + { + uint8_t col = node / PATH_COLUMN_NODES_NB; + uint8_t line = node - col * PATH_COLUMN_NODES_NB; + pos->x = 400 + 50 + 350 / 2 + col * 350 / 2; + pos->y = 2100 - 350 - 350 / 2 + + (col % 2 ? 350 / 2 : 0) + - line * 350; + } + else + { + *pos = path.endpoints[node - PATH_GRID_NODES_NB]; + } +} + +/** Return 1 if the direct path between a and b nodes is blocked, also compute + * distance. */ +static uint8_t +path_blocking (uint8_t a, uint8_t b, int16_t *dp) +{ + uint8_t i; + vect_t va; + vect_t vb; + uint8_t escape_factor = 0; + if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX) + escape_factor = path.escape_factor; + path_pos (a, &va); + path_pos (b, &vb); + /* Test for a blocking obstacle. */ + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid) + { + uint16_t d = distance_segment_point (&va, &vb, + &path.obstacles[i].c); + if (d < path.obstacles[i].r) + { + if (escape_factor) + { + int16_t d = distance_point_point (&va, &vb); + *dp = d * escape_factor; + return 0; + } + else + return 1; + } + } + } + /* Compute distance. */ + int16_t d = distance_point_point (&va, &vb); + if (d == 0) + { + *dp = 0; + return 0; + } + /* No blocking. */ + *dp = d; + return 0; +} + +/** Update the cache of blocked nodes. */ +static void +path_blocked_update (void) +{ + uint8_t i, j; + for (i = 0; i < PATH_GRID_NODES_NB; i++) + { + uint8_t valid = 1; + /* First, gather information from tables. */ + if (!path_nodes[i].usable) + valid = 0; + else + { + vect_t pos; + path_pos (i, &pos); + /* Then, test for obstacles. */ + for (j = 0; j < PATH_OBSTACLES_NB; j++) + { + if (path.obstacles[j].valid) + { + vect_t v = pos; vect_sub (&v, &path.obstacles[j].c); + uint32_t dsq = vect_dot_product (&v, &v); + uint32_t r = path.obstacles[j].r; + if (dsq <= r * r) + { + valid = 0; + break; + } + } + } + } + /* Update cache. */ + path.valid[i] = valid; + } +} + void path_init (void) { @@ -50,23 +267,51 @@ path_endpoints (vect_t s, vect_t d) void path_escape (uint8_t factor) { + path.escape_factor = factor; } void path_obstacle (uint8_t i, vect_t c, uint16_t r, uint8_t factor, uint16_t valid) { + assert (i < AC_PATH_OBSTACLES_NB); + assert (factor == 0); + path.obstacles[i].c = c; + path.obstacles[i].r = r; + path.obstacles[i].valid = valid; } void path_decay (void) { + uint8_t i; + for (i = 0; i < PATH_OBSTACLES_NB; i++) + { + if (path.obstacles[i].valid + && path.obstacles[i].valid != PATH_OBSTACLE_VALID_ALWAYS) + path.obstacles[i].valid--; + } } void path_update (void) { - path.found = 1; + path_blocked_update (); + path.found = astar (path.astar_nodes, PATH_NODES_NB, PATH_DST_NODE_INDEX, + PATH_SRC_NODE_INDEX); + path.get = PATH_SRC_NODE_INDEX; +#if AC_PATH_REPORT + if (path.found) + { + uint8_t n, len = 0; + vect_t points[PATH_NODES_NB]; + for (n = path.get; n != PATH_DST_NODE_INDEX; n = path.astar_nodes[n].prev) + path_pos (n, &points[len++]); + path_pos (n, &points[len++]); + AC_PATH_REPORT_CALLBACK (points, len, path.obstacles, + PATH_OBSTACLES_NB); + } +#endif } uint8_t @@ -74,10 +319,159 @@ path_get_next (vect_t *p) { if (path.found) { - *p = path.endpoints[0]; + assert (path.get != PATH_DST_NODE_INDEX); + uint8_t prev = path.get; + vect_t pp; + path_pos (prev, &pp); + uint8_t next = path.astar_nodes[path.get].prev; + path.get = next; + path_pos (next, p); + while (next != 0xff) + { + /* Try to remove useless points. */ + uint8_t next = path.astar_nodes[path.get].prev; + if (next == 0xff || next == PATH_DST_NODE_INDEX) + break; + vect_t np; + path_pos (next, &np); + vect_t vnp = np; vect_sub (&vnp, &pp); + vect_t vp = *p; vect_sub (&vp, &pp); + if (vect_normal_dot_product (&vp, &vnp) == 0) + { + path.get = next; + *p = np; + } + else + break; + } return 1; } else return 0; } +/** Neighbors callback for nodes in grid. */ +static uint8_t +path_astar_neighbor_callback_grid (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + uint8_t i; + /* Add neighbors in all 8 directions. */ + static const struct { + /** Column offset of this neighbor. */ + int8_t column_offset; + /** Line offset of this neighbor, for even columns. */ + int8_t even_line_offset; + /** Line offset for odd columns. */ + int8_t odd_line_offset; + /** Distance to this neighbor. */ + uint16_t weight; + } star_n[] = { + { 0, -1, -1, 350 }, /* N */ + { -1, 0, -1, 248 }, /* NW */ + { -2, 0, 0, 350 }, /* W */ + { -1, 1, 0, 248 }, /* SW */ + { 0, 1, 1, 350 }, /* S */ + { 1, 1, 0, 248 }, /* SE */ + { 2, 0, 0, 350 }, /* E */ + { 1, 0, -1, 248 }, /* NE */ + }; + uint8_t col = node / PATH_COLUMN_NODES_NB; + uint8_t line = node - col * PATH_COLUMN_NODES_NB; + uint8_t odd = col % 2; + for (i = 0; i < UTILS_COUNT (star_n); i++) + { + int8_t new_col = col + star_n[i].column_offset; + int8_t new_line = line + (odd ? star_n[i].odd_line_offset + : star_n[i].even_line_offset); + int8_t new_node = new_col * PATH_COLUMN_NODES_NB + new_line; + if (new_col >= 0 && new_col < PATH_COLUMNS_NB + && new_line >= 0 && new_line < PATH_COLUMN_NODES_NB) + { + uint8_t valid = path.valid[new_node]; + if (valid) + { + neighbors[neighbors_nb].node = new_node; + neighbors[neighbors_nb].weight = star_n[i].weight + 1; + neighbors_nb++; + } + } + } + /* Check if direct path OK. */ + int16_t d; + if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +/** Neighbors callback for endpoints. */ +static uint8_t +path_astar_neighbor_callback_endpoints (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + uint8_t i; + assert (node == PATH_DST_NODE_INDEX); + /* Select neighbors in the grid. */ + for (i = 0; i < PATH_GRID_NODES_NB; i++) + { + /* Discard blocking nodes. */ + if (!path.valid[i]) + continue; + /* Check if there is an obstacle along the path. */ + int16_t d; + if (path_blocking (PATH_DST_NODE_INDEX, i, &d)) + continue; + /* Add this neighbor. */ + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } + /* Check if direct path OK. */ + int16_t d; + if (!path_blocking (PATH_DST_NODE_INDEX, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +uint8_t +path_astar_neighbor_callback (uint8_t node, + struct astar_neighbor_t *neighbors) +{ +#if PATH_DEBUG + DPRINTF ("neighbor %d\n", node); +#endif + if (node < PATH_GRID_NODES_NB) + return path_astar_neighbor_callback_grid (node, neighbors); + else + return path_astar_neighbor_callback_endpoints (node, neighbors); +} + +uint16_t +path_astar_heuristic_callback (uint8_t node) +{ + /* TODO: a better and faster heuristic can be found, considering that + * movement is only allowed on the grid. */ + vect_t pos; + path_pos (node, &pos); + return distance_point_point (&pos, &path.endpoints[0]); +} diff --git a/digital/io-hub/src/robospierre/simu.host.c b/digital/io-hub/src/robospierre/simu.host.c index 7ca7364b..70055d7d 100644 --- a/digital/io-hub/src/robospierre/simu.host.c +++ b/digital/io-hub/src/robospierre/simu.host.c @@ -29,6 +29,7 @@ #include "modules/host/host.h" #include "modules/host/mex.h" #include "modules/adc/adc.h" +#include "modules/path/path.h" #include "io.h" /** AVR registers. */ @@ -36,6 +37,7 @@ uint8_t PORTA, DDRA, PINA, PINE, PINF; /** Message types. */ uint8_t simu_mex_pos_report; +uint8_t simu_mex_path; static void simu_adc_handle (void *user, mex_msg_t *msg) @@ -56,6 +58,7 @@ simu_init (void) uint8_t mtype = mex_node_reservef ("%s:adc", mex_instance); mex_node_register (mtype, simu_adc_handle, 0); simu_mex_pos_report = mex_node_reservef ("%s:pos-report", mex_instance); + simu_mex_path = mex_node_reservef ("%s:path", mex_instance); } /** Make a simulation step. */ @@ -78,6 +81,19 @@ timer_wait (void) return 0; } +/** Send computed path. */ +void +simu_send_path (vect_t *points, uint8_t len, + struct path_obstacle_t *obstacles, uint8_t obstacles_nb) +{ + int i; + mex_msg_t *m; + m = mex_msg_new (simu_mex_path); + for (i = 0; i < len; i++) + mex_msg_push (m, "hh", points[i].x, points[i].y); + mex_node_send (m); +} + void simu_send_pos_report (vect_t *pos, uint8_t pos_nb, uint8_t id) { diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 66060ea1..8d758382 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,24 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class Path (Observable): + """Path finding algorithm report. + + - path: sequence of (x, y) coordinates (millimeters). + + """ + + def __init__ (self, node, instance): + Observable.__init__ (self) + self.path = [ ] + node.register (instance + ':path', self.__handle) + + def __handle (self, msg): + self.path = [ ] + while len (msg) >= 4: + self.path.append (msg.pop ('hh')) + self.notify () + class PosReport (Observable): """General purpose position report. @@ -149,5 +167,6 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.path = self.Path (node, instance) self.pos_report = self.PosReport (node, instance) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index b98dafd9..284e5bb8 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -55,5 +55,6 @@ class Bag: DistanceSensorSensopart (link_bag.io_hub.adc[3], scheduler, table, (-20, 20), pi - pi * 10 / 180, (self.position, ), 2), ] + self.path = link_bag.io_hub.path self.pos_report = link_bag.io_hub.pos_report diff --git a/host/simu/robots/robospierre/view/bag.py b/host/simu/robots/robospierre/view/bag.py index b718f6fe..d87cecb9 100644 --- a/host/simu/robots/robospierre/view/bag.py +++ b/host/simu/robots/robospierre/view/bag.py @@ -45,5 +45,6 @@ class Bag: ClampSide.height), model_bag.clamp)) self.distance_sensor = [DistanceSensorUS (self.robot, ds) for ds in model_bag.distance_sensor] + self.path = Path (table, model_bag.path) self.pos_report = PosReport (table, model_bag.pos_report) -- cgit v1.2.3 From 2aff3a82387b86b390348adb63cde3fe92b041db Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Tue, 31 May 2011 13:39:12 +0200 Subject: digital/io: new logistic system --- digital/io-hub/src/robospierre/logistic.c | 582 ++++++++++++++---------------- digital/io-hub/src/robospierre/logistic.h | 33 ++ 2 files changed, 298 insertions(+), 317 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index a9712c23..0822a852 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -65,28 +65,147 @@ logistic_debug_dump (void) #endif } -static void -logistic_tower_possible () +/** Return 1 if location corresponds to element. */ +inline uint8_t +logistic_case_test (uint8_t loc, uint8_t e) { - uint8_t i, head = 0, pawn = 0; - ctx.ready = 0; - /* If clamp broken. */ - if (ctx.prepare == 3 && ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]) + if (e == LOG_a && ctx.slots[loc]) + return 1; + if (e == LOG__) + return 1; + if ((e == LOG_e || e == LOG_D) && !ctx.slots[loc]) + return 1; + if ((e == LOG_P || e == LOG_p) && ctx.slots[loc] && + !ELEMENT_IS_HEAD (ctx.slots[loc])) + return 1; + if ((e == LOG_H || e == LOG_h) && ELEMENT_IS_HEAD (ctx.slots[loc])) + return 1; + return 0; +} + +static uint8_t +logistic_case (uint8_t e1, uint8_t e2, uint8_t e3, uint8_t e4, uint8_t e5, + uint8_t e6, uint8_t e7, uint8_t new_dir, uint8_t ready, + uint8_t check_symetric) +{ + /* Define direction bay and opposed bay. */ + uint8_t dir_bay, opp_bay; + if (ctx.collect_direction == DIRECTION_FORWARD) { - ctx.construct_possible = 2; - ctx.ready = 1; - ctx.collect_direction = DIRECTION_BACKWARD; - return; + dir_bay = CLAMP_SLOT_FRONT_BOTTOM; + opp_bay = CLAMP_SLOT_BACK_BOTTOM; } - else if (ctx.prepare == 3 && ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) + else { - ctx.construct_possible = 2; - ctx.ready = 1; - ctx.collect_direction = DIRECTION_FORWARD; - return; + dir_bay = CLAMP_SLOT_BACK_BOTTOM; + opp_bay = CLAMP_SLOT_FRONT_BOTTOM; + } + /*if (check_symetric) + { + uint8_t tmp = dir_bay; + dir_bay = opp_bay; + opp_bay = tmp; + }*/ + + /* Emplacement of elements. */ + uint8_t + e1_loc = dir_bay + 2, + e3_loc = dir_bay + 1, + e6_loc = dir_bay, + e4_loc = CLAMP_SLOT_SIDE, + e2_loc = opp_bay + 2, + e5_loc = opp_bay + 1, + e7_loc = opp_bay; + + /* Check elements are here. */ + if (!(logistic_case_test (e1_loc, e1) && + logistic_case_test (e2_loc, e2) && + logistic_case_test (e3_loc, e3) && + logistic_case_test (e4_loc, e4) && + logistic_case_test (e5_loc, e5) && + logistic_case_test (e6_loc, e6) && + logistic_case_test (e7_loc, e7))) + { + /* Check mirror. */ + /*if (!check_symetric) + logistic_case (e1, e2, e3, e4, e5, e6, e7, new_dir, ready, 1);*/ + return 0; + } + + /* Find source/destination if we have to make a move. */ + /* Find source. */ + uint8_t src = CLAMP_SLOT_NB, dst = CLAMP_SLOT_NB; + if (e1 == LOG_P ||e1 == LOG_H) + src = e1_loc; + else if (e3 == LOG_P ||e3 == LOG_H) + src = e3_loc; + else if (e6 == LOG_P ||e6 == LOG_H) + src = e6_loc; + else if (e4 == LOG_P ||e4 == LOG_H) + src = e4_loc; + else if (e2 == LOG_P ||e2 == LOG_H) + src = e2_loc; + else if (e5 == LOG_P ||e5 == LOG_H) + src = e5_loc; + else if (e7 == LOG_P ||e7 == LOG_H) + src = e7_loc; + + /* Find destination. */ + if (e1 == LOG_D) + dst = e1_loc; + else if (e3 == LOG_D) + dst = e3_loc; + else if (e6 == LOG_D) + dst = e6_loc; + else if (e4 == LOG_D) + dst = e4_loc; + else if (e2 == LOG_D) + dst = e2_loc; + else if (e5 == LOG_D) + dst = e5_loc; + else if (e7 == LOG_D) + dst = e7_loc; + + /* We are making a move. */ + if (src != CLAMP_SLOT_NB && dst != CLAMP_SLOT_NB) + { + if (ctx.slots[src] && !ctx.slots[dst]) + { + ctx.moving_from = src; + ctx.moving_to = dst; + ctx.ready = 0; + } + } + + /* Set collect direction. */ + /* LEFT means we keep the same side, RIGHT means we put the opposed side. */ + if (new_dir == LOG_DIR_RIGHT) + { + if (ctx.collect_direction == DIRECTION_FORWARD) + ctx.collect_direction = DIRECTION_BACKWARD; + else + ctx.collect_direction = DIRECTION_FORWARD; } - for (i = CLAMP_SLOT_FRONT_BOTTOM; i <= CLAMP_SLOT_NB; i++) + /* Don't touch clamp's idle position if broken. */ + if (ctx.prepare != 3) + { + /* Set clamp position idle. */ + if (ctx.collect_direction == DIRECTION_FORWARD) + ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; + else + ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; + } + /* Set ready */ + ctx.ready = ready; + return 1; +} + +static void +logistic_update_construct_possible () +{ + uint8_t pawn = 0, head = 0, i; + for (i = CLAMP_SLOT_FRONT_BOTTOM; i < CLAMP_SLOT_NB; i++) { if (ELEMENT_IS_HEAD (ctx.slots[i])) head++; @@ -102,13 +221,10 @@ logistic_tower_possible () ctx.construct_possible = 2; else ctx.construct_possible = 0; - return; } -/* Check if we can't take element anymore and we need to be authorized to - * unload. */ static void -logistic_check_need_prepare () +logistic_update_need_prepare () { uint8_t i, head = 0, pawn = 0; for (i = CLAMP_SLOT_FRONT_BOTTOM; i < CLAMP_SLOT_NB; i++) @@ -119,162 +235,126 @@ logistic_check_need_prepare () pawn++; } if ((head == 1 && pawn == 3) || - head == 2 || - pawn > 4) + head == 2 || pawn >= 4) ctx.need_prepare = 1; else ctx.need_prepare = 0; } -/** Build something not a tower. */ static void -logistic_put_element () +logisitic_make_broken () { - uint8_t dir_bay, nodir_bay; - if (ctx.collect_direction == DIRECTION_FORWARD) - { - dir_bay = CLAMP_SLOT_FRONT_BOTTOM; - nodir_bay = CLAMP_SLOT_BACK_BOTTOM; - } - else - { - dir_bay = CLAMP_SLOT_BACK_BOTTOM; - nodir_bay = CLAMP_SLOT_FRONT_BOTTOM; - } + LOGISTIC_CASE (_, _, + _, _, _, + a, _, RIGHT, 1); - /* If we may build a little tower ? */ - /* Put the tower on the pawn. */ - if (ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 2]) && - ctx.slots[nodir_bay] && - !ctx.slots[nodir_bay + 1]) - { - ctx.moving_from = nodir_bay + 2; - ctx.moving_to = nodir_bay + 1; - return; - } - /* Little tower is ready. */ - else if (ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 1]) && - ctx.slots[nodir_bay]) - ctx.ready = 1; - else if (ELEMENT_IS_HEAD (ctx.slots[nodir_bay])) - ctx.ready = 1; - /* Find a pawn for a lonely head. */ - else if (ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 2])) - { - if (!ctx.slots[nodir_bay] && ctx.slots[CLAMP_SLOT_SIDE]) - { - ctx.moving_from = CLAMP_SLOT_SIDE; - ctx.moving_to = nodir_bay; - } - else if (!ctx.slots[nodir_bay] && !ELEMENT_IS_HEAD (ctx.slots[dir_bay + 2]) && - ctx.slots[dir_bay + 2]) - { - ctx.moving_from = dir_bay + 2; - ctx.moving_to = nodir_bay; - } - else if (!ctx.slots[nodir_bay] && - !ELEMENT_IS_HEAD (ctx.slots[dir_bay + 1]) && - ctx.slots[dir_bay + 1]) - { - ctx.moving_from = dir_bay + 1; - ctx.moving_to = nodir_bay; - } - /* No pawn ! */ - else if (!ctx.slots[nodir_bay]) - { - ctx.moving_from = nodir_bay + 2; - ctx.moving_to = nodir_bay; - } - } - /* If we may build a little tower ? */ - /* Put the tower on the pawn. */ - else if (ELEMENT_IS_HEAD (ctx.slots[dir_bay + 2]) && - ctx.slots[dir_bay] && - !ctx.slots[dir_bay + 1]) - { - ctx.moving_from = dir_bay + 2; - ctx.moving_to = dir_bay + 1; - return; - } - /* Little tower is ready. */ - else if (ELEMENT_IS_HEAD (ctx.slots[dir_bay + 1]) && - ctx.slots[dir_bay]) - ctx.ready = 1; - else if (ELEMENT_IS_HEAD (ctx.slots[dir_bay])) - ctx.ready = 1; - /* Find a pawn for a lonely head. */ - else if (ELEMENT_IS_HEAD (ctx.slots[dir_bay + 2])) - { - if (!ctx.slots[dir_bay] && ctx.slots[CLAMP_SLOT_SIDE]) - { - ctx.moving_from = CLAMP_SLOT_SIDE; - ctx.moving_to = dir_bay; - } - else if (!ctx.slots[dir_bay] && - !ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 2]) && - ctx.slots[nodir_bay + 2]) - { - ctx.moving_from = nodir_bay + 2; - ctx.moving_to = dir_bay; - } - else if (!ctx.slots[dir_bay] && - !ELEMENT_IS_HEAD (ctx.slots[nodir_bay + 1]) && - ctx.slots[nodir_bay + 1]) - { - ctx.moving_from = nodir_bay + 1; - ctx.moving_to = dir_bay; - } - /* No pawn ! */ - else if (!ctx.slots[dir_bay]) - { - ctx.moving_from = dir_bay + 2; - ctx.moving_to = dir_bay; - } - } - /* No head, find any pawn. */ - else if (!ctx.slots[nodir_bay]) - { - ctx.moving_to = nodir_bay; - if (ctx.slots[nodir_bay + 2]) - ctx.moving_from = nodir_bay + 2; - else if (ctx.slots[CLAMP_SLOT_SIDE]) - ctx.moving_from = CLAMP_SLOT_SIDE; - else if (ctx.slots[dir_bay + 2]) - ctx.moving_from = dir_bay + 2; - else if (ctx.slots[dir_bay]) - ctx.moving_from = dir_bay; - } - else if (ctx.slots[nodir_bay]) - { - ctx.ready = 1; - } + LOGISTIC_CASE (_, _, + _, _, _, + _, a, LEFT, 1); } -/** If we picked up a tower. */ static void -logistic_new_tower () +logistic_make_tower () { - uint8_t dir_bay, nodir_bay; - if (ctx.collect_direction == DIRECTION_FORWARD) - { - dir_bay = CLAMP_SLOT_FRONT_BOTTOM; - nodir_bay = CLAMP_SLOT_BACK_BOTTOM; - } - else - { - dir_bay = CLAMP_SLOT_BACK_BOTTOM; - nodir_bay = CLAMP_SLOT_FRONT_BOTTOM; - } - if (ctx.slots[dir_bay] == ELEMENT_TOWER) - { - ctx.ready = 1; - if (ctx.collect_direction == DIRECTION_FORWARD) - ctx.collect_direction = DIRECTION_BACKWARD; - else - ctx.collect_direction = DIRECTION_FORWARD; - } - if (ctx.slots[nodir_bay] == ELEMENT_TOWER) - ctx.ready = 1; + LOGISTIC_CASE (D, _, + e, _, _, + H, _, RIGHT, 1); + + LOGISTIC_CASE (_, h, + _, _, p, + _, p, LEFT, 1); + + LOGISTIC_CASE (P, h, + e, _, e, + _, D, LEFT, 0); + + LOGISTIC_CASE (P, h, + e, _, e, + _, D, LEFT, 0); + + LOGISTIC_CASE (_, h, + e, _, e, + P, D, LEFT, 0); + + LOGISTIC_CASE (_, h, + _, P, e, + _, D, LEFT, 0); + + LOGISTIC_CASE (P, h, + e, _, D, + _, p, LEFT, 0); + + LOGISTIC_CASE (_, h, + e, _, D, + P, p, LEFT, 0); + + LOGISTIC_CASE (_, h, + _, P, D, + _, p, LEFT, 0); +} + +static void +logistic_make_unload () +{ + LOGISTIC_CASE (_, _, + _, _, _, + _, a, LEFT, 1); + + LOGISTIC_CASE (_, _, + _, _, _, + a, _, RIGHT, 1); + + LOGISTIC_CASE (H, _, + e, _, _, + D, _, RIGHT, 0); + + LOGISTIC_CASE (_, H, + _, _, e, + _, D, LEFT, 0); + + LOGISTIC_CASE (P, _, + e, _, _, + D, _, RIGHT, 0); + + LOGISTIC_CASE (_, P, + _, _, e, + _, D, LEFT, 0); + + LOGISTIC_CASE (_, _, + _, P, e, + e, D, LEFT, 0); + + LOGISTIC_CASE (_, _, + e, P, _, + D, _, RIGHT, 0); +} + +static void +logisitic_make_switches () +{ + LOGISTIC_CASE (_, _, + e, D, _, + P, _, LEFT, 0); + + LOGISTIC_CASE (_, D, + e, p, e, + P, _, LEFT, 0); + + LOGISTIC_CASE (_, p, + e, p, e, + P, D, LEFT, 0); + + LOGISTIC_CASE (D, _, + e, _, _, + H, _, RIGHT, 0); + + LOGISTIC_CASE (h, e, + e, _, e, + D, P, LEFT, 0); + + LOGISTIC_CASE (h, P, + e, _, e, + D, P, LEFT, 0); } /** Examine current state and take a decision. */ @@ -288,172 +368,39 @@ logistic_decision (void) ctx.ready = 0; ctx.need_prepare = 0; + /* Update context. */ + logistic_update_construct_possible (); /* Update if a something is possible. */ - logistic_tower_possible (); + logistic_update_need_prepare (); - /* Clamp is broken, can't move anything. */ - if (ctx.prepare == 3) + /* Broken clamp. */ + if (ctx.construct_possible && ctx.prepare == 3) { - logistic_debug_dump (); - return; + DPRINTF ("\nlogisitic_make_broken\n"); + logisitic_make_broken (); } - - /* We founded a tower ! */ - logistic_new_tower (); - if (ctx.ready) + /* Prepare tower. */ + else if (ctx.construct_possible == 1 && ctx.prepare != 0) { - logistic_debug_dump (); - return; - } - /* Check if we really need to prepare something. */ - logistic_check_need_prepare (); - - /* A head is here, put it in the top ! */ - if (ctx.prepare != 2) - { - if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]) && - !ctx.slots[CLAMP_SLOT_FRONT_TOP]) - { - ctx.moving_from = CLAMP_SLOT_FRONT_BOTTOM; - ctx.moving_to = CLAMP_SLOT_FRONT_TOP; - if (!ctx.slots[CLAMP_SLOT_BACK_TOP]) - { - ctx.collect_direction = DIRECTION_BACKWARD; - ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; - } - logistic_debug_dump (); - return; - } - if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) && - !ctx.slots[CLAMP_SLOT_BACK_TOP]) - { - ctx.moving_from = CLAMP_SLOT_BACK_BOTTOM; - ctx.moving_to = CLAMP_SLOT_BACK_TOP; - if (!ctx.slots[CLAMP_SLOT_FRONT_TOP]) - { - ctx.collect_direction = DIRECTION_FORWARD; - ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; - } - logistic_debug_dump (); - return; - } + DPRINTF ("\nlogisitic_make_tower\n"); + logistic_make_tower (); } - /* We can build a tower and we are authorized to do so. */ - if (ctx.construct_possible == 1 && ctx.prepare) + /* Need to unload. */ + else if (ctx.construct_possible == 2 && ctx.prepare == 2) { - /* Where to build the tower. */ - uint8_t build_dir; - if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_TOP]) && - ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_FRONT_TOP])) - { - if (ctx.slots[CLAMP_SLOT_BACK_BOTTOM] && - ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]) - build_dir = ctx.collect_direction; - else if (ctx.slots[CLAMP_SLOT_BACK_BOTTOM]) - build_dir = DIRECTION_BACKWARD; - else - build_dir = DIRECTION_FORWARD; - } - else if (ELEMENT_IS_HEAD (ctx.slots[CLAMP_SLOT_BACK_TOP])) - build_dir = DIRECTION_BACKWARD; - else - build_dir = DIRECTION_FORWARD; - /* Adapt collect direction. */ - if (build_dir == DIRECTION_FORWARD) - ctx.collect_direction = DIRECTION_BACKWARD; - else - ctx.collect_direction = DIRECTION_FORWARD; - /* Fill with pawns. */ - uint8_t build_bay, collect_bay; - if (build_dir == DIRECTION_FORWARD) - { - build_bay = CLAMP_SLOT_FRONT_BOTTOM; - collect_bay = CLAMP_SLOT_BACK_BOTTOM; - } - else - { - build_bay = CLAMP_SLOT_BACK_BOTTOM; - collect_bay = CLAMP_SLOT_FRONT_BOTTOM; - } - /* Destination. */ - if (!ctx.slots[build_bay]) - ctx.moving_to = build_bay; - else if (!ctx.slots[build_bay + 1]) - ctx.moving_to = build_bay + 1; - else - { - /* Build is finished. */ - ctx.ready = 1; - logistic_debug_dump (); - return; - } - /* Source. */ - if (ctx.slots[collect_bay + 2] && - !ELEMENT_IS_HEAD (ctx.slots[collect_bay + 2])) - ctx.moving_from = collect_bay + 2; - else if (ctx.slots[collect_bay] && - !ELEMENT_IS_HEAD (ctx.slots[collect_bay])) - ctx.moving_from = collect_bay; - else if (ctx.slots[CLAMP_SLOT_SIDE]) - ctx.moving_from = CLAMP_SLOT_SIDE; - logistic_debug_dump (); - return; + DPRINTF ("\nlogisitic_make_unload\n"); + logistic_make_unload (); } - /* We have to prepare an element to put out. */ - else if (ctx.prepare == 2 && ctx.construct_possible) - { - logistic_put_element (); - } - /* Time to adjust element inside de robot, not build. */ + /* Internal switches. */ else { - uint8_t get_bay, put_bay; - if (ctx.collect_direction == DIRECTION_FORWARD) - { - get_bay = CLAMP_SLOT_FRONT_BOTTOM; - put_bay = CLAMP_SLOT_BACK_BOTTOM; - } - else - { - get_bay = CLAMP_SLOT_BACK_BOTTOM; - put_bay = CLAMP_SLOT_FRONT_BOTTOM; - } - /* Adjust some pawns. */ - /* Search source. */ - if (ctx.slots[get_bay] == ELEMENT_PAWN) - ctx.moving_from = get_bay; - else if (ctx.slots[get_bay + 2] == ELEMENT_PAWN) - ctx.moving_from = get_bay + 2; - else if (ctx.slots[put_bay + 2] == ELEMENT_PAWN) - ctx.moving_from = put_bay + 2; - else if (ctx.slots[put_bay] == ELEMENT_PAWN) - ctx.moving_from = put_bay; - - /* Search destination. */ - if (ctx.moving_from != CLAMP_SLOT_NB) - { - if (!ctx.slots[CLAMP_SLOT_SIDE]) - ctx.moving_to = CLAMP_SLOT_SIDE; - else if (!ctx.slots[put_bay]) - ctx.moving_to = put_bay; - else if (!ctx.slots[put_bay + 2] && ctx.moving_from != put_bay) - ctx.moving_to = put_bay + 2; - else - ctx.moving_from = CLAMP_SLOT_NB; - } - if (ctx.moving_from == ctx.moving_to) - { - ctx.moving_from = CLAMP_SLOT_NB; - ctx.moving_to = CLAMP_SLOT_NB; - } - - if (ctx.collect_direction == DIRECTION_FORWARD) - ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; - else - ctx.clamp_pos_idle = CLAMP_SLOT_BACK_MIDDLE; + DPRINTF ("\nlogisitic_make_switches\n"); + logisitic_make_switches (); } + logistic_debug_dump (); + return; } void @@ -464,6 +411,7 @@ logistic_init (void) ctx.slots[i] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; ctx.collect_direction = DIRECTION_FORWARD; + ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; ctx.construct_possible = 0; ctx.prepare = 1; ctx.ready = 0; diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 05b640db..13c849a5 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -27,6 +27,39 @@ #include "element.h" #include "clamp.h" +/** Defines for logistic macro, see LOGISTIC_CASE. */ +/** Any element must be present (not empty). */ +#define LOG_a 0 +/** Do not care if there is an element or not. */ +#define LOG__ 1 +/** There must not be element here. */ +#define LOG_e 2 +/** Pawn to pick up. */ +#define LOG_P 4 +/** A head to pick up. */ +#define LOG_H 5 +/** Pawn who has to be present. */ +#define LOG_p 6 +/** Head who has to be present. */ +#define LOG_h 7 +/** Destination (who has to be empty). */ +#define LOG_D 8 +/* LEFT means we keep the same side, RIGHT means we put the opposed side. */ +#define LOG_DIR_LEFT 0 +#define LOG_DIR_RIGHT 1 +/** Logistic macro to test if a case correspond and to perform movement. + * We have to consider that the moving direction is on the left. + * New direction is indicated (same or opposed). + * Set ready or not. */ +#define LOGISTIC_CASE(e1, e2, \ + e3, e4, e5, \ + e6, e7, new_dir, ready) \ +{ \ + if (logistic_case (LOG_##e1, LOG_##e2, LOG_##e3, LOG_##e4, LOG_##e5, \ + LOG_##e6, LOG_##e7, LOG_DIR_##new_dir, ready, 0)) \ + return; \ +} + /** Logistic context. */ struct logistic_t { -- cgit v1.2.3 From 31f85163b5981a78a2b6704168d0ae12929b53be Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 09:39:16 +0200 Subject: digital/mimot: better find zero/limit --- digital/ai/src/twi_master/mimot.c | 34 ++++++++++++++++++++++++++++------ digital/ai/src/twi_master/mimot.h | 10 ++++++++++ digital/mimot/src/dirty/aux.c | 12 ++++++++---- digital/mimot/src/dirty/aux.h | 8 ++++++-- digital/mimot/src/dirty/main.c | 13 ++++++++++--- digital/mimot/src/dirty/twi_proto.c | 26 +++++++++++++++++--------- 6 files changed, 79 insertions(+), 24 deletions(-) diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index c3b82823..6159e6c9 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -165,20 +165,42 @@ mimot_move_motor1_absolute (uint16_t position, uint8_t speed) void mimot_motor0_zero_position (int8_t speed) +{ + mimot_motor0_find_zero (speed, 0, 0); +} + +void +mimot_motor1_zero_position (int8_t speed) +{ + mimot_motor1_find_zero (speed, 0, 0); +} + +void +mimot_motor0_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); buffer[0] = 'B'; - buffer[1] = speed; - twi_master_send_buffer (2); + buffer[1] = 0; + buffer[2] = speed; + buffer[3] = use_switch; + buffer[4] = v16_to_v8 (reset_position, 1); + buffer[5] = v16_to_v8 (reset_position, 0); + twi_master_send_buffer (6); } void -mimot_motor1_zero_position (int8_t speed) +mimot_motor1_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position) { uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE); - buffer[0] = 'C'; - buffer[1] = speed; - twi_master_send_buffer (2); + buffer[0] = 'B'; + buffer[1] = 1; + buffer[2] = speed; + buffer[3] = use_switch; + buffer[4] = v16_to_v8 (reset_position, 1); + buffer[5] = v16_to_v8 (reset_position, 0); + twi_master_send_buffer (6); } void diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index 97575b0d..c59c1608 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -92,6 +92,16 @@ mimot_motor0_zero_position (int8_t speed); void mimot_motor1_zero_position (int8_t speed); +/** Find zero position. */ +void +mimot_motor0_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position); + +/** Find zero position. */ +void +mimot_motor1_find_zero (int8_t speed, uint8_t use_switch, + uint16_t reset_position); + /** Clamp motor0. */ void mimot_motor0_clamp (int8_t speed, int16_t pwm); diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c index e00e2c08..e36489ba 100644 --- a/digital/mimot/src/dirty/aux.c +++ b/digital/mimot/src/dirty/aux.c @@ -184,7 +184,7 @@ aux_traj_find_zero (struct aux_t *aux) { aux->speed->cons = 0; state_finish (aux->state); - aux->pos = 0; + aux->pos = aux->reset_pos; aux->traj_mode = AUX_TRAJ_DONE; } break; @@ -193,12 +193,14 @@ aux_traj_find_zero (struct aux_t *aux) /** Start find zero mode. */ void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq) +aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq) { aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT; aux->speed->use_pos = 0; aux->speed->cons = speed << 8; state_start (aux->state, MODE_TRAJ, seq); + aux->reset_pos = reset_pos; } /** Find limit mode. */ @@ -225,7 +227,7 @@ aux_traj_find_limit (struct aux_t *aux) if (!--aux->wait) { state_finish (aux->state); - aux->pos = 0; + aux->pos = aux->reset_pos; aux->traj_mode = AUX_TRAJ_DONE; } break; @@ -234,13 +236,15 @@ aux_traj_find_limit (struct aux_t *aux) /** Start find limit mode. */ void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq) +aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq) { aux->traj_mode = AUX_TRAJ_FIND_LIMIT; aux->speed->use_pos = 0; aux->speed->cons = speed << 8; state_start (aux->state, MODE_TRAJ, seq); aux->state->variant = 4; + aux->reset_pos = reset_pos; } /** Update trajectories for one motor. */ diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h index f31b789a..7ec60617 100644 --- a/digital/mimot/src/dirty/aux.h +++ b/digital/mimot/src/dirty/aux.h @@ -50,6 +50,8 @@ struct aux_t uint8_t zero_bv; /** Handle blocking by aux instead of pos. */ uint8_t handle_blocking; + /** Reset position after zero is found. */ + int16_t reset_pos; }; extern struct aux_t aux[AC_ASSERV_AUX_NB]; @@ -68,10 +70,12 @@ aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm, uint8_t seq); void -aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, uint8_t seq); +aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq); void -aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, uint8_t seq); +aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos, + uint8_t seq); void aux_traj_update (void); diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c index 5dcfd8c7..2b55259b 100644 --- a/digital/mimot/src/dirty/main.c +++ b/digital/mimot/src/dirty/main.c @@ -287,15 +287,22 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]), args[4]); break; - case c ('y', 3): + case c ('y', 6): /* Auxiliary find zero. * - b: aux index. * - b: speed. + * - b: use switch. + * - w: reset position. * - b: sequence number. */ if (!auxp) { proto_send0 ('?'); return; } - if (args[2] == state->sequence) + if (args[5] == state->sequence) break; - aux_traj_find_limit_start (auxp, args[1], args[2]); + if (args[2]) + aux_traj_find_zero_start (auxp, args[1], + v8_to_v16 (args[3], args[4]), args[5]); + else + aux_traj_find_limit_start (auxp, args[1], + v8_to_v16 (args[3], args[4]), args[5]); break; case c ('a', 2): /* Set all acknoledge. diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c index bda255ef..2f935fda 100644 --- a/digital/mimot/src/dirty/twi_proto.c +++ b/digital/mimot/src/dirty/twi_proto.c @@ -122,11 +122,6 @@ twi_proto_callback (u8 *buf, u8 size) speed_aux[0].max = buf[4]; aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]), 0); break; - case c ('B', 1): - /* Find the zero position of the aux0. - * - b: speed. */ - aux_traj_find_limit_start (&aux[0], buf[2], 0); - break; case c ('c', 3): /* Move the aux1. * - w: new position. @@ -134,10 +129,23 @@ twi_proto_callback (u8 *buf, u8 size) speed_aux[1].max = buf[4]; aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]), 0); break; - case c ('C', 1): - /* Find the zero position of the aux1. - * - b: speed. */ - aux_traj_find_limit_start (&aux[1], buf[2], 0); + case c ('B', 5): + /* Find the zero position. + * - b: aux index. + * - b: speed. + * - b: use switch. + * - w: reset position. */ + if (buf[2] < AC_ASSERV_AUX_NB) + { + if (buf[4]) + aux_traj_find_zero_start (&aux[buf[2]], buf[3], + v8_to_v16 (buf[5], buf[6]), 0); + else + aux_traj_find_limit_start (&aux[buf[2]], buf[3], + v8_to_v16 (buf[5], buf[6]), 0); + } + else + buf[0] = 0; break; case c ('l', 4): /* Clamp. -- cgit v1.2.3 From a309ae5825b179a0c77c8e7572f16f06fd058e10 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 09:43:01 +0200 Subject: digital/mimot: add simulated robospierre contact --- digital/mimot/src/dirty/models.host.c | 2 +- digital/mimot/src/dirty/simu.host.c | 9 +++++++++ digital/mimot/src/dirty/simu.host.h | 3 +++ 3 files changed, 13 insertions(+), 1 deletion(-) diff --git a/digital/mimot/src/dirty/models.host.c b/digital/mimot/src/dirty/models.host.c index b1141748..db4152e0 100644 --- a/digital/mimot/src/dirty/models.host.c +++ b/digital/mimot/src/dirty/models.host.c @@ -91,7 +91,7 @@ static const struct robot_t robospierre_robot = /** Number of steps for each auxiliary motor encoder. */ { 256, 250 }, /** Sensor update function. */ - NULL, + simu_sensor_update_robospierre, }; /* Table of models. */ diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c index af07d744..8afd73ca 100644 --- a/digital/mimot/src/dirty/simu.host.c +++ b/digital/mimot/src/dirty/simu.host.c @@ -152,6 +152,15 @@ simu_sensor_update_marcel (void) { } +/** Update sensors for Robospierre. */ +void +simu_sensor_update_robospierre (void) +{ + PINC = 0; + if (simu_aux_model[0].th < 120.0 * 5.0 / 6.0 * simu_aux_model[0].m.i_G) + PINC |= IO_BV (CONTACT_AUX0_ZERO_IO); +} + /** Do a simulation step. */ static void simu_step (void) diff --git a/digital/mimot/src/dirty/simu.host.h b/digital/mimot/src/dirty/simu.host.h index 6250bed1..87a728b6 100644 --- a/digital/mimot/src/dirty/simu.host.h +++ b/digital/mimot/src/dirty/simu.host.h @@ -45,4 +45,7 @@ simu_sensor_update_aquajim (void); void simu_sensor_update_marcel (void); +void +simu_sensor_update_robospierre (void); + #endif /* simu_host_h */ -- cgit v1.2.3 From 9e20538296e4ed747e728cd6101d09fed3184d10 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 09:43:25 +0200 Subject: digital/io-hub: change clamp init algorithm --- digital/io-hub/src/robospierre/bot.h | 11 +++-- digital/io-hub/src/robospierre/clamp.c | 73 +++++++++------------------------- 2 files changed, 23 insertions(+), 61 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 4333ba8a..21e936cb 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -84,8 +84,8 @@ # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP (0x3b0b / 2 + 1000) # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP (0x3b0b / 2) -# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP 0x18ca -# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP 0x18ca +#define BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP \ + BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 @@ -117,9 +117,8 @@ # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) -// TODO: to be measured. -# define BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP (0x1da7 / 2) -# define BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP (0x1f03 / 2) +#define BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP \ + BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP // TODO: measure on robot # define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 # define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 @@ -150,7 +149,7 @@ ? BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET \ : BOT_CLAMP_CLOSED_SIDE_ROTATION_OFFSET)) -#define BOT_CLAMP_INIT_ELEVATION_SPEED 0x08 +#define BOT_CLAMP_INIT_ELEVATION_SPEED 0x10 #define BOT_CLAMP_INIT_ROTATION_SPEED -0x04 #define BOT_CLAMP_ELEVATION_SPEED 0x60 #define BOT_CLAMP_ROTATION_SPEED 0x60 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index db15639c..b2a6bf76 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -35,8 +35,6 @@ #include "fsm.h" #include "fsm_queue.h" -#include "modules/proto/proto.h" - #include "logistic.h" #include "pawn_sensor.h" @@ -55,13 +53,12 @@ FSM_STATES ( CLAMP_START, /* Initialisation sequence: opening everything. */ CLAMP_INIT_OPENING, - /* Initialisation sequence: going up until a middle pawn sensor - * see the clamp. */ - CLAMP_INIT_UPING_UNTIL_SEEN, /* Initialisation sequence: going to the middle level. */ CLAMP_INIT_GOING_MIDDLE, /* Initialisation sequence: finding front right edge. */ - CLAMP_INIT_FINDING_EDGE, + CLAMP_INIT_FINDING_ROTATION_EDGE, + /* Initialisation sequence: finding top switch. */ + CLAMP_INIT_FINDING_TOP, /* Returning to idle position. */ CLAMP_GOING_IDLE, @@ -96,8 +93,6 @@ FSM_STATES ( CLAMP_MOVE_DST_CLAMP_OPENING) FSM_EVENTS ( - /* During initialisation sequence, clamp seen by a sensor. */ - clamp_init_seen, /* New element inside bottom slot. */ clamp_new_element, /* Order to drop elements. */ @@ -144,10 +139,6 @@ struct clamp_t uint8_t open; /** True if clamp position is controled. */ uint8_t controled; - /** Position of clamp when seen by sensor. */ - uint16_t init_seen_step; - /** Position to initialise at seen position. */ - uint16_t init_seen_init_step; }; /** Global context. */ @@ -300,24 +291,6 @@ clamp_handle_event (void) /* Go directly to next point. */ clamp_route (); } - /* Handle initialisation. */ - if (FSM_CAN_HANDLE (AI, clamp_init_seen)) - { - if (!IO_GET (CONTACT_FRONT_MIDDLE)) - { - ctx.init_seen_step = mimot_get_motor0_position (); - ctx.init_seen_init_step = BOT_CLAMP_INIT_FRONT_SEEN_ELEVATION_STEP; - FSM_HANDLE (AI, clamp_init_seen); - return 1; - } - if (!IO_GET (CONTACT_BACK_MIDDLE)) - { - ctx.init_seen_step = mimot_get_motor0_position (); - ctx.init_seen_init_step = BOT_CLAMP_INIT_BACK_SEEN_ELEVATION_STEP; - FSM_HANDLE (AI, clamp_init_seen); - return 1; - } - } return 0; } @@ -422,7 +395,7 @@ FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING) } FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT, - CLAMP_INIT_UPING_UNTIL_SEEN) + CLAMP_INIT_GOING_MIDDLE) { mimot_move_motor0_absolute (mimot_get_motor0_position () + BOT_CLAMP_INIT_ELEVATION_STEP, @@ -430,37 +403,27 @@ FSM_TRANS_TIMEOUT (CLAMP_INIT_OPENING, BOT_PWM_CLAMP_DOOR_INIT, return FSM_NEXT_TIMEOUT (CLAMP_INIT_OPENING); } -FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success, CLAMP_START) -{ - /* Dead end. */ - mimot_motor0_free (); - return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_elevation_success); -} - -FSM_TRANS (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen, - CLAMP_INIT_GOING_MIDDLE) +FSM_TRANS (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success, + CLAMP_INIT_FINDING_ROTATION_EDGE) { - mimot_motor0_free (); - mimot_set_motor0_position (ctx.init_seen_init_step - + mimot_get_motor0_position () - - ctx.init_seen_step); - proto_send1w ('C', ctx.init_seen_step); - mimot_move_motor0_absolute (BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP, - BOT_CLAMP_ELEVATION_SPEED); - return FSM_NEXT (CLAMP_INIT_UPING_UNTIL_SEEN, clamp_init_seen); + mimot_motor1_find_zero (BOT_CLAMP_INIT_ROTATION_SPEED, 0, 0); + return FSM_NEXT (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success); } -FSM_TRANS (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success, - CLAMP_INIT_FINDING_EDGE) +FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success, + CLAMP_INIT_FINDING_TOP) { - mimot_motor1_zero_position (BOT_CLAMP_INIT_ROTATION_SPEED); - return FSM_NEXT (CLAMP_INIT_GOING_MIDDLE, clamp_elevation_success); + mimot_motor0_find_zero (BOT_CLAMP_INIT_ELEVATION_SPEED, 1, + BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP); + return FSM_NEXT (CLAMP_INIT_FINDING_ROTATION_EDGE, + clamp_rotation_success); } -FSM_TRANS (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success, CLAMP_GOING_IDLE) +FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, + CLAMP_GOING_IDLE) { - clamp_move (CLAMP_SLOT_FRONT_MIDDLE); - return FSM_NEXT (CLAMP_INIT_FINDING_EDGE, clamp_rotation_success); + clamp_move (CLAMP_SLOT_SIDE); + return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success); } FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) -- cgit v1.2.3 From 0843e5f91424e7e2957a33d3386eccbfcee1b480 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 09:46:02 +0200 Subject: digital/mimot: moved zero contacts --- digital/mimot/src/dirty/contacts.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/mimot/src/dirty/contacts.h b/digital/mimot/src/dirty/contacts.h index 1de0c57d..366692c4 100644 --- a/digital/mimot/src/dirty/contacts.h +++ b/digital/mimot/src/dirty/contacts.h @@ -26,7 +26,7 @@ * }}} */ /** Define contacts. */ -#define CONTACT_AUX0_ZERO_IO C, 0 -#define CONTACT_AUX1_ZERO_IO C, 1 +#define CONTACT_AUX0_ZERO_IO C, 2 +#define CONTACT_AUX1_ZERO_IO C, 3 #endif /* contacts_h */ -- cgit v1.2.3 From 8f4d4d55ed0c9adf5defdea892d4bf33ae76ca39 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 12:04:19 +0200 Subject: digital/mimot: stop motor directly when zero found --- digital/mimot/src/dirty/aux.c | 1 + 1 file changed, 1 insertion(+) diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c index e36489ba..65b4e3d5 100644 --- a/digital/mimot/src/dirty/aux.c +++ b/digital/mimot/src/dirty/aux.c @@ -183,6 +183,7 @@ aux_traj_find_zero (struct aux_t *aux) if (!zero) { aux->speed->cons = 0; + aux->speed->cur = 0; state_finish (aux->state); aux->pos = aux->reset_pos; aux->traj_mode = AUX_TRAJ_DONE; -- cgit v1.2.3 From 9b8e88e56e6d22bc182cc0df605c69aa6b10d9e9 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 12:05:36 +0200 Subject: digital/io-hub: change clamp parameters after init --- digital/io-hub/src/robospierre/bot.h | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 21e936cb..8e32a317 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -117,21 +117,20 @@ # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) -#define BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP \ - BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP // TODO: measure on robot +#define BOT_CLAMP_INIT_ELEVATION_SWITCH_STEP 0x363f -# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 0 -# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 0 -# define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 0 -# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x23d7 -# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x23d7 -# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x23d7 +# define BOT_CLAMP_SLOT_FRONT_BOTTOM_ROTATION_STEP 92 +# define BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP 92 +# define BOT_CLAMP_SLOT_FRONT_TOP_ROTATION_STEP 92 +# define BOT_CLAMP_SLOT_BACK_BOTTOM_ROTATION_STEP 0x2433 +# define BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP 0x2433 +# define BOT_CLAMP_SLOT_BACK_TOP_ROTATION_STEP 0x2433 # define BOT_CLAMP_BAY_FRONT_ROTATION_STEP \ BOT_CLAMP_SLOT_FRONT_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP -# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x1127 + 120) +# define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x1183 + 120) #define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET -129 #define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET -60 -- cgit v1.2.3 From b3442eedc8233a81afe074f4dda6c4dc7e45df72 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 14:52:48 +0200 Subject: digital/asserv: faster host speed --- digital/asserv/tools/asserv/init.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/digital/asserv/tools/asserv/init.py b/digital/asserv/tools/asserv/init.py index 5bbffe7d..c845af03 100644 --- a/digital/asserv/tools/asserv/init.py +++ b/digital/asserv/tools/asserv/init.py @@ -12,10 +12,19 @@ host_marcel = dict ( E = 0x3ff, D = 0x1ff, l = 0x1000, ) +host_robospierre = dict ( + scale = 0.0395840674352314, f = 0xdd1, + tkp = 1, tkd = 16, + ta = 0.75, tsm = 0x60, tss = 0x10, + akp = 2, akd = 16, + aa = 0.25, asm = 0x60, ass = 0x10, + E = 0x3ff, D = 0x1ff, + l = 0x1000, + ) host = { 'giboulee': host_marcel, 'marcel': host_marcel, - 'robospierre': host_marcel, + 'robospierre': host_robospierre, } target_marcel = dict ( scale = 0.0415178942124, f = 0xcef, -- cgit v1.2.3 From 7110e5fc8d629f10a85a4355bf34e5d022845f1c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 00:44:37 +0200 Subject: digital/io-hub: handle green area for path finding --- digital/io-hub/src/robospierre/path.c | 150 +++++++++++++++++++++++++++++----- 1 file changed, 130 insertions(+), 20 deletions(-) diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index 1122c684..bf6c2471 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -49,6 +49,8 @@ * 352/2 mm, and that is the reason why code should handle odd and even * columns differently. * + * There is also extra grid nodes in front of the green zone. + * * All those tricks are used to reduce the number of nodes. */ @@ -64,12 +66,18 @@ /** Number of nodes in the grid. */ #define PATH_GRID_NODES_NB (PATH_COLUMNS_NB * PATH_COLUMN_NODES_NB) +/** Number of nodes in front of each green zone. */ +#define PATH_GREEN_NODES_NB 4 + +/** Number of fixed nodes. */ +#define PATH_FIXED_NODES_NB (PATH_GRID_NODES_NB + 2 * PATH_GREEN_NODES_NB) + /** Number of nodes in search graph, last two nodes are destination and source * nodes. */ -#define PATH_NODES_NB (PATH_GRID_NODES_NB + 2) +#define PATH_NODES_NB (PATH_FIXED_NODES_NB + 2) /** Index of destination node. */ -#define PATH_DST_NODE_INDEX PATH_GRID_NODES_NB +#define PATH_DST_NODE_INDEX PATH_FIXED_NODES_NB /** Index of source node. */ #define PATH_SRC_NODE_INDEX (PATH_DST_NODE_INDEX + 1) @@ -91,7 +99,7 @@ struct path_t /** List of nodes used for A*. */ struct astar_node_t astar_nodes[PATH_NODES_NB]; /** Cache of whether a node is blocked. */ - uint8_t valid[PATH_GRID_NODES_NB]; + uint8_t valid[PATH_FIXED_NODES_NB]; /** Position of end points. */ vect_t endpoints[2]; /** Whether the last update was a success. */ @@ -102,7 +110,7 @@ struct path_t static struct path_t path; /** Static information on nodes. */ -static const struct path_node_t path_nodes[PATH_GRID_NODES_NB] = { +static const struct path_node_t path_nodes[PATH_FIXED_NODES_NB] = { /* {{{ */ { 1 }, /* 0 column 0. */ { 1 }, /* 1 */ @@ -148,6 +156,14 @@ static const struct path_node_t path_nodes[PATH_GRID_NODES_NB] = { { 1 }, /* 41 */ { 1 }, /* 42 */ { 1 }, /* 43 */ + { 1 }, /* 44 left green. */ + { 1 }, /* 45 */ + { 1 }, /* 46 */ + { 1 }, /* 47 */ + { 1 }, /* 48 right green. */ + { 1 }, /* 49 */ + { 1 }, /* 50 */ + { 1 }, /* 51 */ /* }}} */ }; @@ -165,9 +181,18 @@ path_pos (uint8_t node, vect_t *pos) + (col % 2 ? 350 / 2 : 0) - line * 350; } + else if (node < PATH_GRID_NODES_NB + 2 * PATH_GREEN_NODES_NB) + { + node -= PATH_GRID_NODES_NB; + uint8_t col = node / PATH_GREEN_NODES_NB; + uint8_t line = node - col * PATH_GREEN_NODES_NB; + pos->x = col == 0 ? BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM + : PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM; + pos->y = (5 - line) * 280 + 10; + } else { - *pos = path.endpoints[node - PATH_GRID_NODES_NB]; + *pos = path.endpoints[node - PATH_FIXED_NODES_NB]; } } @@ -180,30 +205,52 @@ path_blocking (uint8_t a, uint8_t b, int16_t *dp) vect_t va; vect_t vb; uint8_t escape_factor = 0; + uint8_t factor = 1; + uint8_t blocking = 0; if (a == PATH_SRC_NODE_INDEX || b == PATH_SRC_NODE_INDEX) escape_factor = path.escape_factor; path_pos (a, &va); path_pos (b, &vb); + /* Test for green zone. */ + uint8_t a_green, b_green; + a_green = va.x < 400 || va.x > PG_WIDTH - 400; + b_green = vb.x < 400 || vb.x > PG_WIDTH - 400; + if ((va.x < BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM + && vb.x > BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM) + || (va.x > BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM + && vb.x < BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM) + || (va.x > PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM + && vb.x < PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM) + || (va.x < PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM + && vb.x > PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM)) + blocking = 1; + if (a_green && b_green) + blocking = 1; + if (a_green || b_green) + factor = 4; /* Test for a blocking obstacle. */ - for (i = 0; i < PATH_OBSTACLES_NB; i++) + for (i = 0; i < PATH_OBSTACLES_NB && !blocking; i++) { if (path.obstacles[i].valid) { uint16_t d = distance_segment_point (&va, &vb, &path.obstacles[i].c); if (d < path.obstacles[i].r) - { - if (escape_factor) - { - int16_t d = distance_point_point (&va, &vb); - *dp = d * escape_factor; - return 0; - } - else - return 1; - } + blocking = 1; } } + /* Handle escaping. */ + if (blocking) + { + if (escape_factor) + { + int16_t d = distance_point_point (&va, &vb); + *dp = d * escape_factor; + return 0; + } + else + return 1; + } /* Compute distance. */ int16_t d = distance_point_point (&va, &vb); if (d == 0) @@ -212,7 +259,7 @@ path_blocking (uint8_t a, uint8_t b, int16_t *dp) return 0; } /* No blocking. */ - *dp = d; + *dp = d * factor; return 0; } @@ -221,7 +268,7 @@ static void path_blocked_update (void) { uint8_t i, j; - for (i = 0; i < PATH_GRID_NODES_NB; i++) + for (i = 0; i < PATH_FIXED_NODES_NB; i++) { uint8_t valid = 1; /* First, gather information from tables. */ @@ -398,8 +445,69 @@ path_astar_neighbor_callback_grid (uint8_t node, } } } + /* Check path to green nodes. */ + int16_t d; + if (col <= 1 || col >= PATH_COLUMNS_NB - 1) + { + uint8_t green = PATH_GRID_NODES_NB + + (col <= 1 ? 0 : PATH_GREEN_NODES_NB); + for (i = green; i < green + PATH_GREEN_NODES_NB; i++) + { + if (!path_blocking (node, i, &d)) + { + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } + } + } /* Check if direct path OK. */ + if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d)) + { + /* Add this neighbor. */ + neighbors[neighbors_nb].node = PATH_SRC_NODE_INDEX; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } +#if PATH_DEBUG + for (i = 0; i < neighbors_nb; i++) + DPRINTF (" n %d %d\n", neighbors[i].node, neighbors[i].weight); +#endif + return neighbors_nb; +} + +/** Neighbors callback for green nodes. */ +static uint8_t +path_astar_neighbor_callback_green (uint8_t node, + struct astar_neighbor_t *neighbors) +{ + uint8_t neighbors_nb = 0; + uint8_t i; + uint8_t col = (node - PATH_GRID_NODES_NB) / PATH_GREEN_NODES_NB; int16_t d; + /* Check path to grid nodes. */ + uint8_t grid = col ? PATH_GRID_NODES_NB - 2 * PATH_COLUMN_NODES_NB : 0; + for (i = grid; i < grid + 2 * PATH_COLUMN_NODES_NB; i++) + { + if (!path_blocking (node, i, &d)) + { + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } + } + /* Check path to other green nodes. */ + uint8_t green = PATH_GRID_NODES_NB + (col ? PATH_GREEN_NODES_NB : 0); + for (i = green; i < green + PATH_GREEN_NODES_NB; i++) + { + if (i != node && !path_blocking (node, i, &d)) + { + neighbors[neighbors_nb].node = i; + neighbors[neighbors_nb].weight = d + 1; + neighbors_nb++; + } + } + /* Check if direct path OK. */ if (!path_blocking (node, PATH_SRC_NODE_INDEX, &d)) { /* Add this neighbor. */ @@ -422,8 +530,8 @@ path_astar_neighbor_callback_endpoints (uint8_t node, uint8_t neighbors_nb = 0; uint8_t i; assert (node == PATH_DST_NODE_INDEX); - /* Select neighbors in the grid. */ - for (i = 0; i < PATH_GRID_NODES_NB; i++) + /* Select neighbors in the fixed nodes. */ + for (i = 0; i < PATH_FIXED_NODES_NB; i++) { /* Discard blocking nodes. */ if (!path.valid[i]) @@ -462,6 +570,8 @@ path_astar_neighbor_callback (uint8_t node, #endif if (node < PATH_GRID_NODES_NB) return path_astar_neighbor_callback_grid (node, neighbors); + else if (node < PATH_GRID_NODES_NB + 2 * PATH_GREEN_NODES_NB) + return path_astar_neighbor_callback_green (node, neighbors); else return path_astar_neighbor_callback_endpoints (node, neighbors); } -- cgit v1.2.3 From 32a21a69bb7adb44c4fc91d025ad8f0ccdaf45a1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 15:07:25 +0200 Subject: digital/io-hub: remove special case for green elements --- digital/io-hub/src/robospierre/bot.h | 7 +-- digital/io-hub/src/robospierre/element.c | 13 ++-- digital/io-hub/src/robospierre/element.h | 5 +- digital/io-hub/src/robospierre/test_element.c | 4 +- digital/io-hub/src/robospierre/top.c | 85 ++++----------------------- 5 files changed, 21 insertions(+), 93 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 8e32a317..de32ebb2 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -58,10 +58,9 @@ /** Distance from border to position in front of a green element. */ #define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600 -/** Distance to go to capture a green element. */ -#define BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM \ - (BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM - BOT_ELEMENT_RADIUS \ - - BOT_SIZE_FRONT - 20) +/** Distance from border to go to capture a green element. */ +#define BOT_GREEN_ELEMENT_DISTANCE_MM \ + (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 20) /** Speed used for initialisation. */ #ifdef HOST diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index bc76a2b0..71f754e0 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -706,24 +706,21 @@ element_opposed (uint8_t element_id) return op; } -position_t +vect_t element_get_pos (uint8_t element_id) { element_t e = element_get (element_id); - position_t pos; - pos.v = e.pos; - pos.a = 0xffff; + vect_t pos; + pos = e.pos; if (e.attr == (ELEMENT_GREEN |ELEMENT_RIGHT)) { /* To the right. */ - pos.a = 0x0000; - pos.v.x = PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM; + pos.x = PG_WIDTH - BOT_GREEN_ELEMENT_DISTANCE_MM; } if (e.attr == (ELEMENT_GREEN |ELEMENT_LEFT)) { /* To the left. */ - pos.a = 0x8000; - pos.v.x = BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM; + pos.x = BOT_GREEN_ELEMENT_DISTANCE_MM; } return pos; } diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 01e8f22e..3ac1f9ba 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -167,11 +167,8 @@ element_nearest_element_id (position_t robot_pos); /** Give the position where the robot need to be placed to get an element. - The position correspond to the emplacement of the element with an angle of - 0xffff. If the element is in the green zone, the returned position include - the angle the robot need to set before moving to the element. */ -position_t +vect_t element_get_pos (uint8_t element_id); #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index 2de92361..7e62a492 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -328,8 +328,8 @@ int main () case 'g': printf ("element id: "); scanf ("%i", &x); - position_t pos = element_get_pos (x); - printf ("x: %u y: %u a: %u\n", pos.v.x, pos.v.y, pos.a); + vect_t pos = element_get_pos (x); + printf ("x: %u y: %u\n", pos.x, pos.y); break; case 'q': exit = 1; diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 5c10a0f3..a3ad4c72 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -49,11 +49,7 @@ FSM_STATES ( TOP_GOING_OUT2, TOP_GOING_TO_DROP, - TOP_GOING_TO_ELEMENT, - TOP_GOING_TO_GREEN_POS, - TOP_GOING_TO_GREEN_ELEMENT, - TOP_TAKING_GREEN_ELEMENT, - TOP_GOING_FROM_GREEN_ELEMENT) + TOP_GOING_TO_ELEMENT) FSM_START_WITH (TOP_START) @@ -62,8 +58,6 @@ struct top_t { /** Target element. */ uint8_t target_element_id; - /** Green element move distance. */ - int16_t green_move_distance_mm; }; /** Global context. */ @@ -89,27 +83,11 @@ top_go_element (void) position_t robot_pos; asserv_get_position (&robot_pos); ctx.target_element_id = element_best (robot_pos); - position_t element_pos = element_get_pos (ctx.target_element_id); + vect_t element_pos = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; - if (element_pos.a != 0xffff) - { - /* Green zone element. */ - ctx.green_move_distance_mm = BOT_GREEN_ELEMENT_MOVE_DISTANCE_MM; - if (backward) - { - element_pos.a += 0x8000; - ctx.green_move_distance_mm = -ctx.green_move_distance_mm; - } - move_start (element_pos, backward); - return 2; - } - else - { - /* Regular element. */ - move_start_noangle (element_pos.v, backward, 0); - return 1; - } + move_start_noangle (element_pos, backward, 0); + return 1; } static uint8_t @@ -118,10 +96,10 @@ top_go_drop (void) position_t robot_pos; asserv_get_position (&robot_pos); uint8_t drop_pos_id = 37; - position_t drop_pos = element_get_pos (drop_pos_id); + vect_t drop_pos = element_get_pos (drop_pos_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; - move_start_noangle (drop_pos.v, backward, 0); + move_start_noangle (drop_pos, backward, 0); return 0; } @@ -137,79 +115,36 @@ top_decision (void) FSM_TRANS (TOP_GOING_OUT2, robot_move_success, drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT, - green_element, TOP_GOING_TO_GREEN_POS) + element, TOP_GOING_TO_ELEMENT) { switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, drop); case 1: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, element); - case 2: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, - green_element); } } FSM_TRANS (TOP_GOING_TO_DROP, move_success, drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT, - green_element, TOP_GOING_TO_GREEN_POS) + element, TOP_GOING_TO_ELEMENT) { clamp_drop (logistic_global.collect_direction); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, drop); case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, element); - case 2: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, - green_element); } } FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT, - green_element, TOP_GOING_TO_GREEN_POS) + element, TOP_GOING_TO_ELEMENT) { + element_taken (ctx.target_element_id, ELEMENT_PAWN); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop); case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, element); - case 2: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, - green_element); - } -} - -FSM_TRANS (TOP_GOING_TO_GREEN_POS, move_success, TOP_GOING_TO_GREEN_ELEMENT) -{ - asserv_move_linearly (ctx.green_move_distance_mm); - return FSM_NEXT (TOP_GOING_TO_GREEN_POS, move_success); -} - -FSM_TRANS (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success, - TOP_TAKING_GREEN_ELEMENT) -{ - element_taken (ctx.target_element_id, ELEMENT_PAWN); - return FSM_NEXT (TOP_GOING_TO_GREEN_ELEMENT, robot_move_success); -} - -FSM_TRANS_TIMEOUT (TOP_TAKING_GREEN_ELEMENT, 250, TOP_GOING_FROM_GREEN_ELEMENT) -{ - asserv_move_linearly (-ctx.green_move_distance_mm); - return FSM_NEXT_TIMEOUT (TOP_TAKING_GREEN_ELEMENT); -} - -FSM_TRANS (TOP_GOING_FROM_GREEN_ELEMENT, robot_move_success, - drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT, - green_element, TOP_GOING_TO_GREEN_POS) -{ - switch (top_decision ()) - { - default: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, - robot_move_success, drop); - case 1: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, - robot_move_success, element); - case 2: return FSM_NEXT (TOP_GOING_FROM_GREEN_ELEMENT, - robot_move_success, green_element); } } -- cgit v1.2.3 From 4092ab572dc5664a5528d6e85f6052fccdae14e7 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Tue, 31 May 2011 17:51:18 +0200 Subject: digital/io-hub: fixed ready flag in tower construction --- digital/io-hub/src/robospierre/logistic.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 0822a852..fc21823c 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -258,7 +258,7 @@ logistic_make_tower () { LOGISTIC_CASE (D, _, e, _, _, - H, _, RIGHT, 1); + H, _, RIGHT, 0); LOGISTIC_CASE (_, h, _, _, p, -- cgit v1.2.3 From d00e50a990ad1dda71fcd0902056b42d53f82089 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Tue, 31 May 2011 18:22:33 +0200 Subject: digital/io-hub: fixed some logistic cases --- digital/io-hub/src/robospierre/logistic.c | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index fc21823c..d2abbb92 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -268,10 +268,6 @@ logistic_make_tower () e, _, e, _, D, LEFT, 0); - LOGISTIC_CASE (P, h, - e, _, e, - _, D, LEFT, 0); - LOGISTIC_CASE (_, h, e, _, e, P, D, LEFT, 0); @@ -322,7 +318,7 @@ logistic_make_unload () LOGISTIC_CASE (_, _, _, P, e, - e, D, LEFT, 0); + _, D, LEFT, 0); LOGISTIC_CASE (_, _, e, P, _, @@ -332,10 +328,22 @@ logistic_make_unload () static void logisitic_make_switches () { + LOGISTIC_CASE (_, P, + _, D, e, + _, _, LEFT, 0); + + LOGISTIC_CASE (P, _, + e, D, _, + _, _, LEFT, 0); + LOGISTIC_CASE (_, _, e, D, _, P, _, LEFT, 0); + LOGISTIC_CASE (_, _, + _, D, e, + _, P, LEFT, 0); + LOGISTIC_CASE (_, D, e, p, e, P, _, LEFT, 0); @@ -348,13 +356,13 @@ logisitic_make_switches () e, _, _, H, _, RIGHT, 0); - LOGISTIC_CASE (h, e, + LOGISTIC_CASE (h, _, e, _, e, - D, P, LEFT, 0); + D, P, RIGHT, 0); LOGISTIC_CASE (h, P, e, _, e, - D, P, LEFT, 0); + D, _, RIGHT, 0); } /** Examine current state and take a decision. */ -- cgit v1.2.3 From 13b762a23b3448bf6195985957fbf473b104e236 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Tue, 31 May 2011 18:44:33 +0200 Subject: digital/io-hub: fixed some test order in logistic --- digital/io-hub/src/robospierre/logistic.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index d2abbb92..a1b2a705 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -300,15 +300,11 @@ logistic_make_unload () _, _, _, a, _, RIGHT, 1); - LOGISTIC_CASE (H, _, - e, _, _, - D, _, RIGHT, 0); - LOGISTIC_CASE (_, H, _, _, e, _, D, LEFT, 0); - LOGISTIC_CASE (P, _, + LOGISTIC_CASE (H, _, e, _, _, D, _, RIGHT, 0); @@ -316,6 +312,10 @@ logistic_make_unload () _, _, e, _, D, LEFT, 0); + LOGISTIC_CASE (P, _, + e, _, _, + D, _, RIGHT, 0); + LOGISTIC_CASE (_, _, _, P, e, _, D, LEFT, 0); -- cgit v1.2.3 From 8e18792e1c921690ac99364ff84a3ab056ffcad6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 15:37:29 +0200 Subject: digital/io-hub: fixed contact --- digital/io-hub/src/robospierre/contact_defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h index 8c60342a..2c300ece 100644 --- a/digital/io-hub/src/robospierre/contact_defs.h +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -27,7 +27,7 @@ #define CONTACT_COLOR E, 5 #define CONTACT_JACK E, 6 -#define CONTACT_STRAT E, 5 +#define CONTACT_STRAT E, 3 #define CONTACT_FRONT_BOTTOM A, 4 #define CONTACT_FRONT_MIDDLE F, 4 #define CONTACT_BACK_BOTTOM A, 5 -- cgit v1.2.3 From 079e9cd0832d9854b172d120c822b578a2788e9a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 15:45:24 +0200 Subject: digital/io-hub: fix pawn sensor --- digital/io-hub/src/robospierre/pawn_sensor.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index 3a59e908..be6f3c87 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -32,6 +32,7 @@ #include "clamp.h" #include "bot.h" +#include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" /* Handle pawn sensors. When a pawn is detected, it can not be taken @@ -94,6 +95,12 @@ pawn_sensor_get (uint8_t direction) ctx->active = 0; return pawn_sensor_get_type (direction); } + else if (d > UTILS_ABS (dist)) + { + vect_from_polar_uf016 (&ctx->active_position, dist, + robot_position.a); + vect_translate (&ctx->active_position, &robot_position.v); + } } else { -- cgit v1.2.3 From 0b188fa1a29f2b7f73a88e87c82724132ea53f1b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 16:15:43 +0200 Subject: digital/io-hub: add clamp waiting --- digital/io-hub/src/robospierre/clamp.c | 40 ++++++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 17 ++++++++++++++- 3 files changed, 60 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index b2a6bf76..0a7654f3 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -95,6 +95,10 @@ FSM_STATES ( FSM_EVENTS ( /* New element inside bottom slot. */ clamp_new_element, + /* Sent when clamp is working. */ + clamp_working, + /* Sent when clamp return to idle state. */ + clamp_done, /* Order to drop elements. */ clamp_drop, /* Sent once drop is done, but robot should advance to completely @@ -123,6 +127,8 @@ FSM_START_WITH (CLAMP_MOVE_IDLE) /** Clamp context. */ struct clamp_t { + /** True if clamp is working. */ + uint8_t working; /** Current position. */ uint8_t pos_current; /** Requested position. */ @@ -196,6 +202,12 @@ clamp_init (void) ctx.open = 1; } +uint8_t +clamp_working (void) +{ + return ctx.working; +} + void clamp_move (uint8_t pos) { @@ -428,11 +440,15 @@ FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); } FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) { + ctx.working = 1; + fsm_queue_post_event (FSM_EVENT (AI, clamp_working)); pwm_set_timed (clamp_slot_door[ctx.pos_new], BOT_PWM_DOOR_CLOSE (ctx.pos_new)); return FSM_NEXT (CLAMP_IDLE, clamp_new_element); @@ -470,10 +486,18 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle); } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked); + } } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done); + } } FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, @@ -500,12 +524,20 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, move_to_idle); } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, clamp_locked); + } } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, done); + } } FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME, @@ -539,12 +571,20 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, move_to_idle); } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, clamp_locked); + } } else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, done); + } } FSM_TRANS (CLAMP_LOCKED, clamp_new_element, CLAMP_LOCKED) diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 09233cce..60da1615 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -64,6 +64,10 @@ enum { void clamp_init (void); +/** Is clamp working? */ +uint8_t +clamp_working (void); + /** Move clamp to given position. */ void clamp_move (uint8_t pos); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index a3ad4c72..92e42b1c 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -49,7 +49,9 @@ FSM_STATES ( TOP_GOING_OUT2, TOP_GOING_TO_DROP, - TOP_GOING_TO_ELEMENT) + TOP_GOING_TO_ELEMENT, + /* Waiting clamp has finished its work. */ + TOP_WAITING_CLAMP) FSM_START_WITH (TOP_START) @@ -137,10 +139,13 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success, } FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, + clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { element_taken (ctx.target_element_id, ELEMENT_PAWN); + if (clamp_working ()) + return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop); @@ -148,3 +153,13 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, } } +FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT) +{ + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, drop); + case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element); + } +} -- cgit v1.2.3 From 4d6cb481580591f35bfbc201671dbd45ec759d15 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:18:55 +0200 Subject: digital/io-hub: increase door voltage --- digital/io-hub/src/robospierre/bot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index de32ebb2..02ec8f07 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -173,7 +173,7 @@ -0x1ff, (((slot) == CLAMP_SLOT_FRONT_BOTTOM \ || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? 80 : 62), \ (((slot) == CLAMP_SLOT_FRONT_BOTTOM \ - || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x180) + || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x200) #define BOT_PWM_CLAMP_INIT 0x1ff, 150, 0 #define BOT_PWM_DOOR_INIT 0x1ff, 80, 0x55 -- cgit v1.2.3 From 280b556d985801e1528119ed87ac12f563d90745 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:27:23 +0200 Subject: digital/mimot: lower clamp elevator acceleration --- digital/mimot/tools/mimot/init.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/mimot/tools/mimot/init.py b/digital/mimot/tools/mimot/init.py index 5cdab647..49b16cc1 100644 --- a/digital/mimot/tools/mimot/init.py +++ b/digital/mimot/tools/mimot/init.py @@ -11,7 +11,7 @@ target_marcel = dict ( ) target_robospierre = dict ( a0kp = 8, a0kd = 1, - a0a = 8, a0sm = 0x60, a0ss = 0x10, + a0a = 2, a0sm = 0x60, a0ss = 0x10, a0be = 32, a0bs = 0x08, a0bc = 125, a1kp = 4, a1a = 0.5, a1sm = 0x30, a1ss = 0x08, -- cgit v1.2.3 From 546594731c6c7d2727382154a991a564b8b8d6da Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:28:36 +0200 Subject: digital/io-hub: fix move backward --- digital/io-hub/src/robospierre/move.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index e89e6d6b..6c204f61 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -187,21 +187,24 @@ move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle, uint8_t backward) { position_t robot_position; + asserv_get_position (&robot_position); + uint16_t robot_angle = robot_position.a; + if (backward & ASSERV_BACKWARD) + robot_angle += 0x8000; /* Remember step. */ move_data.step = dst; move_data.step_angle = angle; move_data.step_with_angle = with_angle; move_data.step_backward = backward; /* Compute angle to destination. */ - asserv_get_position (&robot_position); vect_t v = dst; vect_sub (&v, &robot_position.v); uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); if (backward & ASSERV_BACKWARD) dst_angle += 0x8000; if ((backward & ASSERV_REVERT_OK) - && (dst_angle ^ robot_position.a) & 0x8000) + && (dst_angle ^ robot_angle) & 0x8000) dst_angle += 0x8000; - int16_t diff = dst_angle - robot_position.a; + int16_t diff = dst_angle - robot_angle; /* Move or rotate. */ if (UTILS_ABS (diff) < 0x1000) { -- cgit v1.2.3 From 18a1c04ba9b61d484c745b075bfbaad101629a2c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:34:26 +0200 Subject: digital/io-hub: add green zone width define --- digital/io-hub/src/robospierre/path.c | 6 ++--- digital/io-hub/src/robospierre/playground_2011.h | 33 ++++++++++++++++++++++++ digital/io-hub/src/robospierre/top.c | 4 +-- 3 files changed, 38 insertions(+), 5 deletions(-) create mode 100644 digital/io-hub/src/robospierre/playground_2011.h diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index bf6c2471..61ea9293 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -26,7 +26,7 @@ #include "defs.h" #include "path.h" #include "bot.h" -#include "playground.h" +#include "playground_2011.h" #include "modules/path/astar/astar.h" #include "modules/utils/utils.h" @@ -213,8 +213,8 @@ path_blocking (uint8_t a, uint8_t b, int16_t *dp) path_pos (b, &vb); /* Test for green zone. */ uint8_t a_green, b_green; - a_green = va.x < 400 || va.x > PG_WIDTH - 400; - b_green = vb.x < 400 || vb.x > PG_WIDTH - 400; + a_green = va.x < PG_GREEN_WIDTH_MM || va.x > PG_WIDTH - PG_GREEN_WIDTH_MM; + b_green = vb.x < PG_GREEN_WIDTH_MM || vb.x > PG_WIDTH - PG_GREEN_WIDTH_MM; if ((va.x < BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM && vb.x > BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM) || (va.x > BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM diff --git a/digital/io-hub/src/robospierre/playground_2011.h b/digital/io-hub/src/robospierre/playground_2011.h new file mode 100644 index 00000000..a9963e34 --- /dev/null +++ b/digital/io-hub/src/robospierre/playground_2011.h @@ -0,0 +1,33 @@ +#ifndef playground_2011_h +#define playground_2011_h +/* playground_2011.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#include "playground.h" + +/** Width of a green zone. */ +#define PG_GREEN_WIDTH_MM 400 + +#endif /* playground_2011_h */ diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 92e42b1c..5756d38e 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -24,7 +24,7 @@ * }}} */ #include "common.h" -#include "playground.h" +#include "playground_2011.h" #include "asserv.h" #define FSM_NAME AI @@ -69,7 +69,7 @@ struct top_t top_global; FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1) { element_init (); - asserv_goto (PG_X (400 + 100), PG_Y (PG_LENGTH - 200), 0); + asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), PG_Y (PG_LENGTH - 200), 0); return FSM_NEXT (TOP_START, init_start_round); } -- cgit v1.2.3 From f86980a9c1a8a7daf2b45629a66288f2a8fd03a2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:35:06 +0200 Subject: digital/io-hub: authorize backward movement from green zone --- digital/io-hub/src/robospierre/move.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index 6c204f61..09faf8f9 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -24,6 +24,7 @@ * }}} */ #include "common.h" #include "move.h" +#include "playground_2011.h" #include "main.h" #include "asserv.h" @@ -191,6 +192,10 @@ move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle, uint16_t robot_angle = robot_position.a; if (backward & ASSERV_BACKWARD) robot_angle += 0x8000; + /* Check for green zone. */ + if (robot_position.v.x < PG_GREEN_WIDTH_MM + || robot_position.v.x > PG_WIDTH - PG_GREEN_WIDTH_MM) + backward |= ASSERV_REVERT_OK; /* Remember step. */ move_data.step = dst; move_data.step_angle = angle; -- cgit v1.2.3 From 8350707ab8e1cbea3a83ddc82bbdc814210b0e66 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:36:08 +0200 Subject: digital/io-hub: change initial position --- digital/io-hub/src/robospierre/clamp.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 0a7654f3..a9a1680b 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -360,14 +360,16 @@ clamp_route (void) } else if (pos_current == CLAMP_BAY_FRONT_LEAVING) { - if (pos_request == CLAMP_SLOT_SIDE) + if (pos_request == CLAMP_SLOT_SIDE + || pos_request == CLAMP_BAY_SIDE_ENTER_LEAVE) pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; else pos_new = CLAMP_SLOT_BACK_MIDDLE; } else if (pos_current == CLAMP_BAY_BACK_LEAVING) { - if (pos_request == CLAMP_SLOT_SIDE) + if (pos_request == CLAMP_SLOT_SIDE + || pos_request == CLAMP_BAY_SIDE_ENTER_LEAVE) pos_new = CLAMP_BAY_SIDE_ENTER_LEAVE; else pos_new = CLAMP_SLOT_FRONT_MIDDLE; @@ -434,7 +436,7 @@ FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success, FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, CLAMP_GOING_IDLE) { - clamp_move (CLAMP_SLOT_SIDE); + clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE); return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success); } -- cgit v1.2.3 From 5ad4761bb3fc9b4d96d470630ee89b5a5bbd14e7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 22:42:33 +0200 Subject: digital/io-hub: move clamp to idle position once match started --- digital/io-hub/src/robospierre/clamp.c | 17 ++++++++++++++++- digital/io-hub/src/robospierre/logistic.c | 3 ++- 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index a9a1680b..979cc3c4 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -59,6 +59,10 @@ FSM_STATES ( CLAMP_INIT_FINDING_ROTATION_EDGE, /* Initialisation sequence: finding top switch. */ CLAMP_INIT_FINDING_TOP, + /* Initialisation sequence: going to rest position. */ + CLAMP_INIT_GOING_REST, + /* Clamp ready, waiting in rest position. */ + CLAMP_INIT_READY, /* Returning to idle position. */ CLAMP_GOING_IDLE, @@ -434,12 +438,23 @@ FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success, } FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, - CLAMP_GOING_IDLE) + CLAMP_INIT_GOING_REST) { clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE); return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success); } +FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY) +{ + return FSM_NEXT (CLAMP_INIT_GOING_REST, clamp_move_success); +} + +FSM_TRANS (CLAMP_INIT_READY, init_start_round, CLAMP_GOING_IDLE) +{ + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_INIT_READY, init_start_round); +} + FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) { ctx.working = 0; diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index a1b2a705..4fff865d 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -419,7 +419,8 @@ logistic_init (void) ctx.slots[i] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; ctx.collect_direction = DIRECTION_FORWARD; - ctx.clamp_pos_idle = CLAMP_SLOT_FRONT_MIDDLE; + ctx.clamp_pos_idle = ctx.collect_direction == DIRECTION_FORWARD + ? CLAMP_SLOT_FRONT_MIDDLE : CLAMP_SLOT_BACK_MIDDLE; ctx.construct_possible = 0; ctx.prepare = 1; ctx.ready = 0; -- cgit v1.2.3 From 4f3135cce80991a0f4930ada3975b1eba46231e6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Tue, 31 May 2011 23:59:36 +0200 Subject: digital/io-hub: default to backward direction Back sensors work better. --- digital/io-hub/src/robospierre/logistic.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 4fff865d..fea121cf 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -418,7 +418,7 @@ logistic_init (void) for (i = 0; i < CLAMP_SLOT_NB; i++) ctx.slots[i] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; - ctx.collect_direction = DIRECTION_FORWARD; + ctx.collect_direction = DIRECTION_BACKWARD; ctx.clamp_pos_idle = ctx.collect_direction == DIRECTION_FORWARD ? CLAMP_SLOT_FRONT_MIDDLE : CLAMP_SLOT_BACK_MIDDLE; ctx.construct_possible = 0; -- cgit v1.2.3 From 9e61ccb17af699691cd2e16c102d81cda2e119d5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 00:00:15 +0200 Subject: digital/io-hub: change temporary hack drop position --- digital/io-hub/src/robospierre/top.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 5756d38e..6e2fb6ba 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -97,7 +97,7 @@ top_go_drop (void) { position_t robot_pos; asserv_get_position (&robot_pos); - uint8_t drop_pos_id = 37; + uint8_t drop_pos_id = 43; vect_t drop_pos = element_get_pos (drop_pos_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; -- cgit v1.2.3 From bbc0bfccf74022a37e8afd312cfde802444e281c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 00:40:10 +0200 Subject: digital/io-hub: fix move revert ok --- digital/io-hub/src/robospierre/move.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index 09faf8f9..623dabdf 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -206,8 +206,9 @@ move_go_or_rotate (vect_t dst, uint16_t angle, uint8_t with_angle, uint16_t dst_angle = atan2 (v.y, v.x) * ((1l << 16) / (2 * M_PI)); if (backward & ASSERV_BACKWARD) dst_angle += 0x8000; + int16_t diff_angle = dst_angle - robot_angle; if ((backward & ASSERV_REVERT_OK) - && (dst_angle ^ robot_angle) & 0x8000) + && (diff_angle > 0x4000 || diff_angle < -0x4000)) dst_angle += 0x8000; int16_t diff = dst_angle - robot_angle; /* Move or rotate. */ -- cgit v1.2.3 From fd9d4c27c0fefce4bfde4d85fde34444e4f1421f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 00:46:13 +0200 Subject: digital/io-hub: save energy while waiting start --- digital/io-hub/src/robospierre/bot.h | 3 ++- digital/io-hub/src/robospierre/clamp.c | 6 ++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 02ec8f07..e31825e3 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -176,7 +176,8 @@ || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x200) #define BOT_PWM_CLAMP_INIT 0x1ff, 150, 0 -#define BOT_PWM_DOOR_INIT 0x1ff, 80, 0x55 +#define BOT_PWM_DOOR_INIT 0x1ff, 80, 0 +#define BOT_PWM_DOOR_INIT_START 0x55 #define BOT_PWM_CLAMP_DOOR_INIT 150 #endif /* bot_h */ diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 979cc3c4..e3e04cf6 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -446,11 +446,17 @@ FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY) { + mimot_motor0_free (); + mimot_motor1_free (); return FSM_NEXT (CLAMP_INIT_GOING_REST, clamp_move_success); } FSM_TRANS (CLAMP_INIT_READY, init_start_round, CLAMP_GOING_IDLE) { + pwm_set (BOT_PWM_DOOR_FRONT_BOTTOM, BOT_PWM_DOOR_INIT_START); + pwm_set (BOT_PWM_DOOR_FRONT_TOP, BOT_PWM_DOOR_INIT_START); + pwm_set (BOT_PWM_DOOR_BACK_BOTTOM, BOT_PWM_DOOR_INIT_START); + pwm_set (BOT_PWM_DOOR_BACK_TOP, BOT_PWM_DOOR_INIT_START); clamp_move (logistic_global.clamp_pos_idle); return FSM_NEXT (CLAMP_INIT_READY, init_start_round); } -- cgit v1.2.3 From 6d3e532a65e14fddb242ecc4841becec44253a6b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 12:01:58 +0200 Subject: digital/io-hub: handle element in first position --- digital/io-hub/src/robospierre/top.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 6e2fb6ba..b99e3baf 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -116,9 +116,12 @@ top_decision (void) } FSM_TRANS (TOP_GOING_OUT2, robot_move_success, + clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { + if (clamp_working ()) + return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_OUT2, robot_move_success, drop); -- cgit v1.2.3 From b2c12931a8378cdb2112da8b21b63099683516f5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 12:51:46 +0200 Subject: digital/io-hub: do not prepare in green zone --- digital/io-hub/src/robospierre/clamp.c | 45 ++++++++++++++++++++++++++ digital/io-hub/src/robospierre/clamp.h | 4 +++ digital/io-hub/src/robospierre/element.c | 7 ---- digital/io-hub/src/robospierre/element.h | 6 ++++ digital/io-hub/src/robospierre/logistic.c | 3 +- digital/io-hub/src/robospierre/logistic.h | 4 +++ digital/io-hub/src/robospierre/top.c | 54 +++++++++++++++++++++++++++---- 7 files changed, 107 insertions(+), 16 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index e3e04cf6..3e7fa9fd 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -99,6 +99,8 @@ FSM_STATES ( FSM_EVENTS ( /* New element inside bottom slot. */ clamp_new_element, + /* Order to prepare tower. */ + clamp_prepare, /* Sent when clamp is working. */ clamp_working, /* Sent when clamp return to idle state. */ @@ -243,6 +245,13 @@ clamp_new_element (uint8_t pos, uint8_t element_type) FSM_HANDLE (AI, clamp_new_element); } +void +clamp_prepare (uint8_t prepare) +{ + logistic_global.prepare = 1; + FSM_HANDLE (AI, clamp_prepare); +} + uint8_t clamp_drop (uint8_t drop_direction) { @@ -477,6 +486,42 @@ FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) return FSM_NEXT (CLAMP_IDLE, clamp_new_element); } +FSM_TRANS (CLAMP_IDLE, clamp_prepare, + move_element, CLAMP_MOVING_ELEMENT, + move_to_idle, CLAMP_GOING_IDLE, + clamp_locked, CLAMP_LOCKED, + done, CLAMP_IDLE) +{ + logistic_decision (); + if (logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element); + } + else if (logistic_global.clamp_pos_idle != ctx.pos_current) + { + if (logistic_path_clear (ctx.pos_current, + logistic_global.clamp_pos_idle)) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_to_idle); + } + else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + return FSM_NEXT (CLAMP_IDLE, clamp_prepare, clamp_locked); + } + } + else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + return FSM_NEXT (CLAMP_IDLE, clamp_prepare, done); + } +} + FSM_TRANS (CLAMP_IDLE, clamp_drop, CLAMP_DROPING_DOOR_OPENING) { /* If going forward, drop at back. */ diff --git a/digital/io-hub/src/robospierre/clamp.h b/digital/io-hub/src/robospierre/clamp.h index 60da1615..76dcc34b 100644 --- a/digital/io-hub/src/robospierre/clamp.h +++ b/digital/io-hub/src/robospierre/clamp.h @@ -80,6 +80,10 @@ clamp_move_element (uint8_t from, uint8_t to); void clamp_new_element (uint8_t pos, uint8_t element_type); +/** Change logisitic preparation level and update clamp state. */ +void +clamp_prepare (uint8_t prepare); + /** Drop an element tower. Return 0 if not currently possible. If * drop_direction is forward, drop at the back. */ uint8_t diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 71f754e0..73b165cc 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -131,13 +131,6 @@ struct element_t element_table[] = {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT} /* right blue */ }; -inline element_t -element_get (uint8_t element_id) -{ - assert (element_id < UTILS_COUNT (element_table)); - return element_table[element_id]; -} - inline void element_set (uint8_t element_id, element_t element) { diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 3ac1f9ba..bcc40f43 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -171,4 +171,10 @@ element_nearest_element_id (position_t robot_pos); vect_t element_get_pos (uint8_t element_id); +extern inline element_t +element_get (uint8_t element_id) +{ + return element_table[element_id]; +} + #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index fea121cf..32b9fdb0 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -365,8 +365,7 @@ logisitic_make_switches () D, _, RIGHT, 0); } -/** Examine current state and take a decision. */ -static void +void logistic_decision (void) { /* Reset. */ diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 13c849a5..334b9db4 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -97,6 +97,10 @@ extern struct logistic_t logistic_global; void logistic_init (void); +/** Examine current state and take a decision. */ +void +logistic_decision (void); + /** To be called at regular interval to check for bad robot state. */ void logistic_update (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index b99e3baf..0af8ce80 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -51,7 +51,13 @@ FSM_STATES ( TOP_GOING_TO_DROP, TOP_GOING_TO_ELEMENT, /* Waiting clamp has finished its work. */ - TOP_WAITING_CLAMP) + TOP_WAITING_CLAMP, + /* Waiting construction is ready to drop. */ + TOP_WAITING_READY, + /* Dropping, opening the doors. */ + TOP_DROP_DROPPING, + /* Dropping, clearing so that doors can be closed. */ + TOP_DROP_CLEARING) FSM_START_WITH (TOP_START) @@ -85,6 +91,11 @@ top_go_element (void) position_t robot_pos; asserv_get_position (&robot_pos); ctx.target_element_id = element_best (robot_pos); + element_t e = element_get (ctx.target_element_id); + if (e.attr & ELEMENT_GREEN) + logistic_global.prepare = 0; + else + logistic_global.prepare = 1; vect_t element_pos = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; @@ -130,14 +141,18 @@ FSM_TRANS (TOP_GOING_OUT2, robot_move_success, } FSM_TRANS (TOP_GOING_TO_DROP, move_success, - drop, TOP_GOING_TO_DROP, - element, TOP_GOING_TO_ELEMENT) + ready, TOP_DROP_DROPPING, + wait_clamp, TOP_WAITING_READY) { - clamp_drop (logistic_global.collect_direction); - switch (top_decision ()) + if (logistic_global.ready) + { + clamp_drop (logistic_global.collect_direction); + return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready); + } + else { - default: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, drop); - case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_success, element); + clamp_prepare (1); + return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp); } } @@ -166,3 +181,28 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, case 1: return FSM_NEXT (TOP_WAITING_CLAMP, clamp_done, element); } } + +FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING) +{ + clamp_drop (logistic_global.collect_direction); + return FSM_NEXT (TOP_WAITING_READY, clamp_done); +} + +FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) +{ + asserv_move_linearly (200); + return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting); +} + +FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT) +{ + clamp_drop_clear (); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); + case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, element); + } +} + -- cgit v1.2.3 From 51b917f7994d6d53fde11263ba4da3470fd2a6c4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 15:26:22 +0200 Subject: digital/io-hub, host/simu: add codebar --- digital/io-hub/src/robospierre/Makefile | 1 + digital/io-hub/src/robospierre/codebar.avr.c | 64 +++++++++++++++++++++++++++ digital/io-hub/src/robospierre/codebar.h | 36 +++++++++++++++ digital/io-hub/src/robospierre/codebar.host.c | 56 +++++++++++++++++++++++ digital/io-hub/src/robospierre/main.c | 15 +++++++ digital/io-hub/src/robospierre/pawn_sensor.c | 5 ++- digital/io-hub/tools/io_hub/mex.py | 43 ++++++++++++++++++ host/simu/robots/robospierre/model/bag.py | 2 +- host/simu/robots/robospierre/model/clamp.py | 15 +++++-- 9 files changed, 231 insertions(+), 6 deletions(-) create mode 100644 digital/io-hub/src/robospierre/codebar.avr.c create mode 100644 digital/io-hub/src/robospierre/codebar.h create mode 100644 digital/io-hub/src/robospierre/codebar.host.c diff --git a/digital/io-hub/src/robospierre/Makefile b/digital/io-hub/src/robospierre/Makefile index 714541dc..88e6c79e 100644 --- a/digital/io-hub/src/robospierre/Makefile +++ b/digital/io-hub/src/robospierre/Makefile @@ -6,6 +6,7 @@ HOST_PROGS = test_element # Sources to compile. io_hub_SOURCES = main.c top.c \ clamp.c logistic.c element.c pawn_sensor.c \ + codebar.avr.c codebar.host.c \ radar_defs.c radar.c path.c move.c \ init.c fsm.host.c fsm_AI_gen.avr.c fsm_queue.c \ pwm.avr.c pwm.host.c \ diff --git a/digital/io-hub/src/robospierre/codebar.avr.c b/digital/io-hub/src/robospierre/codebar.avr.c new file mode 100644 index 00000000..d2609012 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.avr.c @@ -0,0 +1,64 @@ +/* codebar.avr.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "codebar.h" + +#include "defs.h" + +#include "modules/twi/twi.h" +#include "modules/utils/utils.h" +#include "modules/utils/crc.h" +#include "modules/utils/byte.h" + +#define CODEBAR_ADDRESS 0x20 +#define CODEBAR_STATUS_LENGTH 7 + +void +codebar_init (void) +{ +} + +uint8_t +codebar_get (uint8_t direction) +{ + uint8_t buffer[CODEBAR_STATUS_LENGTH]; + /* Read status. */ + twi_master_recv (CODEBAR_ADDRESS, buffer, sizeof (buffer)); + uint8_t ret = twi_master_wait (); + if (ret != CODEBAR_STATUS_LENGTH) + return 0; + uint8_t crc = crc_compute (buffer + 1, CODEBAR_STATUS_LENGTH - 1); + if (crc != buffer[0]) + return 0; + /* Get data. */ + uint8_t offset = direction == DIRECTION_FORWARD ? 1 : 4; + uint16_t age = v8_to_v16 (buffer[offset], buffer[offset + 1]); + uint16_t type = buffer[offset + 2]; + if (age > 225) + return 0; + else + return type; +} + diff --git a/digital/io-hub/src/robospierre/codebar.h b/digital/io-hub/src/robospierre/codebar.h new file mode 100644 index 00000000..d334f131 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.h @@ -0,0 +1,36 @@ +#ifndef codebar_h +#define codebar_h +/* codebar.h */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/** Initialise module. */ +void +codebar_init (void); + +/** Get element type on the specified direction. */ +uint8_t +codebar_get (uint8_t direction); + +#endif /* codebar_h */ diff --git a/digital/io-hub/src/robospierre/codebar.host.c b/digital/io-hub/src/robospierre/codebar.host.c new file mode 100644 index 00000000..68ced300 --- /dev/null +++ b/digital/io-hub/src/robospierre/codebar.host.c @@ -0,0 +1,56 @@ +/* codebar.host.c */ +/* robospierre - Eurobot 2011 AI. {{{ + * + * Copyright (C) 2011 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include "common.h" +#include "codebar.h" +#include "defs.h" + +#include "modules/host/host.h" +#include "modules/host/mex.h" + +uint8_t codebar_front, codebar_back; + +static void +codebar_handle (void *user, mex_msg_t *msg) +{ + mex_msg_pop (msg, "BB", &codebar_front, &codebar_back); +} + +void +codebar_init (void) +{ + const char *mex_instance = host_get_instance ("io-hub0", 0); + uint8_t mtype = mex_node_reservef ("%s:codebar", mex_instance); + mex_node_register (mtype, codebar_handle, 0); +} + +uint8_t +codebar_get (uint8_t direction) +{ + if (direction == DIRECTION_FORWARD) + return codebar_front; + else + return codebar_back; +} + diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 79c0fe2a..42a2809a 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -40,6 +40,7 @@ #include "pwm.h" #include "contact.h" +#include "codebar.h" #include "radar.h" #define FSM_NAME AI @@ -77,6 +78,9 @@ static uint8_t main_stats_asserv_, main_stats_asserv_cpt_; /** Contact stats counters. */ static uint8_t main_stats_contact_, main_stats_contact_cpt_; +/** Codebar stats counters. */ +static uint8_t main_stats_codebar_, main_stats_codebar_cpt_; + /** US sensors stats counters. */ static uint8_t main_stats_usdist_, main_stats_usdist_cpt_; @@ -103,6 +107,7 @@ main_init (void) /* IO modules. */ pwm_init (); contact_init (); + codebar_init (); usdist_init (); /* AI modules. */ clamp_init (); @@ -223,6 +228,12 @@ main_loop (void) proto_send1d ('P', contact_all ()); main_stats_contact_cpt_ = main_stats_contact_; } + if (main_stats_codebar_ && !--main_stats_codebar_cpt_) + { + proto_send2b ('B', codebar_get (DIRECTION_FORWARD), + codebar_get (DIRECTION_BACKWARD)); + main_stats_codebar_cpt_ = main_stats_codebar_; + } if (main_stats_usdist_ && !--main_stats_usdist_cpt_) { proto_send4w ('U', usdist_mm[0], usdist_mm[1], usdist_mm[2], @@ -331,6 +342,10 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) /* Contact stats. */ main_stats_contact_ = main_stats_contact_cpt_ = args[0]; break; + case c ('B', 1): + /* Codebar stats. */ + main_stats_codebar_ = main_stats_codebar_cpt_ = args[0]; + break; case c ('U', 1): /* US sensors stats. */ main_stats_usdist_ = main_stats_usdist_cpt_ = args[0]; diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index be6f3c87..cb4cfe4a 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -31,6 +31,7 @@ #include "element.h" #include "clamp.h" #include "bot.h" +#include "codebar.h" #include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" @@ -53,7 +54,9 @@ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; static uint8_t pawn_sensor_get_type (uint8_t direction) { - uint8_t element_type = IO_GET (CONTACT_STRAT) ? ELEMENT_PAWN : ELEMENT_KING; + uint8_t element_type = codebar_get (direction); + if (!element_type) + element_type = ELEMENT_PAWN; return element_type; } diff --git a/digital/io-hub/tools/io_hub/mex.py b/digital/io-hub/tools/io_hub/mex.py index 8d758382..9b72ccde 100644 --- a/digital/io-hub/tools/io_hub/mex.py +++ b/digital/io-hub/tools/io_hub/mex.py @@ -121,6 +121,46 @@ class Mex: m.push ('L', self.contacts) self.node.send (m) + class Codebar (Observable): + """Codebar stub. + + - element_type: 'queen', 'king', or anything else. + + """ + + def __init__ (self, pack, index): + Observable.__init__ (self) + self.pack = pack + self.index = index + self.element_type = None + self.register (self.__notified) + + def __notified (self): + self.pack.set (self.index, self.element_type) + + class Pack: + """Handle emission of several codebar for one message.""" + + def __init__ (self, node, instance): + self.node = node + self.codebars = [0, 0] + self.mtype = node.reserve (instance + ':codebar') + + def set (self, index, element_type): + if element_type == 'queen': + self.codebars[index] = 4 + elif element_type == 'king': + self.codebars[index] = 8 + else: + self.codebars[index] = 0 + self.__send () + + def __send (self): + m = simu.mex.msg.Msg (self.mtype) + for c in self.codebars: + m.push ('b', c) + self.node.send (m) + class Path (Observable): """Path finding algorithm report. @@ -167,6 +207,9 @@ class Mex: self.__contact_pack = self.Contact.Pack (node, instance) self.contact = tuple (self.Contact (self.__contact_pack, i) for i in range (CONTACT_NB)) + self.__codebar_pack = self.Codebar.Pack (node, instance) + self.codebar = tuple (self.Codebar (self.__codebar_pack, i) + for i in (0, 1)) self.path = self.Path (node, instance) self.pos_report = self.PosReport (node, instance) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index 284e5bb8..b9d3bc3f 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -44,7 +44,7 @@ class Bag: 8 * pi, 0, 0.5 * pi) for i in (0, 1, 3, 4) ] self.clamp = Clamp (table, self.position, link_bag.mimot.aux[0], link_bag.mimot.aux[1], self.clamping_motor, self.door_motors, - self.contact[0:7]) + self.contact[0:7], link_bag.io_hub.codebar) self.distance_sensor = [ DistanceSensorSensopart (link_bag.io_hub.adc[0], scheduler, table, (20, 20), pi * 10 / 180, (self.position, ), 2), diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index 0af4ccf3..dbcab5f2 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -30,13 +30,14 @@ from math import pi, cos, sin class Slot: """Slot which can contain a pawn.""" - def __init__ (self, x, y, z, side, door_motor, contact): + def __init__ (self, x, y, z, side, door_motor, contact, codebar = None): self.x = x self.y = y self.z = z self.side = side self.door_motor = door_motor self.contact = contact + self.codebar = codebar self.pawn = None class Clamp (Observable): @@ -62,7 +63,8 @@ class Clamp (Observable): SLOT_SIDE = 6 def __init__ (self, table, robot_position, elevation_motor, - rotation_motor, clamping_motor, door_motors, slot_contacts): + rotation_motor, clamping_motor, door_motors, slot_contacts, + codebars): Observable.__init__ (self) self.table = table self.robot_position = robot_position @@ -75,13 +77,13 @@ class Clamp (Observable): door_motors[2], None, door_motors[3], None) self.slots = ( Slot (self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 0, - door_motors[0], slot_contacts[0]), + door_motors[0], slot_contacts[0], codebars[0]), Slot (self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 0, None, slot_contacts[1]), Slot (self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 0, door_motors[1], slot_contacts[2]), Slot (-self.BAY_OFFSET, 0, 0 * self.BAY_ZOFFSET, 1, - door_motors[2], slot_contacts[3]), + door_motors[2], slot_contacts[3], codebars[1]), Slot (-self.BAY_OFFSET, 0, 1 * self.BAY_ZOFFSET, 1, None, slot_contacts[4]), Slot (-self.BAY_OFFSET, 0, 2 * self.BAY_ZOFFSET, 1, @@ -203,6 +205,11 @@ class Clamp (Observable): and slots[0].pawn.kind == 'tower')) # This one is really high. slots[2].contact.state = not (slots[2].pawn is not None) + if slots[0].pawn: + slots[0].codebar.element_type = slots[0].pawn.kind + else: + slots[0].codebar.element_type = None + slots[0].codebar.notify () slot_side = self.slots[self.SLOT_SIDE] slot_side.contact.state = slot_side.pawn is None clamp_slot = self.__get_clamp_slot () -- cgit v1.2.3 From 5dda25337ca882196e2ffca31cced51055ec670c Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 14:27:57 +0200 Subject: ddigital/io-hub: fixed logistic switches when we are in the green zone --- digital/io-hub/src/robospierre/logistic.c | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 32b9fdb0..56767d57 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -348,21 +348,33 @@ logisitic_make_switches () e, p, e, P, _, LEFT, 0); - LOGISTIC_CASE (_, p, + LOGISTIC_CASE (_, a, e, p, e, P, D, LEFT, 0); LOGISTIC_CASE (D, _, e, _, _, H, _, RIGHT, 0); + + LOGISTIC_CASE (_, D, + _, _, e, + _, H, LEFT, 0); - LOGISTIC_CASE (h, _, + LOGISTIC_CASE (h, P, + e, _, e, + D, _, RIGHT, 0); + + LOGISTIC_CASE (h, e, e, _, e, D, P, RIGHT, 0); - LOGISTIC_CASE (h, P, + LOGISTIC_CASE (P, h, e, _, e, - D, _, RIGHT, 0); + _, D, LEFT, 0); + + LOGISTIC_CASE (e, h, + e, _, e, + P, D, LEFT, 0); } void @@ -421,7 +433,7 @@ logistic_init (void) ctx.clamp_pos_idle = ctx.collect_direction == DIRECTION_FORWARD ? CLAMP_SLOT_FRONT_MIDDLE : CLAMP_SLOT_BACK_MIDDLE; ctx.construct_possible = 0; - ctx.prepare = 1; + ctx.prepare = 0; ctx.ready = 0; ctx.need_prepare = 0; } -- cgit v1.2.3 From cbf2a5beed9a75707f4a2fe00f685c2afd8dfba4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 16:20:26 +0200 Subject: digital/io-hub: add demo mode --- digital/io-hub/src/robospierre/clamp.c | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 3e7fa9fd..812d4890 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -447,10 +447,21 @@ FSM_TRANS (CLAMP_INIT_FINDING_ROTATION_EDGE, clamp_rotation_success, } FSM_TRANS (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, - CLAMP_INIT_GOING_REST) + rest, CLAMP_INIT_GOING_REST, + demo, CLAMP_GOING_IDLE) { - clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE); - return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success); + if (IO_GET (CONTACT_STRAT)) + { + clamp_move (CLAMP_BAY_SIDE_ENTER_LEAVE); + return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, + rest); + } + else + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_INIT_FINDING_TOP, clamp_elevation_success, + demo); + } } FSM_TRANS (CLAMP_INIT_GOING_REST, clamp_move_success, CLAMP_INIT_READY) -- cgit v1.2.3 From 7c9efd7aaf8a65ba032426dcc318a9cab4d37985 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 18:31:05 +0200 Subject: digital/io-hub: add bonus coeff to elements and adjusted scores algorithms --- digital/io-hub/src/robospierre/element.c | 152 +++++++++++++------------- digital/io-hub/src/robospierre/element.h | 4 + digital/io-hub/src/robospierre/test_element.c | 4 + 3 files changed, 81 insertions(+), 79 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 73b165cc..4ea005f1 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -43,92 +43,92 @@ struct element_t element_table[] = See ELEMENT_INTERSEC_START and ELEMENT_INTERSEC_END ELEMENT_INTERSEC_END don't takes central pawn. */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* top left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, /* top right */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* middle left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, /* middle right */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* 2nd line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* 3th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* 4th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, /* 5th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* top left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, /* top right */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* middle left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, /* middle right */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* 2nd line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* 3th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* 4th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* 5th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, /* Central pawn. (see ELEMENT_CENTRAL_PAWN) */ - {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER}, + {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 50}, /* 10 elements on green zones. To be symmetric, alternate % 2. See ELEMENT_GREEN_START and ELEMENT_GREEN_END */ - {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT}, /* top left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT}, /* top right */ - {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT}, /* 2nd line left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT}, /* 2nd line right */ - {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT}, /* ... */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT}, - {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT}, - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT}, - {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT}, - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT}, + {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 0}, /* top left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 0}, /* top right */ + {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 0}, /* 2nd line left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 0}, /* 2nd line right */ + {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 0}, /* ... */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 0}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT, -100}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT, -100}, /* 32 elements in the middle of a square. Altern colors in order to retrieve position % 2. See ELEMENT_UNLOAD_START and ELEMENT_UNLOAD_END */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, /* Top left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, /* 2nd line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, /* 3th line left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, /* 4th line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, /* 5th line left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS}, /* middle bonus left, red. */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS}, /* middle bonus right, blue. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 10}, /* Top left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, /* 2nd line left red */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, /* 3th line left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, /* 4th line left red */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, /* 5th line left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0}, /* middle bonus left, red. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0}, /* middle bonus right, blue. */ /* 4 elements in safe zones. see ELEMENT_UNLOAD_SAFE_START and ELEMENT_UNLOAD_SAFE_END */ - {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT}, /* left red */ - {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT}, /* left blue */ - {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT}, /* right red */ - {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT} /* right blue */ + {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, -100}, /* left red */ + {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, -100}, /* left blue */ + {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, -100}, /* right red */ + {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, -100} /* right blue */ }; inline void @@ -182,10 +182,8 @@ element_unload_score (position_t robot_pos, uint8_t element_id) if (e.type != ELEMENT_NONE) return -1; - /* Bonus score. */ - if ((e.attr & ELEMENT_BONUS) && - (chrono_remaining_time () * 100 / CHRONO_MATCH_DURATION_MS > 40)) - score += 1000; + /* Bonus adjust. */ + score += e.bonus * ELEMENT_BONUS_COEFF; /* Unload distance. */ /* TODO: minimal distance may not be the best choice. */ @@ -269,7 +267,7 @@ element_score (position_t robot_pos, uint8_t element_id) /* In our zone. */ if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) - score *= 10; + score *= 150; /* In the other zone, boost score if our green zone is empty. */ else { @@ -286,16 +284,12 @@ element_score (position_t robot_pos, uint8_t element_id) cpt++; } if (cpt == 5) - score *= 10; + score *= 150; } } - //TODO Set right modifier at this time - score *= 100; - - /* Central pawn. */ - if (element_id == ELEMENT_CENTRAL_PAWN) - score += 1000; + /* Add score modifier. */ + score += e.bonus * ELEMENT_BONUS_COEFF; /* We are sure of this element. */ if (!(e.type & ELEMENT_NONE) && diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index bcc40f43..fa9fced7 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -90,6 +90,8 @@ struct element_t vect_t pos; /** Emplacement attributes. */ uint8_t attr; + /** Bonus coefficient (or Mallus if negative). */ + int8_t bonus; }; typedef struct element_t element_t; @@ -104,6 +106,8 @@ typedef struct element_t element_t; #define ELEMENT_UNLOAD_SAFE_START 63 #define ELEMENT_UNLOAD_SAFE_END 66 +#define ELEMENT_BONUS_COEFF 100 + /** Elements on table. */ extern struct element_t element_table[]; diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index 7e62a492..774940ab 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -283,6 +283,10 @@ int main () scanf ("%i", &test_chrono_ms); break; case 'c': + if (team_color == TEAM_COLOR_RIGHT) + team_color = TEAM_COLOR_LEFT; + else + team_color = TEAM_COLOR_RIGHT; team_color ^= TEAM_COLOR_RIGHT; printf ("Side is now %u\n", team_color); break; -- cgit v1.2.3 From 2ccee32e49bce84a31c571cd1b9fff1a03ad1b9e Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 19:17:42 +0200 Subject: digital/io-hub: fixed default prepare value at init. --- digital/io-hub/src/robospierre/logistic.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 56767d57..3ebc26e6 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -433,7 +433,7 @@ logistic_init (void) ctx.clamp_pos_idle = ctx.collect_direction == DIRECTION_FORWARD ? CLAMP_SLOT_FRONT_MIDDLE : CLAMP_SLOT_BACK_MIDDLE; ctx.construct_possible = 0; - ctx.prepare = 0; + ctx.prepare = 1; ctx.ready = 0; ctx.need_prepare = 0; } -- cgit v1.2.3 From 409b07274f855c01d27046400610cc88a3d6ef60 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 21:00:22 +0200 Subject: digital/io-hub: element block path or not --- digital/io-hub/src/robospierre/element.c | 40 ++++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/element.h | 8 +++++++ 2 files changed, 48 insertions(+) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 4ea005f1..f73d380d 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -711,3 +711,43 @@ element_get_pos (uint8_t element_id) } return pos; } + +uint8_t +element_blocking_path (vect_t a, vect_t b, int16_t ab) +{ + uint8_t i; + element_t e; + /* For each obstacle, try to find an intersection. */ + for (i = 0; i < UTILS_COUNT (element_table); i++) + { + e = element_get (i); + /* Compute square of distance to obstacle, see + * distance_segment_point in modules/math/geometry for the method + * explanation. */ + int32_t absq = (int32_t) ab * ab; + vect_t vab = b; vect_sub (&vab, &a); + vect_t vao = e.pos; vect_sub (&vao, &a); + int32_t dp = vect_dot_product (&vab, &vao); + int32_t dsq; + if (dp < 0) + { + dsq = vect_dot_product (&vao, &vao); + } + else if (dp > absq) + { + vect_t vbo = e.pos; vect_sub (&vbo, &b); + dsq = vect_dot_product (&vbo, &vbo); + } + else + { + vect_t vabn = vab; vect_normal (&vabn); + dsq = vect_dot_product (&vabn, &vao) / ab; + dsq *= dsq; + } + /* Compare with square of authorised distance. */ + if (dsq < (int32_t) (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20) * + (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20)) + return 1; + } + return 0; +} diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index fa9fced7..07cb3b6b 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -181,4 +181,12 @@ element_get (uint8_t element_id) return element_table[element_id]; } +/** Return whether an element is blocking a line segment. + * - a: line segment first point. + * - b: line segment second point. + * - ab: line segment length. + * - returns: 1 if the path should not be used. */ +uint8_t +element_blocking_path (vect_t a, vect_t b, int16_t ab); + #endif /* element_h */ -- cgit v1.2.3 From b4878630cbe9e8623f1da2b675957401e87138c1 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 23:10:24 +0200 Subject: digital/io-hub: moved safe zone in elements --- digital/io-hub/src/robospierre/element.c | 11 +++-------- digital/io-hub/src/robospierre/element.h | 4 +--- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index f73d380d..1529b224 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -84,7 +84,7 @@ struct element_t element_table[] = {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT, -100}, /* - 32 elements in the middle of a square. + 36 elements in the middle of a square. Altern colors in order to retrieve position % 2. See ELEMENT_UNLOAD_START and ELEMENT_UNLOAD_END */ @@ -118,15 +118,10 @@ struct element_t element_table[] = {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0}, /* middle bonus left, red. */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0}, /* middle bonus right, blue. */ - - /* - 4 elements in safe zones. - see ELEMENT_UNLOAD_SAFE_START and ELEMENT_UNLOAD_SAFE_END - */ {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, -100}, /* left red */ {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, -100}, /* left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0}, /* middle bonus left, red. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0}, /* middle bonus right, blue. */ {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, -100}, /* right red */ {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, -100} /* right blue */ }; diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 07cb3b6b..a014b423 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -102,9 +102,7 @@ typedef struct element_t element_t; #define ELEMENT_GREEN_START 21 #define ELEMENT_GREEN_END 30 #define ELEMENT_UNLOAD_START 31 -#define ELEMENT_UNLOAD_END 62 -#define ELEMENT_UNLOAD_SAFE_START 63 -#define ELEMENT_UNLOAD_SAFE_END 66 +#define ELEMENT_UNLOAD_END 66 #define ELEMENT_BONUS_COEFF 100 -- cgit v1.2.3 From beb7e0fb164fc4356543a272c884dde45dd83572 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 23:34:54 +0200 Subject: digital/io-hub: fixed warning --- digital/io-hub/src/robospierre/element.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 1529b224..2530454c 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -527,7 +527,7 @@ element_taken (uint8_t element_id, uint8_t element_type) { assert (element_id < UTILS_COUNT (element_table)); static uint8_t pawn_c = 3, queen_c = 1, king_c = 1, any_nb = 5; - uint8_t other_side_id, other_side_id_element; + uint8_t other_side_id, other_side_id_element = 0; if (element_type != ELEMENT_PAWN && element_type != ELEMENT_QUEEN && element_type != ELEMENT_KING) return; -- cgit v1.2.3 From 4307b557e5be14c42a1335abce5149712923733a Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 23:40:22 +0200 Subject: digital/io-hub: fixed test elements --- digital/io-hub/src/robospierre/test_element.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index 774940ab..b26db210 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -168,10 +168,10 @@ test_element_convert (const char *l, uint8_t number) switch (number) { case 0: return (29); - case 1: return (63); - case 3: return (64); - case 5: return (61); - case 7: return (62); + case 1: return (61); + case 3: return (62); + case 5: return (63); + case 7: return (64); case 9: return (65); case 11: return (66); case 12: return (30); -- cgit v1.2.3 From 719fd6e016ac32955e3b95111a42e992897b14f5 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 23:45:25 +0200 Subject: digital/io-hub: fixed bonus elements at bottom --- digital/io-hub/src/robospierre/element.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 2530454c..a9d8808a 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -120,8 +120,8 @@ struct element_t element_table[] = {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, -100}, /* left red */ {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, -100}, /* left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0}, /* middle bonus left, red. */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0}, /* middle bonus right, blue. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, -40}, /* middle bonus left, red. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, -40}, /* middle bonus right, blue. */ {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, -100}, /* right red */ {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, -100} /* right blue */ }; @@ -379,8 +379,6 @@ element_proba (uint8_t element_id) { p_pos = (element_id - ELEMENT_UNLOAD_START) / 2 + 1; out = (1000 - p_t) * p_pos; - if (e.attr & ELEMENT_BONUS) - out += 1000; } } -- cgit v1.2.3 From 02f4acf332923b80776d2d25972cb3089da3e2a8 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Wed, 1 Jun 2011 23:48:07 +0200 Subject: digital/io-hub: remove probability from score --- digital/io-hub/src/robospierre/element.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index a9d8808a..896382b6 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -314,8 +314,6 @@ element_score (position_t robot_pos, uint8_t element_id) score += align; } - /* Adjust score with time probability. */ - score += element_proba (element_id); return score; } -- cgit v1.2.3 From e15457c84dc58e3433d66e2790fdd3a9368d7830 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 16:40:19 +0200 Subject: digital/io-hub: fix bad element type values --- digital/io-hub/src/robospierre/element.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index a014b423..362c7ff9 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -39,10 +39,10 @@ /** Any element, pawn, queen, or king. */ #define ELEMENT_ANY (ELEMENT_HEAD |ELEMENT_PAWN) /** Tower types. */ -#define ELEMENT_TOWER_1_QUEEN 8 -#define ELEMENT_TOWER_2_QUEEN 16 -#define ELEMENT_TOWER_1_KING 32 -#define ELEMENT_TOWER_2_KING 64 +#define ELEMENT_TOWER_1_QUEEN 16 +#define ELEMENT_TOWER_2_QUEEN 32 +#define ELEMENT_TOWER_1_KING 64 +#define ELEMENT_TOWER_2_KING 128 #define ELEMENT_TOWER (ELEMENT_TOWER_1_QUEEN | \ ELEMENT_TOWER_2_QUEEN | \ -- cgit v1.2.3 From 8d758c96d257b15edc49dd4cb774ce67becf8cf1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 17:23:31 +0200 Subject: digital/io-hub: drop in center of squares --- digital/io-hub/src/robospierre/top.c | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 0af8ce80..3a5ef25e 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -109,10 +109,25 @@ top_go_drop (void) position_t robot_pos; asserv_get_position (&robot_pos); uint8_t drop_pos_id = 43; - vect_t drop_pos = element_get_pos (drop_pos_id); + position_t drop_pos; + drop_pos.v = element_get_pos (drop_pos_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; - move_start_noangle (drop_pos, backward, 0); + /* Go above or below the drop point. */ + if (drop_pos.v.y > PG_LENGTH / 2) + { + drop_pos.v.y -= 350 / 2; + drop_pos.a = PG_A_DEG (-90); + } + else + { + drop_pos.v.y += 350 / 2; + drop_pos.a = PG_A_DEG (90); + } + if (logistic_global.collect_direction == DIRECTION_BACKWARD) + drop_pos.a += PG_A_DEG (180); + /* Go! */ + move_start (drop_pos, backward); return 0; } @@ -190,7 +205,8 @@ FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING) FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { - asserv_move_linearly (200); + asserv_move_linearly (logistic_global.collect_direction + == DIRECTION_FORWARD ? 200 : -200); return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting); } -- cgit v1.2.3 From dadddea9392f78a360969b72a4dc045b4d87dfec Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 19:16:38 +0200 Subject: digital/io-hub: handle element with path finding --- digital/io-hub/src/robospierre/element.c | 60 +++++++++++++++++++------------- digital/io-hub/src/robospierre/element.h | 4 +++ digital/io-hub/src/robospierre/path.c | 45 +++++++++++++++++++----- 3 files changed, 75 insertions(+), 34 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 896382b6..b0c38265 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -703,6 +703,13 @@ element_get_pos (uint8_t element_id) return pos; } +uint8_t +element_blocking (uint8_t element_id) +{ + element_t e = element_get (element_id); + return e.type == ELEMENT_TOWER; +} + uint8_t element_blocking_path (vect_t a, vect_t b, int16_t ab) { @@ -712,33 +719,36 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab) for (i = 0; i < UTILS_COUNT (element_table); i++) { e = element_get (i); - /* Compute square of distance to obstacle, see - * distance_segment_point in modules/math/geometry for the method - * explanation. */ - int32_t absq = (int32_t) ab * ab; - vect_t vab = b; vect_sub (&vab, &a); - vect_t vao = e.pos; vect_sub (&vao, &a); - int32_t dp = vect_dot_product (&vab, &vao); - int32_t dsq; - if (dp < 0) - { - dsq = vect_dot_product (&vao, &vao); - } - else if (dp > absq) - { - vect_t vbo = e.pos; vect_sub (&vbo, &b); - dsq = vect_dot_product (&vbo, &vbo); - } - else + if (e.type == ELEMENT_TOWER) { - vect_t vabn = vab; vect_normal (&vabn); - dsq = vect_dot_product (&vabn, &vao) / ab; - dsq *= dsq; + /* Compute square of distance to obstacle, see + * distance_segment_point in modules/math/geometry for the method + * explanation. */ + int32_t absq = (int32_t) ab * ab; + vect_t vab = b; vect_sub (&vab, &a); + vect_t vao = e.pos; vect_sub (&vao, &a); + int32_t dp = vect_dot_product (&vab, &vao); + int32_t dsq; + if (dp < 0) + { + dsq = vect_dot_product (&vao, &vao); + } + else if (dp > absq) + { + vect_t vbo = e.pos; vect_sub (&vbo, &b); + dsq = vect_dot_product (&vbo, &vbo); + } + else + { + vect_t vabn = vab; vect_normal (&vabn); + dsq = vect_dot_product (&vabn, &vao) / ab; + dsq *= dsq; + } + /* Compare with square of authorised distance. */ + if (dsq < (int32_t) (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20) * + (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20)) + return 1; } - /* Compare with square of authorised distance. */ - if (dsq < (int32_t) (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20) * - (BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20)) - return 1; } return 0; } diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 362c7ff9..a31d9f4d 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -179,6 +179,10 @@ element_get (uint8_t element_id) return element_table[element_id]; } +/** Return whether an element is blocking robot. */ +uint8_t +element_blocking (uint8_t element_id); + /** Return whether an element is blocking a line segment. * - a: line segment first point. * - b: line segment second point. diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index 61ea9293..049c577f 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -27,6 +27,7 @@ #include "path.h" #include "bot.h" #include "playground_2011.h" +#include "element.h" #include "modules/path/astar/astar.h" #include "modules/utils/utils.h" @@ -196,6 +197,29 @@ path_pos (uint8_t node, vect_t *pos) } } +static uint8_t +path_element_blocking (uint8_t node) +{ + vect_t pos; + path_pos (node, &pos); + int16_t square_x = (pos.x - 450 - 1) / 350; + int16_t square_y = (2100 - pos.y - 1) / 350; + uint8_t element_id = ELEMENT_UNLOAD_START + square_x + 6 * square_y; + if (element_blocking (element_id)) + return 1; + uint8_t intersection = ((pos.x - 450) / 350) != square_x; + if (intersection) + { + if (element_blocking (element_id + 1)) + return 1; + if (element_blocking (element_id + 6)) + return 1; + if (element_blocking (element_id + 6 + 1)) + return 1; + } + return 0; +} + /** Return 1 if the direct path between a and b nodes is blocked, also compute * distance. */ static uint8_t @@ -239,25 +263,27 @@ path_blocking (uint8_t a, uint8_t b, int16_t *dp) blocking = 1; } } + /* Compute distance. */ + int16_t d = distance_point_point (&va, &vb); + if (d == 0) + { + *dp = 0; + return 0; + } + /* Test for a blocking element. */ + if (element_blocking_path (va, vb, d)) + blocking = 1; /* Handle escaping. */ if (blocking) { if (escape_factor) { - int16_t d = distance_point_point (&va, &vb); *dp = d * escape_factor; return 0; } else return 1; } - /* Compute distance. */ - int16_t d = distance_point_point (&va, &vb); - if (d == 0) - { - *dp = 0; - return 0; - } /* No blocking. */ *dp = d * factor; return 0; @@ -272,7 +298,8 @@ path_blocked_update (void) { uint8_t valid = 1; /* First, gather information from tables. */ - if (!path_nodes[i].usable) + if (!path_nodes[i].usable + || path_element_blocking (i)) valid = 0; else { -- cgit v1.2.3 From 44e19c5b04ae34c912c9c0302c1c43e770e0eef1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 20:52:04 +0200 Subject: digital/io-hub: signal drop to element module --- digital/io-hub/src/robospierre/top.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 3a5ef25e..e52c2106 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -108,9 +108,9 @@ top_go_drop (void) { position_t robot_pos; asserv_get_position (&robot_pos); - uint8_t drop_pos_id = 43; + ctx.target_element_id = 43; position_t drop_pos; - drop_pos.v = element_get_pos (drop_pos_id); + drop_pos.v = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; /* Go above or below the drop point. */ @@ -205,6 +205,7 @@ FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING) FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { + element_down (ctx.target_element_id, ELEMENT_TOWER); asserv_move_linearly (logistic_global.collect_direction == DIRECTION_FORWARD ? 200 : -200); return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting); -- cgit v1.2.3 From f78f32f27a922a9c33e0592d9287d76eddedcb57 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 1 Jun 2011 22:09:30 +0200 Subject: digital/io-hub: fix element unload score --- digital/io-hub/src/robospierre/element.c | 2 +- digital/io-hub/src/robospierre/top.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index b0c38265..ed126cc5 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -216,7 +216,7 @@ element_unload_best (position_t robot_pos) i <= ELEMENT_UNLOAD_END; i++) { - score = element_score (robot_pos , i); + score = element_unload_score (robot_pos , i); if (best == 0xff || best_score < score) { best = i; diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index e52c2106..044821e8 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -108,7 +108,7 @@ top_go_drop (void) { position_t robot_pos; asserv_get_position (&robot_pos); - ctx.target_element_id = 43; + ctx.target_element_id = element_unload_best (robot_pos); position_t drop_pos; drop_pos.v = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD -- cgit v1.2.3 From 056acc0205213f1aa5263ad8243aec55e38fb0bf Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Thu, 2 Jun 2011 00:20:21 +0200 Subject: digital/io-hub: change bonus for load and unload and adapt some scores --- digital/io-hub/src/robospierre/element.c | 138 +++++++++++++++---------------- digital/io-hub/src/robospierre/element.h | 7 +- 2 files changed, 74 insertions(+), 71 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index ed126cc5..f23d079b 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -43,87 +43,87 @@ struct element_t element_table[] = See ELEMENT_INTERSEC_START and ELEMENT_INTERSEC_END ELEMENT_INTERSEC_END don't takes central pawn. */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* top left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, /* top right */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* middle left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, /* middle right */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* 2nd line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* 3th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* 4th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, /* 5th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* top left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, /* top right */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* middle left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, /* middle right */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* 2nd line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* 3th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* 4th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* 5th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, /* Central pawn. (see ELEMENT_CENTRAL_PAWN) */ - {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 50}, + {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 50, 0}, /* 10 elements on green zones. To be symmetric, alternate % 2. See ELEMENT_GREEN_START and ELEMENT_GREEN_END */ - {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 0}, /* top left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 0}, /* top right */ - {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 0}, /* 2nd line left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 0}, /* 2nd line right */ - {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 0}, /* ... */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 0}, - {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 0}, - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 0}, - {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT, -100}, - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT, -100}, + {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0}, /* top left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0}, /* top right */ + {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0}, /* 2nd line left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0}, /* 2nd line right */ + {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0}, /* ... */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT, -100, 0}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT, -100, 0}, /* 36 elements in the middle of a square. Altern colors in order to retrieve position % 2. See ELEMENT_UNLOAD_START and ELEMENT_UNLOAD_END */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 10}, /* Top left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, /* 2nd line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, /* 3th line left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, /* 4th line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, /* 5th line left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0}, - {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, -100}, /* left red */ - {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, -100}, /* left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, -40}, /* middle bonus left, red. */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, -40}, /* middle bonus right, blue. */ - {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, -100}, /* right red */ - {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, -100} /* right blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10}, /* Top left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, /* 2nd line left red */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, /* 3th line left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, /* 4th line left red */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, /* 5th line left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, + {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100}, /* left red */ + {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100}, /* left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, -40}, /* middle bonus left, red. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, -40}, /* middle bonus right, blue. */ + {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100}, /* right red */ + {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100} /* right blue */ }; inline void @@ -178,7 +178,7 @@ element_unload_score (position_t robot_pos, uint8_t element_id) return -1; /* Bonus adjust. */ - score += e.bonus * ELEMENT_BONUS_COEFF; + score += e.bonus_unload * ELEMENT_BONUS_COEFF; /* Unload distance. */ /* TODO: minimal distance may not be the best choice. */ @@ -284,7 +284,7 @@ element_score (position_t robot_pos, uint8_t element_id) } /* Add score modifier. */ - score += e.bonus * ELEMENT_BONUS_COEFF; + score += e.bonus_load * ELEMENT_BONUS_COEFF; /* We are sure of this element. */ if (!(e.type & ELEMENT_NONE) && diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index a31d9f4d..5a15f6b0 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -90,8 +90,11 @@ struct element_t vect_t pos; /** Emplacement attributes. */ uint8_t attr; - /** Bonus coefficient (or Mallus if negative). */ - int8_t bonus; + /** Bonus coefficient (or Mallus if negative) for load scores. */ + int8_t bonus_load; + /** Bonus coefficient (or Mallus if negative) for unload scores. */ + int8_t bonus_unload; + }; typedef struct element_t element_t; -- cgit v1.2.3 From 9d8a717578c70db2dfb28cb19c834d55c0bb7b86 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 00:19:08 +0200 Subject: digital/avr/modules/adc: use Vcc reference --- digital/avr/modules/adc/adc.avr.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/avr/modules/adc/adc.avr.c b/digital/avr/modules/adc/adc.avr.c index 3bfdefa9..eff441a9 100644 --- a/digital/avr/modules/adc/adc.avr.c +++ b/digital/avr/modules/adc/adc.avr.c @@ -48,7 +48,7 @@ * 11: Internal 2.56V Voltage Reference with external capacitor * at AREF pin. */ #define ADMUX_CFG (regv (REFS1, REFS0, ADLAR, MUX4, MUX3, MUX2, MUX1, MUX0, \ - 1, 1, 0, 0, 0, 0, 0, 0)) + 0, 1, 0, 0, 0, 0, 0, 0)) #define ADCSR_CFG (regv (ADEN, ADSC, ADFR, ADIF, ADIE, ADPS2, ADPS1, ADPS0, \ 1, 0, 0, 1, 0, 0, 0, 0)) #define ADCSR_CFG_115200 7 -- cgit v1.2.3 From 6c6119e3f1ff8451d5d364726da68ce40691af88 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 00:20:52 +0200 Subject: digital/io-hub: increase head timeout --- digital/io-hub/src/robospierre/codebar.avr.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/codebar.avr.c b/digital/io-hub/src/robospierre/codebar.avr.c index d2609012..fcddb079 100644 --- a/digital/io-hub/src/robospierre/codebar.avr.c +++ b/digital/io-hub/src/robospierre/codebar.avr.c @@ -56,7 +56,7 @@ codebar_get (uint8_t direction) uint8_t offset = direction == DIRECTION_FORWARD ? 1 : 4; uint16_t age = v8_to_v16 (buffer[offset], buffer[offset + 1]); uint16_t type = buffer[offset + 2]; - if (age > 225) + if (age > 3 * 250) return 0; else return type; -- cgit v1.2.3 From b0c915ccf0c05f5cc4054b7f6b0d48e7472a5a82 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 00:21:50 +0200 Subject: digital/io-hub: fix bad angle macro usage --- digital/io-hub/src/robospierre/top.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 044821e8..8e9bea9a 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -125,7 +125,7 @@ top_go_drop (void) drop_pos.a = PG_A_DEG (90); } if (logistic_global.collect_direction == DIRECTION_BACKWARD) - drop_pos.a += PG_A_DEG (180); + drop_pos.a += POSITION_A_DEG (180); /* Go! */ move_start (drop_pos, backward); return 0; -- cgit v1.2.3 From 00e7e806c78324e687aa5b2f2ca167ebbcbe91be Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 00:23:06 +0200 Subject: digital/io-hub: revert starting collect direction to forward --- digital/io-hub/src/robospierre/logistic.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 3ebc26e6..46f8736f 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -429,7 +429,7 @@ logistic_init (void) for (i = 0; i < CLAMP_SLOT_NB; i++) ctx.slots[i] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; - ctx.collect_direction = DIRECTION_BACKWARD; + ctx.collect_direction = DIRECTION_FORWARD; ctx.clamp_pos_idle = ctx.collect_direction == DIRECTION_FORWARD ? CLAMP_SLOT_FRONT_MIDDLE : CLAMP_SLOT_BACK_MIDDLE; ctx.construct_possible = 0; -- cgit v1.2.3 From 49caa98385eacd49a84316e9b5e195843551b61a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 01:47:19 +0200 Subject: digital/io-hub: harder on green zone blocking --- digital/io-hub/src/robospierre/path.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index 049c577f..4f871213 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -247,9 +247,9 @@ path_blocking (uint8_t a, uint8_t b, int16_t *dp) && vb.x < PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM) || (va.x < PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM && vb.x > PG_WIDTH - BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM)) - blocking = 1; + return 1; if (a_green && b_green) - blocking = 1; + return 1; if (a_green || b_green) factor = 4; /* Test for a blocking obstacle. */ -- cgit v1.2.3 From ded65008b4474e86fc16838018d453252d2c7689 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 03:35:47 +0200 Subject: digital/io-hub: go nearer to the wall --- digital/io-hub/src/robospierre/bot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index e31825e3..7acd1100 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -60,7 +60,7 @@ #define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600 /** Distance from border to go to capture a green element. */ #define BOT_GREEN_ELEMENT_DISTANCE_MM \ - (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 20) + (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 10) /** Speed used for initialisation. */ #ifdef HOST -- cgit v1.2.3 From 4bf3121c81de96a6c67d7e5d4bb53f0c33fc5e74 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 03:36:20 +0200 Subject: digital/io-hub: near the wall, take the element --- digital/io-hub/src/robospierre/pawn_sensor.c | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index cb4cfe4a..ccb5eb5b 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -31,6 +31,7 @@ #include "element.h" #include "clamp.h" #include "bot.h" +#include "playground.h" #include "codebar.h" #include "modules/utils/utils.h" @@ -91,6 +92,16 @@ pawn_sensor_get (uint8_t direction) asserv_get_position (&robot_position); if (ctx->active) { + /* In green zone, take it when near enougth from the wall. */ + if (robot_position.v.x < BOT_GREEN_ELEMENT_DISTANCE_MM + 10 + || (robot_position.v.x > PG_WIDTH + - BOT_GREEN_ELEMENT_DISTANCE_MM - 10)) + { + ctx->active = 0; + return pawn_sensor_get_type (direction); + } + /* Else, take it if near enough from the supposed element + * position. */ int32_t d = distance_point_point (&ctx->active_position, &robot_position.v); if (d < BOT_PAWN_TAKING_DISTANCE_MM) -- cgit v1.2.3 From b1050f4948c2fcb6b952094e1679b6d02edf0b9b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 04:19:25 +0200 Subject: digital/io-hub: handle failure to choose another element_id --- digital/io-hub/src/robospierre/element.c | 152 +++++++++++++++++-------------- digital/io-hub/src/robospierre/element.h | 7 ++ 2 files changed, 92 insertions(+), 67 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index f23d079b..1f877543 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -43,87 +43,87 @@ struct element_t element_table[] = See ELEMENT_INTERSEC_START and ELEMENT_INTERSEC_END ELEMENT_INTERSEC_END don't takes central pawn. */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* top left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, /* top right */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* middle left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, /* middle right */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* 2nd line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* 3th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* 4th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, /* 5th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* top left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, /* top right */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* middle left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 5 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, /* middle right */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* 2nd line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 4 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* 3th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* 4th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* 5th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, /* Central pawn. (see ELEMENT_CENTRAL_PAWN) */ - {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 50, 0}, + {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 50, 0, 0}, /* 10 elements on green zones. To be symmetric, alternate % 2. See ELEMENT_GREEN_START and ELEMENT_GREEN_END */ - {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0}, /* top left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0}, /* top right */ - {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0}, /* 2nd line left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0}, /* 2nd line right */ - {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0}, /* ... */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0}, - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT, -100, 0}, - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT, -100, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, /* top left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, /* top right */ + {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, /* 2nd line right */ + {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, /* ... */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT, -100, 0, 0}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT, -100, 0, 0}, /* 36 elements in the middle of a square. Altern colors in order to retrieve position % 2. See ELEMENT_UNLOAD_START and ELEMENT_UNLOAD_END */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10}, /* Top left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, /* 2nd line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 0}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, /* 3th line left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, /* 4th line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 0}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 0}, /* bonus */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, /* 5th line left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0}, - {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100}, /* left red */ - {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100}, /* left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, -40}, /* middle bonus left, red. */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, -40}, /* middle bonus right, blue. */ - {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100}, /* right red */ - {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100} /* right blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, /* Top left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left red */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* 3th line left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 4th line left red */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* 5th line left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, + {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100, 0}, /* left red */ + {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100, 0}, /* left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, -40, 0}, /* middle bonus left, red. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, -40, 0}, /* middle bonus right, blue. */ + {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100, 0}, /* right red */ + {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100, 0} /* right blue */ }; inline void @@ -177,6 +177,11 @@ element_unload_score (position_t robot_pos, uint8_t element_id) if (e.type != ELEMENT_NONE) return -1; + /* Failed element. */ + if (e.failure_until_s + && e.failure_until_s < chrono_remaining_time () / 1000) + return -1; + /* Bonus adjust. */ score += e.bonus_unload * ELEMENT_BONUS_COEFF; @@ -245,6 +250,11 @@ element_score (position_t robot_pos, uint8_t element_id) ((e.attr & ELEMENT_INTERSEC) ||(e.attr & ELEMENT_GREEN))) return -1; + /* Failed element. */ + if (e.failure_until_s + && e.failure_until_s < chrono_remaining_time () / 1000) + return -1; + if (e.type & ELEMENT_PAWN) score += ELEMENT_PAWN_SCORE; if (e.type & ELEMENT_QUEEN) @@ -648,6 +658,14 @@ element_down (uint8_t element_id, uint8_t element_type) element_set (element_id, e); } +void +element_failure (uint8_t element_id) +{ + element_t e = element_get (element_id); + e.failure_until_s = chrono_remaining_time () / 1000 - 7; + element_set (element_id, e); +} + uint8_t element_nearest_element_id (position_t robot_pos) { diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 5a15f6b0..8366907b 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -94,6 +94,9 @@ struct element_t int8_t bonus_load; /** Bonus coefficient (or Mallus if negative) for unload scores. */ int8_t bonus_unload; + /** Failure expiration date, can not take this element until chrono is + * lower than this date in seconds. */ + uint8_t failure_until_s; }; typedef struct element_t element_t; @@ -152,6 +155,10 @@ element_taken (uint8_t element_id, uint8_t element_type); void element_down (uint8_t element_id, uint8_t element_type); +/** Call this function when taking an element failled. */ +void +element_failure (uint8_t element_id); + /** Gives the nearest element from a position. */ uint8_t element_give_position (position_t pos); -- cgit v1.2.3 From f8c5a9371f9d77a99a8a95033fd8d747d21a0356 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 04:27:56 +0200 Subject: digital/io-hub: lower drop distance --- digital/io-hub/src/robospierre/top.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 8e9bea9a..43eaf255 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -207,7 +207,7 @@ FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { element_down (ctx.target_element_id, ELEMENT_TOWER); asserv_move_linearly (logistic_global.collect_direction - == DIRECTION_FORWARD ? 200 : -200); + == DIRECTION_FORWARD ? 150 : -150); return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting); } -- cgit v1.2.3 From 0d1aaa87a85b7306c66d217c345596c84fd9f80f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 04:29:28 +0200 Subject: digital/io-hub: handle blocking situations --- digital/io-hub/src/robospierre/top.c | 82 +++++++++++++++++++++++++++++++++++- 1 file changed, 81 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 43eaf255..4554c12b 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -45,8 +45,12 @@ FSM_STATES ( TOP_START, /* Going out of start area. */ TOP_GOING_OUT1, + /* Problem going out, wait before retry. */ + TOP_GOING_OUT1_BLOCK_WAIT, /* Going out, first pawn emplacement. */ TOP_GOING_OUT2, + /* Problem going out, wait before retry. */ + TOP_GOING_OUT2_BLOCK_WAIT, TOP_GOING_TO_DROP, TOP_GOING_TO_ELEMENT, @@ -66,6 +70,8 @@ struct top_t { /** Target element. */ uint8_t target_element_id; + /** Chaos counter. */ + uint8_t chaos; }; /** Global context. */ @@ -75,7 +81,8 @@ struct top_t top_global; FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1) { element_init (); - asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), PG_Y (PG_LENGTH - 200), 0); + asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), + PG_Y (PG_LENGTH - 200), 0); return FSM_NEXT (TOP_START, init_start_round); } @@ -85,6 +92,18 @@ FSM_TRANS (TOP_GOING_OUT1, robot_move_success, TOP_GOING_OUT2) return FSM_NEXT (TOP_GOING_OUT1, robot_move_success); } +FSM_TRANS (TOP_GOING_OUT1, robot_move_failure, TOP_GOING_OUT1_BLOCK_WAIT) +{ + return FSM_NEXT (TOP_GOING_OUT1, robot_move_failure); +} + +FSM_TRANS_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT, 250, TOP_GOING_OUT1) +{ + asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), + PG_Y (PG_LENGTH - 200 - (++ctx.chaos % 4) * 10), 0); + return FSM_NEXT_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT); +} + static uint8_t top_go_element (void) { @@ -155,6 +174,25 @@ FSM_TRANS (TOP_GOING_OUT2, robot_move_success, } } +FSM_TRANS (TOP_GOING_OUT2, robot_move_failure, TOP_GOING_OUT2_BLOCK_WAIT) +{ + return FSM_NEXT (TOP_GOING_OUT2, robot_move_failure); +} + +FSM_TRANS_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, 250, + clamp_working, TOP_WAITING_CLAMP, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT) +{ + if (clamp_working ()) + return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, clamp_working); + switch (top_decision ()) + { + default: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, drop); + case 1: return FSM_NEXT_TIMEOUT (TOP_GOING_OUT2_BLOCK_WAIT, element); + } +} + FSM_TRANS (TOP_GOING_TO_DROP, move_success, ready, TOP_DROP_DROPPING, wait_clamp, TOP_WAITING_READY) @@ -171,6 +209,21 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success, } } +FSM_TRANS (TOP_GOING_TO_DROP, move_failure, + clamp_working, TOP_WAITING_CLAMP, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT) +{ + element_failure (ctx.target_element_id); + if (clamp_working ()) + return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, clamp_working); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, drop); + case 1: return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, element); + } +} + FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, @@ -186,6 +239,21 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, } } +FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, + clamp_working, TOP_WAITING_CLAMP, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT) +{ + element_failure (ctx.target_element_id); + if (clamp_working ()) + return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, drop); + case 1: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, element); + } +} + FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) @@ -223,3 +291,15 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, } } +FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure, + drop, TOP_GOING_TO_DROP, + element, TOP_GOING_TO_ELEMENT) +{ + clamp_drop_clear (); + switch (top_decision ()) + { + default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop); + case 1: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, element); + } +} + -- cgit v1.2.3 From f3d0dcce650be384c672ed8815b0118d1e8adb4f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 05:01:03 +0200 Subject: digital/io-hub: change contacts positions --- digital/io-hub/src/robospierre/contact_defs.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/contact_defs.h b/digital/io-hub/src/robospierre/contact_defs.h index 2c300ece..132ec050 100644 --- a/digital/io-hub/src/robospierre/contact_defs.h +++ b/digital/io-hub/src/robospierre/contact_defs.h @@ -25,9 +25,9 @@ * * }}} */ -#define CONTACT_COLOR E, 5 -#define CONTACT_JACK E, 6 -#define CONTACT_STRAT E, 3 +#define CONTACT_COLOR E, 3 +#define CONTACT_JACK E, 5 +#define CONTACT_STRAT E, 1 #define CONTACT_FRONT_BOTTOM A, 4 #define CONTACT_FRONT_MIDDLE F, 4 #define CONTACT_BACK_BOTTOM A, 5 -- cgit v1.2.3 From c47f5e0c5a741f16ea115e6912ced21e53daf94f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 06:20:54 +0200 Subject: digital/io-serial: also accept qween for Fred elements --- digital/io-serial/src/codebar/codebar.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/digital/io-serial/src/codebar/codebar.c b/digital/io-serial/src/codebar/codebar.c index 8d845784..916e4ef1 100644 --- a/digital/io-serial/src/codebar/codebar.c +++ b/digital/io-serial/src/codebar/codebar.c @@ -64,6 +64,11 @@ string_to_element (char *data, uint8_t data_len) data[i] = 0; return ELEMENT_QUEEN; } + if (memcmp (data + i, "QWEEN", 5) == 0) + { + data[i] = 0; + return ELEMENT_QUEEN; + } } for (i = 0; i < data_len - 4; i++) { -- cgit v1.2.3 From becf1a33459af05d60f70a9b18bdeb05a987c558 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 07:09:25 +0200 Subject: digital/io-hub: stop robot when a element is taken --- digital/io-hub/src/robospierre/move.c | 32 ++++++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/move.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 22 ++++++++++++++++++++++ 3 files changed, 58 insertions(+) diff --git a/digital/io-hub/src/robospierre/move.c b/digital/io-hub/src/robospierre/move.c index 623dabdf..a44b5715 100644 --- a/digital/io-hub/src/robospierre/move.c +++ b/digital/io-hub/src/robospierre/move.c @@ -101,6 +101,13 @@ move_start_noangle (vect_t position, uint8_t backward, int16_t shorten) fsm_queue_post_event (FSM_EVENT (AI, move_start)); } +void +move_stop (void) +{ + /* Stop the FSM. */ + fsm_queue_post_event (FSM_EVENT (AI, move_stop)); +} + void move_obstacles_update (void) { @@ -144,6 +151,8 @@ FSM_EVENTS ( robot_move_failure, /* Initialize the FSM and start the movement directly. */ move_start, + /* Stop movement. */ + move_stop, /* Movement success. */ move_success, /* Movement failure. */ @@ -343,6 +352,12 @@ FSM_TRANS_TIMEOUT (MOVE_ROTATING, 1250, return FSM_NEXT_TIMEOUT (MOVE_ROTATING); } +FSM_TRANS (MOVE_ROTATING, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_ROTATING, move_stop); +} + FSM_TRANS (MOVE_MOVING, robot_move_success, done, MOVE_IDLE, path_found_rotate, MOVE_ROTATING, @@ -415,6 +430,12 @@ FSM_TRANS (MOVE_MOVING, obstacle_in_front, return FSM_NEXT (MOVE_MOVING, obstacle_in_front, tryagain); } +FSM_TRANS (MOVE_MOVING, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_MOVING, move_stop); +} + FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_success, tryout, MOVE_IDLE, path_found_rotate, MOVE_ROTATING, @@ -479,6 +500,12 @@ FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, robot_move_failure, } } +FSM_TRANS (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop, MOVE_IDLE) +{ + asserv_stop_motor (); + return FSM_NEXT (MOVE_MOVING_BACKWARD_TO_TURN_FREELY, move_stop); +} + FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250, path_found_rotate, MOVE_ROTATING, path_found, MOVE_MOVING, @@ -507,3 +534,8 @@ FSM_TRANS_TIMEOUT (MOVE_WAIT_FOR_CLEAR_PATH, 250, } } +FSM_TRANS (MOVE_WAIT_FOR_CLEAR_PATH, move_stop, MOVE_IDLE) +{ + return FSM_NEXT (MOVE_WAIT_FOR_CLEAR_PATH, move_stop); +} + diff --git a/digital/io-hub/src/robospierre/move.h b/digital/io-hub/src/robospierre/move.h index 877002fd..72af5b84 100644 --- a/digital/io-hub/src/robospierre/move.h +++ b/digital/io-hub/src/robospierre/move.h @@ -48,6 +48,10 @@ move_start (position_t position, uint8_t backward); void move_start_noangle (vect_t position, uint8_t backward, int16_t shorten); +/** Stop movement. */ +void +move_stop (void); + /** To be called when obstacles positions are computed. */ void move_obstacles_update (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 4554c12b..05568099 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -160,6 +160,14 @@ top_decision (void) return top_go_element (); } +static void +top_taken_pawn (void) +{ + position_t robot_pos; + asserv_get_position (&robot_pos); + element_taken (element_nearest_element_id (robot_pos), ELEMENT_PAWN); +} + FSM_TRANS (TOP_GOING_OUT2, robot_move_success, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, @@ -224,6 +232,13 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_failure, } } +FSM_TRANS (TOP_GOING_TO_DROP, clamp_working, TOP_WAITING_CLAMP) +{ + top_taken_pawn (); + move_stop (); + return FSM_NEXT (TOP_GOING_TO_DROP, clamp_working); +} + FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, @@ -254,6 +269,13 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, } } +FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) +{ + top_taken_pawn (); + move_stop (); + return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); +} + FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) -- cgit v1.2.3 From 463cf200cf2fdfd5abd0359240e45526746ee588 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 07:17:33 +0200 Subject: digital/io-hub: power off doors --- digital/io-hub/src/robospierre/main.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 42a2809a..6b2c8d31 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -188,6 +188,12 @@ main_loop (void) /* Is match over? */ if (chrono_is_match_over ()) { + /* Power off doors. */ + pwm_set (BOT_PWM_DOOR_FRONT_BOTTOM, 0); + pwm_set (BOT_PWM_DOOR_FRONT_TOP, 0); + pwm_set (BOT_PWM_DOOR_BACK_BOTTOM, 0); + pwm_set (BOT_PWM_DOOR_BACK_TOP, 0); + pwm_set (BOT_PWM_CLAMP, 0); /* End it and block here indefinitely. */ chrono_end_match (42); return; -- cgit v1.2.3 From f335b6c0c4fa4315229e9cc7ee01cb200207acdf Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 07:42:13 +0200 Subject: digital/io-hub: detect loss of side slot element --- digital/io-hub/src/robospierre/logistic.c | 18 ++++++++++++++++++ digital/io-hub/src/robospierre/logistic.h | 4 ++++ digital/io-hub/src/robospierre/test_element.c | 2 ++ 3 files changed, 24 insertions(+) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 46f8736f..eacecbe1 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -28,6 +28,9 @@ #include "clamp.h" #include "defs.h" +#include "contact.h" +#include "io.h" + #include "debug.host.h" /** Handle elements stored inside the robot. */ @@ -441,6 +444,21 @@ logistic_init (void) void logistic_update (void) { + uint8_t side_now = !IO_GET (CONTACT_SIDE); + /* Filter side contact. */ + if (side_now) + { + ctx.side_filter = 0; + ctx.side_state = 1; + } + else if (ctx.side_filter++ == 2 * 250) + { + ctx.side_state = 0; + ctx.side_filter = 0; + } + /* Side slot element can be lost. */ + if (ctx.moving_from != CLAMP_SLOT_SIDE && !ctx.side_state) + ctx.slots[CLAMP_SLOT_SIDE] = 0; } void diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 334b9db4..f27c082b 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -88,6 +88,10 @@ struct logistic_t /* Inform TOP that we can't take any more elements and needs to put * construction somewhere. */ uint8_t need_prepare; + /** Filtered side slot sensor. */ + uint8_t side_state; + /** Filtered side slot sensor counter. */ + uint16_t side_filter; }; /** Global context. */ diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index b26db210..8c93e876 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -35,6 +35,8 @@ int test_print_type_ = TEST_PRINT_TYPE_SCORE_PICK; position_t test_robot_pos_ = {{0, 2100}, 1}; +uint8_t PINE; + enum team_color_e team_color; /** Stubbed chrono. */ -- cgit v1.2.3 From 257d3ab2a29e56b75d13dc66f69e4b4a70f532e2 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 07:54:45 +0200 Subject: digital/io-hub: avoid dangerous places --- digital/io-hub/src/robospierre/element.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 1f877543..b4ade33f 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -59,10 +59,10 @@ struct element_t element_table[] = {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 2 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, /* 5th line left */ - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 30, 0, 0}, - {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, 0, 0, 0}, /* 5th line left */ + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 2 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 - 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_LEFT, -30, 0, 0}, + {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, -30, 0, 0}, /* Central pawn. (see ELEMENT_CENTRAL_PAWN) */ {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 50, 0, 0}, -- cgit v1.2.3 From e98eb0d5767956b7aba353e922323657b10f41c8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 07:55:06 +0200 Subject: digital/io-hub: drop pawn when there is no room in robot --- digital/io-hub/src/robospierre/top.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 05568099..c9362203 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -156,6 +156,11 @@ top_decision (void) /* If we can make a tower. */ if (logistic_global.construct_possible == 1) return top_go_drop (); + if (logistic_global.need_prepare) + { + clamp_prepare (2); + return top_go_drop (); + } else return top_go_element (); } -- cgit v1.2.3 From 4bae7237c6d4509838870dd23eb50b0b92f6e6a4 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Thu, 2 Jun 2011 11:36:14 +0200 Subject: digital/io-hub: set need unload when a tower poped in our back --- digital/io-hub/src/robospierre/logistic.c | 32 +++++++++++++++++-------------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index eacecbe1..82e4f5f2 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -242,6 +242,22 @@ logistic_update_need_prepare () ctx.need_prepare = 1; else ctx.need_prepare = 0; + + /* Define direction bay and opposed bay. */ + uint8_t dir_bay, opp_bay; + if (ctx.collect_direction == DIRECTION_FORWARD) + { + dir_bay = CLAMP_SLOT_FRONT_BOTTOM; + opp_bay = CLAMP_SLOT_BACK_BOTTOM; + } + else + { + dir_bay = CLAMP_SLOT_BACK_BOTTOM; + opp_bay = CLAMP_SLOT_FRONT_BOTTOM; + } + /* If a head appear at the back (?) */ + if (ELEMENT_IS_HEAD (ctx.slots[opp_bay])) + ctx.need_prepare = 1; } static void @@ -303,19 +319,11 @@ logistic_make_unload () _, _, _, a, _, RIGHT, 1); - LOGISTIC_CASE (_, H, - _, _, e, - _, D, LEFT, 0); - - LOGISTIC_CASE (H, _, - e, _, _, - D, _, RIGHT, 0); - - LOGISTIC_CASE (_, P, + LOGISTIC_CASE (_, a, _, _, e, _, D, LEFT, 0); - LOGISTIC_CASE (P, _, + LOGISTIC_CASE (a, _, e, _, _, D, _, RIGHT, 0); @@ -358,10 +366,6 @@ logisitic_make_switches () LOGISTIC_CASE (D, _, e, _, _, H, _, RIGHT, 0); - - LOGISTIC_CASE (_, D, - _, _, e, - _, H, LEFT, 0); LOGISTIC_CASE (h, P, e, _, e, -- cgit v1.2.3 From 8fef548edab0f336bb86d0aeaa070e8fcbdf65d6 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Thu, 2 Jun 2011 15:36:47 +0200 Subject: digital/io-hub: take care of founded towers in logistic --- digital/io-hub/src/robospierre/logistic.c | 23 +++++++++++++++++++++++ digital/io-hub/src/robospierre/logistic.h | 2 ++ 2 files changed, 25 insertions(+) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 82e4f5f2..4b0536f2 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -83,6 +83,8 @@ logistic_case_test (uint8_t loc, uint8_t e) return 1; if ((e == LOG_H || e == LOG_h) && ELEMENT_IS_HEAD (ctx.slots[loc])) return 1; + if (e == LOG_t && ctx.slots[loc] == ELEMENT_TOWER) + return 1; return 0; } @@ -208,6 +210,14 @@ static void logistic_update_construct_possible () { uint8_t pawn = 0, head = 0, i; + /* Check for tower. */ + if (ctx.slots[CLAMP_SLOT_FRONT_BOTTOM] == ELEMENT_TOWER || + ctx.slots[CLAMP_SLOT_BACK_BOTTOM] == ELEMENT_TOWER) + { + ctx.construct_possible = 1; + return; + } + for (i = CLAMP_SLOT_FRONT_BOTTOM; i < CLAMP_SLOT_NB; i++) { if (ELEMENT_IS_HEAD (ctx.slots[i])) @@ -258,6 +268,11 @@ logistic_update_need_prepare () /* If a head appear at the back (?) */ if (ELEMENT_IS_HEAD (ctx.slots[opp_bay])) ctx.need_prepare = 1; + + /* We founded a tower ! */ + if (ctx.slots[CLAMP_SLOT_FRONT_BOTTOM] == ELEMENT_TOWER || + ctx.slots[CLAMP_SLOT_FRONT_BOTTOM] == ELEMENT_TOWER) + ctx.need_prepare = 1; } static void @@ -275,6 +290,14 @@ logisitic_make_broken () static void logistic_make_tower () { + LOGISTIC_CASE (_, _, + _, _, _, + _, t, LEFT, 1); + + LOGISTIC_CASE (_, _, + _, _, _, + t, _, RIGHT, 1); + LOGISTIC_CASE (D, _, e, _, _, H, _, RIGHT, 0); diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index f27c082b..9962874b 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -44,6 +44,8 @@ #define LOG_h 7 /** Destination (who has to be empty). */ #define LOG_D 8 +/** We have a tower (always at bottom). */ +#define LOG_t 9 /* LEFT means we keep the same side, RIGHT means we put the opposed side. */ #define LOG_DIR_LEFT 0 #define LOG_DIR_RIGHT 1 -- cgit v1.2.3 From d5b5a0f2d6afa1f9799f284e7eb9b8cdffad9da3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 09:32:07 +0200 Subject: digital/io-hub: larger green margin --- digital/io-hub/src/robospierre/bot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 7acd1100..d63ccf66 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -60,7 +60,7 @@ #define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600 /** Distance from border to go to capture a green element. */ #define BOT_GREEN_ELEMENT_DISTANCE_MM \ - (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 10) + (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 25) /** Speed used for initialisation. */ #ifdef HOST -- cgit v1.2.3 From a7015e8589617e8c0d908bc34c5a7d12b829074c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 09:32:27 +0200 Subject: digital/io-hub: add 3 mm to side slot --- digital/io-hub/src/robospierre/bot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index d63ccf66..12ca7c30 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -112,7 +112,7 @@ # define BOT_CLAMP_SLOT_BACK_BOTTOM_ELEVATION_STEP 0x0169 # define BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP (0x1f03 - 250) # define BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP 0x3610 -# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP 0x3596 +# define BOT_CLAMP_SLOT_SIDE_ELEVATION_STEP (0x3596 + 3 * 0x72) # define BOT_CLAMP_BAY_FRONT_LEAVE_ELEVATION_STEP 0x1da7 # define BOT_CLAMP_BAY_BACK_LEAVE_ELEVATION_STEP 0x1f03 # define BOT_CLAMP_BAY_SIDE_ENTER_LEAVE_ELEVATION_STEP ((0x1da7 + 0x1f03) / 2) -- cgit v1.2.3 From 63407d7ed7b959e883c67107b3a80e859144e01d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 11:06:14 +0200 Subject: digital/io-hub: handle clamp blocking --- digital/io-hub/src/robospierre/clamp.c | 101 ++++++++++++++++++++++++++++-- digital/io-hub/src/robospierre/logistic.c | 8 +++ digital/io-hub/src/robospierre/logistic.h | 4 ++ digital/io-hub/src/robospierre/main.c | 3 + digital/io-hub/src/robospierre/top.c | 65 ++++++++++++++++--- 5 files changed, 169 insertions(+), 12 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 812d4890..e05f79d7 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -78,6 +78,8 @@ FSM_STATES ( CLAMP_DROPING_WAITING_ROBOT, /* Clamp locked in a bay. */ CLAMP_LOCKED, + /* Clamp blocked. */ + CLAMP_BLOCKED, /* Waiting movement order. */ CLAMP_MOVE_IDLE, @@ -105,6 +107,8 @@ FSM_EVENTS ( clamp_working, /* Sent when clamp return to idle state. */ clamp_done, + /* Sent when clamp is blocked. */ + clamp_blocked, /* Order to drop elements. */ clamp_drop, /* Sent once drop is done, but robot should advance to completely @@ -116,8 +120,12 @@ FSM_EVENTS ( clamp_move, /* Clamp movement success. */ clamp_move_success, + /* Clamp movement failure. */ + clamp_move_failure, /* Elevation and elevation motor success. */ clamp_elevation_rotation_success, + /* Elevation or elevation motor failure. */ + clamp_elevation_or_rotation_failure, /* Elevation motor success. */ clamp_elevation_success, /* Elevation motor failure. */ @@ -248,7 +256,7 @@ clamp_new_element (uint8_t pos, uint8_t element_type) void clamp_prepare (uint8_t prepare) { - logistic_global.prepare = 1; + logistic_global.prepare = prepare; FSM_HANDLE (AI, clamp_prepare); } @@ -409,6 +417,22 @@ clamp_route (void) ctx.pos_current = pos_new; } +static void +clamp_blocked (void) +{ + /* Free everything. */ + clamp_openclose (1); + clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1); + clamp_door (CLAMP_SLOT_FRONT_TOP, 1); + clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1); + clamp_door (CLAMP_SLOT_BACK_TOP, 1); + mimot_motor0_free (); + mimot_motor1_free (); + logistic_dump (); + /* Signal problem. */ + fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure)); +} + /* CLAMP FSM */ FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING) @@ -488,6 +512,12 @@ FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_success, CLAMP_IDLE) return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_success); } +FSM_TRANS (CLAMP_GOING_IDLE, clamp_move_failure, CLAMP_BLOCKED) +{ + fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked)); + return FSM_NEXT (CLAMP_GOING_IDLE, clamp_move_failure); +} + FSM_TRANS (CLAMP_IDLE, clamp_new_element, CLAMP_TAKING_DOOR_CLOSING) { ctx.working = 1; @@ -510,7 +540,8 @@ FSM_TRANS (CLAMP_IDLE, clamp_prepare, logistic_global.moving_to); return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element); } - else if (logistic_global.clamp_pos_idle != ctx.pos_current) + else if (logistic_global.prepare != 3 + && logistic_global.clamp_pos_idle != ctx.pos_current) { if (logistic_path_clear (ctx.pos_current, logistic_global.clamp_pos_idle)) @@ -556,7 +587,8 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, logistic_global.moving_to); return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element); } - else if (logistic_global.clamp_pos_idle != ctx.pos_current) + else if (logistic_global.prepare != 3 + && logistic_global.clamp_pos_idle != ctx.pos_current) { if (logistic_path_clear (ctx.pos_current, logistic_global.clamp_pos_idle)) @@ -593,7 +625,8 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, move_element); } - else if (logistic_global.clamp_pos_idle != ctx.pos_current) + else if (logistic_global.prepare != 3 + && logistic_global.clamp_pos_idle != ctx.pos_current) { if (logistic_path_clear (ctx.pos_current, logistic_global.clamp_pos_idle)) @@ -619,6 +652,12 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, } } +FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_failure, CLAMP_BLOCKED) +{ + fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked)); + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_failure); +} + FSM_TRANS_TIMEOUT (CLAMP_DROPING_DOOR_OPENING, BOT_PWM_CLAMP_OPEN_TIME, CLAMP_DROPING_WAITING_ROBOT) { @@ -640,7 +679,8 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, move_element); } - else if (logistic_global.clamp_pos_idle != ctx.pos_current) + else if (logistic_global.prepare != 3 + && logistic_global.clamp_pos_idle != ctx.pos_current) { if (logistic_path_clear (ctx.pos_current, logistic_global.clamp_pos_idle)) @@ -684,6 +724,34 @@ FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING) return FSM_NEXT (CLAMP_LOCKED, clamp_drop); } +FSM_TRANS (CLAMP_BLOCKED, clamp_prepare, + move_to_idle, CLAMP_GOING_IDLE, + clamp_locked, CLAMP_LOCKED, + done, CLAMP_IDLE) +{ + if (logistic_global.prepare != 3) + { + if (logistic_path_clear (ctx.pos_current, + logistic_global.clamp_pos_idle)) + { + clamp_move (logistic_global.clamp_pos_idle); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle); + } + else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked); + } + } + else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, done); + } +} + /* CLAMP_MOVE FSM */ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, @@ -729,6 +797,13 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_rotation_success, } } +FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure, + CLAMP_MOVE_IDLE) +{ + clamp_blocked (); + return FSM_NEXT (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure); +} + FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, done, CLAMP_MOVE_SRC_CLAMP_CLOSING, next, CLAMP_MOVE_SRC_ROUTING) @@ -747,6 +822,14 @@ FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, } } +FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_or_rotation_failure, + CLAMP_MOVE_IDLE) +{ + clamp_blocked (); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, + clamp_elevation_or_rotation_failure); +} + FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME, open_door, CLAMP_MOVE_SRC_DOOR_OPENDING, move, CLAMP_MOVE_DST_ROUTING) @@ -804,6 +887,14 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, } } +FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_or_rotation_failure, + CLAMP_MOVE_IDLE) +{ + clamp_blocked (); + return FSM_NEXT (CLAMP_MOVE_DST_ROUTING, + clamp_elevation_or_rotation_failure); +} + FSM_TRANS_TIMEOUT (CLAMP_MOVE_DST_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, CLAMP_MOVE_DST_CLAMP_OPENING) { diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 4b0536f2..afa625bc 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -518,6 +518,14 @@ logistic_drop (uint8_t direction) logistic_decision (); } +void +logistic_dump (void) +{ + uint8_t i; + for (i = 0; i < CLAMP_SLOT_NB; i++) + ctx.slots[i] = 0; +} + static uint8_t logistic_slot_clear (uint8_t slot) { diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 9962874b..6acb681e 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -124,6 +124,10 @@ logistic_element_move_done (void); void logistic_drop (uint8_t direction); +/** Dump every element. */ +void +logistic_dump (void); + /** Is path clear between two positions? */ uint8_t logistic_path_clear (uint8_t slot1, uint8_t slot2); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 6b2c8d31..1e12c427 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -144,6 +144,9 @@ main_event_to_fsm (void) if (mimot_motor0_status == success && mimot_motor1_status == success) FSM_HANDLE_E (AI, clamp_elevation_rotation_success); + if (mimot_motor0_status == failure + || mimot_motor1_status == failure) + FSM_HANDLE_E (AI, clamp_elevation_or_rotation_failure); if (mimot_motor0_status == success) FSM_HANDLE_E (AI, clamp_elevation_success); else if (mimot_motor0_status == failure) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index c9362203..8ff69ee6 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -56,6 +56,10 @@ FSM_STATES ( TOP_GOING_TO_ELEMENT, /* Waiting clamp has finished its work. */ TOP_WAITING_CLAMP, + /* Unblocking: waiting a little bit. */ + TOP_UNBLOCKING_SHAKE_WAIT, + /* Unblocking: shaking. */ + TOP_UNBLOCKING_SHAKE, /* Waiting construction is ready to drop. */ TOP_WAITING_READY, /* Dropping, opening the doors. */ @@ -72,6 +76,8 @@ struct top_t uint8_t target_element_id; /** Chaos counter. */ uint8_t chaos; + /** Broken clamp. */ + uint8_t broken; }; /** Global context. */ @@ -111,10 +117,13 @@ top_go_element (void) asserv_get_position (&robot_pos); ctx.target_element_id = element_best (robot_pos); element_t e = element_get (ctx.target_element_id); - if (e.attr & ELEMENT_GREEN) - logistic_global.prepare = 0; - else - logistic_global.prepare = 1; + if (!ctx.broken) + { + if (e.attr & ELEMENT_GREEN) + logistic_global.prepare = 0; + else + logistic_global.prepare = 1; + } vect_t element_pos = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; @@ -154,11 +163,12 @@ static uint8_t top_decision (void) { /* If we can make a tower. */ - if (logistic_global.construct_possible == 1) + if (logistic_global.construct_possible == 1 + || (ctx.broken && logistic_global.construct_possible == 2)) return top_go_drop (); if (logistic_global.need_prepare) { - clamp_prepare (2); + clamp_prepare (ctx.broken ? 3 : 2); return top_go_drop (); } else @@ -217,7 +227,7 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success, } else { - clamp_prepare (1); + clamp_prepare (ctx.broken ? 3 : 1); return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp); } } @@ -292,12 +302,53 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, } } +FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) +{ + return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked); +} + +FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250, + try_again, TOP_UNBLOCKING_SHAKE, + tryout, TOP_WAITING_CLAMP) +{ + if (ctx.chaos < 4) + { + int16_t dist = logistic_global.collect_direction + == DIRECTION_FORWARD ? 100 : -100; + asserv_move_linearly (++ctx.chaos % 2 ? -dist : dist); + return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, try_again); + } + else + { + ctx.broken = 1; + clamp_prepare (3); + return FSM_NEXT_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, tryout); + } +} + +FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_success, TOP_WAITING_CLAMP) +{ + clamp_prepare (0); + return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_success); +} + +FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_failure, + TOP_UNBLOCKING_SHAKE_WAIT) +{ + return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_failure); +} + FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING) { clamp_drop (logistic_global.collect_direction); return FSM_NEXT (TOP_WAITING_READY, clamp_done); } +FSM_TRANS (TOP_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) +{ + return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked); +} + FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { element_down (ctx.target_element_id, ELEMENT_TOWER); -- cgit v1.2.3 From ffdccc5e2427be69a6ca31c0d4c8e37ebf04f385 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 14:42:16 +0200 Subject: digital/io-hub: do not dump side slot --- digital/io-hub/src/robospierre/logistic.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index afa625bc..5fe0d548 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -522,7 +522,8 @@ void logistic_dump (void) { uint8_t i; - for (i = 0; i < CLAMP_SLOT_NB; i++) + /* Drop all except side. */ + for (i = 0; i < CLAMP_SLOT_SIDE; i++) ctx.slots[i] = 0; } -- cgit v1.2.3 From 3e3944aa0e0c638cf4f75cb8866789d6653e6d6a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 14:46:10 +0200 Subject: digital/io-hub: take one out of two green element --- digital/io-hub/src/robospierre/element.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index b4ade33f..af05467a 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -72,12 +72,12 @@ struct element_t element_table[] = To be symmetric, alternate % 2. See ELEMENT_GREEN_START and ELEMENT_GREEN_END */ - {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, /* top left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, /* top right */ + {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 10, 0, 0}, /* top left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 10, 0, 0}, /* top right */ {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left */ {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, /* 2nd line right */ - {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, /* ... */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 10, 0, 0}, /* ... */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 10, 0, 0}, {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT, -100, 0, 0}, -- cgit v1.2.3 From e4a7a4d9fe80a0bafa13a01c3573076b856f9271 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 14:51:22 +0200 Subject: digital/io-hub: add 10 mm to init elevator position --- digital/io-hub/src/robospierre/bot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 12ca7c30..4395deeb 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -138,7 +138,7 @@ #endif /* !HOST */ #define BOT_CLAMP_INIT_ELEVATION_STEP \ - BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP + (BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP + 10 * 0x72) #define BOT_CLAMP_CLOSED_ROTATION_OFFSET(pos) \ (CLAMP_IS_SLOT_IN_FRONT_BAY (pos) \ -- cgit v1.2.3 From 6db0f054898e6873a32f392a2b8abe330f95a3b4 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Thu, 2 Jun 2011 17:11:55 +0200 Subject: digital/io-hub: make small towers if possible --- digital/io-hub/src/robospierre/logistic.c | 32 +++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 5fe0d548..1132b116 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -334,6 +334,38 @@ logistic_make_tower () static void logistic_make_unload () { + /* Making a small tower. Move head when having pawn. */ + LOGISTIC_CASE (D, _, + e, p, _, + H, _, RIGHT, 0); + + LOGISTIC_CASE (D, p, + e, e, e, + H, _, RIGHT, 0); + + LOGISTIC_CASE (D, _, + e, e, e, + H, p, RIGHT, 0); + + /* Making a small tower. Move pawn under head. */ + LOGISTIC_CASE (_, h, + _, P, e, + _, D, LEFT, 0); + + LOGISTIC_CASE (P, h, + e, e, e, + _, D, LEFT, 0); + + LOGISTIC_CASE (_, h, + e, e, e, + P, D, LEFT, 0); + + /* Making a small tower. Finally move head on pawn. */ + LOGISTIC_CASE (_, H, + _, _, D, + _, p, LEFT, 1); + + /* Having any element. */ LOGISTIC_CASE (_, _, _, _, _, _, a, LEFT, 1); -- cgit v1.2.3 From 79a1048f84c27da1dce7afb1266fa8423329a73b Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Thu, 2 Jun 2011 17:29:38 +0200 Subject: digital/io-hub: corrected mini tower construction --- digital/io-hub/src/robospierre/logistic.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 1132b116..dea6e0a5 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -363,6 +363,10 @@ logistic_make_unload () /* Making a small tower. Finally move head on pawn. */ LOGISTIC_CASE (_, H, _, _, D, + _, p, LEFT, 0); + + LOGISTIC_CASE (_, _, + _, _, h, _, p, LEFT, 1); /* Having any element. */ -- cgit v1.2.3 From 4937eebb669121b5ee15a79267c6cbdc1ca88b97 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 15:18:08 +0200 Subject: digital/io-hub: drop single pawn when end of match approach --- digital/io-hub/src/robospierre/top.c | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 8ff69ee6..59a168ed 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -32,6 +32,7 @@ #include "logistic.h" #include "move.h" +#include "chrono.h" /* * Here is the top FSM. This FSM is suppose to give life to the robot with an @@ -110,6 +111,17 @@ FSM_TRANS_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT, 250, TOP_GOING_OUT1) return FSM_NEXT_TIMEOUT (TOP_GOING_OUT1_BLOCK_WAIT); } +static uint8_t +top_prepare_level (void) +{ + if (ctx.broken) + return 3; + else if (logistic_global.need_prepare || chrono_remaining_time () < 45000l) + return 2; + else + return 1; +} + static uint8_t top_go_element (void) { @@ -122,7 +134,7 @@ top_go_element (void) if (e.attr & ELEMENT_GREEN) logistic_global.prepare = 0; else - logistic_global.prepare = 1; + logistic_global.prepare = top_prepare_level (); } vect_t element_pos = element_get_pos (ctx.target_element_id); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD @@ -164,11 +176,12 @@ top_decision (void) { /* If we can make a tower. */ if (logistic_global.construct_possible == 1 - || (ctx.broken && logistic_global.construct_possible == 2)) + || ((ctx.broken || logistic_global.prepare == 2) + && logistic_global.construct_possible == 2)) return top_go_drop (); if (logistic_global.need_prepare) { - clamp_prepare (ctx.broken ? 3 : 2); + clamp_prepare (top_prepare_level ()); return top_go_drop (); } else @@ -227,7 +240,7 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success, } else { - clamp_prepare (ctx.broken ? 3 : 1); + clamp_prepare (top_prepare_level ()); return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp); } } -- cgit v1.2.3 From 6ef0e7de88e2c4896ebdbd2ed1617ded5c89c31c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 16:02:20 +0200 Subject: digital/io-hub: add bonus to some bonus zones --- digital/io-hub/src/robospierre/element.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index af05467a..45b782fb 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -95,10 +95,10 @@ struct element_t element_table[] = {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, /* bonus */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 10, 0}, /* bonus */ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* 3th line left blue */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, -- cgit v1.2.3 From 4eb705e639457a07e6d5e9423ba81f8cede1b979 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 04:37:35 +0200 Subject: digital/io-hub: handle element change to head --- digital/io-hub/src/robospierre/clamp.c | 39 +++++++++++++++++++++++++++-- digital/io-hub/src/robospierre/logistic.c | 8 ++++++ digital/io-hub/src/robospierre/logistic.h | 4 +++ host/simu/robots/robospierre/model/clamp.py | 6 ++--- 4 files changed, 52 insertions(+), 5 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index e05f79d7..6b48c56a 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -373,11 +373,17 @@ clamp_route (void) } else if (pos_current == CLAMP_BAY_FRONT_LEAVE) { - pos_new = CLAMP_BAY_FRONT_LEAVING; + if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) + pos_new = pos_request; + else + pos_new = CLAMP_BAY_FRONT_LEAVING; } else if (pos_current == CLAMP_BAY_BACK_LEAVE) { - pos_new = CLAMP_BAY_BACK_LEAVING; + if (CLAMP_IS_SLOT_IN_BACK_BAY (pos_request)) + pos_new = pos_request; + else + pos_new = CLAMP_BAY_BACK_LEAVING; } else if (pos_current == CLAMP_BAY_FRONT_LEAVING) { @@ -417,6 +423,34 @@ clamp_route (void) ctx.pos_current = pos_new; } +/* When lifting an element, we can discover it is actually a head. In this + * case, change destination. */ +void +clamp_change (void) +{ + uint8_t from = logistic_global.moving_from; + if (logistic_global.slots[from] == ELEMENT_PAWN) + { + /* Look head contact. */ + uint8_t contact_head = 0; + if (from == CLAMP_SLOT_FRONT_BOTTOM) + contact_head = !IO_GET (CONTACT_FRONT_TOP); + else if (from == CLAMP_SLOT_BACK_BOTTOM) + contact_head = !IO_GET (CONTACT_BACK_TOP); + /* Change? */ + if (contact_head) + { + logistic_element_change (from, ELEMENT_KING); + if (logistic_global.moving_from != from) + /* Cancel move. */ + ctx.pos_request = ctx.moving_to = from; + else + /* Change move. */ + ctx.pos_request = ctx.moving_to = logistic_global.moving_to; + } + } +} + static void clamp_blocked (void) { @@ -861,6 +895,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, next, CLAMP_MOVE_DST_ROUTING) { + clamp_change (); if (ctx.pos_current == ctx.pos_request) { if (clamp_slot_door[ctx.pos_current] != 0xff) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index dea6e0a5..f59f2da4 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -533,6 +533,14 @@ logistic_element_new (uint8_t pos, uint8_t element_type) logistic_decision (); } +void +logistic_element_change (uint8_t pos, uint8_t element_type) +{ + assert (pos < CLAMP_SLOT_NB); + ctx.slots[pos] = element_type; + logistic_decision (); +} + void logistic_element_move_done (void) { diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 6acb681e..4a06dcb7 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -115,6 +115,10 @@ logistic_update (void); void logistic_element_new (uint8_t pos, uint8_t element_type); +/** Oh la la, the pawn was not a pawn, it's a head. */ +void +logistic_element_change (uint8_t pos, uint8_t element_type); + /** To be called when a element movement is done. */ void logistic_element_move_done (void); diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index dbcab5f2..c199259e 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -203,8 +203,7 @@ class Clamp (Observable): slots[1].pawn is not None or (slots[0].pawn is not None and slots[0].pawn.kind == 'tower')) - # This one is really high. - slots[2].contact.state = not (slots[2].pawn is not None) + slots[2].contact.state = True if slots[0].pawn: slots[0].codebar.element_type = slots[0].pawn.kind else: @@ -213,7 +212,8 @@ class Clamp (Observable): slot_side = self.slots[self.SLOT_SIDE] slot_side.contact.state = slot_side.pawn is None clamp_slot = self.__get_clamp_slot () - if clamp_slot is not None: + if clamp_slot is not None and clamp_slot != self.SLOT_FRONT_TOP \ + and clamp_slot != self.SLOT_BACK_TOP: clamp_slot.contact.state = False for slot in self.slots: slot.contact.notify () -- cgit v1.2.3 From bebd46dc44ce0d257c4f8e2d67dbfe8a5478803d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 04:43:14 +0200 Subject: digital/io-hub: lower init PWM --- digital/io-hub/src/robospierre/bot.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 4395deeb..a2f8b820 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -176,8 +176,8 @@ || (slot) == CLAMP_SLOT_BACK_BOTTOM) ? -0x100 : -0x200) #define BOT_PWM_CLAMP_INIT 0x1ff, 150, 0 -#define BOT_PWM_DOOR_INIT 0x1ff, 80, 0 +#define BOT_PWM_DOOR_INIT 0x100, 160, 0 #define BOT_PWM_DOOR_INIT_START 0x55 -#define BOT_PWM_CLAMP_DOOR_INIT 150 +#define BOT_PWM_CLAMP_DOOR_INIT 250 #endif /* bot_h */ -- cgit v1.2.3 From 50408adbb1ecf0e1a73a3339d053a346bc2cba24 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 05:22:15 +0200 Subject: digital/io-hub: fix prepare pawn after 45 s --- digital/io-hub/src/robospierre/top.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 59a168ed..286e819c 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -39,6 +39,8 @@ * impression of intelligence... Well... */ +#define TOP_PAWN_TIME 45000l + FSM_INIT FSM_STATES ( @@ -116,7 +118,8 @@ top_prepare_level (void) { if (ctx.broken) return 3; - else if (logistic_global.need_prepare || chrono_remaining_time () < 45000l) + else if (logistic_global.need_prepare + || chrono_remaining_time () < TOP_PAWN_TIME) return 2; else return 1; @@ -151,6 +154,8 @@ top_go_drop (void) ctx.target_element_id = element_unload_best (robot_pos); position_t drop_pos; drop_pos.v = element_get_pos (ctx.target_element_id); + if (!ctx.broken) + logistic_global.prepare = top_prepare_level (); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; /* Go above or below the drop point. */ @@ -176,7 +181,7 @@ top_decision (void) { /* If we can make a tower. */ if (logistic_global.construct_possible == 1 - || ((ctx.broken || logistic_global.prepare == 2) + || ((ctx.broken || chrono_remaining_time () < TOP_PAWN_TIME) && logistic_global.construct_possible == 2)) return top_go_drop (); if (logistic_global.need_prepare) -- cgit v1.2.3 From f9f6fbde971e8a7d4698e197b09a0ea2dd8a8526 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 05:22:43 +0200 Subject: host/simu/robots/robospierre: fix level 2 drop --- host/simu/robots/robospierre/model/clamp.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index c199259e..d32edf25 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -193,6 +193,9 @@ class Clamp (Observable): and slots[2].pawn and slots[2].door_motor.angle: slots[0].pawn.tower.append (slots[2].pawn) slots[2].pawn = None + if slots[0].pawn is None and slots[1].pawn is None \ + and slots[2].pawn and slots[2].door_motor.angle: + slots[0].pawn, slots[2].pawn = slots[2].pawn, None def update_contacts (self): """Update pawn contacts.""" -- cgit v1.2.3 From 2eca57339e792160fe21a17956a1257403296643 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 06:07:49 +0200 Subject: digital/io-hub: cancel bonus on far unload area --- digital/io-hub/src/robospierre/element.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 45b782fb..cbb07a65 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -88,12 +88,12 @@ struct element_t element_table[] = Altern colors in order to retrieve position % 2. See ELEMENT_UNLOAD_START and ELEMENT_UNLOAD_END */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, /* Top left blue */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* Top left blue */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left red */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, /* bonus */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, -- cgit v1.2.3 From 002c8f8819a34cc8d589115cca433de716036d8c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 06:15:17 +0200 Subject: digital/io-hub: little step backward in green zone --- digital/io-hub/src/robospierre/bot.h | 2 +- digital/io-hub/src/robospierre/clamp.c | 3 +++ digital/io-hub/src/robospierre/top.c | 13 +++++++++++++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index a2f8b820..fbbeeb7e 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -60,7 +60,7 @@ #define BOT_GREEN_ELEMENT_PLACE_DISTANCE_MM 600 /** Distance from border to go to capture a green element. */ #define BOT_GREEN_ELEMENT_DISTANCE_MM \ - (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 25) + (BOT_ELEMENT_RADIUS + BOT_SIZE_FRONT + 10) /** Speed used for initialisation. */ #ifdef HOST diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 6b48c56a..50afda1e 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -103,6 +103,8 @@ FSM_EVENTS ( clamp_new_element, /* Order to prepare tower. */ clamp_prepare, + /* Sent when an element has just been taken (door closed). */ + clamp_taken, /* Sent when clamp is working. */ clamp_working, /* Sent when clamp return to idle state. */ @@ -614,6 +616,7 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { + fsm_queue_post_event (FSM_EVENT (AI, clamp_taken)); logistic_element_new (ctx.pos_new, ctx.new_element_type); if (logistic_global.moving_from != CLAMP_SLOT_NB) { diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 286e819c..9cdefc3e 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -81,6 +81,8 @@ struct top_t uint8_t chaos; /** Broken clamp. */ uint8_t broken; + /** Saved, direction when picking element. */ + uint8_t go_to_element_direction; }; /** Global context. */ @@ -140,6 +142,7 @@ top_go_element (void) logistic_global.prepare = top_prepare_level (); } vect_t element_pos = element_get_pos (ctx.target_element_id); + ctx.go_to_element_direction = logistic_global.collect_direction; uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; move_start_noangle (element_pos, backward, 0); @@ -325,6 +328,16 @@ FSM_TRANS (TOP_WAITING_CLAMP, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) return FSM_NEXT (TOP_WAITING_CLAMP, clamp_blocked); } +FSM_TRANS (TOP_WAITING_CLAMP, clamp_taken, TOP_WAITING_CLAMP) +{ + position_t robot_pos; + asserv_get_position (&robot_pos); + if (robot_pos.v.x < 400 || robot_pos.v.x > PG_WIDTH - 400) + asserv_move_linearly (ctx.go_to_element_direction + == DIRECTION_FORWARD ? -50 : 50); + return FSM_NEXT (TOP_WAITING_CLAMP, clamp_taken); +} + FSM_TRANS_TIMEOUT (TOP_UNBLOCKING_SHAKE_WAIT, 250, try_again, TOP_UNBLOCKING_SHAKE, tryout, TOP_WAITING_CLAMP) -- cgit v1.2.3 From d5ca3805ed0eb4829b491352a31165b9fd58b15e Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 08:24:48 +0200 Subject: digital/io-hub: handle bumpers --- digital/ai/src/twi_master/mimot.c | 6 +++ digital/ai/src/twi_master/mimot.h | 4 ++ digital/io-hub/src/robospierre/main.c | 4 +- digital/io-hub/src/robospierre/pawn_sensor.c | 77 ++++++++++++++++++++++++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 8 +++ digital/io-hub/src/robospierre/top.c | 28 ++++++++-- digital/mimot/src/dirty/simu.host.c | 2 +- 7 files changed, 123 insertions(+), 6 deletions(-) diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c index 6159e6c9..8df84538 100644 --- a/digital/ai/src/twi_master/mimot.c +++ b/digital/ai/src/twi_master/mimot.c @@ -97,6 +97,12 @@ mimot_motor1_cmd_status (void) return none; } +uint8_t +mimot_get_input (void) +{ + return mimot_status.input_port; +} + uint16_t mimot_get_motor0_position (void) { diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h index c59c1608..c9de7090 100644 --- a/digital/ai/src/twi_master/mimot.h +++ b/digital/ai/src/twi_master/mimot.h @@ -56,6 +56,10 @@ mimot_motor0_cmd_status (void); asserv_status_e mimot_motor1_cmd_status (void); +/** Return input port state. */ +uint8_t +mimot_get_input (void); + /** Get motor0 position in steps. */ uint16_t mimot_get_motor0_position (void); diff --git a/digital/io-hub/src/robospierre/main.c b/digital/io-hub/src/robospierre/main.c index 1e12c427..9ae644e6 100644 --- a/digital/io-hub/src/robospierre/main.c +++ b/digital/io-hub/src/robospierre/main.c @@ -41,6 +41,7 @@ #include "pwm.h" #include "contact.h" #include "codebar.h" +#include "pawn_sensor.h" #include "radar.h" #define FSM_NAME AI @@ -207,6 +208,7 @@ main_loop (void) /* Update IO modules. */ pwm_update (); contact_update (); + pawn_sensor_update (); if (usdist_update ()) { position_t robot_pos; @@ -234,7 +236,7 @@ main_loop (void) } if (main_stats_contact_ && !--main_stats_contact_cpt_) { - proto_send1d ('P', contact_all ()); + proto_send1d ('P', contact_all () | (uint32_t) mimot_get_input () << 24); main_stats_contact_cpt_ = main_stats_contact_; } if (main_stats_codebar_ && !--main_stats_codebar_cpt_) diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index ccb5eb5b..5ca18759 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -23,6 +23,7 @@ * * }}} */ #include "common.h" +#include "defs.h" #include "pawn_sensor.h" #include "asserv.h" @@ -33,6 +34,12 @@ #include "bot.h" #include "playground.h" #include "codebar.h" +#include "mimot.h" +#include "main.h" + +#define FSM_NAME AI +#include "fsm.h" +#include "fsm_queue.h" #include "modules/utils/utils.h" #include "modules/math/geometry/distance.h" @@ -49,8 +56,18 @@ struct pawn_sensor_t vect_t active_position; }; +/** Pawn sensor general context. */ +struct pawn_sensor_general_t +{ + /** Last bumped element position. */ + vect_t last_bumped; + /** Bumper triggered, wait until the next one. */ + uint16_t bump_wait; +}; + /** Global contexts. */ struct pawn_sensor_t pawn_sensor_front, pawn_sensor_back; +struct pawn_sensor_general_t pawn_sensor_global; static uint8_t pawn_sensor_get_type (uint8_t direction) @@ -132,3 +149,63 @@ pawn_sensor_get (uint8_t direction) return 0; } +void +pawn_sensor_bumper (uint8_t bumped, uint16_t dx, uint16_t dy) +{ + uint8_t i; + if (bumped) + { + /* Compute pawn position. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + vect_t bumped_pawn; + bumped_pawn.x = dx; + bumped_pawn.y = dy; + vect_rotate_uf016 (&bumped_pawn, robot_pos.a); + vect_translate (&bumped_pawn, &robot_pos.v); + /* Do not count if out of the table. */ + if (bumped_pawn.x < 400 + BOT_ELEMENT_RADIUS + && bumped_pawn.x >= PG_WIDTH - 400 - BOT_ELEMENT_RADIUS + && bumped_pawn.y < BOT_ELEMENT_RADIUS + && bumped_pawn.y >= PG_WIDTH - BOT_ELEMENT_RADIUS) + return; + /* Do not count the opponent as a pawn. */ + for (i = 0; i < main_obstacles_nb; i++) + { + uint16_t dist = distance_point_point (&bumped_pawn, + &main_obstacles_pos[i]); + if (dist < 300 + BOT_ELEMENT_RADIUS) + return; + } + /* OK, take it. */ + pawn_sensor_global.last_bumped = bumped_pawn; + fsm_queue_post_event (FSM_EVENT (AI, top_bumper)); + pawn_sensor_global.bump_wait = 3 * 250; + } +} + +void +pawn_sensor_update (void) +{ +#define BUMPER_FRONT_LEFT _BV (6) +#define BUMPER_FRONT_RIGHT _BV (7) +#define BUMPER_BACK_RIGHT _BV (5) +#define BUMPER_BACK_LEFT _BV (4) + if (pawn_sensor_global.bump_wait) + pawn_sensor_global.bump_wait--; + else + { + uint8_t bumpers = mimot_get_input (); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265); + } +} + +vect_t +pawn_sensor_get_last_bumped (void) +{ + return pawn_sensor_global.last_bumped; +} + diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 14ce2d62..04ea1a2f 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -30,4 +30,12 @@ uint8_t pawn_sensor_get (uint8_t direction); +/** Update pawn sensors. */ +void +pawn_sensor_update (void); + +/** Return last bumped pawn. */ +vect_t +pawn_sensor_get_last_bumped (void); + #endif /* pawn_sensor_h */ diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 9cdefc3e..52bb05ae 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -33,6 +33,7 @@ #include "logistic.h" #include "move.h" #include "chrono.h" +#include "pawn_sensor.h" /* * Here is the top FSM. This FSM is suppose to give life to the robot with an @@ -70,6 +71,10 @@ FSM_STATES ( /* Dropping, clearing so that doors can be closed. */ TOP_DROP_CLEARING) +FSM_EVENTS ( + /* Bumpers have seen something. */ + top_bumper) + FSM_START_WITH (TOP_START) /** Top context. */ @@ -127,6 +132,15 @@ top_prepare_level (void) return 1; } +static void +top_go_this_element (vect_t pos, int16_t shorten) +{ + ctx.go_to_element_direction = logistic_global.collect_direction; + uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD + ? 0 : ASSERV_BACKWARD; + move_start_noangle (pos, backward, shorten); +} + static uint8_t top_go_element (void) { @@ -142,10 +156,7 @@ top_go_element (void) logistic_global.prepare = top_prepare_level (); } vect_t element_pos = element_get_pos (ctx.target_element_id); - ctx.go_to_element_direction = logistic_global.collect_direction; - uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD - ? 0 : ASSERV_BACKWARD; - move_start_noangle (element_pos, backward, 0); + top_go_this_element (element_pos, 0); return 1; } @@ -312,6 +323,15 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); } +FSM_TRANS (TOP_GOING_TO_ELEMENT, top_bumper, TOP_GOING_TO_ELEMENT) +{ + if (!ctx.broken) + logistic_global.prepare = top_prepare_level (); + move_stop (); + top_go_this_element (pawn_sensor_get_last_bumped (), BOT_ELEMENT_RADIUS - 50); + return FSM_NEXT (TOP_GOING_TO_ELEMENT, top_bumper); +} + FSM_TRANS (TOP_WAITING_CLAMP, clamp_done, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) diff --git a/digital/mimot/src/dirty/simu.host.c b/digital/mimot/src/dirty/simu.host.c index 8afd73ca..d27ffe5f 100644 --- a/digital/mimot/src/dirty/simu.host.c +++ b/digital/mimot/src/dirty/simu.host.c @@ -156,7 +156,7 @@ simu_sensor_update_marcel (void) void simu_sensor_update_robospierre (void) { - PINC = 0; + PINC = 0xf0; if (simu_aux_model[0].th < 120.0 * 5.0 / 6.0 * simu_aux_model[0].m.i_G) PINC |= IO_BV (CONTACT_AUX0_ZERO_IO); } -- cgit v1.2.3 From 09193b811d682c035fca4a15caf0728c1d36a9c1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 08:35:10 +0200 Subject: digital/ai/src/fsm: add proto debug --- digital/ai/src/fsm/fsm.host.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/digital/ai/src/fsm/fsm.host.c b/digital/ai/src/fsm/fsm.host.c index ed93221d..50336722 100644 --- a/digital/ai/src/fsm/fsm.host.c +++ b/digital/ai/src/fsm/fsm.host.c @@ -1144,6 +1144,7 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) /* Introduction. */ fprintf (f, "/* This file has been generated, do not edit. */\n\n"); fprintf (f, "#include \"fsm_%s_gen.h\"\n\n", fsm->name); + fprintf (f, "#include \"modules/proto/proto.h\"\n\n"); /* Gen state strings. */ if (embedded_strings) @@ -1348,6 +1349,8 @@ fsm_build_gen_avr_c (fsm_build_t *fsm, uint embedded_strings) fsm->name, fsm->name); fprintf (f, "\t\t{\n"); + fprintf (f, "\t\t\tproto_send2b ('F', fsm_%s_active_states[i], e);\n", + fsm->name); fprintf (f, "\t\t\tfsm_%s_active_states[i] = fsm_%s_read_trans (e, " "fsm_%s_active_states[i])();\n", fsm->name, -- cgit v1.2.3 From 7f3014a642702c61a5c2e619a8c2f9d7ca7ec3bb Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 08:48:52 +0200 Subject: digital/io-hub: make the little step back after clamp is closed --- digital/io-hub/src/robospierre/clamp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 50afda1e..0c6573d7 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -616,7 +616,6 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { - fsm_queue_post_event (FSM_EVENT (AI, clamp_taken)); logistic_element_new (ctx.pos_new, ctx.new_element_type); if (logistic_global.moving_from != CLAMP_SLOT_NB) { @@ -871,6 +870,7 @@ FSM_TRANS_TIMEOUT (CLAMP_MOVE_SRC_CLAMP_CLOSING, BOT_PWM_CLAMP_CLOSE_TIME, open_door, CLAMP_MOVE_SRC_DOOR_OPENDING, move, CLAMP_MOVE_DST_ROUTING) { + fsm_queue_post_event (FSM_EVENT (AI, clamp_taken)); if (clamp_slot_door[ctx.pos_current] != 0xff) { pwm_set_timed (clamp_slot_door[ctx.pos_current], -- cgit v1.2.3 From c8cfe9c4b5498a6d993f224935f38869e782352b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 12:53:24 +0200 Subject: digital/io-hub: give the right element to element module --- digital/io-hub/src/robospierre/clamp.c | 11 +++++++++++ digital/io-hub/src/robospierre/top.c | 12 +----------- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 0c6573d7..f91af6a0 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -425,6 +425,14 @@ clamp_route (void) ctx.pos_current = pos_new; } +static void +clamp_taken_pawn (uint8_t element_type) +{ + position_t robot_pos; + asserv_get_position (&robot_pos); + element_taken (element_nearest_element_id (robot_pos), element_type); +} + /* When lifting an element, we can discover it is actually a head. In this * case, change destination. */ void @@ -443,6 +451,7 @@ clamp_change (void) if (contact_head) { logistic_element_change (from, ELEMENT_KING); + clamp_taken_pawn (ELEMENT_HEAD); if (logistic_global.moving_from != from) /* Cancel move. */ ctx.pos_request = ctx.moving_to = from; @@ -617,6 +626,7 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, done, CLAMP_IDLE) { logistic_element_new (ctx.pos_new, ctx.new_element_type); + clamp_taken_pawn (ctx.new_element_type); if (logistic_global.moving_from != CLAMP_SLOT_NB) { clamp_move_element (logistic_global.moving_from, @@ -747,6 +757,7 @@ FSM_TRANS (CLAMP_LOCKED, clamp_new_element, CLAMP_LOCKED) pwm_set_timed (clamp_slot_door[ctx.pos_new], BOT_PWM_DOOR_CLOSE (ctx.pos_new)); logistic_element_new (ctx.pos_new, ctx.new_element_type); + clamp_taken_pawn (ctx.new_element_type); return FSM_NEXT (CLAMP_LOCKED, clamp_new_element); } diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 52bb05ae..09de28f6 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -207,14 +207,6 @@ top_decision (void) return top_go_element (); } -static void -top_taken_pawn (void) -{ - position_t robot_pos; - asserv_get_position (&robot_pos); - element_taken (element_nearest_element_id (robot_pos), ELEMENT_PAWN); -} - FSM_TRANS (TOP_GOING_OUT2, robot_move_success, clamp_working, TOP_WAITING_CLAMP, drop, TOP_GOING_TO_DROP, @@ -281,7 +273,6 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_failure, FSM_TRANS (TOP_GOING_TO_DROP, clamp_working, TOP_WAITING_CLAMP) { - top_taken_pawn (); move_stop (); return FSM_NEXT (TOP_GOING_TO_DROP, clamp_working); } @@ -291,7 +282,7 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { - element_taken (ctx.target_element_id, ELEMENT_PAWN); + element_failure (ctx.target_element_id); /* Do not take this one again. */ if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); switch (top_decision ()) @@ -318,7 +309,6 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) { - top_taken_pawn (); move_stop (); return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); } -- cgit v1.2.3 From fe415bb389823f1557ef05324bbc966bd42de0a4 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 13:00:45 +0200 Subject: digital/io-hub: do not signal bumped element --- digital/io-hub/src/robospierre/top.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 09de28f6..595371f7 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -261,7 +261,8 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_failure, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { - element_failure (ctx.target_element_id); + if (ctx.target_element_id != 0xff) + element_failure (ctx.target_element_id); if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_DROP, move_failure, clamp_working); switch (top_decision ()) @@ -282,7 +283,8 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { - element_failure (ctx.target_element_id); /* Do not take this one again. */ + if (ctx.target_element_id != 0xff) + element_failure (ctx.target_element_id); /* Do not take this one again. */ if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); switch (top_decision ()) @@ -297,7 +299,8 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, drop, TOP_GOING_TO_DROP, element, TOP_GOING_TO_ELEMENT) { - element_failure (ctx.target_element_id); + if (ctx.target_element_id != 0xff) + element_failure (ctx.target_element_id); if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working); switch (top_decision ()) @@ -318,6 +321,7 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, top_bumper, TOP_GOING_TO_ELEMENT) if (!ctx.broken) logistic_global.prepare = top_prepare_level (); move_stop (); + ctx.target_element_id = 0xff; top_go_this_element (pawn_sensor_get_last_bumped (), BOT_ELEMENT_RADIUS - 50); return FSM_NEXT (TOP_GOING_TO_ELEMENT, top_bumper); } @@ -392,7 +396,8 @@ FSM_TRANS (TOP_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { - element_down (ctx.target_element_id, ELEMENT_TOWER); + if (ctx.target_element_id != 0xff) + element_down (ctx.target_element_id, ELEMENT_TOWER); asserv_move_linearly (logistic_global.collect_direction == DIRECTION_FORWARD ? 150 : -150); return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting); -- cgit v1.2.3 From 631860f79e52a0c155bb2e191c2729c0ca7e3149 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 13:28:10 +0200 Subject: digital/io-hub: on pathologic failure, add failure offset --- digital/io-hub/src/robospierre/element.c | 47 +++++++++++++++++++++----------- 1 file changed, 31 insertions(+), 16 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index cbb07a65..599e9175 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -34,6 +34,9 @@ #include "bot.h" #include "playground.h" +/** Offset to be used when everything fail. */ +static uint8_t failure_offset_s; + /** Elements on table. */ struct element_t element_table[] = { @@ -179,7 +182,7 @@ element_unload_score (position_t robot_pos, uint8_t element_id) /* Failed element. */ if (e.failure_until_s - && e.failure_until_s < chrono_remaining_time () / 1000) + && e.failure_until_s + failure_offset_s < chrono_remaining_time () / 1000) return -1; /* Bonus adjust. */ @@ -217,17 +220,23 @@ element_unload_best (position_t robot_pos) uint8_t i; uint8_t best = 0xff; int32_t score, best_score = 0; - for (i = ELEMENT_UNLOAD_START; - i <= ELEMENT_UNLOAD_END; - i++) + uint8_t retry = 0; + do { - score = element_unload_score (robot_pos , i); - if (best == 0xff || best_score < score) + if (retry && failure_offset_s != 255) + failure_offset_s++; + for (i = ELEMENT_UNLOAD_START; + i <= ELEMENT_UNLOAD_END; + i++) { - best = i; - best_score = score; + score = element_unload_score (robot_pos , i); + if (best == 0xff || best_score < score) + { + best = i; + best_score = score; + } } - } + } while (best_score == -1 && retry++ < 10); return best; } @@ -252,7 +261,7 @@ element_score (position_t robot_pos, uint8_t element_id) /* Failed element. */ if (e.failure_until_s - && e.failure_until_s < chrono_remaining_time () / 1000) + && e.failure_until_s + failure_offset_s < chrono_remaining_time () / 1000) return -1; if (e.type & ELEMENT_PAWN) @@ -401,15 +410,21 @@ element_best (position_t robot_pos) uint8_t i; uint8_t best = 0xff; int32_t score = 0, best_score = 0; - for (i = 0; i < UTILS_COUNT (element_table); i++) + uint8_t retry = 0; + do { - score = element_score (robot_pos ,i); - if (best == 0xff || best_score < score) + if (retry && failure_offset_s != 255) + failure_offset_s++; + for (i = 0; i < UTILS_COUNT (element_table); i++) { - best = i; - best_score = score; + score = element_score (robot_pos ,i); + if (best == 0xff || best_score < score) + { + best = i; + best_score = score; + } } - } + } while (best_score == -1 && retry++ < 10); return best; } -- cgit v1.2.3 From 1fb8f5180fa0dd78327cfc1d51296ecfa3da9737 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Fri, 3 Jun 2011 14:39:10 +0200 Subject: digital/io-hub: moved score for green zone --- digital/io-hub/src/robospierre/element.c | 56 ++++++++++++-------------------- 1 file changed, 21 insertions(+), 35 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 599e9175..ae84bd20 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -75,14 +75,14 @@ struct element_t element_table[] = To be symmetric, alternate % 2. See ELEMENT_GREEN_START and ELEMENT_GREEN_END */ - {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 10, 0, 0}, /* top left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 10, 0, 0}, /* top right */ - {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, /* 2nd line right */ - {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 10, 0, 0}, /* ... */ - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 10, 0, 0}, - {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 0, 0, 0}, - {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 5}, ELEMENT_GREEN |ELEMENT_LEFT, 110, 0, 0}, /* top left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 5}, ELEMENT_GREEN | ELEMENT_RIGHT, 110, 0, 0}, /* top right */ + {ELEMENT_ANY, {200, 10 + 280 * 4}, ELEMENT_GREEN |ELEMENT_LEFT, 100, 0, 0}, /* 2nd line left */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 4}, ELEMENT_GREEN | ELEMENT_RIGHT, 100, 0, 0}, /* 2nd line right */ + {ELEMENT_ANY, {200, 10 + 280 * 3}, ELEMENT_GREEN |ELEMENT_LEFT, 110, 0, 0}, /* ... */ + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 3}, ELEMENT_GREEN | ELEMENT_RIGHT, 110, 0, 0}, + {ELEMENT_ANY, {200, 10 + 280 * 2}, ELEMENT_GREEN |ELEMENT_LEFT, 100, 0, 0}, + {ELEMENT_ANY, {3000 - 200, 10 + 280 * 2}, ELEMENT_GREEN | ELEMENT_RIGHT, 100, 0, 0}, {ELEMENT_ANY, {200, 10 + 280 * 1}, ELEMENT_GREEN |ELEMENT_LEFT, -100, 0, 0}, {ELEMENT_ANY, {3000 - 200, 10 + 280 * 1}, ELEMENT_GREEN | ELEMENT_RIGHT, -100, 0, 0}, @@ -155,6 +155,19 @@ element_init () element_set (i, e); } } + + /* Negative bonus for the other green zone at start. */ + /* Do not touch last green emplacement. */ + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++) + { + element_t e = element_get (i); + if (!((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))) + { + e.bonus_load *= -1; + element_set (i, e); + } + } } int32_t @@ -275,33 +288,6 @@ element_score (position_t robot_pos, uint8_t element_id) else if (e.type & ELEMENT_NONE) score /= 2; - /* Big score for our green zone. */ - if (e.type != ELEMENT_NONE && (e.attr & ELEMENT_GREEN)) - { - /* In our zone. */ - if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || - (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) - score *= 150; - /* In the other zone, boost score if our green zone is empty. */ - else - { - element_t el; - int cpt = 0; - u8 i; - /* Is our green zone empty ? */ - for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END; i++) - { - el = element_get (i); - if (((team_color == TEAM_COLOR_LEFT && (el.attr & ELEMENT_LEFT)) || - (team_color == TEAM_COLOR_RIGHT && (el.attr & ELEMENT_RIGHT))) - && el.type == ELEMENT_NONE) - cpt++; - } - if (cpt == 5) - score *= 150; - } - } - /* Add score modifier. */ score += e.bonus_load * ELEMENT_BONUS_COEFF; -- cgit v1.2.3 From d1e8794ecc6f837ee84e3a2a50bcca1ab3eaa565 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Fri, 3 Jun 2011 15:25:58 +0200 Subject: digital/io-hub: i like green (and tests) --- digital/io-hub/src/robospierre/element.c | 26 ++++++++++++++++++++++++-- digital/io-hub/src/robospierre/element.h | 4 ++++ digital/io-hub/src/robospierre/test_element.c | 27 +++++++++++++++++++++++++++ 3 files changed, 55 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index ae84bd20..18a3e4bd 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -164,8 +164,11 @@ element_init () if (!((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT)))) { - e.bonus_load *= -1; - element_set (i, e); + if (e.bonus_load > 0) + { + e.bonus_load *= -1; + element_set (i, e); + } } } } @@ -771,3 +774,22 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab) } return 0; } + +void +element_i_like_green () +{ + /* Negative bonus for the other green zone at start. */ + /* Do not touch last green emplacement. */ + int i; + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++) + { + element_t e = element_get (i); + if (!((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + && e.bonus_load < 0) + { + e.bonus_load = 10; + element_set (i, e); + } + } +} diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 8366907b..9464c1f9 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -201,4 +201,8 @@ element_blocking (uint8_t element_id); uint8_t element_blocking_path (vect_t a, vect_t b, int16_t ab); +/** Ask to adjust score for the opposed green zone. */ +void +element_i_like_green (); + #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/test_element.c b/digital/io-hub/src/robospierre/test_element.c index 8c93e876..e200902a 100644 --- a/digital/io-hub/src/robospierre/test_element.c +++ b/digital/io-hub/src/robospierre/test_element.c @@ -32,6 +32,8 @@ #define TEST_PRINT_TYPE_SCORE_PICK 0 #define TEST_PRINT_TYPE_SCORE_UNLOAD 1 #define TEST_PRINT_TYPE_ELEMENT 2 +#define TEST_PRINT_TYPE_BONUS_UNLOAD 3 +#define TEST_PRINT_TYPE_BONUS_LOAD 4 int test_print_type_ = TEST_PRINT_TYPE_SCORE_PICK; position_t test_robot_pos_ = {{0, 2100}, 1}; @@ -189,6 +191,16 @@ test_element_get_score (uint8_t element_id) return (long int) element_score (test_robot_pos_, element_id); else if (test_print_type_ == TEST_PRINT_TYPE_SCORE_UNLOAD) return (long int) element_unload_score (test_robot_pos_, element_id); + else if (test_print_type_ == TEST_PRINT_TYPE_BONUS_LOAD) + { + element_t e = element_get (element_id); + return (long int) e.bonus_load; + } + else if (test_print_type_ == TEST_PRINT_TYPE_BONUS_UNLOAD) + { + element_t e = element_get (element_id); + return (long int) e.bonus_unload; + } else return (long int) element_table[element_id].type; } @@ -253,7 +265,9 @@ int main () element_init (); printf ("\ncommands:\n"); printf ("s: print scores, "); + printf ("a: print load bonus, "); printf ("p: print unload scores, "); + printf ("z: print unload bonus, "); printf ("e: print elements, "); printf ("t: set match time, "); printf ("c: set robot side, "); @@ -262,6 +276,7 @@ int main () printf ("n: indicate there is no element, "); printf ("m: id of nearest element, "); printf ("g: get position and angle of desired element,"); + printf ("u: call I like green,"); printf ("q: quit\n"); printf ("your choice: "); fflush (stdin); @@ -272,10 +287,18 @@ int main () test_print_type_ = TEST_PRINT_TYPE_SCORE_PICK; test_element_print_table (); break; + case 'a': + test_print_type_ = TEST_PRINT_TYPE_BONUS_LOAD; + test_element_print_table (); + break; case 'p': test_print_type_ = TEST_PRINT_TYPE_SCORE_UNLOAD; test_element_print_table (); break; + case 'z': + test_print_type_ = TEST_PRINT_TYPE_BONUS_UNLOAD; + test_element_print_table (); + break; case 'e': test_print_type_ = TEST_PRINT_TYPE_ELEMENT; test_element_print_table (); @@ -337,6 +360,10 @@ int main () vect_t pos = element_get_pos (x); printf ("x: %u y: %u\n", pos.x, pos.y); break; + case 'u': + printf ("call I like green ..."); + element_i_like_green (); + break; case 'q': exit = 1; break; -- cgit v1.2.3 From c5a6dd43df4a0557a1d624dabd9bccd46ba86f34 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 13:42:03 +0200 Subject: digital/io-hub: I like green when strat switch selected --- digital/io-hub/src/robospierre/top.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 595371f7..7ed145a2 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -23,6 +23,7 @@ * * }}} */ #include "common.h" +#include "io.h" #include "playground_2011.h" #include "asserv.h" @@ -34,6 +35,7 @@ #include "move.h" #include "chrono.h" #include "pawn_sensor.h" +#include "contact.h" /* * Here is the top FSM. This FSM is suppose to give life to the robot with an @@ -398,6 +400,8 @@ FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { if (ctx.target_element_id != 0xff) element_down (ctx.target_element_id, ELEMENT_TOWER); + if (!IO_GET (CONTACT_STRAT)) + element_i_like_green (); asserv_move_linearly (logistic_global.collect_direction == DIRECTION_FORWARD ? 150 : -150); return FSM_NEXT (TOP_DROP_DROPPING, clamp_drop_waiting); -- cgit v1.2.3 From 1f04178d259c3b595cb59b24bd8948250ca1c236 Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Fri, 3 Jun 2011 15:41:37 +0200 Subject: digital/io-hub: remove score from our green zone at first unload --- digital/io-hub/src/robospierre/element.c | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 18a3e4bd..b3c8cfb6 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -660,6 +660,19 @@ element_down (uint8_t element_id, uint8_t element_type) element_t e = element_get (element_id); e.type = element_type; element_set (element_id, e); + + /* Remove our green zone score at first unload. */ + uint8_t i; + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++) + { + element_t e = element_get (i); + if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + { + e.bonus_load = 0; + element_set (i, e); + } + } } void -- cgit v1.2.3 From bcff628479cb60e0c0bf8f7091f128b3a4cb3d83 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 14:06:57 +0200 Subject: digital/io-hub: remove bonus on center top area --- digital/io-hub/src/robospierre/element.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index b3c8cfb6..0598b6a5 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -93,8 +93,8 @@ struct element_t element_table[] = */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* Top left blue */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 10, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left red */ -- cgit v1.2.3 From c77c8aef56a120af8bea74186a20db9a4f05bbd3 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 14:07:20 +0200 Subject: digital/io-hub: add larger bonus when I like green --- digital/io-hub/src/robospierre/element.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 0598b6a5..5b192e4f 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -801,7 +801,7 @@ element_i_like_green () (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) && e.bonus_load < 0) { - e.bonus_load = 10; + e.bonus_load = 40; element_set (i, e); } } -- cgit v1.2.3 From e41b10c7d7ddeb9560848b63859b58614d1362ef Mon Sep 17 00:00:00 2001 From: Jérôme Jutteau Date: Fri, 3 Jun 2011 14:21:41 +0200 Subject: digital/io-hub: raise king score --- digital/io-hub/src/robospierre/element.c | 14 +++++++------- digital/io-hub/src/robospierre/element.h | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 5b192e4f..37efa46f 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -280,15 +280,15 @@ element_score (position_t robot_pos, uint8_t element_id) && e.failure_until_s + failure_offset_s < chrono_remaining_time () / 1000) return -1; - if (e.type & ELEMENT_PAWN) - score += ELEMENT_PAWN_SCORE; - if (e.type & ELEMENT_QUEEN) - score += ELEMENT_QUEEN_SCORE; - if (e.type & ELEMENT_KING) + if (e.type == ELEMENT_KING) score += ELEMENT_KING_SCORE; - if (e.type & ELEMENT_ANY) + else if (e.type == ELEMENT_QUEEN) + score += ELEMENT_QUEEN_SCORE; + else if (e.type == ELEMENT_PAWN) + score += ELEMENT_PAWN; + else if (e.type == ELEMENT_ANY) score += ELEMENT_ANY_SCORE; - else if (e.type & ELEMENT_NONE) + if (e.type & ELEMENT_NONE) score /= 2; /* Add score modifier. */ diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 9464c1f9..5b6ad9a4 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -57,8 +57,8 @@ #define ELEMENT_PAWN_SCORE 10 #define ELEMENT_ANY_SCORE 15 -#define ELEMENT_QUEEN_SCORE 20 -#define ELEMENT_KING_SCORE 30 +#define ELEMENT_QUEEN_SCORE 20 * 30 +#define ELEMENT_KING_SCORE 30 * 30 /** Emplacement attributes bits. */ #define ELEMENT_BONUS 1 -- cgit v1.2.3 From 53483bc33fd092d70be211fcaba1db5372e1706d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 16:35:00 +0200 Subject: digital/io-hub: fix bug in head detection workaround --- digital/io-hub/src/robospierre/clamp.c | 25 +++++++++++++++++++++---- digital/io-hub/src/robospierre/logistic.c | 2 +- host/simu/robots/robospierre/model/clamp.py | 2 +- 3 files changed, 23 insertions(+), 6 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index f91af6a0..a74c643c 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -161,6 +161,8 @@ struct clamp_t uint8_t open; /** True if clamp position is controled. */ uint8_t controled; + /** Contact state at start of move, for head check. */ + uint8_t contact_head_before_move; }; /** Global context. */ @@ -212,6 +214,9 @@ clamp_openclose (uint8_t open); static void clamp_route (void); +static void +clamp_head_check_prepare (uint8_t from); + void clamp_init (void) { @@ -243,6 +248,7 @@ clamp_move_element (uint8_t from, uint8_t to) assert (from != to); ctx.pos_request = from; ctx.moving_to = to; + clamp_head_check_prepare (from); FSM_HANDLE (AI, clamp_move); } @@ -435,11 +441,11 @@ clamp_taken_pawn (uint8_t element_type) /* When lifting an element, we can discover it is actually a head. In this * case, change destination. */ -void -clamp_change (void) +static void +clamp_head_check (void) { uint8_t from = logistic_global.moving_from; - if (logistic_global.slots[from] == ELEMENT_PAWN) + if (!ctx.contact_head_before_move && logistic_global.slots[from] == ELEMENT_PAWN) { /* Look head contact. */ uint8_t contact_head = 0; @@ -462,6 +468,17 @@ clamp_change (void) } } +/** Prepare head check, if contact is set yet, this is not a new head. */ +static void +clamp_head_check_prepare (uint8_t from) +{ + ctx.contact_head_before_move = 0; + if (CLAMP_IS_SLOT_IN_FRONT_BAY (from)) + ctx.contact_head_before_move = !IO_GET (CONTACT_FRONT_TOP); + else if (CLAMP_IS_SLOT_IN_BACK_BAY (from)) + ctx.contact_head_before_move = !IO_GET (CONTACT_BACK_TOP); +} + static void clamp_blocked (void) { @@ -909,7 +926,7 @@ FSM_TRANS (CLAMP_MOVE_DST_ROUTING, clamp_elevation_rotation_success, done_open_clamp, CLAMP_MOVE_DST_CLAMP_OPENING, next, CLAMP_MOVE_DST_ROUTING) { - clamp_change (); + clamp_head_check (); if (ctx.pos_current == ctx.pos_request) { if (clamp_slot_door[ctx.pos_current] != 0xff) diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index f59f2da4..f9b13427 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -544,7 +544,7 @@ logistic_element_change (uint8_t pos, uint8_t element_type) void logistic_element_move_done (void) { - assert (!ctx.slots[ctx.moving_to]); + assert (!ctx.slots[ctx.moving_to] || ctx.moving_to == ctx.moving_from); ctx.slots[ctx.moving_to] = ctx.slots[ctx.moving_from]; ctx.slots[ctx.moving_from] = 0; ctx.moving_from = ctx.moving_to = CLAMP_SLOT_NB; diff --git a/host/simu/robots/robospierre/model/clamp.py b/host/simu/robots/robospierre/model/clamp.py index d32edf25..4bbe988f 100644 --- a/host/simu/robots/robospierre/model/clamp.py +++ b/host/simu/robots/robospierre/model/clamp.py @@ -206,7 +206,7 @@ class Clamp (Observable): slots[1].pawn is not None or (slots[0].pawn is not None and slots[0].pawn.kind == 'tower')) - slots[2].contact.state = True + slots[2].contact.state = not (slots[2] is not None) if slots[0].pawn: slots[0].codebar.element_type = slots[0].pawn.kind else: -- cgit v1.2.3 From 5c915c07f8440097109763848d4fc0e06ea8078d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 3 Jun 2011 16:47:04 +0200 Subject: digital/io-hub: fix undroppable states --- digital/io-hub/src/robospierre/top.c | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7ed145a2..3313a908 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -248,8 +248,10 @@ FSM_TRANS (TOP_GOING_TO_DROP, move_success, { if (logistic_global.ready) { - clamp_drop (logistic_global.collect_direction); - return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready); + if (clamp_drop (logistic_global.collect_direction)) + return FSM_NEXT (TOP_GOING_TO_DROP, move_success, ready); + else + return FSM_NEXT (TOP_GOING_TO_DROP, move_success, wait_clamp); } else { @@ -385,10 +387,14 @@ FSM_TRANS (TOP_UNBLOCKING_SHAKE, robot_move_failure, return FSM_NEXT (TOP_UNBLOCKING_SHAKE, robot_move_failure); } -FSM_TRANS (TOP_WAITING_READY, clamp_done, TOP_DROP_DROPPING) +FSM_TRANS (TOP_WAITING_READY, clamp_done, + drop, TOP_DROP_DROPPING, + not_ready, TOP_WAITING_READY) { - clamp_drop (logistic_global.collect_direction); - return FSM_NEXT (TOP_WAITING_READY, clamp_done); + if (clamp_drop (logistic_global.collect_direction)) + return FSM_NEXT (TOP_WAITING_READY, clamp_done, drop); + else + return FSM_NEXT (TOP_WAITING_READY, clamp_done, not_ready); } FSM_TRANS (TOP_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) -- cgit v1.2.3 From 0ee500d39cf65c776b02d26bd44d7c0e8fcfb8b7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 2 Jun 2011 13:38:25 +0200 Subject: digital/io-hub: handle tower detection --- digital/io-hub/src/robospierre/clamp.c | 36 ++++++++++++++++++++++++++++++- digital/io-hub/src/robospierre/logistic.c | 22 +++++++++++++------ 2 files changed, 51 insertions(+), 7 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index a74c643c..2340b54f 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -479,6 +479,33 @@ clamp_head_check_prepare (uint8_t from) ctx.contact_head_before_move = !IO_GET (CONTACT_BACK_TOP); } +/* When clamp moved to bottom slot, we can discover it is actually a tower. + * In this case, stop movement. */ +static uint8_t +clamp_tower_check (void) +{ + uint8_t from = logistic_global.moving_from; + if ((from == CLAMP_SLOT_FRONT_BOTTOM || from == CLAMP_SLOT_BACK_BOTTOM) + && ctx.pos_current == from + && logistic_global.slots[from] == ELEMENT_PAWN) + { + /* Look tower contact. */ + uint8_t contact_tower; + if (from == CLAMP_SLOT_FRONT_BOTTOM) + contact_tower = !IO_GET (CONTACT_FRONT_MIDDLE); + else + contact_tower = !IO_GET (CONTACT_BACK_MIDDLE); + /* Change? */ + if (contact_tower) + { + logistic_element_change (from, ELEMENT_TOWER); + clamp_taken_pawn (ELEMENT_TOWER); + return 1; + } + } + return 0; +} + static void clamp_blocked (void) { @@ -869,10 +896,17 @@ FSM_TRANS (CLAMP_MOVE_ROUTING, clamp_elevation_or_rotation_failure, } FSM_TRANS (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, + cancel, CLAMP_MOVE_IDLE, done, CLAMP_MOVE_SRC_CLAMP_CLOSING, next, CLAMP_MOVE_SRC_ROUTING) { - if (ctx.pos_current == ctx.pos_request) + if (clamp_tower_check ()) + { + fsm_queue_post_event (FSM_EVENT (AI, clamp_move_success)); + return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, clamp_elevation_rotation_success, + cancel); + } + else if (ctx.pos_current == ctx.pos_request) { clamp_openclose (0); return FSM_NEXT (CLAMP_MOVE_SRC_ROUTING, diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index f9b13427..629ee0c1 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -574,12 +574,22 @@ logistic_dump (void) static uint8_t logistic_slot_clear (uint8_t slot) { - if (CLAMP_IS_SLOT_IN_FRONT_BAY (slot) - && ctx.slots[CLAMP_SLOT_FRONT_MIDDLE]) - return 0; - if (CLAMP_IS_SLOT_IN_BACK_BAY (slot) - && ctx.slots[CLAMP_SLOT_BACK_MIDDLE]) - return 0; + if (CLAMP_IS_SLOT_IN_FRONT_BAY (slot)) + { + if (ctx.slots[CLAMP_SLOT_FRONT_MIDDLE]) + return 0; + uint8_t middle_type = ctx.slots[CLAMP_SLOT_FRONT_BOTTOM]; + if (ELEMENT_IS_HEAD (middle_type) || middle_type == ELEMENT_TOWER) + return 0; + } + else if (CLAMP_IS_SLOT_IN_BACK_BAY (slot)) + { + if (ctx.slots[CLAMP_SLOT_BACK_MIDDLE]) + return 0; + uint8_t middle_type = ctx.slots[CLAMP_SLOT_BACK_BOTTOM]; + if (ELEMENT_IS_HEAD (middle_type) || middle_type == ELEMENT_TOWER) + return 0; + } return 1; } -- cgit v1.2.3 From eabb56da8bd0004fb665fc2838e3db62c780e72b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 00:21:27 +0200 Subject: digital/io-hub: factorize clamp functions --- digital/io-hub/src/robospierre/clamp.c | 200 ++++++++++++++------------------- 1 file changed, 82 insertions(+), 118 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 2340b54f..e385d876 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -522,6 +522,45 @@ clamp_blocked (void) fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure)); } +#define CLAMP_DECISION_MOVE_ELEMENT 0 +#define CLAMP_DECISION_MOVE_TO_IDLE 1 +#define CLAMP_DECISION_CLAMP_LOCKED 2 +#define CLAMP_DECISION_DONE 3 + +static uint8_t +clamp_decision (uint8_t unblock) +{ + if (!unblock && logistic_global.moving_from != CLAMP_SLOT_NB) + { + clamp_move_element (logistic_global.moving_from, + logistic_global.moving_to); + return CLAMP_DECISION_MOVE_ELEMENT; + } + else if (logistic_global.prepare != 3 + && (unblock + || logistic_global.clamp_pos_idle != ctx.pos_current)) + { + if (logistic_path_clear (ctx.pos_current, + logistic_global.clamp_pos_idle)) + { + clamp_move (logistic_global.clamp_pos_idle); + return CLAMP_DECISION_MOVE_TO_IDLE; + } + else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + return CLAMP_DECISION_CLAMP_LOCKED; + } + } + else + { + ctx.working = 0; + fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + return CLAMP_DECISION_DONE; + } +} + /* CLAMP FSM */ FSM_TRANS (CLAMP_START, init_actuators, CLAMP_INIT_OPENING) @@ -623,32 +662,16 @@ FSM_TRANS (CLAMP_IDLE, clamp_prepare, done, CLAMP_IDLE) { logistic_decision (); - if (logistic_global.moving_from != CLAMP_SLOT_NB) + switch (clamp_decision (0)) { - clamp_move_element (logistic_global.moving_from, - logistic_global.moving_to); + default: + case CLAMP_DECISION_MOVE_ELEMENT: return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_element); - } - else if (logistic_global.prepare != 3 - && logistic_global.clamp_pos_idle != ctx.pos_current) - { - if (logistic_path_clear (ctx.pos_current, - logistic_global.clamp_pos_idle)) - { - clamp_move (logistic_global.clamp_pos_idle); - return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_to_idle); - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); - return FSM_NEXT (CLAMP_IDLE, clamp_prepare, clamp_locked); - } - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + case CLAMP_DECISION_MOVE_TO_IDLE: + return FSM_NEXT (CLAMP_IDLE, clamp_prepare, move_to_idle); + case CLAMP_DECISION_CLAMP_LOCKED: + return FSM_NEXT (CLAMP_IDLE, clamp_prepare, clamp_locked); + case CLAMP_DECISION_DONE: return FSM_NEXT (CLAMP_IDLE, clamp_prepare, done); } } @@ -671,32 +694,16 @@ FSM_TRANS_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, BOT_PWM_DOOR_CLOSE_TIME, { logistic_element_new (ctx.pos_new, ctx.new_element_type); clamp_taken_pawn (ctx.new_element_type); - if (logistic_global.moving_from != CLAMP_SLOT_NB) + switch (clamp_decision (0)) { - clamp_move_element (logistic_global.moving_from, - logistic_global.moving_to); + default: + case CLAMP_DECISION_MOVE_ELEMENT: return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_element); - } - else if (logistic_global.prepare != 3 - && logistic_global.clamp_pos_idle != ctx.pos_current) - { - if (logistic_path_clear (ctx.pos_current, - logistic_global.clamp_pos_idle)) - { - clamp_move (logistic_global.clamp_pos_idle); - return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle); - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); - return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked); - } - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + case CLAMP_DECISION_MOVE_TO_IDLE: + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, move_to_idle); + case CLAMP_DECISION_CLAMP_LOCKED: + return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, clamp_locked); + case CLAMP_DECISION_DONE: return FSM_NEXT_TIMEOUT (CLAMP_TAKING_DOOR_CLOSING, done); } } @@ -708,35 +715,19 @@ FSM_TRANS (CLAMP_MOVING_ELEMENT, clamp_move_success, done, CLAMP_IDLE) { logistic_element_move_done (); - if (logistic_global.moving_from != CLAMP_SLOT_NB) + switch (clamp_decision (0)) { - clamp_move_element (logistic_global.moving_from, - logistic_global.moving_to); + default: + case CLAMP_DECISION_MOVE_ELEMENT: return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, move_element); - } - else if (logistic_global.prepare != 3 - && logistic_global.clamp_pos_idle != ctx.pos_current) - { - if (logistic_path_clear (ctx.pos_current, - logistic_global.clamp_pos_idle)) - { - clamp_move (logistic_global.clamp_pos_idle); - return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, - move_to_idle); - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); - return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, - clamp_locked); - } - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + case CLAMP_DECISION_MOVE_TO_IDLE: + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + move_to_idle); + case CLAMP_DECISION_CLAMP_LOCKED: + return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, + clamp_locked); + case CLAMP_DECISION_DONE: return FSM_NEXT (CLAMP_MOVING_ELEMENT, clamp_move_success, done); } @@ -762,35 +753,19 @@ FSM_TRANS (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, done, CLAMP_IDLE) { logistic_drop (ctx.drop_direction); - if (logistic_global.moving_from != CLAMP_SLOT_NB) + switch (clamp_decision (0)) { - clamp_move_element (logistic_global.moving_from, - logistic_global.moving_to); + default: + case CLAMP_DECISION_MOVE_ELEMENT: return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, move_element); - } - else if (logistic_global.prepare != 3 - && logistic_global.clamp_pos_idle != ctx.pos_current) - { - if (logistic_path_clear (ctx.pos_current, - logistic_global.clamp_pos_idle)) - { - clamp_move (logistic_global.clamp_pos_idle); - return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, - move_to_idle); - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); - return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, - clamp_locked); - } - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + case CLAMP_DECISION_MOVE_TO_IDLE: + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + move_to_idle); + case CLAMP_DECISION_CLAMP_LOCKED: + return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, + clamp_locked); + case CLAMP_DECISION_DONE: return FSM_NEXT (CLAMP_DROPING_WAITING_ROBOT, clamp_drop_clear, done); } @@ -820,25 +795,14 @@ FSM_TRANS (CLAMP_BLOCKED, clamp_prepare, clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { - if (logistic_global.prepare != 3) + switch (clamp_decision (1)) { - if (logistic_path_clear (ctx.pos_current, - logistic_global.clamp_pos_idle)) - { - clamp_move (logistic_global.clamp_pos_idle); - return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle); - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); - return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked); - } - } - else - { - ctx.working = 0; - fsm_queue_post_event (FSM_EVENT (AI, clamp_done)); + default: + case CLAMP_DECISION_MOVE_TO_IDLE: + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle); + case CLAMP_DECISION_CLAMP_LOCKED: + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked); + case CLAMP_DECISION_DONE: return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, done); } } -- cgit v1.2.3 From cff3a91df81737d5f9ce2173375b719ff21d0969 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 00:41:24 +0200 Subject: digital/io-hub: drop the right element type --- digital/io-hub/src/robospierre/element.c | 4 ++-- digital/io-hub/src/robospierre/logistic.c | 19 +++++++++++++++++++ digital/io-hub/src/robospierre/logistic.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 4 +++- 4 files changed, 28 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 37efa46f..4c669c1c 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -742,7 +742,7 @@ uint8_t element_blocking (uint8_t element_id) { element_t e = element_get (element_id); - return e.type == ELEMENT_TOWER; + return e.type == ELEMENT_TOWER || e.type == ELEMENT_PAWN; } uint8_t @@ -754,7 +754,7 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab) for (i = 0; i < UTILS_COUNT (element_table); i++) { e = element_get (i); - if (e.type == ELEMENT_TOWER) + if (e.type == ELEMENT_TOWER || e.type == ELEMENT_PAWN) { /* Compute square of distance to obstacle, see * distance_segment_point in modules/math/geometry for the method diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index 629ee0c1..db0c8b94 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -562,6 +562,25 @@ logistic_drop (uint8_t direction) logistic_decision (); } +uint8_t +logistic_drop_element_type (uint8_t direction) +{ + uint8_t bay = direction == DIRECTION_FORWARD + ? CLAMP_SLOT_BACK_BOTTOM : CLAMP_SLOT_FRONT_BOTTOM; + uint8_t nb = 0; + uint8_t element_type = ELEMENT_NONE; + uint8_t i; + for (i = bay; i < bay + 3; i++) + { + if (ctx.slots[i]) + { + nb++; + element_type = ctx.slots[i]; + } + } + return nb > 1 ? ELEMENT_TOWER : element_type; +} + void logistic_dump (void) { diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 4a06dcb7..0a076477 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -128,6 +128,10 @@ logistic_element_move_done (void); void logistic_drop (uint8_t direction); +/** Get element type to be dropped. */ +uint8_t +logistic_drop_element_type (uint8_t direction); + /** Dump every element. */ void logistic_dump (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 3313a908..f1f14cdd 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -405,7 +405,9 @@ FSM_TRANS (TOP_WAITING_READY, clamp_blocked, TOP_UNBLOCKING_SHAKE_WAIT) FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) { if (ctx.target_element_id != 0xff) - element_down (ctx.target_element_id, ELEMENT_TOWER); + element_down (ctx.target_element_id, + logistic_drop_element_type + (logistic_global.collect_direction)); if (!IO_GET (CONTACT_STRAT)) element_i_like_green (); asserv_move_linearly (logistic_global.collect_direction -- cgit v1.2.3 From aad98e1b215d1e4d8513964904fc6fbeea7976ae Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 00:45:42 +0200 Subject: digital/io-hub: fix missing bonus flag --- digital/io-hub/src/robospierre/element.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 4c669c1c..e0e566a8 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -98,7 +98,7 @@ struct element_t element_table[] = {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 5 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 2nd line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 10, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 10, 0}, /* bonus */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 4 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 10, 0}, /* bonus */ -- cgit v1.2.3 From 13776d2ecc35e6470bc159c7065a0cdf0e7db588 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 00:52:51 +0200 Subject: digital/io-hub: handle path to protected zone --- digital/io-hub/src/robospierre/path.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index 4f871213..7da57ec6 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -252,6 +252,13 @@ path_blocking (uint8_t a, uint8_t b, int16_t *dp) return 1; if (a_green || b_green) factor = 4; + /* Test for protected zone. */ + if (va.y <= 350 && va.x > PG_WIDTH / 2 - 350 && va.y < PG_WIDTH / 2 + 350 + && (vb.x < PG_WIDTH / 2 - 350 || vb.x > PG_WIDTH / 2 + 350)) + return 1; + if (vb.y <= 350 && vb.x > PG_WIDTH / 2 - 350 && vb.y < PG_WIDTH / 2 + 350 + && (va.x < PG_WIDTH / 2 - 350 || va.x > PG_WIDTH / 2 + 350)) + return 1; /* Test for a blocking obstacle. */ for (i = 0; i < PATH_OBSTACLES_NB && !blocking; i++) { -- cgit v1.2.3 From 9a7714630675392c35aabaaee01e0d83b69230b7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 01:05:07 +0200 Subject: digital/io-hub: add score to bonus squares, handle protected squares --- digital/io-hub/src/robospierre/element.c | 8 ++++---- digital/io-hub/src/robospierre/top.c | 9 ++++++++- 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index e0e566a8..3c73a72c 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -110,10 +110,10 @@ struct element_t element_table[] = {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 3 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, /* 4th line left red */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 4, 0}, /* bonus */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 0 * 350 - 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 + 0 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 0, 0}, /* bonus */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 1 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 4, 0}, /* bonus */ {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 2 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, {ELEMENT_NONE | ELEMENT_ANY, {1500 - 2 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_RIGHT, 0, 0, 0}, /* 5th line left blue */ {ELEMENT_NONE | ELEMENT_ANY, {1500 - 1 * 350 - 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, @@ -123,8 +123,8 @@ struct element_t element_table[] = {ELEMENT_NONE | ELEMENT_ANY, {1500 + 2 * 350 + 175, 1 * 350 + 175}, ELEMENT_CENTER | ELEMENT_LEFT, 0, 0, 0}, {ELEMENT_NONE, {1500 - 2 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100, 0}, /* left red */ {ELEMENT_NONE, {1500 - 1 * 350 - 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100, 0}, /* left blue */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, -40, 0}, /* middle bonus left, red. */ - {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, -40, 0}, /* middle bonus right, blue. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 - 175, 175}, ELEMENT_CENTER | ELEMENT_LEFT | ELEMENT_BONUS, 0, 9, 0}, /* middle bonus left, red. */ + {ELEMENT_NONE | ELEMENT_ANY, {1500 + 175, 175}, ELEMENT_CENTER | ELEMENT_RIGHT | ELEMENT_BONUS, 0, 9, 0}, /* middle bonus right, blue. */ {ELEMENT_NONE, {1500 + 1 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_LEFT, 0, -100, 0}, /* right red */ {ELEMENT_NONE, {1500 + 2 * 350 + 175, 175}, ELEMENT_CENTER | ELEMENT_SAFE | ELEMENT_RIGHT, 0, -100, 0} /* right blue */ }; diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index f1f14cdd..7e3f4413 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -175,7 +175,14 @@ top_go_drop (void) uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; /* Go above or below the drop point. */ - if (drop_pos.v.y > PG_LENGTH / 2) + if (drop_pos.v.y < 350) + { + /* Protected zone. */ + drop_pos.v.x = PG_WIDTH / 2; + drop_pos.v.y = 350; + drop_pos.a = PG_A_DEG (45); + } + else if (drop_pos.v.y > PG_LENGTH / 2) { drop_pos.v.y -= 350 / 2; drop_pos.a = PG_A_DEG (-90); -- cgit v1.2.3 From e4f860bb81377ef7a6a60c0ccd55e3688eb5af94 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 01:09:26 +0200 Subject: digital/io-hub: fix failure code There is still a bug (when 0), but I do not care that much. --- digital/io-hub/src/robospierre/element.c | 4 ++-- digital/io-hub/src/robospierre/element.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 3c73a72c..35bf73f9 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -198,7 +198,7 @@ element_unload_score (position_t robot_pos, uint8_t element_id) /* Failed element. */ if (e.failure_until_s - && e.failure_until_s + failure_offset_s < chrono_remaining_time () / 1000) + && e.failure_until_s + failure_offset_s < (int) (chrono_remaining_time () / 1000)) return -1; /* Bonus adjust. */ @@ -277,7 +277,7 @@ element_score (position_t robot_pos, uint8_t element_id) /* Failed element. */ if (e.failure_until_s - && e.failure_until_s + failure_offset_s < chrono_remaining_time () / 1000) + && e.failure_until_s + failure_offset_s < (int) (chrono_remaining_time () / 1000)) return -1; if (e.type == ELEMENT_KING) diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 5b6ad9a4..f9420cdb 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -96,7 +96,7 @@ struct element_t int8_t bonus_unload; /** Failure expiration date, can not take this element until chrono is * lower than this date in seconds. */ - uint8_t failure_until_s; + int8_t failure_until_s; }; typedef struct element_t element_t; -- cgit v1.2.3 From 618872c7705a7f71afe77cd13b694a07660ebcb8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 02:48:28 +0200 Subject: digital/io-hub: conditionaly enable bumpers --- digital/io-hub/src/robospierre/pawn_sensor.c | 27 +++++++++++++++++++-------- digital/io-hub/src/robospierre/pawn_sensor.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 7 +++++++ 3 files changed, 30 insertions(+), 8 deletions(-) diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index 5ca18759..cc9e6900 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -59,6 +59,8 @@ struct pawn_sensor_t /** Pawn sensor general context. */ struct pawn_sensor_general_t { + /** Activate bumpers. */ + uint8_t bumper_enabled; /** Last bumped element position. */ vect_t last_bumped; /** Bumper triggered, wait until the next one. */ @@ -191,18 +193,27 @@ pawn_sensor_update (void) #define BUMPER_FRONT_RIGHT _BV (7) #define BUMPER_BACK_RIGHT _BV (5) #define BUMPER_BACK_LEFT _BV (4) - if (pawn_sensor_global.bump_wait) - pawn_sensor_global.bump_wait--; - else + if (pawn_sensor_global.bumper_enabled) { - uint8_t bumpers = mimot_get_input (); - pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265); - pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265); - pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265); - pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265); + if (pawn_sensor_global.bump_wait) + pawn_sensor_global.bump_wait--; + else + { + uint8_t bumpers = mimot_get_input (); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_LEFT), 120, 265); + pawn_sensor_bumper (!(bumpers & BUMPER_FRONT_RIGHT), 120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_RIGHT), -120, -265); + pawn_sensor_bumper (!(bumpers & BUMPER_BACK_LEFT), -120, 265); + } } } +void +pawn_sensor_bumper_enable (uint8_t enabled) +{ + pawn_sensor_global.bumper_enabled = enabled; +} + vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 04ea1a2f..8d6d2651 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -34,6 +34,10 @@ pawn_sensor_get (uint8_t direction); void pawn_sensor_update (void); +/** Enable bumpers. */ +void +pawn_sensor_bumper_enable (uint8_t enabled); + /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7e3f4413..f4afaa17 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -153,9 +153,15 @@ top_go_element (void) if (!ctx.broken) { if (e.attr & ELEMENT_GREEN) + { logistic_global.prepare = 0; + pawn_sensor_bumper_enable (0); + } else + { logistic_global.prepare = top_prepare_level (); + pawn_sensor_bumper_enable (1); + } } vect_t element_pos = element_get_pos (ctx.target_element_id); top_go_this_element (element_pos, 0); @@ -172,6 +178,7 @@ top_go_drop (void) drop_pos.v = element_get_pos (ctx.target_element_id); if (!ctx.broken) logistic_global.prepare = top_prepare_level (); + pawn_sensor_bumper_enable (0); uint8_t backward = logistic_global.collect_direction == DIRECTION_FORWARD ? 0 : ASSERV_BACKWARD; /* Go above or below the drop point. */ -- cgit v1.2.3 From 2a2233955465883b27d5da6121377464a3d87be1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 03:02:06 +0200 Subject: digital/io-hub: lower center element bonus --- digital/io-hub/src/robospierre/element.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 35bf73f9..1a4dfdb1 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -68,7 +68,7 @@ struct element_t element_table[] = {ELEMENT_NONE | ELEMENT_PAWN, {1500 + 1 * 350, 1 * 350}, ELEMENT_INTERSEC | ELEMENT_RIGHT, -30, 0, 0}, /* Central pawn. (see ELEMENT_CENTRAL_PAWN) */ - {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 50, 0, 0}, + {ELEMENT_PAWN, {1500, 3 * 350}, ELEMENT_INTERSEC | ELEMENT_CENTER, 30, 0, 0}, /* 10 elements on green zones. -- cgit v1.2.3 From b7167cd72c8f7b2e362ef3ace58388e99bc815b8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 03:14:41 +0200 Subject: digital/io-hub: elements do not block paths --- digital/io-hub/src/robospierre/element.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 1a4dfdb1..a776b764 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -742,7 +742,7 @@ uint8_t element_blocking (uint8_t element_id) { element_t e = element_get (element_id); - return e.type == ELEMENT_TOWER || e.type == ELEMENT_PAWN; + return e.type == ELEMENT_TOWER; } uint8_t @@ -754,7 +754,7 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab) for (i = 0; i < UTILS_COUNT (element_table); i++) { e = element_get (i); - if (e.type == ELEMENT_TOWER || e.type == ELEMENT_PAWN) + if (e.type == ELEMENT_TOWER) { /* Compute square of distance to obstacle, see * distance_segment_point in modules/math/geometry for the method -- cgit v1.2.3 From 61f927014ae986ab1aabec520853beb306d4464a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 03:15:39 +0200 Subject: digital/io-hub: invert strat switch --- digital/io-hub/src/robospierre/top.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index f4afaa17..7732fb50 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -422,7 +422,7 @@ FSM_TRANS (TOP_DROP_DROPPING, clamp_drop_waiting, TOP_DROP_CLEARING) element_down (ctx.target_element_id, logistic_drop_element_type (logistic_global.collect_direction)); - if (!IO_GET (CONTACT_STRAT)) + if (IO_GET (CONTACT_STRAT)) element_i_like_green (); asserv_move_linearly (logistic_global.collect_direction == DIRECTION_FORWARD ? 150 : -150); -- cgit v1.2.3 From d5c9ae0c043887ac7b68f13aa300761fcbb0b008 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 03:37:57 +0200 Subject: digital/io-hub: do not detect tower in green zone --- digital/io-hub/src/robospierre/clamp.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index e385d876..cb6d9e3b 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -30,6 +30,7 @@ #include "contact.h" #include "bot.h" #include "element.h" +#include "playground.h" #define FSM_NAME AI #include "fsm.h" @@ -489,6 +490,11 @@ clamp_tower_check (void) && ctx.pos_current == from && logistic_global.slots[from] == ELEMENT_PAWN) { + /* Check for green zone. */ + position_t robot_pos; + asserv_get_position (&robot_pos); + if (robot_pos.v.x < 450 || robot_pos.v.x > PG_WIDTH - 450) + return 0; /* Look tower contact. */ uint8_t contact_tower; if (from == CLAMP_SLOT_FRONT_BOTTOM) -- cgit v1.2.3 From 7a25755b8ffc842460b42152367161392cb955af Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 03:44:05 +0200 Subject: digital/io-hub: only take heads in opponent green zone --- digital/io-hub/src/robospierre/element.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index a776b764..adc90a3a 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -799,7 +799,8 @@ element_i_like_green () element_t e = element_get (i); if (!((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) - && e.bonus_load < 0) + && e.bonus_load < 0 + && (e.type == ELEMENT_KING || e.type == ELEMENT_QUEEN)) { e.bonus_load = 40; element_set (i, e); -- cgit v1.2.3 From dcac94034fb87608cb113d474abacdafc424ca07 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 4 Jun 2011 09:51:49 +0200 Subject: digital/io-hub: a little bit quicker --- digital/io-hub/src/robospierre/bot.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index fbbeeb7e..3996f10e 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -69,7 +69,7 @@ # define BOT_SPEED_INIT 0x10, 0x10, 0x10, 0x10 #endif /** Normal cruise speed. */ -#define BOT_SPEED_NORMAL 0x40, 0x40, 0x20, 0x20 +#define BOT_SPEED_NORMAL 0x50, 0x60, 0x20, 0x20 #ifdef HOST -- cgit v1.2.3 From 90a89850cbc3e4540543784813a71a7b2ac97dfc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 8 Jun 2011 22:33:05 +0200 Subject: admin/logo: add avatar --- admin/logo/apbteam_logo.svg | 214 +++++++++++++++++++++++++++++++++++++------- 1 file changed, 184 insertions(+), 30 deletions(-) diff --git a/admin/logo/apbteam_logo.svg b/admin/logo/apbteam_logo.svg index a4ebfb2c..ebe64c09 100644 --- a/admin/logo/apbteam_logo.svg +++ b/admin/logo/apbteam_logo.svg @@ -1,8 +1,9 @@ + + inkscape:version="0.47 r22583" + sodipodi:docname="apbteam_logo.svg" + version="1.1"> + + + + + + + + + + + + + + + + + + + + + + + + + + + + inkscape:guide-bbox="true" + inkscape:window-maximized="0"> + + @@ -279,11 +414,11 @@ sodipodi:cy="184.87318" sodipodi:rx="8.9183369" sodipodi:ry="18.559782" - d="M 151.37068 184.87318 A 8.9183369 18.559782 0 1 1 133.53401,184.87318 A 8.9183369 18.559782 0 1 1 151.37068 184.87318 z" + d="m 151.37068,184.87318 c 0,10.25029 -3.99287,18.55979 -8.91833,18.55979 -4.92546,0 -8.91834,-8.3095 -8.91834,-18.55979 0,-10.25028 3.99288,-18.55978 8.91834,-18.55978 4.92546,0 8.91833,8.3095 8.91833,18.55978 z" transform="matrix(0.67667,0.62343,0,0.67667,7.96926,-135.9796)" /> + + Avatar -- cgit v1.2.3 From 1ba50389bff517512e417500f4c52ceeafdb06a5 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 13 Jun 2011 00:40:31 +0200 Subject: host/simu: randomize team color --- host/simu/robots/robospierre/model/bag.py | 3 +++ host/simu/view/switch.py | 2 ++ 2 files changed, 5 insertions(+) diff --git a/host/simu/robots/robospierre/model/bag.py b/host/simu/robots/robospierre/model/bag.py index b9d3bc3f..cedb3805 100644 --- a/host/simu/robots/robospierre/model/bag.py +++ b/host/simu/robots/robospierre/model/bag.py @@ -28,11 +28,14 @@ from simu.model.motor_basic import MotorBasic from simu.model.distance_sensor_sensopart import DistanceSensorSensopart from simu.robots.robospierre.model.clamp import Clamp from math import pi +import random class Bag: def __init__ (self, scheduler, table, link_bag): self.color_switch = Switch (link_bag.io_hub.contact[0], invert = True) + self.color_switch.state = random.choice ((False, True)) + self.color_switch.notify () self.jack = Switch (link_bag.io_hub.contact[1], invert = True) self.strat_switch = Switch (link_bag.io_hub.contact[9], invert = True) self.contact = [ Switch (contact) diff --git a/host/simu/view/switch.py b/host/simu/view/switch.py index c0ec2d58..445cbd02 100644 --- a/host/simu/view/switch.py +++ b/host/simu/view/switch.py @@ -28,6 +28,8 @@ class Switch: def __init__ (self, frame, model, text): self.var = IntVar () + if model.state is not None: + self.var.set ((0, 1)[model.state]) self.button = Checkbutton (frame, variable = self.var, command = self.__update, text = text, indicatoron = False) self.button.pack () -- cgit v1.2.3 From a86abce7d1f1567b2f67b42d97334d17fc52fd4d Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 13 Jun 2011 00:41:38 +0200 Subject: digital/io-hub: try to avoid dropped pawn --- digital/io-hub/src/robospierre/element.c | 12 ++++++------ digital/io-hub/src/robospierre/element.h | 5 +++-- digital/io-hub/src/robospierre/path.c | 14 +++++++------- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index adc90a3a..e8ee44e0 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -739,22 +739,22 @@ element_get_pos (uint8_t element_id) } uint8_t -element_blocking (uint8_t element_id) +element_blocking (uint8_t element_id, uint8_t escape) { element_t e = element_get (element_id); - return e.type == ELEMENT_TOWER; + return e.type == ELEMENT_TOWER || (!escape && e.type == ELEMENT_PAWN); } uint8_t -element_blocking_path (vect_t a, vect_t b, int16_t ab) +element_blocking_path (vect_t a, vect_t b, int16_t ab, uint8_t escape) { uint8_t i; element_t e; - /* For each obstacle, try to find an intersection. */ - for (i = 0; i < UTILS_COUNT (element_table); i++) + /* Only unload area are blocking. */ + for (i = ELEMENT_UNLOAD_START; i <= ELEMENT_UNLOAD_END; i++) { e = element_get (i); - if (e.type == ELEMENT_TOWER) + if (e.type == ELEMENT_TOWER || (!escape && e.type == ELEMENT_PAWN)) { /* Compute square of distance to obstacle, see * distance_segment_point in modules/math/geometry for the method diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index f9420cdb..243f5b94 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -191,15 +191,16 @@ element_get (uint8_t element_id) /** Return whether an element is blocking robot. */ uint8_t -element_blocking (uint8_t element_id); +element_blocking (uint8_t element_id, uint8_t blocking); /** Return whether an element is blocking a line segment. * - a: line segment first point. * - b: line segment second point. * - ab: line segment length. + * - escape: trying to escape, be a little bit more permissive. * - returns: 1 if the path should not be used. */ uint8_t -element_blocking_path (vect_t a, vect_t b, int16_t ab); +element_blocking_path (vect_t a, vect_t b, int16_t ab, uint8_t escape); /** Ask to adjust score for the opposed green zone. */ void diff --git a/digital/io-hub/src/robospierre/path.c b/digital/io-hub/src/robospierre/path.c index 7da57ec6..ecdddbf7 100644 --- a/digital/io-hub/src/robospierre/path.c +++ b/digital/io-hub/src/robospierre/path.c @@ -198,23 +198,23 @@ path_pos (uint8_t node, vect_t *pos) } static uint8_t -path_element_blocking (uint8_t node) +path_element_blocking (uint8_t node, uint8_t escape) { vect_t pos; path_pos (node, &pos); int16_t square_x = (pos.x - 450 - 1) / 350; int16_t square_y = (2100 - pos.y - 1) / 350; uint8_t element_id = ELEMENT_UNLOAD_START + square_x + 6 * square_y; - if (element_blocking (element_id)) + if (element_blocking (element_id, escape)) return 1; uint8_t intersection = ((pos.x - 450) / 350) != square_x; if (intersection) { - if (element_blocking (element_id + 1)) + if (element_blocking (element_id + 1, escape)) return 1; - if (element_blocking (element_id + 6)) + if (element_blocking (element_id + 6, escape)) return 1; - if (element_blocking (element_id + 6 + 1)) + if (element_blocking (element_id + 6 + 1, escape)) return 1; } return 0; @@ -278,7 +278,7 @@ path_blocking (uint8_t a, uint8_t b, int16_t *dp) return 0; } /* Test for a blocking element. */ - if (element_blocking_path (va, vb, d)) + if (element_blocking_path (va, vb, d, path.escape_factor)) blocking = 1; /* Handle escaping. */ if (blocking) @@ -306,7 +306,7 @@ path_blocked_update (void) uint8_t valid = 1; /* First, gather information from tables. */ if (!path_nodes[i].usable - || path_element_blocking (i)) + || path_element_blocking (i, path.escape_factor)) valid = 0; else { -- cgit v1.2.3 From e1028794261fd6b829656c15b7bf25f4f09eaf0a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 23 Jun 2011 23:48:53 +0200 Subject: digital/dev2/src: add #error to avoid programming with the bad clock setting --- digital/dev2/src/usb_gpio/avrconfig.h | 1 + digital/dev2/src/usb_serial_isp/avrconfig.h | 1 + digital/dev2/src/usb_twi/avrconfig.h | 1 + 3 files changed, 3 insertions(+) diff --git a/digital/dev2/src/usb_gpio/avrconfig.h b/digital/dev2/src/usb_gpio/avrconfig.h index 0a90876d..eb780e77 100644 --- a/digital/dev2/src/usb_gpio/avrconfig.h +++ b/digital/dev2/src/usb_gpio/avrconfig.h @@ -28,6 +28,7 @@ /* utils */ /** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#error "please check frequency" #define AC_FREQ 8000000 /* usb */ diff --git a/digital/dev2/src/usb_serial_isp/avrconfig.h b/digital/dev2/src/usb_serial_isp/avrconfig.h index 1dfd63d2..60ddf99b 100644 --- a/digital/dev2/src/usb_serial_isp/avrconfig.h +++ b/digital/dev2/src/usb_serial_isp/avrconfig.h @@ -28,6 +28,7 @@ /* utils */ /** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#error "please check frequency" #define AC_FREQ 8000000 /* uart - UART module. */ diff --git a/digital/dev2/src/usb_twi/avrconfig.h b/digital/dev2/src/usb_twi/avrconfig.h index 5c2d92ce..83482f75 100644 --- a/digital/dev2/src/usb_twi/avrconfig.h +++ b/digital/dev2/src/usb_twi/avrconfig.h @@ -28,6 +28,7 @@ /* utils */ /** AVR Frequency : 1000000, 1843200, 2000000, 3686400, 4000000, 7372800, * 8000000, 11059200, 14745600, 16000000, 18432000, 20000000. */ +#error "please check frequency" #define AC_FREQ 8000000 /* twi - TWI module. */ -- cgit v1.2.3 From 0b9ae480b928a24664bfd30edcd17d760db593d1 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 14:43:16 +0200 Subject: digital/io-hub/src/robospierre: disable pawn sensors when tower dropped --- digital/io-hub/src/robospierre/pawn_sensor.c | 6 ++++++ digital/io-hub/src/robospierre/pawn_sensor.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 2 ++ 3 files changed, 12 insertions(+) diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index cc9e6900..faeee879 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -214,6 +214,12 @@ pawn_sensor_bumper_enable (uint8_t enabled) pawn_sensor_global.bumper_enabled = enabled; } +void +pawn_sensor_bumper_wait (uint16_t wait) +{ + pawn_sensor_global.bump_wait = wait; +} + vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 8d6d2651..9c76f77a 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -38,6 +38,10 @@ pawn_sensor_update (void); void pawn_sensor_bumper_enable (uint8_t enabled); +/** Temporarily disable bumpers. */ +void +pawn_sensor_bumper_wait (uint16_t wait); + /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7732fb50..3d39f60a 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -434,6 +434,7 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); + pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); @@ -446,6 +447,7 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); + pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop); -- cgit v1.2.3 From ee469d47e6520647423f91eb9a29aa57ca4fb74b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 15:50:12 +0200 Subject: digital/io-hub/src/robospierre: move to previous position on blocking --- digital/io-hub/src/robospierre/clamp.c | 47 +++++++++++++++++++++++++++++----- 1 file changed, 41 insertions(+), 6 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index cb6d9e3b..b42226cb 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -81,6 +81,8 @@ FSM_STATES ( CLAMP_LOCKED, /* Clamp blocked. */ CLAMP_BLOCKED, + /* Clamp unblocking, returning to prevous position. */ + CLAMP_UNBLOCKING, /* Waiting movement order. */ CLAMP_MOVE_IDLE, @@ -150,6 +152,8 @@ struct clamp_t uint8_t pos_current; /** Requested position. */ uint8_t pos_request; + /** Last position before move. */ + uint8_t pos_previous; /** Element moving destination. */ uint8_t moving_to; /** Position of a new element. */ @@ -336,6 +340,16 @@ clamp_handle_event (void) return 0; } +/** Return to last position. */ +static void +clamp_unblock (void) +{ + ctx.pos_current = ctx.pos_previous; + ctx.pos_request = ctx.pos_current; + ctx.moving_to = CLAMP_POS_NB; + FSM_HANDLE (AI, clamp_move); +} + /** Open or close clamp and adjust rotation. */ static void clamp_openclose (uint8_t open) @@ -361,8 +375,14 @@ clamp_route (void) uint8_t pos_new; uint8_t pos_current = ctx.pos_current; uint8_t pos_request = ctx.pos_request; + /* Save previous position for unblocking. */ + ctx.pos_previous = pos_current; /* Compute new position. */ - if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_current)) + if (pos_current == pos_request) + { + pos_new = pos_current; + } + else if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_current)) { if (!CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) pos_new = CLAMP_BAY_FRONT_LEAVE; @@ -796,23 +816,38 @@ FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING) return FSM_NEXT (CLAMP_LOCKED, clamp_drop); } -FSM_TRANS (CLAMP_BLOCKED, clamp_prepare, +FSM_TRANS (CLAMP_BLOCKED, clamp_prepare, CLAMP_UNBLOCKING) +{ + clamp_unblock (); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare); +} + +FSM_TRANS (CLAMP_UNBLOCKING, clamp_move_success, + move_element, CLAMP_MOVING_ELEMENT, move_to_idle, CLAMP_GOING_IDLE, clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { - switch (clamp_decision (1)) + switch (clamp_decision (0)) { default: + case CLAMP_DECISION_MOVE_ELEMENT: + return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_success, move_element); case CLAMP_DECISION_MOVE_TO_IDLE: - return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle); + return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_success, move_to_idle); case CLAMP_DECISION_CLAMP_LOCKED: - return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked); + return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_success, clamp_locked); case CLAMP_DECISION_DONE: - return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, done); + return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_success, done); } } +FSM_TRANS (CLAMP_UNBLOCKING, clamp_move_failure, CLAMP_BLOCKED) +{ + fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked)); + return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_failure); +} + /* CLAMP_MOVE FSM */ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, -- cgit v1.2.3 From a42a9f908fc0cad8760ecda5670a6b1322d56a83 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 16:39:27 +0200 Subject: Revert "digital/io-hub/src/robospierre: disable pawn sensors when tower dropped" This reverts commit 0b9ae480b928a24664bfd30edcd17d760db593d1. --- digital/io-hub/src/robospierre/pawn_sensor.c | 6 ------ digital/io-hub/src/robospierre/pawn_sensor.h | 4 ---- digital/io-hub/src/robospierre/top.c | 2 -- 3 files changed, 12 deletions(-) diff --git a/digital/io-hub/src/robospierre/pawn_sensor.c b/digital/io-hub/src/robospierre/pawn_sensor.c index faeee879..cc9e6900 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.c +++ b/digital/io-hub/src/robospierre/pawn_sensor.c @@ -214,12 +214,6 @@ pawn_sensor_bumper_enable (uint8_t enabled) pawn_sensor_global.bumper_enabled = enabled; } -void -pawn_sensor_bumper_wait (uint16_t wait) -{ - pawn_sensor_global.bump_wait = wait; -} - vect_t pawn_sensor_get_last_bumped (void) { diff --git a/digital/io-hub/src/robospierre/pawn_sensor.h b/digital/io-hub/src/robospierre/pawn_sensor.h index 9c76f77a..8d6d2651 100644 --- a/digital/io-hub/src/robospierre/pawn_sensor.h +++ b/digital/io-hub/src/robospierre/pawn_sensor.h @@ -38,10 +38,6 @@ pawn_sensor_update (void); void pawn_sensor_bumper_enable (uint8_t enabled); -/** Temporarily disable bumpers. */ -void -pawn_sensor_bumper_wait (uint16_t wait); - /** Return last bumped pawn. */ vect_t pawn_sensor_get_last_bumped (void); diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 3d39f60a..7732fb50 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -434,7 +434,6 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_success, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); - pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_success, drop); @@ -447,7 +446,6 @@ FSM_TRANS (TOP_DROP_CLEARING, robot_move_failure, element, TOP_GOING_TO_ELEMENT) { clamp_drop_clear (); - pawn_sensor_bumper_wait (3 * 250); switch (top_decision ()) { default: return FSM_NEXT (TOP_DROP_CLEARING, robot_move_failure, drop); -- cgit v1.2.3 From 04d27a86b1225e02f80d20962f9168fc9c0e76a7 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 16:57:46 +0200 Subject: digital/io-hub/src/robospierre: malus to intersections near dropped elements --- digital/io-hub/src/robospierre/element.c | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index e8ee44e0..7319adbb 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -657,12 +657,26 @@ element_taken (uint8_t element_id, uint8_t element_type) void element_down (uint8_t element_id, uint8_t element_type) { + uint8_t i; element_t e = element_get (element_id); e.type = element_type; element_set (element_id, e); + /* Malus elements near this element. */ + for (i = ELEMENT_INTERSEC_START; i <= ELEMENT_INTERSEC_END; i++) + { + element_t ie = element_get (i); + if (UTILS_ABS (e.pos.x - ie.pos.x) + < BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20 + && UTILS_ABS (e.pos.y - ie.pos.y) + < BOT_ELEMENT_RADIUS + BOT_SIZE_SIDE + 20) + { + ie.bonus_load = -50; + element_set (i, ie); + } + } + /* Remove our green zone score at first unload. */ - uint8_t i; for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++) { element_t e = element_get (i); -- cgit v1.2.3 From 284ebff6aab7beed71c78e58996b267c4c0dca08 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 17:27:07 +0200 Subject: digital/io-hub/src/robospierre: do not drop everything on blocking --- digital/io-hub/src/robospierre/bot.h | 2 ++ digital/io-hub/src/robospierre/clamp.c | 35 ++++++++++++++++++++++++++----- digital/io-hub/src/robospierre/logistic.c | 8 ++++--- digital/io-hub/src/robospierre/logistic.h | 4 ++-- 4 files changed, 39 insertions(+), 10 deletions(-) diff --git a/digital/io-hub/src/robospierre/bot.h b/digital/io-hub/src/robospierre/bot.h index 3996f10e..499046e2 100644 --- a/digital/io-hub/src/robospierre/bot.h +++ b/digital/io-hub/src/robospierre/bot.h @@ -99,6 +99,7 @@ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_SIDE_ROTATION_STEP \ (BOT_CLAMP_BAY_BACK_ROTATION_STEP / 2) +# define BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP 1000 #define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET 0 #define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET 0 @@ -130,6 +131,7 @@ # define BOT_CLAMP_BAY_BACK_ROTATION_STEP \ BOT_CLAMP_SLOT_BACK_MIDDLE_ROTATION_STEP # define BOT_CLAMP_BAY_SIDE_ROTATION_STEP (0x1183 + 120) +# define BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP 1000 #define BOT_CLAMP_CLOSED_FRONT_ROTATION_OFFSET -129 #define BOT_CLAMP_CLOSED_BACK_ROTATION_OFFSET -60 diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index b42226cb..68fbd44f 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -537,13 +537,38 @@ clamp_blocked (void) { /* Free everything. */ clamp_openclose (1); - clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1); - clamp_door (CLAMP_SLOT_FRONT_TOP, 1); - clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1); - clamp_door (CLAMP_SLOT_BACK_TOP, 1); + uint16_t rotation_position = mimot_get_motor1_position (); + uint16_t elevation_position = mimot_get_motor0_position (); + if (rotation_position < BOT_CLAMP_BAY_SIDE_ROTATION_STEP + - BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP) + { + clamp_door (CLAMP_SLOT_FRONT_BOTTOM, 1); + if (elevation_position + > (BOT_CLAMP_SLOT_FRONT_MIDDLE_ELEVATION_STEP + + BOT_CLAMP_SLOT_FRONT_TOP_ELEVATION_STEP) / 2) + { + clamp_door (CLAMP_SLOT_FRONT_TOP, 1); + logistic_dump (DIRECTION_FORWARD, 1); + } + else + logistic_dump (DIRECTION_FORWARD, 0); + } + else if (rotation_position > BOT_CLAMP_BAY_SIDE_ROTATION_STEP + + BOT_CLAMP_BAY_SIDE_MARGIN_ROTATION_STEP) + { + clamp_door (CLAMP_SLOT_BACK_BOTTOM, 1); + if (elevation_position + > (BOT_CLAMP_SLOT_BACK_MIDDLE_ELEVATION_STEP + + BOT_CLAMP_SLOT_BACK_TOP_ELEVATION_STEP) / 2) + { + clamp_door (CLAMP_SLOT_BACK_TOP, 1); + logistic_dump (DIRECTION_BACKWARD, 1); + } + else + logistic_dump (DIRECTION_BACKWARD, 0); + } mimot_motor0_free (); mimot_motor1_free (); - logistic_dump (); /* Signal problem. */ fsm_queue_post_event (FSM_EVENT (AI, clamp_move_failure)); } diff --git a/digital/io-hub/src/robospierre/logistic.c b/digital/io-hub/src/robospierre/logistic.c index db0c8b94..82f5a4fa 100644 --- a/digital/io-hub/src/robospierre/logistic.c +++ b/digital/io-hub/src/robospierre/logistic.c @@ -582,11 +582,13 @@ logistic_drop_element_type (uint8_t direction) } void -logistic_dump (void) +logistic_dump (uint8_t direction, uint8_t drop_top) { + /* Drop. */ uint8_t i; - /* Drop all except side. */ - for (i = 0; i < CLAMP_SLOT_SIDE; i++) + uint8_t bay = direction == DIRECTION_FORWARD + ? CLAMP_SLOT_FRONT_BOTTOM : CLAMP_SLOT_BACK_BOTTOM; + for (i = bay; i < bay + 2 + drop_top; i++) ctx.slots[i] = 0; } diff --git a/digital/io-hub/src/robospierre/logistic.h b/digital/io-hub/src/robospierre/logistic.h index 0a076477..c6bc0210 100644 --- a/digital/io-hub/src/robospierre/logistic.h +++ b/digital/io-hub/src/robospierre/logistic.h @@ -132,9 +132,9 @@ logistic_drop (uint8_t direction); uint8_t logistic_drop_element_type (uint8_t direction); -/** Dump every element. */ +/** Dump every element on one direction. */ void -logistic_dump (void); +logistic_dump (uint8_t direction, uint8_t drop_top); /** Is path clear between two positions? */ uint8_t -- cgit v1.2.3 From 154ac30376502dd43fc766bd852b77f8e49fcf30 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 25 Jun 2011 17:37:51 +0200 Subject: Revert "digital/io-hub/src/robospierre: move to previous position on blocking" This reverts commit ee469d47e6520647423f91eb9a29aa57ca4fb74b. --- digital/io-hub/src/robospierre/clamp.c | 47 +++++----------------------------- 1 file changed, 6 insertions(+), 41 deletions(-) diff --git a/digital/io-hub/src/robospierre/clamp.c b/digital/io-hub/src/robospierre/clamp.c index 68fbd44f..84b2771c 100644 --- a/digital/io-hub/src/robospierre/clamp.c +++ b/digital/io-hub/src/robospierre/clamp.c @@ -81,8 +81,6 @@ FSM_STATES ( CLAMP_LOCKED, /* Clamp blocked. */ CLAMP_BLOCKED, - /* Clamp unblocking, returning to prevous position. */ - CLAMP_UNBLOCKING, /* Waiting movement order. */ CLAMP_MOVE_IDLE, @@ -152,8 +150,6 @@ struct clamp_t uint8_t pos_current; /** Requested position. */ uint8_t pos_request; - /** Last position before move. */ - uint8_t pos_previous; /** Element moving destination. */ uint8_t moving_to; /** Position of a new element. */ @@ -340,16 +336,6 @@ clamp_handle_event (void) return 0; } -/** Return to last position. */ -static void -clamp_unblock (void) -{ - ctx.pos_current = ctx.pos_previous; - ctx.pos_request = ctx.pos_current; - ctx.moving_to = CLAMP_POS_NB; - FSM_HANDLE (AI, clamp_move); -} - /** Open or close clamp and adjust rotation. */ static void clamp_openclose (uint8_t open) @@ -375,14 +361,8 @@ clamp_route (void) uint8_t pos_new; uint8_t pos_current = ctx.pos_current; uint8_t pos_request = ctx.pos_request; - /* Save previous position for unblocking. */ - ctx.pos_previous = pos_current; /* Compute new position. */ - if (pos_current == pos_request) - { - pos_new = pos_current; - } - else if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_current)) + if (CLAMP_IS_SLOT_IN_FRONT_BAY (pos_current)) { if (!CLAMP_IS_SLOT_IN_FRONT_BAY (pos_request)) pos_new = CLAMP_BAY_FRONT_LEAVE; @@ -841,38 +821,23 @@ FSM_TRANS (CLAMP_LOCKED, clamp_drop, CLAMP_DROPING_DOOR_OPENING) return FSM_NEXT (CLAMP_LOCKED, clamp_drop); } -FSM_TRANS (CLAMP_BLOCKED, clamp_prepare, CLAMP_UNBLOCKING) -{ - clamp_unblock (); - return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare); -} - -FSM_TRANS (CLAMP_UNBLOCKING, clamp_move_success, - move_element, CLAMP_MOVING_ELEMENT, +FSM_TRANS (CLAMP_BLOCKED, clamp_prepare, move_to_idle, CLAMP_GOING_IDLE, clamp_locked, CLAMP_LOCKED, done, CLAMP_IDLE) { - switch (clamp_decision (0)) + switch (clamp_decision (1)) { default: - case CLAMP_DECISION_MOVE_ELEMENT: - return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_success, move_element); case CLAMP_DECISION_MOVE_TO_IDLE: - return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_success, move_to_idle); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, move_to_idle); case CLAMP_DECISION_CLAMP_LOCKED: - return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_success, clamp_locked); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, clamp_locked); case CLAMP_DECISION_DONE: - return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_success, done); + return FSM_NEXT (CLAMP_BLOCKED, clamp_prepare, done); } } -FSM_TRANS (CLAMP_UNBLOCKING, clamp_move_failure, CLAMP_BLOCKED) -{ - fsm_queue_post_event (FSM_EVENT (AI, clamp_blocked)); - return FSM_NEXT (CLAMP_UNBLOCKING, clamp_move_failure); -} - /* CLAMP_MOVE FSM */ FSM_TRANS (CLAMP_MOVE_IDLE, clamp_move, -- cgit v1.2.3 From 7c2fd5bfc8e24005e791bc9536e40972b5a4737a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 26 Jun 2011 00:59:02 +0200 Subject: digital/avr/modules/adc: tested on new AVR --- digital/avr/modules/adc/adc.avr.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/digital/avr/modules/adc/adc.avr.c b/digital/avr/modules/adc/adc.avr.c index eff441a9..96392a86 100644 --- a/digital/avr/modules/adc/adc.avr.c +++ b/digital/avr/modules/adc/adc.avr.c @@ -32,6 +32,10 @@ #elif defined (__AVR_ATmega128__) #elif defined (__AVR_ATmega64__) #elif defined (__AVR_ATmega16__) +# elif defined (__AVR_AT90USB646__) +# elif defined (__AVR_AT90USB647__) +# elif defined (__AVR_AT90USB1286__) +# elif defined (__AVR_AT90USB1287__) #else # warning "adc: not tested on this chip." #endif -- cgit v1.2.3 From 456d1d6670a1a1cf6a01dbbf718a84cfa0a0f34c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 21 Sep 2011 20:07:10 +0200 Subject: digital/io-hub/src/robospierre: go to green zone several times --- digital/io-hub/src/robospierre/element.c | 29 +++++++++++++++++------------ digital/io-hub/src/robospierre/element.h | 4 ++++ digital/io-hub/src/robospierre/top.c | 21 +++++++++++++++++++++ 3 files changed, 42 insertions(+), 12 deletions(-) diff --git a/digital/io-hub/src/robospierre/element.c b/digital/io-hub/src/robospierre/element.c index 7319adbb..881e1be6 100644 --- a/digital/io-hub/src/robospierre/element.c +++ b/digital/io-hub/src/robospierre/element.c @@ -675,18 +675,6 @@ element_down (uint8_t element_id, uint8_t element_type) element_set (i, ie); } } - - /* Remove our green zone score at first unload. */ - for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++) - { - element_t e = element_get (i); - if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || - (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) - { - e.bonus_load = 0; - element_set (i, e); - } - } } void @@ -821,3 +809,20 @@ element_i_like_green () } } } + +void +element_no_more_green (void) +{ + uint8_t i; + /* Remove our green zone score at first unload. */ + for (i = ELEMENT_GREEN_START; i <= ELEMENT_GREEN_END - 2; i++) + { + element_t e = element_get (i); + if ((team_color == TEAM_COLOR_LEFT && (e.attr & ELEMENT_LEFT)) || + (team_color == TEAM_COLOR_RIGHT && (e.attr & ELEMENT_RIGHT))) + { + e.bonus_load = 0; + element_set (i, e); + } + } +} diff --git a/digital/io-hub/src/robospierre/element.h b/digital/io-hub/src/robospierre/element.h index 243f5b94..41985e95 100644 --- a/digital/io-hub/src/robospierre/element.h +++ b/digital/io-hub/src/robospierre/element.h @@ -206,4 +206,8 @@ element_blocking_path (vect_t a, vect_t b, int16_t ab, uint8_t escape); void element_i_like_green (); +/** Do not go to our green zone any more. */ +void +element_no_more_green (void); + #endif /* element_h */ diff --git a/digital/io-hub/src/robospierre/top.c b/digital/io-hub/src/robospierre/top.c index 7732fb50..93fb447f 100644 --- a/digital/io-hub/src/robospierre/top.c +++ b/digital/io-hub/src/robospierre/top.c @@ -90,6 +90,8 @@ struct top_t uint8_t broken; /** Saved, direction when picking element. */ uint8_t go_to_element_direction; + /** Return to our green zone again. */ + uint8_t green_again; }; /** Global context. */ @@ -99,6 +101,7 @@ struct top_t top_global; FSM_TRANS (TOP_START, init_start_round, TOP_GOING_OUT1) { element_init (); + ctx.green_again = 3; asserv_goto (PG_X (PG_GREEN_WIDTH_MM + 100), PG_Y (PG_LENGTH - 200), 0); return FSM_NEXT (TOP_START, init_start_round); @@ -305,6 +308,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_success, element_failure (ctx.target_element_id); /* Do not take this one again. */ if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, clamp_working); + if (ctx.green_again) + { + ctx.green_again--; + if (ctx.green_again == 0) + element_no_more_green (); + } switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_success, drop); @@ -321,6 +330,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, element_failure (ctx.target_element_id); if (clamp_working ()) return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, clamp_working); + if (ctx.green_again) + { + ctx.green_again--; + if (ctx.green_again == 0) + element_no_more_green (); + } switch (top_decision ()) { default: return FSM_NEXT (TOP_GOING_TO_ELEMENT, move_failure, drop); @@ -331,6 +346,12 @@ FSM_TRANS (TOP_GOING_TO_ELEMENT, move_failure, FSM_TRANS (TOP_GOING_TO_ELEMENT, clamp_working, TOP_WAITING_CLAMP) { move_stop (); + if (ctx.green_again) + { + ctx.green_again--; + if (ctx.green_again == 0) + element_no_more_green (); + } return FSM_NEXT (TOP_GOING_TO_ELEMENT, clamp_working); } -- cgit v1.2.3 From 677dbdcc67d976cbf2040dad32b5a4600f233eaf Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 15 Oct 2011 21:09:55 +0200 Subject: host/simu/utils: add vector class --- host/simu/utils/vector.py | 267 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 267 insertions(+) create mode 100644 host/simu/utils/vector.py diff --git a/host/simu/utils/vector.py b/host/simu/utils/vector.py new file mode 100644 index 00000000..e5a8bd05 --- /dev/null +++ b/host/simu/utils/vector.py @@ -0,0 +1,267 @@ +# simu - Robot simulation. {{{ +# encoding: utf-8 +# +# Copyright (C) 2011 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT 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 +# (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. +# +# }}} +"""2D vector.""" +import math + +__all__ = ('vector') + +class vector (object): + """2D vector, with useful operators.""" + + __slots__ = ('x', 'y') + + def __init__ (self, *xy): + """Initialise the vector with given coordinates. + + >>> vector (1, 2) + vector(1.0, 2.0) + >>> vector ((1, 2)) + vector(1.0, 2.0) + >>> vector (vector (1, 2)) + vector(1.0, 2.0) + >>> vector ("hello") + Traceback (most recent call last): + ... + TypeError + >>> vector ("ab") + Traceback (most recent call last): + ... + ValueError: invalid literal for float(): a + """ + if len (xy) == 2: + self.x, self.y = float (xy[0]), float (xy[1]) + elif len (xy) == 1: + v = xy[0] + if isinstance (v, vector): + self.x, self.y = v.x, v.y + elif len (v) == 2: + self.x, self.y = float (v[0]), float (v[1]) + else: + raise TypeError + else: + raise TypeError + + def __len__ (self): + """Return vector length, always 2. + + >>> len (vector (1, 2)) + 2 + """ + return 2 + + def __repr__ (self): + """Return text representation. + + >>> vector (1, 2) + vector(1.0, 2.0) + """ + return 'vector(%r, %r)' % (self.x, self.y) + + def __str__ (self): + """Return informal text representation. + + >>> str (vector (1, 2)) + '(1.0, 2.0)' + """ + return '(%s, %s)' % (self.x, self.y) + + def __getitem__ (self, index): + """Return a coordinate by its index, for compatibility with + sequences.""" + return (self.x, self.y)[index] + + def __eq__ (self, other): + """Test for equality. + + >>> vector (1, 2) == vector (1, 3) + False + >>> vector (1, 2) == vector (2, 2) + False + >>> vector (1, 2) == vector (1, 2) + True + """ + return (self.x, self.y) == (other.x, other.y) + + def __ne__ (self, other): + """Test for inequality. + + >>> vector (1, 2) != vector (1, 2) + False + """ + return not (self == other) + + def __cmp__ (self, other): + """No comparison apart from equality test. + + >>> vector (1, 2) < vector (1, 2) + Traceback (most recent call last): + ... + TypeError + """ + raise TypeError + + def __add__ (self, other): + """Addition. + + >>> vector (1, 2) + vector (3, 4) + vector(4.0, 6.0) + """ + return vector (self.x + other.x, self.y + other.y) + + def __sub__ (self, other): + """Subtraction. + + >>> vector (1, 2) - vector (3, 4) + vector(-2.0, -2.0) + """ + return vector (self.x - other.x, self.y - other.y) + + def __mul__ (self, other): + """Multiplication or dot product. + + >>> vector (1, 2) * 2 + vector(2.0, 4.0) + >>> 3 * vector (1, 2) + vector(3.0, 6.0) + >>> vector (1, 2) * vector (3, 4) + 11.0 + """ + if isinstance (other, vector): + return self.x * other.x + self.y * other.y + else: + return vector (self.x * other, self.y * other) + __rmul__ = __mul__ + + def __div__ (self, other): + """Division. + + >>> vector (1, 2) / 2 + vector(0.5, 1.0) + >>> vector (1, 2) / vector (3, 4) + Traceback (most recent call last): + ... + TypeError: unsupported operand type(s) for /: 'float' and 'vector' + >>> 3.0 / vector (1, 2) + Traceback (most recent call last): + ... + TypeError: unsupported operand type(s) for /: 'float' and 'vector' + """ + return vector (self.x / other, self.y / other) + __truediv__ = __div__ + + def __neg__ (self): + """Negate. + + >>> -vector (1.0, 2.0) + vector(-1.0, -2.0) + """ + return vector (-self.x, -self.y) + + def __pos__ (self): + """No-op. + + >>> +vector (1.0, 2.0) + vector(1.0, 2.0) + """ + return self + + def __abs__ (self): + """Norm. + + >>> abs (vector (3.0, 4.0)) + 5.0 + >>> vector (3.0, 4.0).norm () + 5.0 + """ + return math.hypot (self.x, self.y) + norm = __abs__ + + def norm_squared (self): + """Norm squared. + + >>> vector (3.0, 4.0).norm_squared () + 25.0 + """ + return self.x ** 2 + self.y ** 2 + + def unit (self): + """Return normalized vector. + + >>> print vector (3.0, 4.0).unit () + (0.6, 0.8) + """ + norm = math.hypot (self.x, self.y) + return vector (self.x / norm, self.y / norm) + + def normal (self): + """Return the vector rotated by pi/2. + + >>> vector (1, 2).normal () + vector(-2.0, 1.0) + """ + return vector (-self.y, self.x) + + def angle (self): + """Return angle from (1, 0). + + >>> vector (1, 1).angle () + 0.78539816339744828 + >>> vector (0, -1).angle () + -1.5707963267948966 + >>> vector (-1, -1).angle () + -2.3561944901923448 + """ + return math.atan2 (self.y, self.x) + + @staticmethod + def polar (angle, norm): + """Return a vector constructed from polar coordinates (angle, norm). + + >>> print vector.polar (math.pi / 4, math.sqrt (2)) + (1.0, 1.0) + """ + return vector (norm * math.cos (angle), norm * math.sin (angle)) + +if __name__ == '__main__': + def _test (): + import doctest + doctest.testmod () + def _perf_test (): + import timeit + def do (title, stmt, setup = None): + n = 10000 + if setup is None: + setup = 'import vector; v = vector.vector (1.2, 3.4)' + t = timeit.timeit (stmt, setup, number = n) + print title, '%.3f µs' % (1e6 * t / n) + do ('init from floats', 'v = vector.vector (1.2, 3.4)') + do ('init from tuple', 'v = vector.vector ((1.2, 3.4))') + do ('init from vector', 'v2 = vector.vector (v)') + do ('init from polar', 'v = vector.vector.polar (1.0, 1.0)') + do ('abs', 'abs (v)') + do ('norm_squared', 'v.norm_squared()') + do ('unit', 'v.unit()') + _test() + _perf_test () -- cgit v1.2.3 From 46e9274161ff1f964ffa70970c4f43947b7bc951 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 15 Oct 2011 21:11:28 +0200 Subject: host/simu/model: use vector class in round obstacle --- host/simu/model/round_obstacle.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/host/simu/model/round_obstacle.py b/host/simu/model/round_obstacle.py index 6658ed19..e749440b 100644 --- a/host/simu/model/round_obstacle.py +++ b/host/simu/model/round_obstacle.py @@ -24,6 +24,7 @@ """Obstacle with a round shape.""" from math import pi, cos, sin, sqrt from utils.observable import Observable +from simu.utils.vector import vector class RoundObstacle (Observable): @@ -38,17 +39,19 @@ class RoundObstacle (Observable): to intersection point, else, return None.""" if self.pos is None: return None - ab = sqrt ((b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2) # distance AB. - n = ((b[0] - a[0]) / ab, (b[1] - a[1]) / ab) # vector of length 1. - o = self.pos # obstacle center. + a, b = vector (a), vector (b) + vab = b - a + ab = abs (vab) # distance AB. + n = vab.unit () # vector of length 1. + o = vector (self.pos) # obstacle center. # To check if the line (AB) intersects the circle, compute distance # from circle center to line using a dot product. - vao = (o[0] - a[0], o[1] - a[1]) # vector AO. - # dot product, (-n[1], n[0]) is perpendicular to n. - doc = abs (vao[0] * -n[1] + vao[1] * n[0]) + vao = o - a # vector AO. + # abs of dot product. + doc = abs (vao * n.normal ()) if doc < self.radius: # Line intersects, check if segment intersects. - m = vao[0] * n[0] + vao[1] * n[1] + m = vao * n f = sqrt (self.radius ** 2 - doc ** 2) if m - f > 0 and m - f < ab: return m - f -- cgit v1.2.3 From 0f1efb0a55b424c558e3298282bac6cf48b07b89 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 15 Oct 2011 21:12:31 +0200 Subject: host/simu/model/test: add table and obstacle test --- host/simu/model/test/test_table.py | 168 +++++++++++++++++++++++++++++++++++++ 1 file changed, 168 insertions(+) create mode 100644 host/simu/model/test/test_table.py diff --git a/host/simu/model/test/test_table.py b/host/simu/model/test/test_table.py new file mode 100644 index 00000000..58d09a92 --- /dev/null +++ b/host/simu/model/test/test_table.py @@ -0,0 +1,168 @@ +# simu - Robot simulation. {{{ +# +# Copyright (C) 2011 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT 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 +# (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. +# +# }}} +"""Test table model and obstacles.""" +from simu.model.table import Table +from simu.model.round_obstacle import RoundObstacle +from simu.inter.drawable import Drawable, DrawableCanvas +from simu.utils.vector import vector +from Tkinter import * + +class Area (Drawable): + + def __init__ (self, onto): + Drawable.__init__ (self, onto) + self.table = Table () + self.a = (0, 0) + self.b = (10, 10) + self.result = None + + def draw (self): + self.reset () + for o in self.table.obstacles: + if o.pos is None: + continue + if isinstance (o, RoundObstacle): + self.draw_circle (o.pos, o.radius) + else: + raise TypeError ("unknown obstacle") + if self.a is not None: + self.draw_circle (self.a, 0.2, fill = 'green') + if self.b is not None: + self.draw_circle (self.b, 0.2, fill = 'red') + if self.a is not None and self.b is not None: + self.draw_line (self.a, self.b, arrow = 'last') + if self.result is not None: + self.draw_circle (self.result, 0.2, fill = 'yellow') + + def update (self, test, **kwargs): + self.result = None + if self.a is not None and self.b is not None: + if test == 'intersect': + def nearer (a, b): return a < b + i = self.table.intersect (self.a, self.b, comp = nearer, + **kwargs) + if i is not None: + a, b = vector (self.a), vector (self.b) + self.result = (b - a).unit () * i.distance + elif test == 'nearest': + n = self.table.nearest (self.b, **kwargs) + if n is not None: + self.result = n.pos + +class AreaView (DrawableCanvas): + + def __init__ (self, master = None): + DrawableCanvas.__init__ (self, 22, 22, 0, 0, master, borderwidth = 1, + relief = 'sunken', background = 'white') + self.area = Area (self) + + def draw (self): + self.area.draw () + +class TestTable (Frame): + + def __init__ (self, master = None): + Frame.__init__ (self, master) + self.pack (expand = True, fill = 'both') + self.create_widgets () + self.move = None + + def create_widgets (self): + self.right_frame = Frame (self) + self.right_frame.pack (side = 'right', fill = 'y') + self.quit_button = Button (self.right_frame, text = 'Quit', command = + self.quit) + self.quit_button.pack (side = 'top', fill = 'x') + self.test_var = StringVar () + self.test_var.set ('intersect') + self.test_intersect = Radiobutton (self.right_frame, + variable = self.test_var, command = self.update, + text = 'Intersect', value = 'intersect') + self.test_intersect.pack (side = 'top') + self.test_nearest = Radiobutton (self.right_frame, + variable = self.test_var, command = self.update, + text = 'Nearest', value = 'nearest') + self.test_nearest.pack (side = 'top') + self.new_obstacle_round_frame = LabelFrame (self.right_frame) + self.new_obstacle_round_frame.pack (side = 'top') + self.new_obstacle_radius = Scale (self.new_obstacle_round_frame, + label = 'Radius', orient = 'horizontal', from_ = 0.5, to = 10, + resolution = 0.5) + self.new_obstacle_radius.pack (side = 'top') + self.new_obstacle_round = Button (self.new_obstacle_round_frame, + text = 'New round obstacle', + command = self.new_round_obstacle) + self.new_obstacle_round_frame.configure ( + labelwidget = self.new_obstacle_round) + self.area_view = AreaView (self) + self.area_view.pack (expand = True, fill = 'both') + self.area_view.bind ('<1>', self.click) + + def update (self, draw = True): + self.area_view.area.update (self.test_var.get ()) + if draw: + self.area_view.draw () + + def click (self, ev): + def move (o, pos): + if callable (o): + o (pos) + else: + o.pos = pos + pos = vector (self.area_view.screen_coord ((ev.x, ev.y))) + if self.move is None: + def move_a (pos): + self.area_view.area.a = pos + def move_b (pos): + self.area_view.area.b = pos + objs = [ [ self.area_view.area.a, 0.2, move_a ], + [ self.area_view.area.b, 0.2, move_b ] ] + for o in self.area_view.area.table.obstacles: + objs.append ([ o.pos, o.radius, o ]) + for obj in objs: + opos = vector (obj[0]) + radius = obj[1] + if abs (opos - pos) < radius: + self.move = obj[2] + break + if self.move is not None: + move (self.move, None) + else: + move (self.move, pos) + self.move = None + self.update () + + def new_round_obstacle (self): + o = RoundObstacle (self.new_obstacle_radius.get ()) + o.pos = (5, -5) + self.area_view.area.table.obstacles.append (o) + self.update () + +if __name__ == '__main__': + app = TestTable () + o = RoundObstacle (2) + o.pos = (5, 5) + app.area_view.area.table.obstacles.append (o) + app.update (False) + app.mainloop () -- cgit v1.2.3 From 5f202209337a268a4128d691e32b00c23abc06cd Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 16 Oct 2011 15:07:44 +0200 Subject: host/simu/utils: add intersect util --- host/simu/utils/intersect.py | 103 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 103 insertions(+) create mode 100644 host/simu/utils/intersect.py diff --git a/host/simu/utils/intersect.py b/host/simu/utils/intersect.py new file mode 100644 index 00000000..6074dda0 --- /dev/null +++ b/host/simu/utils/intersect.py @@ -0,0 +1,103 @@ +# simu - Robot simulation. {{{ +# encoding: utf-8 +# +# Copyright (C) 2011 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT 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 +# (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. +# +# }}} +"""Intersection between different kind of primitives.""" +from simu.utils.vector import vector + +def segment_segment (a, b, c, d): + """Find intersection between [AB] and [CD] line segments. Return i such + that A + i * (B - A).unit () gives this intersection. Return None if no + intersection found. + + If line segments are parallel or collinear, None is returned. + """ + a, b, c, d = vector (a), vector (b), vector (c), vector (d) + # For each point P on the line segment [AB], there is a real u in [0, 1] + # for which P = A + u (B - A) + # + # An intersection point must be on both line segments: + # + # A + u (B - A) = C + v (D - C) + # + # xa + u (xb - xa) = xc + v (xd - xc) + # ya + u (yb - ya) = yc + v (yd - yc) + # + # (xc - xa) (yd - yc) - (xd - xc) (yc - ya) + # u = ----------------------------------------- + # (xb - xa) (yd - yc) - (xd - xc) (yb - ya) + # (xc - xa) (yb - ya) - (xb - xa) (yc - ya) + # v = ----------------------------------------- + # (xb - xa) (yd - yc) - (xd - xc) (yb - ya) + # + # u = (vac . vcd.normal()) / (vab . vcd.normal()) + # v = (vac . vab.normal()) / (vab . vcd.normal()) + # + # If vab . vcd.normal () is 0, AB and CD are parallel. + vab = b - a + vcd = d - c + vac = c - a + # Cannot test for 0 because we are using float, cannot test for a very + # small number because we do not know what is small enough, therefore, + # compare with numerator. + den = vab * vcd.normal () + unum = vac * vcd.normal () + if abs (den) <= 1e-6 * abs (unum): + return None + else: + u = unum / den + if u >= 0 and u <= 1: + v = vac * vab.normal () / den + if v >= 0 and v <= 1: + return u * vab.norm () + return None + +if __name__ == '__main__': + import sys + import math + v00 = vector (0, 0) + v02 = vector (0, 2) + v20 = vector (2, 0) + v22 = vector (2, 2) + v44 = vector (4, 4) + failed = 0 + verbose = True + def check (test, result): + r = eval (test) + if r != result: + print test, 'is', r, 'but expected', result + return 1 + elif verbose: + print test, 'is', r, 'as expected' + return 0 + v2 = math.sqrt (2) + failed += check ('segment_segment (v00, v22, v20, v02)', v2) + failed += check ('segment_segment (v00, v22, v02, v20)', v2) + failed += check ('segment_segment (v22, v00, v20, v02)', v2) + failed += check ('segment_segment (v22, v00, v02, v20)', v2) + failed += check ('segment_segment (v00, v22, v00, v22)', None) + failed += check ('segment_segment (v00, v22, v00, v44)', None) + failed += check ('segment_segment (v22, v44, v02, v20)', None) + failed += check ('segment_segment (v00, v44, v02, v20)', v2) + failed += check ('segment_segment (v44, v00, v02, v20)', 3 * v2) + sys.exit (0 if not failed else 1) -- cgit v1.2.3 From 99dd816f4859c8d17ea2c3df9435cbbbcce595d8 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 16 Oct 2011 15:23:34 +0200 Subject: host/simu/model: add rectangular obstacle --- host/simu/model/rectangular_obstacle.py | 63 +++++++++++++++++++++++++++++++++ host/simu/model/test/test_table.py | 48 +++++++++++++++++++++++-- 2 files changed, 109 insertions(+), 2 deletions(-) create mode 100644 host/simu/model/rectangular_obstacle.py diff --git a/host/simu/model/rectangular_obstacle.py b/host/simu/model/rectangular_obstacle.py new file mode 100644 index 00000000..71cfdc8b --- /dev/null +++ b/host/simu/model/rectangular_obstacle.py @@ -0,0 +1,63 @@ +# simu - Robot simulation. {{{ +# +# Copyright (C) 2011 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT 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 +# (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. +# +# }}} +"""Obstacle with a rectangular shape.""" +from utils.observable import Observable +from simu.utils.vector import vector +from math import pi +import simu.utils.intersect + +class RectangularObstacle (Observable): + + def __init__ (self, dim, level = 0): + Observable.__init__ (self) + self.pos = None + self.angle = 0 + self.dim = dim + self.level = level + + def intersect (self, a, b): + """If the segment [AB] intersects the obstacle, return distance from a + to intersection point, else, return None.""" + if self.pos is None or self.angle is None: + return None + # Find intersection with each rectangle segments. There is at most + # two intersections, return the nearest. + u = vector.polar (self.angle, self.dim[0] / 2.) + v = vector.polar (self.angle + pi / 2, self.dim[1] / 2.) + o = vector (self.pos) + p1 = o + u + v + p2 = o - u + v + p3 = o - u - v + p4 = o + u - v + found = None + for c, d in ((p1, p2), (p2, p3), (p3, p4), (p4, p1)): + i = simu.utils.intersect.segment_segment (a, b, c, d) + if i is not None: + if found is not None: + found = min (found, i) + break + else: + found = i + return found + diff --git a/host/simu/model/test/test_table.py b/host/simu/model/test/test_table.py index 58d09a92..9595f256 100644 --- a/host/simu/model/test/test_table.py +++ b/host/simu/model/test/test_table.py @@ -24,9 +24,11 @@ """Test table model and obstacles.""" from simu.model.table import Table from simu.model.round_obstacle import RoundObstacle +from simu.model.rectangular_obstacle import RectangularObstacle from simu.inter.drawable import Drawable, DrawableCanvas from simu.utils.vector import vector from Tkinter import * +from math import pi class Area (Drawable): @@ -44,6 +46,14 @@ class Area (Drawable): continue if isinstance (o, RoundObstacle): self.draw_circle (o.pos, o.radius) + elif isinstance (o, RectangularObstacle): + self.trans_push () + self.trans_translate (o.pos) + self.trans_rotate (o.angle) + self.draw_circle ((0, 0), 0.2) + self.draw_rectangle ((o.dim[0] / 2, o.dim[1] / 2), + (-o.dim[0] / 2, -o.dim[1] / 2), fill = '') + self.trans_pop () else: raise TypeError ("unknown obstacle") if self.a is not None: @@ -105,7 +115,7 @@ class TestTable (Frame): text = 'Nearest', value = 'nearest') self.test_nearest.pack (side = 'top') self.new_obstacle_round_frame = LabelFrame (self.right_frame) - self.new_obstacle_round_frame.pack (side = 'top') + self.new_obstacle_round_frame.pack (side = 'top', fill = 'x') self.new_obstacle_radius = Scale (self.new_obstacle_round_frame, label = 'Radius', orient = 'horizontal', from_ = 0.5, to = 10, resolution = 0.5) @@ -115,9 +125,25 @@ class TestTable (Frame): command = self.new_round_obstacle) self.new_obstacle_round_frame.configure ( labelwidget = self.new_obstacle_round) + self.new_obstacle_rect_frame = LabelFrame (self.right_frame) + self.new_obstacle_rect_frame.pack (side = 'top', fill = 'x') + self.new_obstacle_dim0 = Scale (self.new_obstacle_rect_frame, + label = 'Dim[0]', orient = 'horizontal', from_ = 1, to = 10, + resolution = 1) + self.new_obstacle_dim0.pack (side = 'top') + self.new_obstacle_dim1 = Scale (self.new_obstacle_rect_frame, + label = 'Dim[1]', orient = 'horizontal', from_ = 1, to = 10, + resolution = 1) + self.new_obstacle_dim1.pack (side = 'top') + self.new_obstacle_rect = Button (self.new_obstacle_rect_frame, + text = 'New rectangular obstacle', + command = self.new_rectangular_obstacle) + self.new_obstacle_rect_frame.configure ( + labelwidget = self.new_obstacle_rect) self.area_view = AreaView (self) self.area_view.pack (expand = True, fill = 'both') self.area_view.bind ('<1>', self.click) + self.area_view.bind ('<3>', self.rotate) def update (self, draw = True): self.area_view.area.update (self.test_var.get ()) @@ -139,7 +165,7 @@ class TestTable (Frame): objs = [ [ self.area_view.area.a, 0.2, move_a ], [ self.area_view.area.b, 0.2, move_b ] ] for o in self.area_view.area.table.obstacles: - objs.append ([ o.pos, o.radius, o ]) + objs.append ([ o.pos, getattr (o, 'radius', 0.2), o ]) for obj in objs: opos = vector (obj[0]) radius = obj[1] @@ -153,12 +179,30 @@ class TestTable (Frame): self.move = None self.update () + def rotate (self, ev): + pos = vector (self.area_view.screen_coord ((ev.x, ev.y))) + for o in self.area_view.area.table.obstacles: + if o.pos is None or not hasattr (o, 'angle'): + continue + opos = vector (o.pos) + if abs (opos - pos) < 0.2: + o.angle += pi / 4 + self.update () + def new_round_obstacle (self): o = RoundObstacle (self.new_obstacle_radius.get ()) o.pos = (5, -5) self.area_view.area.table.obstacles.append (o) self.update () + def new_rectangular_obstacle (self): + o = RectangularObstacle ((float (self.new_obstacle_dim0.get ()), + float (self.new_obstacle_dim1.get ()))) + o.pos = (5, -5) + o.angle = 0 + self.area_view.area.table.obstacles.append (o) + self.update () + if __name__ == '__main__': app = TestTable () o = RoundObstacle (2) -- cgit v1.2.3 From a0beeed07493245d7048390625bfca7087abfe1b Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 16 Oct 2011 15:26:22 +0200 Subject: host/simu: add eurobot 2012 table --- host/simu/model/table_eurobot2012.py | 93 +++++++++++++++++ host/simu/view/table_eurobot2012.py | 189 +++++++++++++++++++++++++++++++++++ 2 files changed, 282 insertions(+) create mode 100644 host/simu/model/table_eurobot2012.py create mode 100644 host/simu/view/table_eurobot2012.py diff --git a/host/simu/model/table_eurobot2012.py b/host/simu/model/table_eurobot2012.py new file mode 100644 index 00000000..38960513 --- /dev/null +++ b/host/simu/model/table_eurobot2012.py @@ -0,0 +1,93 @@ +# simu - Robot simulation. {{{ +# +# Copyright (C) 2011 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT 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 +# (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. +# +# }}} +"""Table model for Eurobot 2012.""" +import simu.model.table +from simu.model.round_obstacle import RoundObstacle +from simu.model.rectangular_obstacle import RectangularObstacle +from math import pi +import math +import random + +class Table (simu.model.table.Table): + + def __init__ (self, cards = None): + simu.model.table.Table.__init__ (self) + # Well, this is a boring write only code which create every elements. + # Add coins. + self.coins = [ ] + def add_coin (pos, angle, level = 1): + coin = RoundObstacle (60, level) + coin.pos = pos + coin.angle = angle + coin.value = 1 + self.coins.append (coin) + def add_coin_circle (center, radius, start, step, n, level = 1): + angle = start + for i in xrange (n): + pos = (center[0] + radius * math.cos (angle), + center[1] + radius * math.sin (angle)) + add_coin (pos, angle, level) + angle += step + add_coin ((1000, 1500), 0) + add_coin ((2000, 1500), pi) + add_coin ((450, 300), pi) + add_coin ((3000 - 450, 300), 0) + add_coin_circle ((1500, 300), 90, 0, pi / 2, 4) + add_coin_circle ((1500 - 400, 1000), 300 - 60, pi / 4, pi / 4, 7) + add_coin_circle ((1500 + 400, 1000), 300 - 60, pi + pi / 4, pi / 4, 7) + add_coin_circle ((1500 - 400, 1000), 115, pi / 4, pi / 2, 4) + add_coin_circle ((1500 + 400, 1000), 115, pi / 4, pi / 2, 4) + add_coin_circle ((1500 - 400, 1000), 105, pi / 4, pi / 2, 4, 3) + add_coin_circle ((1500 + 400, 1000), 105, pi / 4, pi / 2, 4, 3) + # Add gold bars. + self.gold_bars = [ ] + def add_gold_bar (pos, angle, level = 1): + gold_bar = RectangularObstacle ((150, 70), level) + gold_bar.pos = pos + gold_bar.angle = angle + gold_bar.value = 3 + self.gold_bars.append (gold_bar) + add_gold_bar ((1500, 647), 0) + add_gold_bar ((1500 - 400, 1000 + 125 - 35), 0, 2) + add_gold_bar ((1500 + 400, 1000 + 125 - 35), 0, 2) + add_gold_bar ((1500 - 400, 1000 - 125 + 35), 0, 2) + add_gold_bar ((1500 + 400, 1000 - 125 + 35), 0, 2) + ba = pi / 2 - 0.04995839 + cba = math.cos (ba) + sba = math.sin (ba) + gbx = 400 - (285 + 75) * cba + 35 + gby = 1500 - (285 + 75) * sba + add_gold_bar ((gbx, gby), ba) + add_gold_bar ((3000 - gbx, gby), pi - ba) + # Set random black coins. + nblack = 0 + while nblack < 4: + coin = random.choice (self.coins[2:]) + if coin.value: + coin.value = 0 + nblack += 1 + # Add everything to obstacles. + self.obstacles += self.coins + self.obstacles += self.gold_bars + diff --git a/host/simu/view/table_eurobot2012.py b/host/simu/view/table_eurobot2012.py new file mode 100644 index 00000000..89ada042 --- /dev/null +++ b/host/simu/view/table_eurobot2012.py @@ -0,0 +1,189 @@ +# simu - Robot simulation. {{{ +# +# Copyright (C) 2011 Nicolas Schodet +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT 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 +# (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. +# +# }}} +"""Eurobot 2012 table.""" +from simu.inter.drawable import Drawable +import math +from math import pi + +PURPLE = '#a737ff' +RED = '#fe4543' +BLUE = '#01c3ff' +GREEN = '#81ff00' +BLACK = '#1f1b1d' +YELLOW = '#f7ff00' +WHITE = '#f5fef2' +TRANS = '#c0c2b5' +BROWN = '#9d4e01' + +def draw_coin (d, coin): + d.trans_push () + color = WHITE if coin.value else BLACK + d.draw_circle ((0, 0), coin.radius, fill = color) + d.draw_circle ((0, 0), 7.5, fill = color) + d.trans_translate ((40.5, 0)) + d.draw_rectangle ((-9, -9), (9, 9), fill = color, outline = TRANS) + d.trans_pop () + +class Coin (Drawable): + + def __init__ (self, onto, model): + Drawable.__init__ (self, onto) + self.model = model + self.model.register (self.__notified) + self.__notified () + + def __notified (self): + self.pos = self.model.pos + self.angle = self.model.angle + self.update () + + def draw (self): + self.reset () + if self.pos: + self.trans_translate (self.pos) + self.trans_rotate (self.angle) + draw_coin (self, self.model) + Drawable.draw (self) + +def draw_gold_bar (d, gold_bar): + d.draw_rectangle ((-gold_bar.dim[0] / 2, -gold_bar.dim[1] / 2), + (gold_bar.dim[0] / 2, gold_bar.dim[1] / 2), fill = YELLOW) + d.draw_rectangle ((-gold_bar.dim[0] / 2 + 13, -gold_bar.dim[1] / 2 + 13), + (gold_bar.dim[0] / 2 - 13, gold_bar.dim[1] / 2 - 13), fill = YELLOW) + +class GoldBar (Drawable): + + def __init__ (self, onto, model): + Drawable.__init__ (self, onto) + self.model = model + self.model.register (self.__notified) + self.__notified () + + def __notified (self): + self.pos = self.model.pos + self.angle = self.model.angle + self.update () + + def draw (self): + self.reset () + if self.pos: + self.trans_translate (self.pos) + self.trans_rotate (self.angle) + draw_gold_bar (self, self.model) + Drawable.draw (self) + +class Table (Drawable): + """The table and its elements.""" + + def __init__ (self, onto, model): + Drawable.__init__ (self, onto) + self.model = model + for e in self.model.coins: + if e.level <= 2: + Coin (self, e) + for e in self.model.gold_bars: + GoldBar (self, e) + for e in self.model.coins: + if e.level > 2: + Coin (self, e) + + def draw_both (self, primitive, *args, **kargs): + """Draw a primitive on both sides.""" + primitive (*args, **kargs) + primitive (*((3000 - x, y) for x, y in args), **kargs) + + def draw (self): + # Redraw. + self.reset () + # Table. + self.draw_rectangle ((-22, -22), (3000 + 22, 2000 + 22), fill = BLUE) + self.draw_rectangle ((0, 0), (3000, 2000), fill = BLUE) + # Start zones. + self.draw_rectangle ((0, 2000 - 500), (500, 2000), fill = PURPLE) + self.draw_rectangle ((3000, 2000 - 500), (3000 - 500, 2000), fill = RED) + # Black lines. + pup = 2000 - 500 + 50 + self.draw_both (self.draw_polygon, (500, pup), (650, pup), (650, 0), (630, 0), + (630, pup - 20), (500, pup - 20), fill = BLACK, outline = 'black') + # Ships. + ba = 0.04995839 + self.draw_both (self.draw_polygon, (0, 2000 - 500), (400, 2000 - 500), (325, 0), (0, 0), + fill = BROWN, outline = 'black') + self.draw_both (self.draw_rectangle, (0, 2000 - 500), (400, 2000 - 500 - 18), fill = BROWN) + cba = 750 * math.cos (ba) + sba = 750 * math.sin (ba) + self.draw_both (self.draw_polygon, (325, 0), (325 + sba, cba), (325 + sba + 18, cba), (325 + 18, 0), + fill = BROWN, outline = 'black') + self.draw_both (self.draw_rectangle, (-22, -22), (340, 610 - 22), fill = TRANS) + self.draw_both (self.draw_rectangle, (-4, -4), (340 - 18, 610 - 22 - 18), fill = TRANS) + # Peanut island. + self.draw_both (self.draw_circle, (1500 - 400, 1000), r = 300, fill = YELLOW) + self.draw_rectangle ((1500 - 400 + 141, 1000 + 265), (1500 + 400 - 141, 1000 - 265), fill = YELLOW, outline = '') + self.draw_arc ((1500, 1000 + 750), 550, start = pi + 1, extent = pi - 2, fill = BLUE, outline = '') + self.draw_arc ((1500, 1000 + 750), 550, start = pi + 1.08083, extent = pi - 2 * 1.08083, style = 'arc') + self.draw_arc ((1500, 1000 - 750), 550, start = 1, extent = pi - 2, fill = BLUE, outline = '') + self.draw_arc ((1500, 1000 - 750), 550, start = 1.08083, extent = pi - 2 * 1.08083, style = 'arc') + self.draw_both (self.draw_circle, (1500 - 400, 1000), r = 200, fill = GREEN) + self.draw_circle ((1500, 1000), r = 75, fill = GREEN) + self.draw_both (self.draw_rectangle, (1500 - 400 - 125, 1000 - 125), (1500 - 400 + 125, 1000 + 125), fill = BROWN) + # Map island. + self.draw_arc ((1500, 2000), 400, start = pi, extent = pi, fill = YELLOW) + self.draw_arc ((1500, 2000), 300, start = pi, extent = pi, fill = GREEN) + # Map. + self.draw_rectangle ((1500 - 200 - 22, 2000 + 148), (1500 + 200 + 22, 2000 + 140), fill = BLUE) + self.draw_rectangle ((1500 - 200, 2000 + 140), (1500 + 200, 2000 - 8), fill = BLUE) + self.draw_both (self.draw_rectangle, (1500 - 200 - 22, 2000 + 140), (1500 - 200, 2000 + 125), fill = BLUE) + self.draw_both (self.draw_rectangle, (1500 - 200 - 22, 2000 + 125), (1500 - 200, 2000 - 23), fill = BLUE) + self.draw_rectangle ((1500 - 22, 2000 + 140), (1500 + 22, 2000 + 125), fill = BLUE) + self.draw_rectangle ((1500 - 22, 2000 + 125), (1500 + 22, 2000 + 115), fill = BLUE) + self.draw_rectangle ((1500 - 22, 2000 + 2), (1500 + 22, 2000 - 13), fill = BLUE) + self.draw_rectangle ((1500 - 22, 2000 - 13), (1500 + 22, 2000 - 23), fill = BLUE) + # Bottles. + def draw_bottle (x, color): + self.draw_rectangle ((x - 100, 0), (x + 100, -44), fill = color) + self.draw_rectangle ((x - 100, -44), (x + 100, -66), fill = color) + self.draw_rectangle ((x - 11, 44), (x + 11, 0), fill = color) + self.draw_rectangle ((x - 50, 22), (x + 100, -44), fill = color) + self.draw_polygon ((x - 50, 22), (x - 80, 0), (x - 100, 0), (x - 100, -22), (x - 80, -22), (x - 50, -44), + fill = color, outline = 'black') + draw_bottle (640, PURPLE) + draw_bottle (640 + 477, RED) + draw_bottle (3000 - 640, RED) + draw_bottle (3000 - 640 - 477, PURPLE) + # Axes. + self.draw_line ((0, 200), (0, 0), (200, 0), arrow = 'both') + # Beacons. + self.draw_both (self.draw_rectangle, (-22, 2000 + 22), (-22 - 80, 2000 + 22 + 80), fill = BLACK) + self.draw_both (self.draw_rectangle, (-22, 1000 - 40), (-22 - 80, 1000 + 40), fill = BLACK) + self.draw_both (self.draw_rectangle, (-22, -80 - 22), (-22 - 80, -22), fill = BLACK) + # Children. + Drawable.draw (self) + +if __name__ == '__main__': + from simu.inter.inter import Inter + import simu.model.table_eurobot2012 as model + app = Inter () + m = model.Table () + Table (app.table_view, m) + app.mainloop () -- cgit v1.2.3 From a0d7ee857c15e8ec8bd6c2df98048c2ee41a0e68 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 16 Oct 2011 15:29:10 +0200 Subject: eurobot/2012: new Eurobot rules --- eurobot/2012/E2012_Rules-EN.pdf | Bin 0 -> 1567643 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 eurobot/2012/E2012_Rules-EN.pdf diff --git a/eurobot/2012/E2012_Rules-EN.pdf b/eurobot/2012/E2012_Rules-EN.pdf new file mode 100644 index 00000000..265bc750 Binary files /dev/null and b/eurobot/2012/E2012_Rules-EN.pdf differ -- cgit v1.2.3 From 7872d2b5c319862f943d2c1340ecee1d7721d7f4 Mon Sep 17 00:00:00 2001 From: Florent Duchon Date: Mon, 26 Dec 2011 23:48:32 +0100 Subject: First version of Beacons Simulator --- digital/beacon/simu/Makefile | 12 ++ digital/beacon/simu/avrconfig.h | 39 +++++ digital/beacon/simu/beacon.c | 106 +++++++++++++ digital/beacon/simu/position.c | 103 +++++++++++++ digital/beacon/simu/position.h | 59 +++++++ digital/beacon/simu/simulator.py | 322 +++++++++++++++++++++++++++++++++++++++ 6 files changed, 641 insertions(+) create mode 100644 digital/beacon/simu/Makefile create mode 100644 digital/beacon/simu/avrconfig.h create mode 100644 digital/beacon/simu/beacon.c create mode 100644 digital/beacon/simu/position.c create mode 100644 digital/beacon/simu/position.h create mode 100644 digital/beacon/simu/simulator.py diff --git a/digital/beacon/simu/Makefile b/digital/beacon/simu/Makefile new file mode 100644 index 00000000..132f0b2e --- /dev/null +++ b/digital/beacon/simu/Makefile @@ -0,0 +1,12 @@ +BASE = ../../avr +HOST_PROGS = beacon +beacon_SOURCES = beacon.c position.c +MODULES = math/fixed utils +CONFIGFILE = avrconfig.h +# atmega8, atmega8535, atmega128... +AVR_MCU = atmega128 +# -O2 : speed +# -Os : size +OPTIMIZE = -O2 -lm + +include $(BASE)/make/Makefile.gen diff --git a/digital/beacon/simu/avrconfig.h b/digital/beacon/simu/avrconfig.h new file mode 100644 index 00000000..8da6dbc6 --- /dev/null +++ b/digital/beacon/simu/avrconfig.h @@ -0,0 +1,39 @@ +#ifndef avrconfig_h +#define avrconfig_h +/* avrconfig.h - Path module configuration template. */ +/* avr.path - Path finding module. {{{ + * + * Copyright (C) 2008 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +/* path - Path finding module. */ +/** Report path found for debug. */ +#define AC_PATH_REPORT defined (HOST) +/** Report function name. */ +#define AC_PATH_REPORT_CALLBACK path_report +/** Number of possible obstacles. */ +#define AC_PATH_OBSTACLES_NB 10 +/** Number of points per obstacle. */ +#define AC_PATH_OBSTACLES_POINTS_NB 8 + + +#endif /* avrconfig_h */ diff --git a/digital/beacon/simu/beacon.c b/digital/beacon/simu/beacon.c new file mode 100644 index 00000000..5ea8a263 --- /dev/null +++ b/digital/beacon/simu/beacon.c @@ -0,0 +1,106 @@ +/* beacon.c */ +/* Beacon Simulator Interface. {{{ + * + * Copyright (C) 2011 Florent Duchon + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ +#include +#include +#include +#include "position.h" +#include "common.h" + +/* Globals Declaration */ +beacon_value_s beacon; +opponent_s opponent; + +void syntax (void) +{ + fprintf (stderr, + "beacon sources_number source1 angle1 ... sourceX angleX\n" + "example: beacon 2,1,3.2,3,0.82\n" + ); + exit (1); +} + +void read_tab (const char *s, double *tab, int n) +{ + assert (n > 0); + while (n) + { + char *sp; + double v; + v = strtod (s, &sp); + if (sp == s) + syntax (); + if ((n > 1 && *sp != ',') + || (n == 1 && *sp != '\0')) + syntax (); + s = sp + 1; + *tab++ = v; + n--; + } +} + +int main (int argc, char **argv) +{ + if (argc < 8 || argc > 10) + syntax (); + + double input[7] = {0}; + /* Init global structures */ + init_struct(); + + + /* Read Beacon num */ + read_tab (argv[2], input, 1); + + /* Read Beacon angle */ + read_tab (argv[3], input+1, 1); + + /* Read Beacon num */ + read_tab (argv[4], input+2, 1); + + /* Read Beacon angle */ + read_tab (argv[5], input+3, 1); + + /* Read Activate filter */ + read_tab (argv[6],input+4, 1); + + /* Read last x */ + read_tab (argv[7],input+5, 1); + + /* Read last y */ + read_tab (argv[8],input+6, 1); + +// printf("Balise %d --- Angle %f\n",(int)input[0],input[1]); +// printf("Balise %d --- Angle %f\n",(int)input[2],input[3]); + + /* Compute position */ + update_position(input[0],input[1]); + update_position(input[2],input[3]); + + /* Return position to the simulator */ + printf("%d\n",(int)opponent.x); + printf("%d\n",(int)opponent.y); + + return 0; +} diff --git a/digital/beacon/simu/position.c b/digital/beacon/simu/position.c new file mode 100644 index 00000000..5bc4a8f8 --- /dev/null +++ b/digital/beacon/simu/position.c @@ -0,0 +1,103 @@ +/* position.c */ +/* Beacon triangulation algorithms. {{{ + * + * Copyright (C) 2011 Florent Duchon + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#include +#include "position.h" + +extern beacon_value_s beacon; +extern opponent_s opponent; + +void init_struct(void) +{ + beacon.angle[0]=0; + beacon.angle[1]=0; + beacon.angle[2]=0; + beacon.angle[3]=0; + beacon.last_updated_beacon = 0; + beacon.before_last_updated_beacon = 0; + opponent.x = 0; + opponent.y = 0; +} + +float degree_to_radian(int value) +{ + float temp = 0; + temp = (value * 2 * M_PI) / 360; + return temp; +} + +void update_position(int beacon_number,double angle) +{ + int which_formula = 0; + if(beacon_number == beacon.last_updated_beacon) + { + beacon.last_updated_beacon = beacon_number; + } + else + { + beacon.before_last_updated_beacon = beacon.last_updated_beacon; + beacon.last_updated_beacon = beacon_number; + } + which_formula = beacon.before_last_updated_beacon + beacon.last_updated_beacon; +// printf("[position.c] => Update_position beacon_number = %d angle = %f\n",(int)beacon_number,(double)angle); + beacon.angle[beacon_number] = angle; + + switch(which_formula) + { + case 3: +// printf("[position.c] => Formula 3\r\n"); +// printf("[position.c] => angle[1] = %f angle[2] = %f\n",beacon.angle[1],beacon.angle[2]); + opponent.x = LARGEUR_TABLE * tan(beacon.angle[2]) * tan(beacon.angle[1]) / (tan(beacon.angle[2])+tan(beacon.angle[1])); + opponent.y = LARGEUR_TABLE * tan(beacon.angle[1]) / (tan(beacon.angle[2])+tan(beacon.angle[1])); + break; + case 4: +// printf("[position.c] => Formula 4\r\n"); + if(beacon.angle[3] > M_PI/2) + { + opponent.y = (LARGEUR_DEMI_TABLE*tan(M_PI - beacon.angle[3]) - LARGEUR_TABLE*tan(beacon.angle[1]) + LONGUEUR_TABLE) / (tan(M_PI - beacon.angle[3]) - tan(beacon.angle[1])); + } + else + { + opponent.y = (LARGEUR_DEMI_TABLE*tan(beacon.angle[3]) + LARGEUR_TABLE*tan(beacon.angle[1])-LONGUEUR_TABLE) / (tan(beacon.angle[1]) + tan(beacon.angle[3])); + } + opponent.x = (LARGEUR_TABLE - opponent.y)*tan(beacon.angle[1]); + break; + case 5: +// printf("[position.c] => formula 5\r\n"); + if(beacon.angle[3] > M_PI/2) + { + opponent.y = (LONGUEUR_TABLE + LARGEUR_DEMI_TABLE * tan(M_PI - beacon.angle[3])) / (tan(beacon.angle[2]) + tan(M_PI - beacon.angle[3])); + } + else + { + opponent.y = (LARGEUR_DEMI_TABLE*tan(beacon.angle[3]) - LONGUEUR_TABLE) / (tan(beacon.angle[3]) - tan(beacon.angle[2])); + } + opponent.x = tan(beacon.angle[2]) * opponent.y; + break; + default: +// printf("[position.c] => Unknown Formula\r\n"); + break; + } +} \ No newline at end of file diff --git a/digital/beacon/simu/position.h b/digital/beacon/simu/position.h new file mode 100644 index 00000000..de962e5a --- /dev/null +++ b/digital/beacon/simu/position.h @@ -0,0 +1,59 @@ +/* position.h */ +/* Beacon triangulation algorithms. {{{ + * + * Copyright (C) 2011 Florent Duchon + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT 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 + * (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. + * + * }}} */ + +#ifndef _POSITION_H +#define _POSITION_H + +#define LARGEUR_TABLE 2000 +#define LONGUEUR_TABLE 3000 +#define LARGEUR_DEMI_TABLE LARGEUR_TABLE/2 +#define M_PI 3.14 + +typedef struct +{ + float angle[4]; + int last_updated_beacon; + int before_last_updated_beacon; + +}beacon_value_s; + +typedef struct +{ + int x; + int y; + int old_x; + int old_y; +}opponent_s; + +typedef struct +{ + int lost_beacon; +}status_s; + +void init_struct(void); +void update_position(int beacon_number,double angle); +void distance_filter(void); + +#endif \ No newline at end of file diff --git a/digital/beacon/simu/simulator.py b/digital/beacon/simu/simulator.py new file mode 100644 index 00000000..4284b3d0 --- /dev/null +++ b/digital/beacon/simu/simulator.py @@ -0,0 +1,322 @@ +# simu - Beacon simulation. {{{ +# +# Copyright (C) 2011 Florent DUCHON +# +# APBTeam: +# Web: http://apbteam.org/ +# Email: team AT apbteam DOT 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 +# (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. +# +# }}} + +"""Graphic interface for beacon simulator.""" + +import re +from Tkinter import * +from simu.inter.drawable import * +from subprocess import Popen, PIPE +from math import pi +import math +import random +import decimal + +class Obstacle: + def __init__ (self, pos, radius, factor): + self.pos = pos + self.radius = radius + self.factor = factor + + def move (self, pos): + self.pos = pos + + +class Beacon (Drawable): + def __init__ (self,onto, num, pos, orientation, size): + Drawable.__init__ (self, onto) + self.id = num # ID + self.pos = pos # position (x,y) + self.orientation = orientation # orientation in degrees + self.size = size # side size + self.angle = 0 # obstacles angle + self.mode = 0 # 0 = deactivated + # 1 = activated + def draw_beacon (self): + # Color depends of the beacon mode + if self.mode is 1: + color = 'black' + else: + color = 'red' + # Draw it + self.draw_rectangle ((self.pos[0]-self.size/2,self.pos[1]-self.size/2),(self.pos[0]+self.size/2,self.pos[1]+self.size/2), fill = color) + + def draw_axes (self): + self.draw_line (self.pos,(self.pos[0]+(300)*math.cos(math.radians(self.orientation)),self.pos[1]+300*math.sin(math.radians(self.orientation))),fill='red', arrow = LAST) + self.draw_line (self.pos,(self.pos[0]+(300)*math.cos(math.radians(self.orientation+90)),self.pos[1]+300*math.sin(math.radians(self.orientation+90))),fill='red', arrow = LAST) + + def toogle_mode (self): + if self.mode is 0: + self.mode = 1 + else: + self.mode = 0 + + +class Area (Drawable): + def __init__ (self, onto, border_min, border_max): + Drawable.__init__ (self, onto) + self.border_min = border_min + self.border_max = border_max + self.border = None + self.phantoms_counter = 0 + self.obstacles = [ ] + self.beacons = [ ] + self.phantoms = [ ] # virtual obstacles computed by the beacon module. + + def draw (self): + self.reset () + self.draw_rectangle (self.border_min, self.border_max, fill = 'grey') + self.draw_rectangle ((self.border_min[0],self.border_max[1]-500),(self.border_min[0]+500,self.border_max[1]), fill = 'blue') + self.draw_rectangle ((self.border_max[0]-500,self.border_max[1]-500), self.border_max, fill = 'red') + self.draw_line ((0,0),(150,0),fill='black', arrow = LAST) + self.draw_line ((0,0),(0,150),fill='black', arrow = LAST) + + for b in self.beacons: + if b.pos is not None: + b.draw_beacon () + for o in self.obstacles: + if o.pos is not None: + self.draw_circle (o.pos, o.radius,fill = o.factor and 'gray50' or 'gray25') + for p in self.phantoms: + if p is not None: + self.draw_circle (p, 20,fill = 0 and 'gray50' or 'red') + + def show_angles (self): + for b in self.beacons: + for o in self.obstacles: + self.draw_line ((b.pos[0],b.pos[1]),(o.pos[0],o.pos[1]),fill='cyan', arrow = NONE) + + def populate (self): + self.obstacles.append (Obstacle ((500, 500), 200, 0)) + self.beacons.append (Beacon (self, 1, (-40,2040), 270,80)) + self.beacons.append (Beacon (self, 2, (-40,-40),0,80)) + self.beacons.append (Beacon (self, 3, (3040,1000), 180,80)) + + def add_phantom (self,center): + # Only take care of the 100 latest value. Delete previous one. + if self.phantoms_counter is 100: + del self.phantoms[0] + else: + self.phantoms_counter += 1 + self.phantoms.append(center) + + +class AreaView (DrawableCanvas): + def __init__ (self, border_min, border_max, master = None): + self.border_min = border_min + self.border_max = border_max + width = border_max[0] - border_min[0] + height = border_max[1] - border_min[0] + DrawableCanvas.__init__ (self, width * 1.3, height * 1.3, -width / 2,-height / 2,master, borderwidth = 1, relief = 'sunken', background = 'white') + self.area = Area (self, border_min, border_max) + self.area.populate () + + def draw (self): + self.area.draw () + + def add_phantom (self,center): + self.area.add_phantom(center) + + +class beacon_simu (Frame): + def __init__ (self, border_min, border_max, master = None): + Frame.__init__ (self, master) + self.pack (expand = 1, fill = 'both') + self.createWidgets (border_min, border_max) + self.robot_pos.set("Robot position = (0 , 0)") + self.phantom_pos.set("Last phantom position = ") + + def createWidgets (self, border_min, border_max): + # Bottom Panel + self.bottomFrame = Frame (self) + self.bottomFrame.pack (side = 'bottom', fill = 'both') + + # First subPanel for display options + self.subPanel1 = Frame (self.bottomFrame) + self.subPanel1.pack (side = 'left', fill = 'both') + self.display_axes = IntVar () + self.axesButton = Checkbutton (self.subPanel1,variable = self.display_axes, command = self.update,text = 'Display axes', indicatoron = True) + self.axesButton.pack (anchor='w') + self.display_angles = IntVar () + self.anglesButton = Checkbutton (self.subPanel1,variable = self.display_angles, command = self.update,text = 'Display angles', indicatoron = True) + self.anglesButton.pack (anchor='w') + + # Second subPanel for simulator options + self.subPanel2 = Frame (self.bottomFrame) + self.subPanel2.pack (side = 'left', fill = 'both') + self.mode = StringVar() + self.manualRadioButton = Radiobutton (self.subPanel2, text = 'Manual', variable = self.mode, value='manual').pack(anchor='w') + self.autoRadioButton = Radiobutton (self.subPanel2, text = 'Auto', variable = self.mode, value='auto').pack(anchor='w') + + # Third subPanel for simulator options + self.subPanel3 = Frame (self.bottomFrame) + self.subPanel3.pack (side = 'left', fill = 'both') + self.precisionScale = Scale (self.subPanel3, label = 'Precision', orient = 'horizontal', from_ = 1, to = 3) + self.precisionScale.pack () + + # Fourth subPanel for buttons + self.subPanel4 = Frame (self.bottomFrame) + self.subPanel4.pack (side = 'left', fill = 'both') + self.clearPhantomsButton = Button (self.subPanel4, text = 'Clear', command = self.clear_phantoms) + self.clearPhantomsButton.pack (side = 'bottom') + self.startButton = Button (self.subPanel4, text = 'Start', command = self.start) + self.startButton.pack (side = 'top') + + # Fifth subPanel for Label + self.subPanel5 = Frame (self.bottomFrame) + self.subPanel5.pack (side = 'left', fill = 'both') + self.robot_pos = StringVar() + self.label = Label(self.subPanel5, textvariable=self.robot_pos).pack(anchor='w') + self.phantom_pos = StringVar() + self.label = Label(self.subPanel5, textvariable=self.phantom_pos).pack(anchor='w') + + # Sixth subPanel for Exit button + self.subPanel6 = Frame (self.bottomFrame) + self.subPanel6.pack (side = 'right', fill = 'both') + self.quitButton = Button (self.subPanel6, text = 'Quit', command = self.quit) + self.quitButton.pack (side = 'right', fill = 'both') + + self.areaview = AreaView (border_min, border_max, self) + self.areaview.pack (expand = True, fill = 'both') + self.areaview.bind ('<1>', self.click) + + def clear (self): + self.areaview.area.draw () + + def clear_phantoms (self): + del self.areaview.area.phantoms[:] + self.areaview.area.phantoms_counter = 0 + self.update() + + def update (self): + self.areaview.area.update () + self.areaview.area.draw () + if self.display_angles.get() is 1: + self.areaview.area.show_angles() + if self.display_axes.get() is 1: + for b in self.areaview.area.beacons: + b.draw_axes() + + def click (self, ev): + pos = self.areaview.screen_coord ((ev.x, ev.y)) + + # Update obstacles position + for o in self.areaview.area.obstacles: + if self.areaview.area.border_min[0] < pos[0] < self.areaview.area.border_max[0] and self.areaview.area.border_min[1] < pos[1] < self.areaview.area.border_max[1]: + o.pos = pos + self.robot_pos.set("Robot position = (%.0f , %.0f)" % pos) + + # Check beacon mode + for b in self.areaview.area.beacons: + dx = abs(b.pos[0] - pos[0]) + dy = abs(b.pos[1] - pos[1]) + if dx < b.size and dy < b.size: + b.toogle_mode () + # Update area + self.update () + + def call_algorithm (self,num1,num2): + args = [0,0,0,0,0,0,0,0] + for b in self.areaview.area.beacons: + if b.id is num1: + args[1]=num1 + args[2]=b.angle + if b.id is num2: + args[3]=num2 + args[4]=b.angle + args = [repr(a) for a in args] + args[0:0] = [ './beacon.host' ] + p = Popen (args, stdout = PIPE) + output = p.communicate ()[0] + del p + output = output.split ('\n') + return output + + def rotate_beacons (self): # Simulate a rotation for a all beacons, ie set beacon.angles. + # Set the requested imprecision + imprecision = self.precisionScale.get () #1 to 3 degrees + imprecision = int (math.radians(imprecision)*1000) + imprecision = decimal.Decimal(random.randrange(-imprecision,imprecision))/1000 + # Compute angles for every beaconss + for o in self.areaview.area.obstacles: + for b in self.areaview.area.beacons: + if b.id is 1: + b.angle = math.atan(o.pos[0]/(2000-o.pos[1]))+float(imprecision) + if b.id is 2: + b.angle = math.atan(o.pos[0]/o.pos[1])+float(imprecision) + if b.id is 3: + b.angle = math.atan((3000-o.pos[0])/(1000-o.pos[1]))+float(imprecision) + if b.angle < 0: + b.angle = pi - abs(b.angle) + + def manual_mode (self): + # Manual mode : warning : two beacons must already be activated + self.rotate_beacons () + temp = [0,0,0] + i = 0 + for b in self.areaview.area.beacons: + if b.mode is 1: + temp[i] = b.id + i=i+1 + phantom_pos = self.call_algorithm(temp[0],temp[1]) + self.areaview.add_phantom((int (phantom_pos[0]),int (phantom_pos[1]))) + self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1]))) + self.update () + + def automatic_mode (self): + # Automatic mode : all beacons are used + self.rotate_beacons () + + # Randomly select two beacons and update obstacles position + exclude = random.randrange(1,4) + if exclude is 1: + phantom_pos = self.call_algorithm(2,3) + if exclude is 2: + phantom_pos = self.call_algorithm(1,3) + if exclude is 3: + phantom_pos = self.call_algorithm(1,2) + + # Draw the computed position + self.areaview.add_phantom((int (phantom_pos[0]),int (phantom_pos[1]))) + self.phantom_pos.set("Last phantom position = (%.0f , %.0f)" %(float(phantom_pos[0]),float(phantom_pos[1]))) + + def start (self) : + if self.mode.get() == "manual": + self.manual_mode() + return + if self.mode.get() == "auto": + #for o in self.areaview.area.obstacles: + #x = o.pos[0] + float (decimal.Decimal(random.randrange(-5,40))) + #y = o.pos[1] + float (decimal.Decimal(random.randrange(-25,25))) + #o.pos = (x,y) + self.automatic_mode() + self.after (30,self.start) + self.update() + else: + print "No mode selected" + return + +if __name__ == '__main__': + app = beacon_simu ((0, 0), (3000, 2000)) + app.mainloop () -- cgit v1.2.3 From 8ddd21a6f3c65cd94b794cb25e6fa74dc20da3cc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 9 Feb 2012 21:23:47 +0100 Subject: digital/ai/tools, host/simu/robots: simulate several robots --- digital/ai/tools/marcel.py | 58 ++++++++++++----------- digital/ai/tools/robospierre.py | 62 ++++++++++++++----------- digital/ai/tools/test_simu.py | 80 +++++++++++++------------------- host/simu/robots/aquajim/link/bag.py | 6 +-- host/simu/robots/giboulee/link/bag.py | 6 +-- host/simu/robots/marcel/link/bag.py | 8 ++-- host/simu/robots/robospierre/link/bag.py | 8 ++-- 7 files changed, 113 insertions(+), 115 deletions(-) diff --git a/digital/ai/tools/marcel.py b/digital/ai/tools/marcel.py index 08314ca3..001eca5e 100644 --- a/digital/ai/tools/marcel.py +++ b/digital/ai/tools/marcel.py @@ -1,10 +1,3 @@ -import simu.model.table_eurobot2010 -import simu.view.table_eurobot2010 - -import simu.robots.marcel.link.bag -import simu.robots.marcel.model.bag -import simu.robots.marcel.view.bag - import asserv import asserv.init import mimot @@ -16,24 +9,37 @@ from proto.popen_io import PopenIO import math class Robot: + """Marcel robot instance.""" + + import simu.model.table_eurobot2010 as table_model + import simu.view.table_eurobot2010 as table_view + + import simu.robots.marcel.link.bag as robot_link + import simu.robots.marcel.model.bag as robot_model + import simu.robots.marcel.view.bag as robot_view + + robot_start_pos = { + False: (300, 2100 - 305, math.radians (-270)), + True: (3000 - 300, 2100 - 305, math.radians (-270)) + } + + client_nb = 3 - def __init__ (self, proto_time): - self.table_model = simu.model.table_eurobot2010 - self.table_view = simu.view.table_eurobot2010 - self.robot_link = simu.robots.marcel.link.bag - self.robot_model = simu.robots.marcel.model.bag - self.robot_view = simu.robots.marcel.view.bag - asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', 'marcel') - mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'marcel') - io_cmd = ('../../io/src/io.host') - self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, - **asserv.init.host['marcel']) - self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time, - **mimot.init.host['marcel']) - self.io = io.Proto (PopenIO (io_cmd), proto_time, - **io.init.host['marcel']) - self.robot_start_pos = { - False: (300, 2100 - 305, math.radians (-270)), - True: (3000 - 300, 2100 - 305, math.radians (-270)) - } + def __init__ (self, proto_time, instance = 'robot0'): + self.instance = instance + def proto (proto_class, cmd, init): + cmd = [ s.format (instance = instance) for s in cmd ] + return proto_class (PopenIO (cmd), proto_time, **init) + asserv_cmd = ('../../asserv/src/asserv/asserv.host', + '-i{instance}:asserv0', '-m9', 'marcel') + mimot_cmd = ('../../mimot/src/dirty/dirty.host', + '-i{instance}:mimot0', '-m9', 'marcel') + io_cmd = ('../../io/src/io.host', '-i{instance}:io0') + self.asserv = proto (asserv.Proto, asserv_cmd, + asserv.init.host['marcel']) + self.mimot = proto (mimot.Proto, mimot_cmd, + mimot.init.host['marcel']) + self.io = proto (io.Proto, io_cmd, + io.init.host['marcel']) + self.protos = (self.asserv, self.mimot, self.io) diff --git a/digital/ai/tools/robospierre.py b/digital/ai/tools/robospierre.py index 6a44883e..0d258b34 100644 --- a/digital/ai/tools/robospierre.py +++ b/digital/ai/tools/robospierre.py @@ -1,10 +1,3 @@ -import simu.model.table_eurobot2011 -import simu.view.table_eurobot2011 - -import simu.robots.robospierre.link.bag -import simu.robots.robospierre.model.bag -import simu.robots.robospierre.view.bag - import asserv import asserv.init import mimot @@ -16,26 +9,39 @@ from proto.popen_io import PopenIO import math class Robot: + """Robospierre robot instance.""" + + import simu.model.table_eurobot2011 as table_model + import simu.view.table_eurobot2011 as table_view + + import simu.robots.robospierre.link.bag as robot_link + import simu.robots.robospierre.model.bag as robot_model + import simu.robots.robospierre.view.bag as robot_view + + robot_start_pos = { + # In real life, better place the robot in green zone. + False: (300, 2100 - 200, math.radians (180)), + True: (3000 - 300, 2100 - 200, math.radians (0)) + } + + client_nb = 3 - def __init__ (self, proto_time): - self.table_model = simu.model.table_eurobot2011 - self.table_view = simu.view.table_eurobot2011 - self.robot_link = simu.robots.robospierre.link.bag - self.robot_model = simu.robots.robospierre.model.bag - self.robot_view = simu.robots.robospierre.view.bag - asserv_cmd = ('../../asserv/src/asserv/asserv.host', '-m9', - 'robospierre') - mimot_cmd = ('../../mimot/src/dirty/dirty.host', '-m9', 'robospierre') - io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host') - self.asserv = asserv.Proto (PopenIO (asserv_cmd), proto_time, - **asserv.init.host['robospierre']) - self.mimot = mimot.Proto (PopenIO (mimot_cmd), proto_time, - **mimot.init.host['robospierre']) - self.io = io_hub.Proto (PopenIO (io_hub_cmd), proto_time, - **io_hub.init.host['robospierre']) - self.robot_start_pos = { - # In real life, better place the robot in green zone. - False: (300, 2100 - 200, math.radians (180)), - True: (3000 - 300, 2100 - 200, math.radians (0)) - } + def __init__ (self, proto_time, instance = 'robot0'): + self.instance = instance + def proto (proto_class, cmd, init): + cmd = [ s.format (instance = instance) for s in cmd ] + return proto_class (PopenIO (cmd), proto_time, **init) + asserv_cmd = ('../../asserv/src/asserv/asserv.host', + '-i{instance}:asserv0', '-m9', 'robospierre') + mimot_cmd = ('../../mimot/src/dirty/dirty.host', + '-i{instance}:mimot0', '-m9', 'robospierre') + io_hub_cmd = ('../../io-hub/src/robospierre/io_hub.host', + '-i{instance}:io0') + self.asserv = proto (asserv.Proto, asserv_cmd, + asserv.init.host['robospierre']) + self.mimot = proto (mimot.Proto, mimot_cmd, + mimot.init.host['robospierre']) + self.io = proto (io_hub.Proto, io_hub_cmd, + io_hub.init.host['robospierre']) + self.protos = (self.asserv, self.mimot, self.io) diff --git a/digital/ai/tools/test_simu.py b/digital/ai/tools/test_simu.py index 80008bb9..50eaf265 100644 --- a/digital/ai/tools/test_simu.py +++ b/digital/ai/tools/test_simu.py @@ -49,32 +49,29 @@ class ObstacleWithBeacon (obstacle_view.RoundObstacle): class TestSimu (InterNode): """Interface, with simulated programs.""" - def __init__ (self, robot_class): + def __init__ (self, robot_class, robot_nb = 1): # Hub. - self.hub = mex.hub.Hub (min_clients = 4) + self.hub = mex.hub.Hub (min_clients = 1 + robot_class.client_nb + * robot_nb) self.forked_hub = utils.forked.Forked (self.hub.wait) # InterNode. InterNode.__init__ (self) def proto_time (): return self.node.date / self.node.tick - # Robot parameters. - robot = robot_class (proto_time) - self.robot = robot - # Asserv. - self.asserv = robot.asserv - self.asserv.async = True - self.tk.createfilehandler (self.asserv, READABLE, self.asserv_read) - # Mimot. - self.mimot = robot.mimot - self.mimot.async = True - self.tk.createfilehandler (self.mimot, READABLE, self.mimot_read) - # Io. - self.io = robot.io - self.io.async = True - self.tk.createfilehandler (self.io, READABLE, self.io_read) + # Robot instances. + self.robots = [ robot_class (proto_time, 'robot%d' % i) + for i in xrange (robot_nb) ] + for r in self.robots: + for prog in r.protos: + prog.async = True + def prog_read (f, mask, prog = prog): + prog.proto.read () + prog.proto.sync () + self.tk.createfilehandler (prog, READABLE, prog_read) # Add table. - self.table_model = robot.table_model.Table () - self.table = robot.table_view.Table (self.table_view, self.table_model) + self.table_model = robot_class.table_model.Table () + self.table = robot_class.table_view.Table (self.table_view, + self.table_model) self.obstacle = obstacle_model.RoundObstacle (150) self.table_model.obstacles.append (self.obstacle) self.obstacle_beacon = obstacle_model.RoundObstacle (40, 2) @@ -82,43 +79,30 @@ class TestSimu (InterNode): self.obstacle_view = ObstacleWithBeacon (self.table, self.obstacle, self.obstacle_beacon) self.table_view.bind ('<2>', self.place_obstacle) - # Add robot. - self.robot_link = robot.robot_link.Bag (self.node) - self.robot_model = robot.robot_model.Bag (self.node, self.table_model, - self.robot_link) - self.robot_view = robot.robot_view.Bag (self.table, - self.actuator_view, self.sensor_frame, self.robot_model) - # Color switch. - self.robot_model.color_switch.register (self.change_color) + # Add robots. + for r in self.robots: + r.link = r.robot_link.Bag (self.node, r.instance) + r.model = r.robot_model.Bag (self.node, self.table_model, r.link) + r.view = r.robot_view.Bag (self.table, self.actuator_view, + self.sensor_frame, r.model) + # Color switch. + def change_color (r = r): + i = r.model.color_switch.state + r.asserv.set_simu_pos (*r.robot_start_pos[i]); + r.model.color_switch.register (change_color) def close (self): self.forked_hub.kill () import time time.sleep (1) - def asserv_read (self, file, mask): - self.asserv.proto.read () - self.asserv.proto.sync () - - def mimot_read (self, file, mask): - self.mimot.proto.read () - self.mimot.proto.sync () - - def io_read (self, file, mask): - self.io.proto.read () - self.io.proto.sync () - def step (self): """Overide step to handle retransmissions, could be made cleaner using simulated time.""" InterNode.step (self) - self.asserv.proto.sync () - self.mimot.proto.sync () - self.io.proto.sync () - - def change_color (self, *dummy): - i = self.robot_model.color_switch.state - self.asserv.set_simu_pos (*self.robot.robot_start_pos[i]); + for r in self.robots: + for prog in r.protos: + prog.proto.sync () def place_obstacle (self, ev): pos = self.table_view.screen_coord ((ev.x, ev.y)) @@ -132,6 +116,8 @@ def run (default_robot, test_class = TestSimu): parser = optparse.OptionParser () parser.add_option ('-r', '--robot', help = "use specified robot", metavar = 'NAME', default = default_robot) + parser.add_option ('-n', '--robot-nb', help = "number of robots", + type = 'int', metavar = 'NB', default = 1) (options, args) = parser.parse_args () if args: parser.error ("too many arguments") @@ -143,7 +129,7 @@ def run (default_robot, test_class = TestSimu): robot = robospierre.Robot else: parser.error ("unknown robot") - app = test_class (robot) + app = test_class (robot, options.robot_nb) app.mainloop () app.close () diff --git a/host/simu/robots/aquajim/link/bag.py b/host/simu/robots/aquajim/link/bag.py index 3cb9e1b2..f0f96b9d 100644 --- a/host/simu/robots/aquajim/link/bag.py +++ b/host/simu/robots/aquajim/link/bag.py @@ -27,7 +27,7 @@ import asserv.mex class Bag: - def __init__ (self, node): - self.asserv = asserv.mex.Mex (node) - self.io = io.mex.Mex (node) + def __init__ (self, node, instance = 'robot0'): + self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance) + self.io = io.mex.Mex (node, '%s:io0' % instance) diff --git a/host/simu/robots/giboulee/link/bag.py b/host/simu/robots/giboulee/link/bag.py index c8c1b285..4dbf0554 100644 --- a/host/simu/robots/giboulee/link/bag.py +++ b/host/simu/robots/giboulee/link/bag.py @@ -27,7 +27,7 @@ import asserv.mex class Bag: - def __init__ (self, node): - self.asserv = asserv.mex.Mex (node) - self.io = io.mex.Mex (node) + def __init__ (self, node, instance = 'robot0'): + self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance) + self.io = io.mex.Mex (node, '%s:io0' % instance) diff --git a/host/simu/robots/marcel/link/bag.py b/host/simu/robots/marcel/link/bag.py index 7cb34c28..c0e36dea 100644 --- a/host/simu/robots/marcel/link/bag.py +++ b/host/simu/robots/marcel/link/bag.py @@ -28,8 +28,8 @@ import mimot.mex class Bag: - def __init__ (self, node): - self.asserv = asserv.mex.Mex (node) - self.io = io.mex.Mex (node) - self.mimot = mimot.mex.Mex (node) + def __init__ (self, node, instance = 'robot0'): + self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance) + self.io = io.mex.Mex (node, '%s:io0' % instance) + self.mimot = mimot.mex.Mex (node, '%s:mimot0' % instance) diff --git a/host/simu/robots/robospierre/link/bag.py b/host/simu/robots/robospierre/link/bag.py index ac68889a..e3368d55 100644 --- a/host/simu/robots/robospierre/link/bag.py +++ b/host/simu/robots/robospierre/link/bag.py @@ -28,8 +28,8 @@ import mimot.mex class Bag: - def __init__ (self, node): - self.asserv = asserv.mex.Mex (node) - self.io_hub = io_hub.mex.Mex (node) - self.mimot = mimot.mex.Mex (node) + def __init__ (self, node, instance = 'robot0'): + self.asserv = asserv.mex.Mex (node, '%s:asserv0' % instance) + self.io_hub = io_hub.mex.Mex (node, '%s:io0' % instance) + self.mimot = mimot.mex.Mex (node, '%s:mimot0' % instance) -- cgit v1.2.3 From 5a431895efe1c573c763a8e1197a011a36442837 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 9 Feb 2012 21:30:17 +0100 Subject: eurobot/2012: add project document --- eurobot/2012/planete-sciences/dossier_projet_2012.odt | Bin 0 -> 577262 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 eurobot/2012/planete-sciences/dossier_projet_2012.odt diff --git a/eurobot/2012/planete-sciences/dossier_projet_2012.odt b/eurobot/2012/planete-sciences/dossier_projet_2012.odt new file mode 100644 index 00000000..a9cbe10a Binary files /dev/null and b/eurobot/2012/planete-sciences/dossier_projet_2012.odt differ -- cgit v1.2.3 From c3fe438f1b60f20f6f5425db895777ee5248637f Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sat, 11 Feb 2012 16:22:57 +0100 Subject: digital/mimot, digital/asserv: fix overflow in speed control code When substracting two values, one extra bit is needed. Use an unsigned number to have this extra bit. --- digital/asserv/src/asserv/speed.c | 20 ++++++++++++++------ digital/mimot/src/dirty/speed.c | 20 ++++++++++++++------ 2 files changed, 28 insertions(+), 12 deletions(-) diff --git a/digital/asserv/src/asserv/speed.c b/digital/asserv/src/asserv/speed.c index 5c221b7e..32102177 100644 --- a/digital/asserv/src/asserv/speed.c +++ b/digital/asserv/src/asserv/speed.c @@ -58,13 +58,21 @@ speed_init (void) static void speed_update_by_speed (struct speed_t *speed) { - /* Update current speed. */ - if (UTILS_ABS (speed->cons - speed->cur) < speed->acc) - speed->cur = speed->cons; - else if (speed->cons > speed->cur) - speed->cur += speed->acc; + /* Update current speed (be careful of overflow!). */ + if (speed->cons > speed->cur) + { + if ((uint16_t) (speed->cons - speed->cur) < (uint16_t) speed->acc) + speed->cur = speed->cons; + else + speed->cur += speed->acc; + } else - speed->cur -= speed->acc; + { + if ((uint16_t) (speed->cur - speed->cons) < (uint16_t) speed->acc) + speed->cur = speed->cons; + else + speed->cur -= speed->acc; + } } /** Compute maximum allowed speed according to: distance left, maximum speed, diff --git a/digital/mimot/src/dirty/speed.c b/digital/mimot/src/dirty/speed.c index f9d4193e..80ca3d6b 100644 --- a/digital/mimot/src/dirty/speed.c +++ b/digital/mimot/src/dirty/speed.c @@ -53,13 +53,21 @@ speed_init (void) static void speed_update_by_speed (struct speed_t *speed) { - /* Update current speed. */ - if (UTILS_ABS (speed->cons - speed->cur) < speed->acc) - speed->cur = speed->cons; - else if (speed->cons > speed->cur) - speed->cur += speed->acc; + /* Update current speed (be careful of overflow!). */ + if (speed->cons > speed->cur) + { + if ((uint16_t) (speed->cons - speed->cur) < (uint16_t) speed->acc) + speed->cur = speed->cons; + else + speed->cur += speed->acc; + } else - speed->cur -= speed->acc; + { + if ((uint16_t) (speed->cur - speed->cons) < (uint16_t) speed->acc) + speed->cur = speed->cons; + else + speed->cur -= speed->acc; + } } /** Compute maximum allowed speed according to: distance left, maximum speed, -- cgit v1.2.3 From dcb79f383269440ec2c5a54b6d7792fbb0110d5a Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Mon, 5 Mar 2012 21:47:19 +0100 Subject: digital/ai/tools: fix test_simu_control scripts --- digital/ai/tools/test_simu.py | 11 ++++++----- digital/ai/tools/test_simu_control_marcel.py | 7 +++++-- digital/ai/tools/test_simu_control_robospierre.py | 7 +++++-- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/digital/ai/tools/test_simu.py b/digital/ai/tools/test_simu.py index 50eaf265..1b804e22 100644 --- a/digital/ai/tools/test_simu.py +++ b/digital/ai/tools/test_simu.py @@ -49,7 +49,7 @@ class ObstacleWithBeacon (obstacle_view.RoundObstacle): class TestSimu (InterNode): """Interface, with simulated programs.""" - def __init__ (self, robot_class, robot_nb = 1): + def __init__ (self, robot_class, robot_nb = 1, color_switch = True): # Hub. self.hub = mex.hub.Hub (min_clients = 1 + robot_class.client_nb * robot_nb) @@ -86,10 +86,11 @@ class TestSimu (InterNode): r.view = r.robot_view.Bag (self.table, self.actuator_view, self.sensor_frame, r.model) # Color switch. - def change_color (r = r): - i = r.model.color_switch.state - r.asserv.set_simu_pos (*r.robot_start_pos[i]); - r.model.color_switch.register (change_color) + if color_switch: + def change_color (r = r): + i = r.model.color_switch.state + r.asserv.set_simu_pos (*r.robot_start_pos[i]) + r.model.color_switch.register (change_color) def close (self): self.forked_hub.kill () diff --git a/digital/ai/tools/test_simu_control_marcel.py b/digital/ai/tools/test_simu_control_marcel.py index 2a43b7bb..1d5563e0 100644 --- a/digital/ai/tools/test_simu_control_marcel.py +++ b/digital/ai/tools/test_simu_control_marcel.py @@ -28,8 +28,11 @@ import math class TestSimuControl (TestSimu): """Interface with extra control.""" - def __init__ (self, robot_class): - TestSimu.__init__ (self, robot_class) + def __init__ (self, robot_class, *args): + TestSimu.__init__ (self, robot_class, *args, color_switch = False) + self.io = self.robots[0].io + self.asserv = self.robots[0].asserv + self.mimot = self.robots[0].mimot def create_widgets (self): TestSimu.create_widgets (self) diff --git a/digital/ai/tools/test_simu_control_robospierre.py b/digital/ai/tools/test_simu_control_robospierre.py index 1795e17f..c1a16c3c 100644 --- a/digital/ai/tools/test_simu_control_robospierre.py +++ b/digital/ai/tools/test_simu_control_robospierre.py @@ -32,8 +32,11 @@ class TestSimuControl (TestSimu): ROTATION_STROKE = 0x233e - def __init__ (self, robot_class): - TestSimu.__init__ (self, robot_class) + def __init__ (self, robot_class, *args): + TestSimu.__init__ (self, robot_class, *args, color_switch = False) + self.io = self.robots[0].io + self.asserv = self.robots[0].asserv + self.mimot = self.robots[0].mimot def create_widgets (self): TestSimu.create_widgets (self) -- cgit v1.2.3