current limit on shovel

diff --git a/projects/microb2010/cobboard/commands.c b/projects/microb2010/cobboard/commands.c
index feeb32ff8c5d52eab364dd18104d793f49bbf07c..df0b7013b12316e0a01e5b7c884e98c61e90d7cb 100644 (file)
--- a/projects/microb2010/cobboard/commands.c
+++ b/projects/microb2010/cobboard/commands.c
@@ -79,6 +79,8 @@ extern parse_pgm_inst_t cmd_spickle_params_show;
 extern parse_pgm_inst_t cmd_spickle_params2;
 extern parse_pgm_inst_t cmd_spickle_params2_show;
 extern parse_pgm_inst_t cmd_shovel;
+extern parse_pgm_inst_t cmd_shovel_current;
+extern parse_pgm_inst_t cmd_shovel_current_show;
 extern parse_pgm_inst_t cmd_test;
 
 
@@ -140,6 +142,8 @@ parse_pgm_ctx_t main_ctx[] = {
        (parse_pgm_inst_t *)&cmd_spickle_params2,
        (parse_pgm_inst_t *)&cmd_spickle_params2_show,
        (parse_pgm_inst_t *)&cmd_shovel,
+       (parse_pgm_inst_t *)&cmd_shovel_current,
+       (parse_pgm_inst_t *)&cmd_shovel_current_show,
        (parse_pgm_inst_t *)&cmd_test,
 
        NULL,

diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
index 669c4c8bc8589ac9753377c0ae9ebc6520e02dc2..dc1ec3560fe0b9db4acaaf07f4cb78a7435ef351 100644 (file)
--- a/projects/microb2010/cobboard/commands_cobboard.c
+++ b/projects/microb2010/cobboard/commands_cobboard.c
@@ -759,6 +759,79 @@ parse_pgm_inst_t cmd_spickle_params2_show = {
        },
 };
 
+/**********************************************************/
+/* Set Shovel Params */
+
+/* this structure is filled when cmd_shovel_current is parsed successfully */
+struct cmd_shovel_current_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       int32_t arg2;
+       int32_t arg3;
+};
+
+/* function called when cmd_shovel_current is parsed successfully */
+static void cmd_shovel_current_parsed(void *parsed_result,
+                                     __attribute__((unused)) void *data)
+{
+       struct cmd_shovel_current_result * res = parsed_result;
+       uint8_t enable;
+       int32_t k1, k2;
+
+       if (!strcmp_P(res->arg1, PSTR("set")))
+               shovel_set_current_limit_coefs(res->arg2, res->arg3);
+       else if (!strcmp_P(res->arg1, PSTR("on")))
+               shovel_current_limit_enable(1);
+       else if (!strcmp_P(res->arg1, PSTR("off")))
+               shovel_current_limit_enable(0);
+
+       /* else show */
+       enable = shovel_get_current_limit_coefs(&k1, &k2);
+       printf_P(PSTR("enabled=%d k1=%"PRIi32" k2=%"PRIi32"\r\n"),
+                enable, k1, k2);
+}
+
+prog_char str_shovel_current_arg0[] = "shovel_current";
+parse_pgm_token_string_t cmd_shovel_current_arg0 =
+       TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg0, str_shovel_current_arg0);
+prog_char str_shovel_current_arg1[] = "set";
+parse_pgm_token_string_t cmd_shovel_current_arg1 =
+       TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg1, str_shovel_current_arg1);
+parse_pgm_token_num_t cmd_shovel_current_arg2 =
+       TOKEN_NUM_INITIALIZER(struct cmd_shovel_current_result, arg2, INT32);
+parse_pgm_token_num_t cmd_shovel_current_arg3 =
+       TOKEN_NUM_INITIALIZER(struct cmd_shovel_current_result, arg3, INT32);
+
+prog_char help_shovel_current[] = "Set shovel_current values";
+parse_pgm_inst_t cmd_shovel_current = {
+       .f = cmd_shovel_current_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_shovel_current,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_shovel_current_arg0,
+               (prog_void *)&cmd_shovel_current_arg1,
+               (prog_void *)&cmd_shovel_current_arg2,
+               (prog_void *)&cmd_shovel_current_arg3,
+               NULL,
+       },
+};
+
+prog_char str_shovel_current_arg1_show[] = "show#on#off";
+parse_pgm_token_string_t cmd_shovel_current_arg1_show =
+       TOKEN_STRING_INITIALIZER(struct cmd_shovel_current_result, arg1, str_shovel_current_arg1_show);
+
+prog_char help_shovel_current_show[] = "show shovel params";
+parse_pgm_inst_t cmd_shovel_current_show = {
+       .f = cmd_shovel_current_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_shovel_current_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_shovel_current_arg0,
+               (prog_void *)&cmd_shovel_current_arg1_show,
+               NULL,
+       },
+};
+
 /**********************************************************/
 /* Test */
 

