weak current limit on spickles

diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
index dc1ec35..9cfda19 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);
        }
                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);
        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_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);
 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;
 
 {
        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 */
        }
 
        /* 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_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 =
 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 a6bf6db..741c103 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 "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) */
 #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];
 
        /* cs blocks */
        struct cs_block * const csb[2];


@@ -60,8 +63,10 @@ struct spickle_params {
 };
 
 static struct spickle_params spickle = {
 };
 
 static struct spickle_params spickle = {

-       .k1 = 1000,
-       .k2 = 20,
+       .sk1 = 1000,
+       .sk2 = 20,
+       .wk1 = 200,
+       .wk2 = 20,

        .csb = {
                &cobboard.left_spickle,
                &cobboard.right_spickle,
        .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;
 {
        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 (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;
        }
        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)
        }
 
        speed = pos - oldpos;
        if (speed > 0 && cmd < 0)

-               maxcmd = spickle.k1;
+               maxcmd = k1;

        else if (speed < 0 && cmd > 0)
        else if (speed < 0 && cmd > 0)

-               maxcmd = spickle.k1;
+               maxcmd = k1;

        else {
                speed = ABS(speed);
        else {
                speed = ABS(speed);

-               maxcmd = spickle.k1 + spickle.k2 * speed;
+               maxcmd = k1 + k2 * speed;

        }
        if (cmd > maxcmd)
                cmd = maxcmd;
        }
        if (cmd > maxcmd)
                cmd = maxcmd;


@@ -131,10 +153,16 @@ void spickle_set(void *mot, int32_t cmd)
                oldpos_right = pos;
 }
 
                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,
 }
 
 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)
 {
 
 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],
        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 f34894a..bf5887c 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);
 #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);
 
 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 cbbc829..4eef55f 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;
 }
 
                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)
 {
 /* 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 f3c90ed..aa55b34 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);
 /* 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);
 
 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 bbebd32..b5c9d36 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_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;
 };
        uint8_t lspickle;
        uint8_t rspickle;
 };