From d9b4e922ce1253224adb937d65c8c2e7cd5cfe87 Mon Sep 17 00:00:00 2001 From: zer0 Date: Sat, 8 May 2010 02:15:43 +0200 Subject: [PATCH] kickstand --- projects/microb2010/ballboard/ax12_user.c | 1 - projects/microb2010/cobboard/ax12_user.c | 1 - .../microb2010/cobboard/commands_cobboard.c | 4 +- projects/microb2010/cobboard/shovel.c | 7 +++ projects/microb2010/cobboard/shovel.h | 1 + projects/microb2010/cobboard/state.c | 49 +++++++++++++------ projects/microb2010/common/i2c_commands.h | 6 ++- .../microb2010/mainboard/commands_mainboard.c | 4 +- 8 files changed, 53 insertions(+), 20 deletions(-) diff --git a/projects/microb2010/ballboard/ax12_user.c b/projects/microb2010/ballboard/ax12_user.c index e3a5f9f..ae443f1 100644 --- a/projects/microb2010/ballboard/ax12_user.c +++ b/projects/microb2010/ballboard/ax12_user.c @@ -80,7 +80,6 @@ */ static volatile uint8_t ax12_state = AX12_STATE_READ; -extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */ static volatile uint8_t ax12_nsent = 0; /* Called by ax12 module to send a character on serial line. Count the diff --git a/projects/microb2010/cobboard/ax12_user.c b/projects/microb2010/cobboard/ax12_user.c index b022ef7..ceb8413 100644 --- a/projects/microb2010/cobboard/ax12_user.c +++ b/projects/microb2010/cobboard/ax12_user.c @@ -91,7 +91,6 @@ static microseconds t_prev_msg = 0; */ static volatile uint8_t ax12_state = AX12_STATE_READ; -extern volatile struct cirbuf g_tx_fifo[]; /* uart fifo */ static volatile uint8_t ax12_nsent = 0; /* Called by ax12 module to send a character on serial line. Count the diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 9cfda19..24226e1 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -193,6 +193,8 @@ static void cmd_state1_parsed(void *parsed_result, state_init(); else if (!strcmp_P(res->arg1, PSTR("eject"))) state_set_mode(I2C_COBBOARD_MODE_EJECT); + else if (!strcmp_P(res->arg1, PSTR("kickstand"))) + state_set_mode(I2C_COBBOARD_MODE_KICKSTAND); else if (!strcmp_P(res->arg1, PSTR("ignore_i2c"))) state_set_i2c_ignore(1); else if (!strcmp_P(res->arg1, PSTR("care_i2c"))) @@ -203,7 +205,7 @@ static void cmd_state1_parsed(void *parsed_result, prog_char str_state1_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_state1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg0, str_state1_arg0); -prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c"; +prog_char str_state1_arg1[] = "init#eject#ignore_i2c#care_i2c#kickstand"; parse_pgm_token_string_t cmd_state1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state1_result, arg1, str_state1_arg1); prog_char help_state1[] = "set cobboard mode"; diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c index 56efa96..71139ab 100644 --- a/projects/microb2010/cobboard/shovel.c +++ b/projects/microb2010/cobboard/shovel.c @@ -142,6 +142,13 @@ void shovel_up(void) cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); } +void shovel_kickstand(void) +{ + quadramp_set_1st_order_vars(&cobboard.shovel.qr, 200, 200); + quadramp_set_2nd_order_vars(&cobboard.shovel.qr, 10, 10); + cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); +} + uint8_t shovel_is_up(void) { return shovel_is_at_pos(SHOVEL_UP); diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h index fa0e2a2..3b01e77 100644 --- a/projects/microb2010/cobboard/shovel.h +++ b/projects/microb2010/cobboard/shovel.h @@ -32,6 +32,7 @@ void shovel_set(void *mot, int32_t cmd); void shovel_down(void); void shovel_mid(void); void shovel_up(void); +void shovel_kickstand(void); uint8_t shovel_is_up(void); uint8_t shovel_is_down(void); diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index 4eef55f..d9e6533 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -66,6 +66,7 @@ static uint8_t cob_count; #define INIT(mode) ((mode) == I2C_COBBOARD_MODE_INIT) #define HARVEST(mode) ((mode) == I2C_COBBOARD_MODE_HARVEST) #define EJECT(mode) ((mode) == I2C_COBBOARD_MODE_EJECT) +#define KICKSTAND(mode) ((mode) == I2C_COBBOARD_MODE_KICKSTAND) uint8_t state_debug = 0; @@ -131,6 +132,10 @@ uint8_t state_spicklemode_weak(uint8_t side) /* pack/deploy spickles depending on mode */ static void spickle_prepare(uint8_t side) { + /* pack spickle if we are not in harvest mode */ + if (!HARVEST(state_mode)) + spickle_pack(side); + /* we do nothing in mode no out */ if (state_spicklemode_nomove(side)) return; @@ -337,20 +342,36 @@ void state_machine(void) state_mode = I2C_COBBOARD_MODE_HARVEST; } - /* pack/deply spickles, enable/disable roller */ - cobroller_off(I2C_LEFT_SIDE); - cobroller_off(I2C_RIGHT_SIDE); - spickle_prepare(I2C_LEFT_SIDE); - spickle_prepare(I2C_RIGHT_SIDE); - - /* harvest */ - if (cob_count < 5) { - if (state_spicklemode_deployed(I2C_LEFT_SIDE) && - state_spicklemode_autoharvest(I2C_LEFT_SIDE)) - state_do_harvest(I2C_LEFT_SIDE); - if (state_spicklemode_deployed(I2C_RIGHT_SIDE) && - state_spicklemode_autoharvest(I2C_RIGHT_SIDE)) - state_do_harvest(I2C_RIGHT_SIDE); + if (HARVEST(state_mode)) { + /* init for each loop */ + shovel_down(); + servo_carry_close(); + servo_door_close(); + shovel_current_limit_enable(0); + + /* pack/deply spickles, enable/disable roller */ + cobroller_off(I2C_LEFT_SIDE); + cobroller_off(I2C_RIGHT_SIDE); + spickle_prepare(I2C_LEFT_SIDE); + spickle_prepare(I2C_RIGHT_SIDE); + + /* harvest if not many cobs */ + if (cob_count < 5) { + if (state_spicklemode_deployed(I2C_LEFT_SIDE) && + state_spicklemode_autoharvest(I2C_LEFT_SIDE)) + state_do_harvest(I2C_LEFT_SIDE); + if (state_spicklemode_deployed(I2C_RIGHT_SIDE) && + state_spicklemode_autoharvest(I2C_RIGHT_SIDE)) + state_do_harvest(I2C_RIGHT_SIDE); + } + } + + /* help to climb the hill */ + if (KICKSTAND(state_mode)) { + servo_carry_open(); + servo_door_open(); + shovel_current_limit_enable(1); + shovel_kickstand(); } /* eject */ diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index b5c9d36..5e4a449 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -1,6 +1,6 @@ /* - * Copyright Droids Corporation (2007) - * + * Copyright Droids Corporation (2010) + * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or @@ -78,6 +78,7 @@ struct i2c_cmd_cobboard_set_mode { #define I2C_COBBOARD_MODE_HARVEST 0x01 /* harvest mode */ #define I2C_COBBOARD_MODE_EJECT 0x02 /* eject cobs */ #define I2C_COBBOARD_MODE_INIT 0x03 /* init state machine */ +#define I2C_COBBOARD_MODE_KICKSTAND 0x04 /* help to climb the hill */ uint8_t mode; }; @@ -128,6 +129,7 @@ struct i2c_ans_cobboard_status { #define I2C_COBBOARD_STATUS_LBUSY 0x02 #define I2C_COBBOARD_STATUS_RBUSY 0x03 #define I2C_COBBOARD_STATUS_EJECT 0x04 +#define I2C_COBBOARD_STATUS_KICKSTAND 0x05 uint8_t status; uint8_t cob_count; diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index 2335ce3..04e8c91 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -738,11 +738,13 @@ static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data) i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT); else if (!strcmp_P(res->arg1, PSTR("eject"))) i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT); + else if (!strcmp_P(res->arg1, PSTR("kickstand"))) + i2c_cobboard_set_mode(I2C_COBBOARD_MODE_KICKSTAND); } prog_char str_cobboard_setmode1_arg0[] = "cobboard"; parse_pgm_token_string_t cmd_cobboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg0, str_cobboard_setmode1_arg0); -prog_char str_cobboard_setmode1_arg1[] = "init#eject"; +prog_char str_cobboard_setmode1_arg1[] = "init#eject#kickstand"; parse_pgm_token_string_t cmd_cobboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode1_result, arg1, str_cobboard_setmode1_arg1); prog_char help_cobboard_setmode1[] = "set cobboard mode (mode)"; -- 2.39.5