diff --git a/projects/microb2010/cobboard/cs.c b/projects/microb2010/cobboard/cs.c
index f8c71233cda4c39f4fdc81afd75cf7fbb20fb06d..386b36024b07d108c2480e044b53cb6546eccaa4 100644 (file)
--- a/projects/microb2010/cobboard/cs.c
+++ b/projects/microb2010/cobboard/cs.c
@@ -46,6 +46,7 @@
 #include "main.h"
 #include "actuator.h"
 #include "spickle.h"
+#include "shovel.h"
 
 /* called every 5 ms */
 static void do_cs(__attribute__((unused)) void *dummy)
@@ -173,7 +174,7 @@ void microb_cs_init(void)
        cs_init(&cobboard.shovel.cs);
        cs_set_consign_filter(&cobboard.shovel.cs, quadramp_do_filter, &cobboard.shovel.qr);
        cs_set_correct_filter(&cobboard.shovel.cs, pid_do_filter, &cobboard.shovel.pid);
-       cs_set_process_in(&cobboard.shovel.cs, pwm_ng_set, SHOVEL_PWM);
+       cs_set_process_in(&cobboard.shovel.cs, shovel_set, SHOVEL_PWM);
        cs_set_process_out(&cobboard.shovel.cs, encoders_spi_get_value, SHOVEL_ENCODER);
        cs_set_consign(&cobboard.shovel.cs, 0);
 

diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c
index b2a8cc0501a66f7adad1b015851385700510c217..56efa9685c59b5d7803d77686ae58ba49bbd80ae 100644 (file)
--- a/projects/microb2010/cobboard/shovel.c
+++ b/projects/microb2010/cobboard/shovel.c
@@ -47,6 +47,10 @@
 #define SHOVEL_MID  4500
 #define SHOVEL_UP   11000
 
+static int32_t shovel_k1 = 1000;
+static int32_t shovel_k2 = 20;
+static uint8_t shovel_current_limit = 1;
+
 /* init spickle position at beginning */
 static void shovel_autopos(void)
 {
@@ -69,6 +73,51 @@ static uint8_t shovel_is_at_pos(int32_t pos)
        return 0;
 }
 
