From d23a4fdeed4e90d004d407942b230c188d012465 Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 6 May 2010 01:24:39 +0200 Subject: [PATCH] weak current limit on spickles --- .../microb2010/cobboard/commands_cobboard.c | 15 ++++-- projects/microb2010/cobboard/spickle.c | 51 +++++++++++++++---- projects/microb2010/cobboard/spickle.h | 3 +- projects/microb2010/cobboard/state.c | 8 +++ projects/microb2010/cobboard/state.h | 1 + projects/microb2010/common/i2c_commands.h | 1 + 6 files changed, 63 insertions(+), 16 deletions(-) diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index dc1ec35..9cfda19 100644 --- 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 a6bf6db..741c103 100644 --- 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 f34894a..bf5887c 100644 --- 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 cbbc829..4eef55f 100644 --- 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 f3c90ed..aa55b34 100644 --- 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 bbebd32..b5c9d36 100644 --- 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; }; -- 2.39.5