trajectories on hostsim

diff --git a/modules/devices/robot/robot_system/robot_system.c b/modules/devices/robot/robot_system/robot_system.c
index 5e91561..b5b3d32 100755 (executable)
--- a/modules/devices/robot/robot_system/robot_system.c
+++ b/modules/devices/robot/robot_system/robot_system.c
@@ -1,6 +1,6 @@
-/*  
+/*

  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)
  *  Copyright Droids Corporation, Microb Technology, Eirbot (2005)

- * 
+ *

  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or


@@ -40,7 +40,7 @@
 #include "robot_system.h"
 
 
 #include "robot_system.h"
 
 

-/** Call a pwm() pointer : 
+/** Call a pwm() pointer :

  * - lock the interrupts
  * - read the pointer to the pwm function
  * - unlock the interrupts
  * - lock the interrupts
  * - read the pointer to the pwm function
  * - unlock the interrupts


@@ -62,7 +62,7 @@ safe_setpwm(void (*f)(void *, int32_t), void * param, int32_t value)
        }
 }
 
        }
 }
 

-/** Call a encoder() pointer : 
+/** Call a encoder() pointer :

  * - lock the interrupts
  * - read the pointer to the encoder function
  * - unlock the interrupts
  * - lock the interrupts
  * - read the pointer to the encoder function
  * - unlock the interrupts


@@ -134,7 +134,7 @@ void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *, int32_
 
 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 /** define left motor encoder function and param */
 
 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
 /** define left motor encoder function and param */

