From f5502f60124c62801685777bb7c699bdfb7e1166 Mon Sep 17 00:00:00 2001 From: zer0 Date: Thu, 6 May 2010 01:13:44 +0200 Subject: [PATCH] current limit on shovel --- projects/microb2010/cobboard/commands.c | 4 + .../microb2010/cobboard/commands_cobboard.c | 73 +++++++++++++++++++ projects/microb2010/cobboard/cs.c | 3 +- projects/microb2010/cobboard/shovel.c | 49 +++++++++++++ projects/microb2010/cobboard/shovel.h | 5 ++ projects/microb2010/mainboard/display.py | 38 +++++----- projects/microb2010/mainboard/i2c_protocol.c | 7 +- projects/microb2010/mainboard/main.h | 6 +- projects/microb2010/mainboard/robotsim.c | 19 +++-- projects/microb2010/mainboard/robotsim.h | 1 + 10 files changed, 178 insertions(+), 27 deletions(-) diff --git a/projects/microb2010/cobboard/commands.c b/projects/microb2010/cobboard/commands.c index feeb32f..df0b701 100644 --- 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 669c4c8..dc1ec35 100644 --- 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 f8c7123..386b360 100644 --- 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 b2a8cc0..56efa96 100644 --- 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 80bceec..fa0e2a2 100644 --- 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 f89580c..297dbce 100644 --- 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 7a2c378..3354e39 100644 --- 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 0798699..00b6e86 100755 --- 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 2d0d7be..42a33d9 100644 --- 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 0950af0..7c1eccf 100644 --- 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); -- 2.39.5