+void shovel_set_current_limit_coefs(int32_t k1, int32_t k2)
+{
+       shovel_k1 = k1;
+       shovel_k2 = k2;
+}
+
+uint8_t shovel_get_current_limit_coefs(int32_t *k1, int32_t *k2)
+{
+       *k1 = shovel_k1;
+       *k2 = shovel_k2;
+       return shovel_current_limit;
+}
+
+void shovel_current_limit_enable(uint8_t enable)
+{
+       shovel_current_limit = enable;
+}
+
+/* Set CS command for shovel. Called by CS manager. */
+void shovel_set(void *mot, int32_t cmd)
+{
+       static int32_t oldpos;
+       int32_t pos, maxcmd, speed;
+
+       pos = encoders_spi_get_value(SHOVEL_ENCODER);
+       if (shovel_current_limit) {
+               speed = pos - oldpos;
+               if (speed > 0 && cmd < 0)
+                       maxcmd = shovel_k1;
+               else if (speed < 0 && cmd > 0)
+                       maxcmd = shovel_k1;
+               else {
+                       speed = ABS(speed);
+                       maxcmd = shovel_k1 + shovel_k2 * speed;
+               }
+               if (cmd > maxcmd)
+                       cmd = maxcmd;
+               else if (cmd < -maxcmd)
+                       cmd = -maxcmd;
+       }
+
+       pwm_ng_set(mot, cmd);
+       oldpos = pos;
+}
+
 void shovel_down(void)
 {
        quadramp_set_1st_order_vars(&cobboard.shovel.qr, 2500, 2500);

diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h
index 80bceec148746e9cb7f7a6db3f00661b0d326fc5..fa0e2a29b5a043bf98241bde7f5f37aa66493189 100644 (file)
--- a/projects/microb2010/cobboard/shovel.h
+++ b/projects/microb2010/cobboard/shovel.h
@@ -24,6 +24,11 @@
 
 void shovel_init(void);
 
+void shovel_set_current_limit_coefs(int32_t k1, int32_t k2);
+uint8_t shovel_get_current_limit_coefs(int32_t *k1, int32_t *k2);
+void shovel_current_limit_enable(uint8_t enable);
+void shovel_set(void *mot, int32_t cmd);
+
 void shovel_down(void);
 void shovel_mid(void);
 void shovel_up(void);

diff --git a/projects/microb2010/mainboard/display.py b/projects/microb2010/mainboard/display.py
index f89580cad18aed5395e54893341e199098e96ba7..297dbcec3371a242c17aecb3d1c38b72421c91d3 100644 (file)
--- a/projects/microb2010/mainboard/display.py
+++ b/projects/microb2010/mainboard/display.py
@@ -11,7 +11,7 @@ ROBOT_HEIGHT=5 # 350
 CORN_HEIGHT=5  # 150
 
 ROBOT_WIDTH=320
-ROBOT_LENGTH=360
+ROBOT_LENGTH=250
 
 area = [ (0.0, 0.0, -0.2), (3000.0, 2100.0, 0.2) ]
 areasize = reduce(lambda x,y:tuple([abs(x[i])+abs(y[i]) for i in range(len(x))]) , area)
@@ -63,8 +63,10 @@ sq2 = square(500)
 robot_x = 0.
 robot_y = 0.
 robot_a = 0.
-robot_lspickle = 0
-robot_rspickle = 0
+robot_lspickle_deployed = 0
+robot_rspickle_deployed = 0
+robot_lspickle_autoharvest = 0
+robot_rspickle_autoharvest = 0
 robot_trail = curve()
 robot_trail_list = []
 max_trail = 500
@@ -281,19 +283,25 @@ def set_robot():
     robot.axis = axis
     robot.size = (ROBOT_LENGTH, ROBOT_WIDTH, ROBOT_HEIGHT)
 
-    robot_lspickle = 2 # XXX
-    lspickle.pos = (tmp_x + (robot_lspickle*60) * math.cos((tmp_a+90)*math.pi/180),
-                    tmp_y + (robot_lspickle*60) * math.sin((tmp_a+90)*math.pi/180),
+    lspickle.pos = (tmp_x + (robot_lspickle_deployed*60) * math.cos((tmp_a+90)*math.pi/180),
+                    tmp_y + (robot_lspickle_deployed*60) * math.sin((tmp_a+90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     lspickle.axis = axis
     lspickle.size = (20, ROBOT_WIDTH, 5)
+    if robot_lspickle_autoharvest:
+        lspickle.color = (1, 0, 0)
+    else:
+        lspickle.color = (0.4, 0.4, 0.4)
 
-    robot_rspickle = 2 # XXX
-    rspickle.pos = (tmp_x + (robot_rspickle*60) * math.cos((tmp_a-90)*math.pi/180),
-                    tmp_y + (robot_rspickle*60) * math.sin((tmp_a-90)*math.pi/180),
+    rspickle.pos = (tmp_x + (robot_rspickle_deployed*60) * math.cos((tmp_a-90)*math.pi/180),
+                    tmp_y + (robot_rspickle_deployed*60) * math.sin((tmp_a-90)*math.pi/180),
                     ROBOT_HEIGHT/2)
     rspickle.axis = axis
     rspickle.size = (20, ROBOT_WIDTH, 5)
+    if robot_rspickle_autoharvest:
+        rspickle.color = (1, 0, 0)
+    else:
+        rspickle.color = (0.4, 0.4, 0.4)
 
     # save position
     save_pos.append((robot.pos.x, robot.pos.y, tmp_a))
@@ -360,15 +368,11 @@ while True:
                     side = int(m.groups()[0])
                     flags = int(m.groups()[1])
                     if side == 0:
-                        if (flags & 0x01) == 0:
-                            robot_lspickle = 1
-                        else:
-                            robot_lspickle = 2
+                        robot_lspickle_deployed = ((flags & 1) * 2)
+                        robot_lspickle_autoharvest = ((flags & 2) != 0)
                     else:
-                        if (flags & 0x01) == 0:
-                            robot_rspickle = 1
-                        else:
-                            robot_rspickle = 2
+                        robot_rspickle_deployed = ((flags & 1) * 2)
+                        robot_rspickle_autoharvest = ((flags & 2) != 0)
 
             if scene.kb.keys == 0:
                 continue

diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 7a2c3780f43a196947f1e3d197d5b5580f2d5b3e..3354e394a003aaeffa56600488535065317e0ea6 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -403,7 +403,8 @@ int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
 int8_t i2c_cobboard_set_mode(uint8_t mode)
 {
 #ifdef HOST_VERSION
-       return robotsim_i2c_cobboard_set_mode(mode);
+       cobboard.mode = mode;
+       return 0;
 #else
        struct i2c_cmd_cobboard_set_mode buf;
        buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
@@ -414,11 +415,15 @@ int8_t i2c_cobboard_set_mode(uint8_t mode)
 
 static int8_t i2c_cobboard_set_spickle(uint8_t side, uint8_t flags)
 {
+#ifdef HOST_VERSION
+       return robotsim_i2c_cobboard_set_spickles(side, flags);
+#else
        if (side == I2C_LEFT_SIDE)
                cobboard.lspickle = flags;
        else
                cobboard.rspickle = flags;
        return 0;
+#endif
 }
 
 int8_t i2c_cobboard_pack(uint8_t side)

diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h
index 07986990c0f671d3cba2714a9646f6d6c7f9f527..00b6e8630b7e0d3abeae7d6d9cd5ebb196d9cc6b 100755 (executable)
--- a/projects/microb2010/mainboard/main.h
+++ b/projects/microb2010/mainboard/main.h
@@ -76,11 +76,11 @@
 #define MATCH_TIME 89
 
 /* decrease track to decrease angle */
-#define EXT_TRACK_MM 304.9
+#define EXT_TRACK_MM 304.61875
 #define VIRTUAL_TRACK_MM EXT_TRACK_MM
 
-#define ROBOT_HALF_LENGTH_FRONT 180
-#define ROBOT_HALF_LENGTH_REAR 70
+#define ROBOT_HALF_LENGTH_FRONT 130
+#define ROBOT_HALF_LENGTH_REAR 120
 #define ROBOT_WIDTH 320
 
 /* it is a 1024 imps -> 4096 because we see 1/4 period

diff --git a/projects/microb2010/mainboard/robotsim.c b/projects/microb2010/mainboard/robotsim.c
index 2d0d7bebbba15245ef27f626c76ca308418bd545..42a33d9a95fc5fa2676abab379b3147faf766c0a 100644 (file)
--- a/projects/microb2010/mainboard/robotsim.c
+++ b/projects/microb2010/mainboard/robotsim.c
@@ -111,16 +111,25 @@ robotsim_i2c_ballboard_set_mode(struct i2c_cmd_ballboard_set_mode *cmd)
 }
 
 int8_t
-robotsim_i2c_cobboard_set_mode(uint8_t mode)
+robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags)
 {
        char buf[BUFSIZ];
        int len;
 
-       if (cobboard.mode == mode)
-               return 0;
+       if (side == I2C_LEFT_SIDE) {
+               if (cobboard.lspickle == flags)
+                       return 0;
+               else
+                       cobboard.lspickle = flags;
+       }
+       if (side == I2C_RIGHT_SIDE) {
+               if (cobboard.rspickle == flags)
+                       return 0;
+               else
+                       cobboard.rspickle = flags;
+       }
 
-       cobboard.mode = mode;
-       len = snprintf(buf, sizeof(buf), "cobboard=%d\n", mode);
+       len = snprintf(buf, sizeof(buf), "cobboard=%d,%d\n", side, flags);
        hostsim_lock();
        write(fdw, buf, len);
        hostsim_unlock();

diff --git a/projects/microb2010/mainboard/robotsim.h b/projects/microb2010/mainboard/robotsim.h
index 0950af0a7da23998f4bcc96d33df6bdb619f6397..7c1eccfeff37dc6dc52a0d2e056115455520677a 100644 (file)
--- a/projects/microb2010/mainboard/robotsim.h
+++ b/projects/microb2010/mainboard/robotsim.h
@@ -25,4 +25,5 @@ void robotsim_pwm(void *arg, int32_t val);
 int32_t robotsim_encoder_get(void *arg);
 int robotsim_init(void);
 void robotsim_dump(void);
+int8_t robotsim_i2c_cobboard_set_mode(uint8_t mode);
 int8_t robotsim_i2c_cobboard_set_spickles(uint8_t side, uint8_t flags);