-void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *), 
+void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encoder)(void *),

                             void *left_mot_encoder_param, double gain)
 {
        uint8_t flags;
                             void *left_mot_encoder_param, double gain)
 {
        uint8_t flags;


@@ -147,7 +147,7 @@ void rs_set_left_mot_encoder(struct robot_system * rs, int32_t (*left_mot_encode
 }
 
 /** define right motor encoder function and param */
 }
 
 /** define right motor encoder function and param */

-void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *), 
+void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_encoder)(void *),

                              void *right_mot_encoder_param, double gain)
 {
        uint8_t flags;
                              void *right_mot_encoder_param, double gain)
 {
        uint8_t flags;


@@ -161,7 +161,7 @@ void rs_set_right_mot_encoder(struct robot_system * rs, int32_t (*right_mot_enco
 #endif
 
 /** define left external encoder function and param */
 #endif
 
 /** define left external encoder function and param */

-void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *), 
+void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encoder)(void *),

                             void *left_ext_encoder_param, double gain)
 {
        uint8_t flags;
                             void *left_ext_encoder_param, double gain)
 {
        uint8_t flags;


@@ -178,7 +178,7 @@ void rs_set_left_ext_encoder(struct robot_system * rs, int32_t (*left_ext_encode
 }
 
 /** define right external encoder function and param */
 }
 
 /** define right external encoder function and param */

-void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *), 
+void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_encoder)(void *),

                              void *right_ext_encoder_param, double gain)
 {
        uint8_t flags;
                              void *right_ext_encoder_param, double gain)
 {
        uint8_t flags;


@@ -196,9 +196,9 @@ void rs_set_right_ext_encoder(struct robot_system * rs, int32_t (*right_ext_enco
 
 /**** Virtual encoders and PWM */
 
 
 /**** Virtual encoders and PWM */
 

-/** 
+/**

  * set the real pwms according to the specified angle (it also
  * set the real pwms according to the specified angle (it also

- * depends on the last distance command sent) 
+ * depends on the last distance command sent)

  */
 void rs_set_angle(void * data, int32_t angle)
 {
  */
 void rs_set_angle(void * data, int32_t angle)
 {


@@ -214,14 +214,14 @@ void rs_set_angle(void * data, int32_t angle)
 
        p.angle = angle;
        rs_get_wheels_from_polar(&w, &p);
 
        p.angle = angle;
        rs_get_wheels_from_polar(&w, &p);

-       
+

        safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
        safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
 }
 
        safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
        safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
 }
 

-/** 
+/**

  * set the real pwms according to the specified distance (it also
  * set the real pwms according to the specified distance (it also

- * depends on the last angle command sent) 
+ * depends on the last angle command sent)

  */
 void rs_set_distance(void * data, int32_t distance)
 {
  */
 void rs_set_distance(void * data, int32_t distance)
 {


@@ -237,37 +237,37 @@ void rs_set_distance(void * data, int32_t distance)
 
        p.distance = distance;
        rs_get_wheels_from_polar(&w, &p);
 
        p.distance = distance;
        rs_get_wheels_from_polar(&w, &p);

-       
+

        safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
        safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
 }
 
        safe_setpwm(rs->left_pwm, rs->left_pwm_param, w.left);
        safe_setpwm(rs->right_pwm, rs->right_pwm_param, w.right);
 }
 

-/** 
- * get the virtual angle according to real encoders value. 
+/**
+ * get the virtual angle according to real encoders value.

  */
 int32_t rs_get_angle(void * data)
 {
        struct robot_system * rs = data;
        int32_t angle;
        uint8_t flags;
  */
 int32_t rs_get_angle(void * data)
 {
        struct robot_system * rs = data;
        int32_t angle;
        uint8_t flags;

-       
+

        IRQ_LOCK(flags);
        IRQ_LOCK(flags);

-       angle = rs->virtual_encoders.angle ;    
+       angle = rs->virtual_encoders.angle ;

        IRQ_UNLOCK(flags);
        return angle;
 }
 
        IRQ_UNLOCK(flags);
        return angle;
 }
 

-/** 
- * get the virtual distance according to real encoders value. 
+/**
+ * get the virtual distance according to real encoders value.

  */
 int32_t rs_get_distance(void * data)
 {
        struct robot_system * rs = data;
        int32_t distance;
        uint8_t flags;
  */
 int32_t rs_get_distance(void * data)
 {
        struct robot_system * rs = data;
        int32_t distance;
        uint8_t flags;

-       
+

        IRQ_LOCK(flags);
        IRQ_LOCK(flags);

-       distance = rs->virtual_encoders.distance ;      
+       distance = rs->virtual_encoders.distance ;

        IRQ_UNLOCK(flags);
        return distance;
 }
        IRQ_UNLOCK(flags);
        return distance;
 }


@@ -277,9 +277,9 @@ int32_t rs_get_ext_angle(void * data)
        struct robot_system * rs = data;
        int32_t angle;
        uint8_t flags;
        struct robot_system * rs = data;
        int32_t angle;
        uint8_t flags;

-       
+

        IRQ_LOCK(flags);
        IRQ_LOCK(flags);

-       angle = rs->pext_prev.angle ;   
+       angle = rs->pext_prev.angle ;

        IRQ_UNLOCK(flags);
        return angle;
 }
        IRQ_UNLOCK(flags);
        return angle;
 }


@@ -289,9 +289,9 @@ int32_t rs_get_ext_distance(void * data)
        struct robot_system * rs = data;
        int32_t distance;
        uint8_t flags;
        struct robot_system * rs = data;
        int32_t distance;
        uint8_t flags;

-       
+

        IRQ_LOCK(flags);
        IRQ_LOCK(flags);

-       distance = rs->pext_prev.distance ;     
+       distance = rs->pext_prev.distance ;

        IRQ_UNLOCK(flags);
        return distance;
 }
        IRQ_UNLOCK(flags);
        return distance;
 }


@@ -301,10 +301,10 @@ int32_t rs_get_mot_angle(void * data)
 {
        struct robot_system * rs = data;
        int32_t angle;
 {
        struct robot_system * rs = data;
        int32_t angle;

-       uint8_t flags;  
+       uint8_t flags;

 
        IRQ_LOCK(flags);
 
        IRQ_LOCK(flags);

-       angle = rs->pmot_prev.angle ;   
+       angle = rs->pmot_prev.angle ;

        IRQ_UNLOCK(flags);
        return angle;
 }
        IRQ_UNLOCK(flags);
        return angle;
 }


@@ -314,9 +314,9 @@ int32_t rs_get_mot_distance(void * data)
        struct robot_system * rs = data;
        int32_t distance;
        uint8_t flags;
        struct robot_system * rs = data;
        int32_t distance;
        uint8_t flags;

