weak current limit on spickles

diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
index dc1ec3560fe0b9db4acaaf07f4cb78a7435ef351..9cfda19daf050f92f3fe3d2138203a313081104c 100644 (file)
--- a/projects/microb2010/cobboard/commands_cobboard.c
+++ b/projects/microb2010/cobboard/commands_cobboard.c
@@ -244,6 +244,10 @@ static void cmd_state2_parsed(void *parsed_result,
                state_set_mode(I2C_COBBOARD_MODE_HARVEST);
                state_set_spickle(side, 0);
        }
+       else if (!strcmp_P(res->arg1, PSTR("weak_pack"))) {
+               state_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               state_set_spickle(side, I2C_COBBOARD_SPK_WEAK);
+       }
        else if (!strcmp_P(res->arg1, PSTR("deploy"))) {
                state_set_mode(I2C_COBBOARD_MODE_HARVEST);
                state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY);
@@ -268,7 +272,7 @@ static void cmd_state2_parsed(void *parsed_result,
 
 prog_char str_state2_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
-prog_char str_state2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
+prog_char str_state2_arg1[] = "harvest#deploy#pack#weak_pack#harvest_nomove#deploy_nomove";
 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
 prog_char str_state2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
@@ -710,8 +714,11 @@ static void cmd_spickle_params2_parsed(void *parsed_result,
 {
        struct cmd_spickle_params2_result * res = parsed_result;
 
-       if (!strcmp_P(res->arg1, PSTR("coef"))) {
-               spickle_set_coefs(res->arg2, res->arg3);
+       if (!strcmp_P(res->arg1, PSTR("wcoef"))) {
+               spickle_set_wcoefs(res->arg2, res->arg3);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("scoef"))) {
+               spickle_set_scoefs(res->arg2, res->arg3);
        }
 
        /* else show */
@@ -721,7 +728,7 @@ static void cmd_spickle_params2_parsed(void *parsed_result,
 prog_char str_spickle_params2_arg0[] = "spickle_params2";
 parse_pgm_token_string_t cmd_spickle_params2_arg0 =
        TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg0, str_spickle_params2_arg0);
-prog_char str_spickle_params2_arg1[] = "coef";
+prog_char str_spickle_params2_arg1[] = "wcoef#scoef";
 parse_pgm_token_string_t cmd_spickle_params2_arg1 =
        TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg1, str_spickle_params2_arg1);
 parse_pgm_token_num_t cmd_spickle_params2_arg2 =

diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index a6bf6db1715992d9466ef394a02eaf16ef5da3a2..741c103155e48826dbb383c573ca6b2f363b19e7 100644 (file)
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -42,13 +42,16 @@
 
 #include "sensor.h"
 #include "../common/i2c_commands.h"
+#include "state.h"
 #include "main.h"
 #include "actuator.h"
 
 struct spickle_params {
        /* current limit (common to left and right) */
-       int32_t k1;
-       int32_t k2;
+       int32_t sk1;
+       int32_t sk2;
+       int32_t wk1;
+       int32_t wk2;
 
        /* cs blocks */
        struct cs_block * const csb[2];
@@ -60,8 +63,10 @@ struct spickle_params {
 };
 
 static struct spickle_params spickle = {
-       .k1 = 1000,
-       .k2 = 20,
+       .sk1 = 1000,
+       .sk2 = 20,
+       .wk1 = 200,
+       .wk2 = 20,
        .csb = {
                &cobboard.left_spickle,
                &cobboard.right_spickle,
@@ -99,24 +104,41 @@ void spickle_set(void *mot, int32_t cmd)
 {
        static int32_t oldpos_left, oldpos_right;
        int32_t oldpos, pos, maxcmd, speed;
+       int32_t k1, k2;
 
        if (mot == LEFT_SPICKLE_PWM) {
                pos = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
                oldpos = oldpos_left;
+               if (state_spicklemode_weak(I2C_LEFT_SIDE)) {
+                       k1 = spickle.wk1;
+                       k2 = spickle.wk2;
+               }
+               else {
+                       k1 = spickle.sk1;
+                       k2 = spickle.sk2;
+               }
        }
        else {
                pos = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
                oldpos = oldpos_right;
+               if (state_spicklemode_weak(I2C_RIGHT_SIDE)) {
+                       k1 = spickle.wk1;
+                       k2 = spickle.wk2;
+               }
+               else {
+                       k1 = spickle.sk1;
+                       k2 = spickle.sk2;
+               }
        }
 
        speed = pos - oldpos;
        if (speed > 0 && cmd < 0)
-               maxcmd = spickle.k1;
+               maxcmd = k1;
        else if (speed < 0 && cmd > 0)
-               maxcmd = spickle.k1;
+               maxcmd = k1;
        else {
                speed = ABS(speed);
-               maxcmd = spickle.k1 + spickle.k2 * speed;
+               maxcmd = k1 + k2 * speed;
        }
        if (cmd > maxcmd)
                cmd = maxcmd;
@@ -131,10 +153,16 @@ void spickle_set(void *mot, int32_t cmd)
                oldpos_right = pos;
 }
 
-void spickle_set_coefs(uint32_t k1, uint32_t k2)
+void spickle_set_wcoefs(uint32_t k1, uint32_t k2)
 {
-       spickle.k1 = k1;
-       spickle.k2 = k2;
+       spickle.wk1 = k1;
+       spickle.wk2 = k2;
+}
+
+void spickle_set_scoefs(uint32_t k1, uint32_t k2)
+{
+       spickle.sk1 = k1;
+       spickle.sk2 = k2;
 }
 
 void spickle_set_pos(uint8_t side, int32_t pos_packed,
@@ -147,7 +175,8 @@ void spickle_set_pos(uint8_t side, int32_t pos_packed,
 
 void spickle_dump_params(void)
 {
-       printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2);
+       printf_P(PSTR("strong_coef %ld %ld\r\n"), spickle.sk1, spickle.sk2);
+       printf_P(PSTR("weak_coef %ld %ld\r\n"), spickle.wk1, spickle.wk2);
        printf_P(PSTR("left pos %ld %ld\r\n"),
                 spickle.pos_packed[I2C_LEFT_SIDE],
                 spickle.pos_mid[I2C_LEFT_SIDE],

diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h
index f34894a45c6fee252904f7981d62f251a72107dd..bf5887c8cd208c6fff05fe391c15e2cb834d5f7d 100644 (file)
--- a/projects/microb2010/cobboard/spickle.h
+++ b/projects/microb2010/cobboard/spickle.h
@@ -23,7 +23,8 @@
 #define _SPICKLE_H_
 
 void spickle_set(void *dummy, int32_t cmd);
-void spickle_set_coefs(uint32_t k1, uint32_t k2);
+void spickle_set_scoefs(uint32_t k1, uint32_t k2);
+void spickle_set_wcoefs(uint32_t k1, uint32_t k2);
 void spickle_set_pos(uint8_t side, int32_t pos_pack,
                     int32_t pos_mid, int32_t pos_deployed);
 

diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c
index cbbc8293b5d28ffeb83d3afaf095711cd2799fab..4eef55f5c55247e6de410c51d2e92a3e211acb60 100644 (file)
--- a/projects/microb2010/cobboard/state.c
+++ b/projects/microb2010/cobboard/state.c
@@ -120,6 +120,14 @@ static uint8_t state_spicklemode_nomove(uint8_t side)
                return rspickle & I2C_COBBOARD_SPK_NO_MOVE;
 }
 
+uint8_t state_spicklemode_weak(uint8_t side)
+{
+       if (side == I2C_LEFT_SIDE)
+               return lspickle & I2C_COBBOARD_SPK_WEAK;
+       else
+               return rspickle & I2C_COBBOARD_SPK_WEAK;
+}
+
 /* pack/deploy spickles depending on mode */
 static void spickle_prepare(uint8_t side)
 {

diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h
index f3c90ed271743221399cd507263d55a171bd7ef8..aa55b34d7159d4514c6362a33c8c527ba12f2a15 100644 (file)
--- a/projects/microb2010/cobboard/state.h
+++ b/projects/microb2010/cobboard/state.h
@@ -29,6 +29,7 @@ void state_set_spickle(uint8_t side, uint8_t flags);
 /* get current state */
 uint8_t state_get_mode(void);
 uint8_t state_get_status(void);
+uint8_t state_spicklemode_weak(uint8_t side);
 
 void state_set_i2c_ignore(uint8_t val);
 uint8_t state_get_i2c_ignore(void);

diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index bbebd32a577727e2657e5d568b91c89af7fe2bed..b5c9d36897e6ee80ae32d5209255621ef6b12818 100644 (file)
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -110,6 +110,7 @@ struct i2c_req_cobboard_status {
 #define I2C_COBBOARD_SPK_DEPLOY  0x01 /* deploy the spickle */
 #define I2C_COBBOARD_SPK_AUTOHARVEST 0x02 /* auto harvest the cobs */
 #define I2C_COBBOARD_SPK_NO_MOVE 0x04 /* if enabled, don't change state */
+#define I2C_COBBOARD_SPK_WEAK 0x08 /* if enabled, current limit */
        uint8_t lspickle;
        uint8_t rspickle;
 };