-       
+

        IRQ_LOCK(flags);
        IRQ_LOCK(flags);

-       distance = rs->pmot_prev.distance ;     
+       distance = rs->pmot_prev.distance ;

        IRQ_UNLOCK(flags);
        return distance;
 }
        IRQ_UNLOCK(flags);
        return distance;
 }


@@ -327,9 +327,9 @@ int32_t rs_get_ext_left(void * data)
        struct robot_system * rs = data;
        int32_t left;
        uint8_t flags;
        struct robot_system * rs = data;
        int32_t left;
        uint8_t flags;

-       
+

        IRQ_LOCK(flags);
        IRQ_LOCK(flags);

-       left = rs->wext_prev.left ;     
+       left = rs->wext_prev.left ;

        IRQ_UNLOCK(flags);
        return left;
 }
        IRQ_UNLOCK(flags);
        return left;
 }


@@ -339,9 +339,9 @@ int32_t rs_get_ext_right(void * data)
        struct robot_system * rs = data;
        int32_t right;
        uint8_t flags;
        struct robot_system * rs = data;
        int32_t right;
        uint8_t flags;

-       
+

        IRQ_LOCK(flags);
        IRQ_LOCK(flags);

-       right = rs->wext_prev.right ;   
+       right = rs->wext_prev.right ;

        IRQ_UNLOCK(flags);
        return right;
 }
        IRQ_UNLOCK(flags);
        return right;
 }


@@ -351,10 +351,10 @@ int32_t rs_get_mot_left(void * data)
 {
        struct robot_system * rs = data;
        int32_t left;
 {
        struct robot_system * rs = data;
        int32_t left;

-       uint8_t flags;  
+       uint8_t flags;

 
        IRQ_LOCK(flags);
 
        IRQ_LOCK(flags);

-       left = rs->wmot_prev.left ;     
+       left = rs->wmot_prev.left ;

        IRQ_UNLOCK(flags);
        return left;
 }
        IRQ_UNLOCK(flags);
        return left;
 }


@@ -364,9 +364,9 @@ int32_t rs_get_mot_right(void * data)
        struct robot_system * rs = data;
        int32_t right;
        uint8_t flags;
        struct robot_system * rs = data;
        int32_t right;
        uint8_t flags;

-       
+

        IRQ_LOCK(flags);
        IRQ_LOCK(flags);

-       right = rs->wmot_prev.right ;   
+       right = rs->wmot_prev.right ;

        IRQ_UNLOCK(flags);
        return right;
 }
        IRQ_UNLOCK(flags);
        return right;
 }


@@ -375,13 +375,13 @@ int32_t rs_get_mot_right(void * data)
 void rs_set_flags(struct robot_system * rs, uint8_t flags)
 {
        uint8_t i_flags;
 void rs_set_flags(struct robot_system * rs, uint8_t flags)
 {
        uint8_t i_flags;

-       
+

        IRQ_LOCK(i_flags);
        rs->flags = flags;
        IRQ_UNLOCK(i_flags);
 }
 
        IRQ_LOCK(i_flags);
        rs->flags = flags;
        IRQ_UNLOCK(i_flags);
 }
 

-/** 
+/**

  * Read the encoders, and update internal virtual counters. Call this
  * function is needed before reading the virtual encoders.The program
  * will decide if it the external encoders or the motor encoders are
  * Read the encoders, and update internal virtual counters. Call this
  * function is needed before reading the virtual encoders.The program
  * will decide if it the external encoders or the motor encoders are


@@ -398,7 +398,7 @@ void rs_update(void * data)
 #endif
        int32_t delta_angle, delta_distance;
        uint8_t flags;
 #endif
        int32_t delta_angle, delta_distance;
        uint8_t flags;

-       
+

        /* read encoders */
        wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param);
        wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param);
        /* read encoders */
        wext.left = safe_getencoder(rs->left_ext_encoder, rs->left_ext_encoder_param);
        wext.right = safe_getencoder(rs->right_ext_encoder, rs->right_ext_encoder_param);


@@ -407,7 +407,7 @@ void rs_update(void * data)
        wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param);
        wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param);
 #endif
        wmot.left = safe_getencoder(rs->left_mot_encoder, rs->left_mot_encoder_param);
        wmot.right = safe_getencoder(rs->right_mot_encoder, rs->right_mot_encoder_param);
 #endif

-       
+

        /* apply gains to each wheel */
        if (! (rs->flags & RS_IGNORE_EXT_GAIN )) {
 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64
        /* apply gains to each wheel */
        if (! (rs->flags & RS_IGNORE_EXT_GAIN )) {
 #ifdef CONFIG_MODULE_ROBOT_SYSTEM_USE_F64

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager.h b/modules/devices/robot/trajectory_manager/trajectory_manager.h
index 43730ea..6e7e82e 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager.h
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager.h
@@ -197,6 +197,15 @@ void trajectory_goto_xy_rel(struct trajectory *traj, double x_rel_mm, double y_r
 void trajectory_circle_rel(struct trajectory *traj, double x, double y,
                           double radius_mm, double rel_a_deg, uint8_t flags);
 
 void trajectory_circle_rel(struct trajectory *traj, double x, double y,
                           double radius_mm, double rel_a_deg, uint8_t flags);
 

+/*
+ * Compute the fastest distance and angle speeds matching the radius
+ * from current traj_speed
+ */
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+                                    double radius_mm,
+                                    double *speed_d,
+                                    double *speed_a);
+

 /* do a line */
 void trajectory_line_abs(struct trajectory *traj, double x1, double y1,
                         double x2, double y2, double advance);
 /* do a line */
 void trajectory_line_abs(struct trajectory *traj, double x1, double y1,
                         double x2, double y2, double advance);

diff --git a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
index 33c2a30..0177419 100644 (file)
--- a/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
+++ b/modules/devices/robot/trajectory_manager/trajectory_manager_core.c
@@ -479,10 +479,10 @@ void trajectory_manager_xy_event(struct trajectory *traj)
  * Compute the fastest distance and angle speeds matching the radius
  * from current traj_speed
  */
  * Compute the fastest distance and angle speeds matching the radius
  * from current traj_speed
  */

-/* static  */void circle_get_da_speed_from_radius(struct trajectory *traj,
-                                                 double radius_mm,
-                                                 double *speed_d,
-                                                 double *speed_a)
+void circle_get_da_speed_from_radius(struct trajectory *traj,
+                                    double radius_mm,
+                                    double *speed_d,
+                                    double *speed_a)

 {
        /* speed_d = coef * speed_a */
        double coef;
 {
        /* speed_d = coef * speed_a */
        double coef;

diff --git a/projects/microb2010/mainboard/Makefile b/projects/microb2010/mainboard/Makefile
index 7eff3cb..141943f 100755 (executable)
--- a/projects/microb2010/mainboard/Makefile
+++ b/projects/microb2010/mainboard/Makefile
@@ -7,14 +7,14 @@ SRC  = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c
 SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
 SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
 SRC += strat_utils.c strat_base.c strat.c
 SRC += commands_cs.c commands_mainboard.c commands_traj.c commands.c
 SRC += i2c_protocol.c sensor.c actuator.c cs.c ax12_user.c
 SRC += strat_utils.c strat_base.c strat.c

-ifneq ($(HOST),avr)
+ifeq ($(H),1)

 SRC += robotsim.c
 endif
 
 ASRC =
 
 CFLAGS += -Wall -Werror
 SRC += robotsim.c
 endif
 
 ASRC =
 
 CFLAGS += -Wall -Werror

-ifeq ($(HOST),avr)
+ifneq ($(H),1)

 LDFLAGS = -T ../common/avr6.x
 endif
 
 LDFLAGS = -T ../common/avr6.x
 endif
 

diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index 9f39f86..ced8e58 100644 (file)
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -1074,16 +1074,92 @@ parse_pgm_inst_t cmd_servo_balls = {
 struct cmd_test_result {
        fixed_string_t arg0;
        int32_t radius;
 struct cmd_test_result {
        fixed_string_t arg0;
        int32_t radius;

+       int32_t dist;

 };
 
 };
 

+/* function called when cmd_test is parsed successfully */
+static void line2line(double line1x1, double line1y1,
+                     double line1x2, double line1y2,
+                     double line2x1, double line2y1,
+                     double line2x2, double line2y2,
+                     double radius, double dist)
+{
+       uint8_t err;
+       int32_t dist_imp_target;
+       double speed_d, speed_a;
+       double distance, angle;
+       double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
+       double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
+
+       printf("%s()\n", __FUNCTION__);
+       strat_set_speed(500, 500);
+       circle_get_da_speed_from_radius(&mainboard.traj, radius,
+                                       &speed_d, &speed_a);
+       trajectory_line_abs(&mainboard.traj,
+                           line1x1, line1y1,
+                           line1x2, line1y2, 150.);
+       err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
+                                   dist, TRAJ_FLAGS_NO_NEAR);
+       /* circle */
+       strat_set_speed(speed_d, speed_a);
+       angle = line2_angle - line1_angle;
+       distance = angle * radius;
+       if (distance < 0)
+               distance = -distance;
+       dist_imp_target = rs_get_distance(&mainboard.rs) +
+               distance * mainboard.pos.phys.distance_imp_per_mm;
+       angle = DEG(angle);
+       distance += 100; /* take some margin to avoid deceleration */
+       trajectory_d_a_rel(&mainboard.traj, distance, angle);
+
+       err = WAIT_COND_OR_TRAJ_END(rs_get_distance(&mainboard.rs) > dist_imp_target,
+                                   TRAJ_FLAGS_NO_NEAR);
+
+       strat_set_speed(500, 500);
+       trajectory_line_abs(&mainboard.traj,
+                           line2x1, line2y1,
+                           line2x2, line2y2, 150.);
+}
+

 /* function called when cmd_test is parsed successfully */
 static void cmd_test_parsed(void *parsed_result, void *data)
 {
 /* function called when cmd_test is parsed successfully */
 static void cmd_test_parsed(void *parsed_result, void *data)
 {

+       //      struct cmd_test_result *res = parsed_result;
+#ifdef HOST_VERSION
+       strat_reset_pos(400, 400, 90);
+       mainboard.angle.on = 1;
+       mainboard.distance.on = 1;
+#endif
+       line2line(375, 347, 375, 1847,
+                 375, 1847, 1050, 1472,
+                 100, 200);
+       line2line(825, 1596, 1050, 1472,
+                 1050, 1472, 1500, 1722,
+                 180, 120);
+       line2line(1050, 1472, 1500, 1722,
+                 1500, 1722, 2175, 1347,
+                 180, 120);
+       line2line(1500, 1722, 2175, 1347,
+                 2175, 1347, 2175, 847,
+                 150, 120);
+       line2line(2175, 1347, 2175, 847,
+                 2175, 847, 2400, 722,
+                 150, 120);
+       line2line(2175, 847, 2400, 722,
+                 2400, 722, 2625, 847,
+                 150, 100);
+       line2line(2400, 722, 2625, 847,
+                 2625, 847, 2625, 1847,
+                 150, 100);
+       line2line(2625, 847, 2625, 1847,
+                 2625, 1847, 375, 597,
+                 100, 200);

 }
 
 prog_char str_test_arg0[] = "test";
 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
 }
 
 prog_char str_test_arg0[] = "test";
 parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0);
 parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);

+parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);

 
 prog_char help_test[] = "Test function";
 parse_pgm_inst_t cmd_test = {
 
 prog_char help_test[] = "Test function";
 parse_pgm_inst_t cmd_test = {


@@ -1092,7 +1168,6 @@ parse_pgm_inst_t cmd_test = {
        .help_str = help_test,
        .tokens = {        /* token list, NULL terminated */
                (prog_void *)&cmd_test_arg0,
        .help_str = help_test,
        .tokens = {        /* token list, NULL terminated */
                (prog_void *)&cmd_test_arg0,

-               (prog_void *)&cmd_test_arg1,

                NULL,
        },
 };
                NULL,
        },
 };

diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index 239b2d6..3ca1a47 100644 (file)
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -15,7 +15,7 @@ save_pos = []
 
 robot = box(pos = (0, 0, 150),
             size = (250,320,350),
 
 robot = box(pos = (0, 0, 150),
             size = (250,320,350),

-            color = (1, 0, 0) )
+            color = (0.3, 0.3, 0.3) )

 
 last_pos = robot.pos.x, robot.pos.y, robot.pos.z
 hcenter_line = curve()
 
 last_pos = robot.pos.x, robot.pos.y, robot.pos.z
 hcenter_line = curve()


@@ -23,6 +23,18 @@ hcenter_line.pos = [(-AREA_X/2, 0., 0.3), (AREA_X/2, 0., 0.3)]
 vcenter_line = curve()
 vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
 
 vcenter_line = curve()
 vcenter_line.pos = [(0., -AREA_Y/2, 0.3), (0., AREA_Y/2, 0.3)]
 

+yellowarea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
+yellowareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , yellowarea)
+yellowarea_box = box(pos=(-AREA_X/2+250,-AREA_Y/2+250,0), size=yellowareasize, color=(1.0, 1.0, 0.0))
+
+bluearea = [ (0.0, 0.0, -0.5), (500.0, 500.0, 0.5) ]
+blueareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , bluearea)
+bluearea_box = box(pos=(AREA_X/2-250,-AREA_Y/2+250,0), size=blueareasize, color=(0.0, 0.0, 1.0))
+
+greyarea = [ (0.0, 0.0, -0.5), (1520.0, 500.0, 0.5) ]
+greyareasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , greyarea)
+greyarea_box = box(pos=(0,-AREA_Y/2+250,0), size=greyareasize, color=(0.3, 0.6, 0.3))
+

 def square(sz):
     sq = curve()
     sq.pos = [(-sz, -sz, 0.3),
 def square(sz):
     sq = curve()
     sq.pos = [(-sz, -sz, 0.3),


@@ -54,7 +66,8 @@ TYPE_DANGEROUS=1
 TYPE_WHITE_CORN=2
 TYPE_BLACK_CORN=3
 TYPE_OBSTACLE=4
 TYPE_WHITE_CORN=2
 TYPE_BLACK_CORN=3
 TYPE_OBSTACLE=4

-TYPE_NEIGH=5
+TYPE_BALL=5
+TYPE_NEIGH=6

 
 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]
 
 col = [TYPE_WAYPOINT] * WAYPOINTS_NBY
 waypoints = [col[:] for i in range(WAYPOINTS_NBX)]


@@ -131,6 +144,15 @@ def init_waypoints():
             if c >= 0:
                 waypoints[i][j] = corn_table[c]
                 continue
             if c >= 0:
                 waypoints[i][j] = corn_table[c]
                 continue

+
+            # balls
+            if (i & 1) == 0 and j > 3:
+                waypoints[i][j] = TYPE_BALL
+                continue
+            if (i == 0 or i == WAYPOINTS_NBX-1) and j > 2:
+                waypoints[i][j] = TYPE_BALL
+                continue
+

             # too close of border
             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
                 waypoints[i][j] = TYPE_OBSTACLE
             # too close of border
             if (i & 1) == 1 and j == WAYPOINTS_NBY -1:
                 waypoints[i][j] = TYPE_OBSTACLE


@@ -178,6 +200,14 @@ def toggle_obj_disp():
                                  radius=25, color=(0.2,0.2,0.2),
                                  pos=(x-AREA_X/2,y-AREA_Y/2,75))
                     area_objects.append(c)
                                  radius=25, color=(0.2,0.2,0.2),
                                  pos=(x-AREA_X/2,y-AREA_Y/2,75))
                     area_objects.append(c)

+                elif waypoints[i][j] == TYPE_BALL:
+                    c = sphere(radius=50, color=(1., 0.,0.),
+                               pos=(x-AREA_X/2,y-AREA_Y/2,50))
+                    area_objects.append(c)
+                else:
+                    c = sphere(radius=5, color=(0., 0.,1.),
+                               pos=(x-AREA_X/2,y-AREA_Y/2,5))
+                    area_objects.append(c)

                 j += 1
                 y += STEP_CORN_Y
             i += 1
                 j += 1
                 y += STEP_CORN_Y
             i += 1