From 57b3e145f2ff46e1a7e7d522c85f3c56821dbf48 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Sat, 27 Mar 2010 17:47:44 +0100 Subject: [PATCH] cobboard: prepare state machine --- projects/microb2010/cobboard/Makefile | 2 +- projects/microb2010/cobboard/actuator.c | 34 +- projects/microb2010/cobboard/actuator.h | 6 + projects/microb2010/cobboard/commands.c | 4 + .../microb2010/cobboard/commands_cobboard.c | 308 ++++++++++++++---- projects/microb2010/cobboard/i2c_protocol.c | 5 +- projects/microb2010/cobboard/main.c | 6 +- projects/microb2010/cobboard/main.h | 4 + projects/microb2010/cobboard/shovel.c | 57 ++++ projects/microb2010/cobboard/shovel.h | 46 +++ projects/microb2010/cobboard/spickle.c | 161 +++++---- projects/microb2010/cobboard/spickle.h | 17 +- projects/microb2010/cobboard/state.c | 163 +++++---- projects/microb2010/cobboard/state.h | 2 +- projects/microb2010/common/i2c_commands.h | 23 +- 15 files changed, 616 insertions(+), 222 deletions(-) create mode 100644 projects/microb2010/cobboard/shovel.c create mode 100644 projects/microb2010/cobboard/shovel.h diff --git a/projects/microb2010/cobboard/Makefile b/projects/microb2010/cobboard/Makefile index 8293a3f..81ffee1 100644 --- a/projects/microb2010/cobboard/Makefile +++ b/projects/microb2010/cobboard/Makefile @@ -11,7 +11,7 @@ LDFLAGS = -T ../common/avr6.x SRC = $(TARGET).c cmdline.c commands_ax12.c commands_gen.c SRC += commands_cs.c commands_cobboard.c commands.c SRC += i2c_protocol.c sensor.c actuator.c cs.c -SRC += state.c ax12_user.c spickle.c +SRC += state.c ax12_user.c spickle.c shovel.c # List Assembler source files here. # Make them always end in a capital .S. Files ending in a lowercase .s diff --git a/projects/microb2010/cobboard/actuator.c b/projects/microb2010/cobboard/actuator.c index 8c15480..3217270 100644 --- a/projects/microb2010/cobboard/actuator.c +++ b/projects/microb2010/cobboard/actuator.c @@ -42,23 +42,55 @@ #include "sensor.h" #include "../common/i2c_commands.h" -#include "actuator.h" #include "main.h" +#include "actuator.h" + +#define COBROLLER_SPEED 800 + +#define SERVO_DOOR_PWM ((void *)&gen.servo2) +#define SERVO_DOOR_OPEN 250 +#define SERVO_DOOR_CLOSED 470 + +void actuator_init(void); void servo_carry_open(void) { + /* TODO */ } void servo_carry_close(void) { + /* TODO */ } void servo_door_open(void) { + pwm_ng_set(SERVO_DOOR_PWM, SERVO_DOOR_OPEN); } void servo_door_close(void) { + pwm_ng_set(SERVO_DOOR_PWM, SERVO_DOOR_CLOSED); +} + +void left_cobroller_on(void) +{ + cobboard.left_cobroller_speed = COBROLLER_SPEED; +} + +void right_cobroller_on(void) +{ + cobboard.right_cobroller_speed = COBROLLER_SPEED; +} + +void left_cobroller_off(void) +{ + cobboard.left_cobroller_speed = 0; +} + +void right_cobroller_off(void) +{ + cobboard.right_cobroller_speed = 0; } void actuator_init(void) diff --git a/projects/microb2010/cobboard/actuator.h b/projects/microb2010/cobboard/actuator.h index 7ef1572..90ff487 100644 --- a/projects/microb2010/cobboard/actuator.h +++ b/projects/microb2010/cobboard/actuator.h @@ -23,10 +23,16 @@ #define _ACTUATOR_H_ void actuator_init(void); + void servo_carry_open(void); void servo_carry_close(void); void servo_door_open(void); void servo_door_close(void); +void left_cobroller_on(void); +void right_cobroller_on(void); +void left_cobroller_off(void); +void right_cobroller_off(void); + #endif diff --git a/projects/microb2010/cobboard/commands.c b/projects/microb2010/cobboard/commands.c index ca5ded6..6952a74 100644 --- a/projects/microb2010/cobboard/commands.c +++ b/projects/microb2010/cobboard/commands.c @@ -71,10 +71,12 @@ extern parse_pgm_inst_t cmd_state3; extern parse_pgm_inst_t cmd_state_debug; extern parse_pgm_inst_t cmd_state_machine; extern parse_pgm_inst_t cmd_servo_door; +extern parse_pgm_inst_t cmd_cobroller; extern parse_pgm_inst_t cmd_servo_carry; extern parse_pgm_inst_t cmd_spickle; extern parse_pgm_inst_t cmd_spickle_params; extern parse_pgm_inst_t cmd_spickle_params_show; +extern parse_pgm_inst_t cmd_shovel; extern parse_pgm_inst_t cmd_test; @@ -128,10 +130,12 @@ parse_pgm_ctx_t main_ctx[] = { (parse_pgm_inst_t *)&cmd_state_debug, (parse_pgm_inst_t *)&cmd_state_machine, (parse_pgm_inst_t *)&cmd_servo_door, + (parse_pgm_inst_t *)&cmd_cobroller, (parse_pgm_inst_t *)&cmd_servo_carry, (parse_pgm_inst_t *)&cmd_spickle, (parse_pgm_inst_t *)&cmd_spickle_params, (parse_pgm_inst_t *)&cmd_spickle_params_show, + (parse_pgm_inst_t *)&cmd_shovel, (parse_pgm_inst_t *)&cmd_test, NULL, diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c index 3c6e3e3..249a69b 100644 --- a/projects/microb2010/cobboard/commands_cobboard.c +++ b/projects/microb2010/cobboard/commands_cobboard.c @@ -51,6 +51,7 @@ #include "i2c_protocol.h" #include "actuator.h" #include "spickle.h" +#include "shovel.h" extern uint16_t state_debug; @@ -185,23 +186,18 @@ static void cmd_state1_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state1_result *res = parsed_result; - struct i2c_cmd_cobboard_set_mode command; - if (!strcmp_P(res->arg1, PSTR("init"))) { + if (!strcmp_P(res->arg1, PSTR("init"))) state_init(); - return; - } + else if (!strcmp_P(res->arg1, PSTR("eject"))) + state_set_mode(I2C_COBBOARD_MODE_EJECT); - if (!strcmp_P(res->arg1, PSTR("manual"))) - command.mode = I2C_COBBOARD_MODE_MANUAL; - else if (!strcmp_P(res->arg1, PSTR("harvest"))) - command.mode = I2C_COBBOARD_MODE_HARVEST; - state_set_mode(&command); + /* other commands */ } 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#manual"; +prog_char str_state1_arg1[] = "init#eject"; 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"; @@ -231,26 +227,43 @@ static void cmd_state2_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state2_result *res = parsed_result; - struct i2c_cmd_cobboard_set_mode command; - uint8_t side; + uint8_t side, mode = state_get_mode(); - if (!strcmp_P(res->arg2, PSTR("left"))) + if (!strcmp_P(res->arg2, PSTR("left"))) { side = I2C_LEFT_SIDE; - else if (!strcmp_P(res->arg2, PSTR("right"))) + mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST); + } + else { side = I2C_RIGHT_SIDE; + mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST); + } - if (!strcmp_P(res->arg1, PSTR("yyy"))) { + if (!strcmp_P(res->arg1, PSTR("pack"))) { + /* nothing to do */ } - else if (!strcmp_P(res->arg1, PSTR("xxx"))) { + else if (!strcmp_P(res->arg1, PSTR("deploy"))) { + if (side == I2C_LEFT_SIDE) + mode |= I2C_COBBOARD_MODE_L_DEPLOY; + else + mode |= I2C_COBBOARD_MODE_R_DEPLOY; + } + else if (!strcmp_P(res->arg1, PSTR("harvest"))) { + if (side == I2C_LEFT_SIDE) { + mode |= I2C_COBBOARD_MODE_L_DEPLOY; + mode |= I2C_COBBOARD_MODE_L_HARVEST; + } + else { + mode |= I2C_COBBOARD_MODE_R_DEPLOY; + mode |= I2C_COBBOARD_MODE_R_HARVEST; + } } - - state_set_mode(&command); + state_set_mode(mode); } 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[] = "xxx"; +prog_char str_state2_arg1[] = "harvest#deploy#pack"; 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); @@ -283,14 +296,13 @@ static void cmd_state3_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_state3_result *res = parsed_result; - struct i2c_cmd_cobboard_set_mode command; if (!strcmp_P(res->arg1, PSTR("xxx"))) { /* xxx = res->arg2 */ } else if (!strcmp_P(res->arg1, PSTR("yyy"))) { } - state_set_mode(&command); + state_set_mode(0); } prog_char str_state3_arg0[] = "cobboard"; @@ -411,6 +423,95 @@ parse_pgm_inst_t cmd_servo_door = { }, }; +/**********************************************************/ +/* cobroller */ + +/* this structure is filled when cmd_cobroller is parsed successfully */ +struct cmd_cobroller_result { + fixed_string_t arg0; + fixed_string_t arg1; + fixed_string_t arg2; +}; + +/* function called when cmd_cobroller is parsed successfully */ +static void cmd_cobroller_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_cobroller_result *res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("left"))) { + if (!strcmp_P(res->arg2, PSTR("on"))) + left_cobroller_on(); + else if (!strcmp_P(res->arg2, PSTR("off"))) + left_cobroller_off(); + } + else if (!strcmp_P(res->arg1, PSTR("right"))) { + if (!strcmp_P(res->arg2, PSTR("on"))) + right_cobroller_on(); + else if (!strcmp_P(res->arg2, PSTR("off"))) + right_cobroller_off(); + } +} + +prog_char str_cobroller_arg0[] = "cobroller"; +parse_pgm_token_string_t cmd_cobroller_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg0, str_cobroller_arg0); +prog_char str_cobroller_arg1[] = "left#right"; +parse_pgm_token_string_t cmd_cobroller_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg1, str_cobroller_arg1); +prog_char str_cobroller_arg2[] = "on#off"; +parse_pgm_token_string_t cmd_cobroller_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobroller_result, arg2, str_cobroller_arg2); + +prog_char help_cobroller[] = "Servo door function"; +parse_pgm_inst_t cmd_cobroller = { + .f = cmd_cobroller_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_cobroller, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_cobroller_arg0, + (prog_void *)&cmd_cobroller_arg1, + (prog_void *)&cmd_cobroller_arg2, + NULL, + }, +}; + +/**********************************************************/ +/* shovel */ + +/* this structure is filled when cmd_shovel is parsed successfully */ +struct cmd_shovel_result { + fixed_string_t arg0; + fixed_string_t arg1; +}; + +/* function called when cmd_shovel is parsed successfully */ +static void cmd_shovel_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_shovel_result *res = parsed_result; + if (!strcmp_P(res->arg1, PSTR("down"))) + shovel_down(); + else if (!strcmp_P(res->arg1, PSTR("up"))) + shovel_up(); + else if (!strcmp_P(res->arg1, PSTR("mid"))) + shovel_mid(); +} + +prog_char str_shovel_arg0[] = "shovel"; +parse_pgm_token_string_t cmd_shovel_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_shovel_result, arg0, str_shovel_arg0); +prog_char str_shovel_arg1[] = "down#up#mid"; +parse_pgm_token_string_t cmd_shovel_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_shovel_result, arg1, str_shovel_arg1); + +prog_char help_shovel[] = "Servo shovel function"; +parse_pgm_inst_t cmd_shovel = { + .f = cmd_shovel_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_shovel, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_shovel_arg0, + (prog_void *)&cmd_shovel_arg1, + NULL, + }, +}; + /**********************************************************/ /* Servo_Carry */ @@ -449,12 +550,13 @@ parse_pgm_inst_t cmd_servo_carry = { }; /**********************************************************/ -/* Spickles tests */ +/* Spickle tests */ /* this structure is filled when cmd_spickle is parsed successfully */ struct cmd_spickle_result { fixed_string_t arg0; fixed_string_t arg1; + fixed_string_t arg2; }; /* function called when cmd_spickle is parsed successfully */ @@ -462,29 +564,33 @@ static void cmd_spickle_parsed(void * parsed_result, __attribute__((unused)) void *data) { struct cmd_spickle_result * res = parsed_result; - - if (!strcmp_P(res->arg1, PSTR("up"))) { - spickle_up(); - } - else if (!strcmp_P(res->arg1, PSTR("down"))) { - spickle_down(); - } - else if (!strcmp_P(res->arg1, PSTR("stop"))) { - spickle_stop(); + uint8_t side; + + if (!strcmp_P(res->arg1, PSTR("left"))) + side = I2C_LEFT_SIDE; + else + side = I2C_RIGHT_SIDE; + + if (!strcmp_P(res->arg2, PSTR("deploy"))) { + spickle_deploy(side); } - else if (!strcmp_P(res->arg1, PSTR("auto"))) { - spickle_auto(); + else if (!strcmp_P(res->arg2, PSTR("pack"))) { + spickle_pack(side); } - printf_P(PSTR("done\r\n")); } prog_char str_spickle_arg0[] = "spickle"; -parse_pgm_token_string_t cmd_spickle_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg0, str_spickle_arg0); -prog_char str_spickle_arg1[] = "auto#up#down#stop"; -parse_pgm_token_string_t cmd_spickle_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1); - -prog_char help_spickle[] = "spickle auto mode: spickle auto delay_up delay_down"; +parse_pgm_token_string_t cmd_spickle_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg0, str_spickle_arg0); +prog_char str_spickle_arg1[] = "left#right"; +parse_pgm_token_string_t cmd_spickle_arg1 = + TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg1, str_spickle_arg1); +prog_char str_spickle_arg2[] = "deploy#pack"; +parse_pgm_token_string_t cmd_spickle_arg2 = + TOKEN_STRING_INITIALIZER(struct cmd_spickle_result, arg2, str_spickle_arg2); + +prog_char help_spickle[] = "move spickle"; parse_pgm_inst_t cmd_spickle = { .f = cmd_spickle_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ @@ -492,6 +598,7 @@ parse_pgm_inst_t cmd_spickle = { .tokens = { /* token list, NULL terminated */ (prog_void *)&cmd_spickle_arg0, (prog_void *)&cmd_spickle_arg1, + (prog_void *)&cmd_spickle_arg2, NULL, }, }; @@ -503,8 +610,9 @@ parse_pgm_inst_t cmd_spickle = { struct cmd_spickle_params_result { fixed_string_t arg0; fixed_string_t arg1; - int32_t arg2; + fixed_string_t arg2; int32_t arg3; + int32_t arg4; }; /* function called when cmd_spickle_params is parsed successfully */ @@ -512,30 +620,39 @@ static void cmd_spickle_params_parsed(void *parsed_result, __attribute__((unused)) void *data) { struct cmd_spickle_params_result * res = parsed_result; - - - if (!strcmp_P(res->arg1, PSTR("delay"))) { - spickle_set_delays(res->arg2, res->arg3); - } - else if (!strcmp_P(res->arg1, PSTR("coef"))) { - spickle_set_coefs(res->arg2, res->arg3); - } - else if (!strcmp_P(res->arg1, PSTR("pos"))) { - spickle_set_pos(res->arg2, res->arg3); + uint8_t side; + + if (!strcmp_P(res->arg1, PSTR("show"))) { + spickle_dump_params(); + return; } - /* else show */ - spickle_dump_params(); + if (!strcmp_P(res->arg1, PSTR("left"))) + side = I2C_LEFT_SIDE; + else + side = I2C_RIGHT_SIDE; + + if (!strcmp_P(res->arg2, PSTR("pos"))) + spickle_set_pos(side, res->arg3, res->arg4); + else if (!strcmp_P(res->arg2, PSTR("delay"))) + spickle_set_delay(side, res->arg3, res->arg4); } prog_char str_spickle_params_arg0[] = "spickle_params"; -parse_pgm_token_string_t cmd_spickle_params_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg0, str_spickle_params_arg0); -prog_char str_spickle_params_arg1[] = "delay#pos#coef"; -parse_pgm_token_string_t cmd_spickle_params_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1); -parse_pgm_token_num_t cmd_spickle_params_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg2, INT32); -parse_pgm_token_num_t cmd_spickle_params_arg3 = TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32); - -prog_char help_spickle_params[] = "Set spickle_params values"; +parse_pgm_token_string_t cmd_spickle_params_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg0, str_spickle_params_arg0); +prog_char str_spickle_params_arg1[] = "left#right"; +parse_pgm_token_string_t cmd_spickle_params_arg1 = + TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1); +prog_char str_spickle_params_arg2[] = "pos#delay"; +parse_pgm_token_string_t cmd_spickle_params_arg2 = + TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg2, str_spickle_params_arg2); +parse_pgm_token_num_t cmd_spickle_params_arg3 = + TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg3, INT32); +parse_pgm_token_num_t cmd_spickle_params_arg4 = + TOKEN_NUM_INITIALIZER(struct cmd_spickle_params_result, arg4, INT32); + +prog_char help_spickle_params[] = "Set spickle pos values"; parse_pgm_inst_t cmd_spickle_params = { .f = cmd_spickle_params_parsed, /* function to call */ .data = NULL, /* 2nd arg of func */ @@ -545,12 +662,14 @@ parse_pgm_inst_t cmd_spickle_params = { (prog_void *)&cmd_spickle_params_arg1, (prog_void *)&cmd_spickle_params_arg2, (prog_void *)&cmd_spickle_params_arg3, + (prog_void *)&cmd_spickle_params_arg4, NULL, }, }; prog_char str_spickle_params_arg1_show[] = "show"; -parse_pgm_token_string_t cmd_spickle_params_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1_show); +parse_pgm_token_string_t cmd_spickle_params_arg1_show = + TOKEN_STRING_INITIALIZER(struct cmd_spickle_params_result, arg1, str_spickle_params_arg1_show); prog_char help_spickle_params_show[] = "show spickle params"; parse_pgm_inst_t cmd_spickle_params_show = { @@ -564,6 +683,72 @@ parse_pgm_inst_t cmd_spickle_params_show = { }, }; +/**********************************************************/ +/* Set Spickle Params */ + +/* this structure is filled when cmd_spickle_params2 is parsed successfully */ +struct cmd_spickle_params2_result { + fixed_string_t arg0; + fixed_string_t arg1; + int32_t arg2; + int32_t arg3; +}; + +/* function called when cmd_spickle_params2 is parsed successfully */ +static void cmd_spickle_params2_parsed(void *parsed_result, + __attribute__((unused)) void *data) +{ + struct cmd_spickle_params2_result * res = parsed_result; + + if (!strcmp_P(res->arg1, PSTR("coef"))) { + spickle_set_coefs(res->arg2, res->arg3); + } + + /* else show */ + spickle_dump_params(); +} + +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"; +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 = + TOKEN_NUM_INITIALIZER(struct cmd_spickle_params2_result, arg2, INT32); +parse_pgm_token_num_t cmd_spickle_params2_arg3 = + TOKEN_NUM_INITIALIZER(struct cmd_spickle_params2_result, arg3, INT32); + +prog_char help_spickle_params2[] = "Set spickle_params2 values"; +parse_pgm_inst_t cmd_spickle_params2 = { + .f = cmd_spickle_params2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_spickle_params2, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_spickle_params2_arg0, + (prog_void *)&cmd_spickle_params2_arg1, + (prog_void *)&cmd_spickle_params2_arg2, + (prog_void *)&cmd_spickle_params2_arg3, + NULL, + }, +}; + +prog_char str_spickle_params2_arg1_show[] = "show"; +parse_pgm_token_string_t cmd_spickle_params2_arg1_show = + TOKEN_STRING_INITIALIZER(struct cmd_spickle_params2_result, arg1, str_spickle_params2_arg1_show); + +prog_char help_spickle_params2_show[] = "show spickle params"; +parse_pgm_inst_t cmd_spickle_params2_show = { + .f = cmd_spickle_params2_parsed, /* function to call */ + .data = NULL, /* 2nd arg of func */ + .help_str = help_spickle_params2_show, + .tokens = { /* token list, NULL terminated */ + (prog_void *)&cmd_spickle_params2_arg0, + (prog_void *)&cmd_spickle_params2_arg1_show, + NULL, + }, +}; + /**********************************************************/ /* Test */ @@ -579,7 +764,8 @@ static void cmd_test_parsed(__attribute__((unused)) void *parsed_result, } prog_char str_test_arg0[] = "test"; -parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0); +parse_pgm_token_string_t cmd_test_arg0 = + TOKEN_STRING_INITIALIZER(struct cmd_test_result, arg0, str_test_arg0); prog_char help_test[] = "Test function"; parse_pgm_inst_t cmd_test = { diff --git a/projects/microb2010/cobboard/i2c_protocol.c b/projects/microb2010/cobboard/i2c_protocol.c index b790128..53f2ab7 100644 --- a/projects/microb2010/cobboard/i2c_protocol.c +++ b/projects/microb2010/cobboard/i2c_protocol.c @@ -95,13 +95,16 @@ static void i2c_send_status(void) ans.mode = state_get_mode(); ans.status = 0x55; + ans.left_cobroller_speed = cobboard.left_cobroller_speed; + ans.right_cobroller_speed = cobboard.right_cobroller_speed; + i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, sizeof(ans), I2C_CTRL_GENERIC); } static int8_t i2c_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) { - state_set_mode(cmd); + state_set_mode(cmd->mode); return 0; } diff --git a/projects/microb2010/cobboard/main.c b/projects/microb2010/cobboard/main.c index d0526a5..8ba5d52 100755 --- a/projects/microb2010/cobboard/main.c +++ b/projects/microb2010/cobboard/main.c @@ -58,6 +58,7 @@ #include "state.h" #include "actuator.h" #include "spickle.h" +#include "shovel.h" #include "cs.h" #include "i2c_protocol.h" @@ -166,7 +167,7 @@ int main(void) # error not supported #endif - eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); + //eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_COBBOARD); /* check eeprom to avoid to run the bad program */ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) != EEPROM_MAGIC_COBBOARD) { @@ -248,8 +249,9 @@ int main(void) /* actuators */ actuator_init(); - /* spickle */ + /* spickle, shovel */ spickle_init(); + shovel_init(); /* state_init(); */ diff --git a/projects/microb2010/cobboard/main.h b/projects/microb2010/cobboard/main.h index d50b610..dce4825 100755 --- a/projects/microb2010/cobboard/main.h +++ b/projects/microb2010/cobboard/main.h @@ -125,6 +125,10 @@ struct cobboard { uint8_t our_color; volatile uint8_t cob_count; volatile uint8_t status; + + /* synchronized to mainboard */ + int16_t left_cobroller_speed; + int16_t right_cobroller_speed; }; extern struct genboard gen; diff --git a/projects/microb2010/cobboard/shovel.c b/projects/microb2010/cobboard/shovel.c new file mode 100644 index 0000000..39ce2bc --- /dev/null +++ b/projects/microb2010/cobboard/shovel.c @@ -0,0 +1,57 @@ +/* + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $ + * + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "main.h" + +/* init spickle position at beginning */ +static void shovel_autopos(void) +{ + pwm_ng_set(SHOVEL_PWM, -500); + wait_ms(1000); + pwm_ng_set(LEFT_SPICKLE_PWM, 0); + encoders_spi_set_value(SHOVEL_ENCODER, 0); +} + +void shovel_init(void) +{ + shovel_autopos(); + cobboard.shovel.on = 1; +} diff --git a/projects/microb2010/cobboard/shovel.h b/projects/microb2010/cobboard/shovel.h new file mode 100644 index 0000000..1f2e53b --- /dev/null +++ b/projects/microb2010/cobboard/shovel.h @@ -0,0 +1,46 @@ +/* + * 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 + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + * Revision : $Id: actuator.c,v 1.4 2009-04-24 19:30:41 zer0 Exp $ + * + */ + +#ifndef _SHOVEL_H_ +#define _SHOVEL_H_ + +#define SHOVEL_DOWN 100 +#define SHOVEL_MID 4000 +#define SHOVEL_UP 10000 + +void shovel_init(void); + +static inline void shovel_down(void) +{ + cs_set_consign(&cobboard.shovel.cs, SHOVEL_DOWN); +} + +static inline void shovel_mid(void) +{ + cs_set_consign(&cobboard.shovel.cs, SHOVEL_MID); +} + +static inline void shovel_up(void) +{ + cs_set_consign(&cobboard.shovel.cs, SHOVEL_UP); +} + +#endif /* _SHOVEL_H_ */ diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c index db954ed..03565ac 100644 --- a/projects/microb2010/cobboard/spickle.c +++ b/projects/microb2010/cobboard/spickle.c @@ -42,34 +42,62 @@ #include "sensor.h" #include "../common/i2c_commands.h" -#include "actuator.h" #include "main.h" +#include "actuator.h" - -#define OFF 0 -#define WAIT_SENSOR 1 -#define SENSOR_OK 2 -#define WAIT_DOWN 3 - -static volatile uint8_t spickle_state = OFF; -static volatile uint32_t spickle_pos_up = 35000; -static volatile uint32_t spickle_pos_down = 0; -static volatile uint32_t spickle_delay_up = 500; -static volatile uint32_t spickle_delay_down = 2000; -static volatile uint32_t delay = 0; -static volatile int32_t spickle_k1 = 500, spickle_k2 = 20; -static volatile int32_t spickle_cmd = 0; +struct spickle_params { + /* current limit (common to left and right) */ + int32_t k1; + int32_t k2; + + /* cs blocks */ + struct cs_block * const csb[2]; + + /* params */ + int16_t delay_deployed[2]; + int16_t delay_packed[2]; + int32_t pos_deployed[2]; + int32_t pos_packed[2]; +}; + +static struct spickle_params spickle = { + .k1 = 500, + .k2 = 20, + .csb = { + &cobboard.left_spickle, + &cobboard.right_spickle, + }, + .delay_deployed = { + 500, /* left */ + 500, /* right */ + }, + .delay_packed = { + 500, /* left */ + 500, /* right */ + }, + .pos_deployed = { + 35000, /* left */ + 35000, /* right */ + }, + .pos_packed = { + 0, /* left */ + 0, /* right */ + }, +}; /* init spickle position at beginning */ static void spickle_autopos(void) { pwm_ng_set(LEFT_SPICKLE_PWM, -500); - wait_ms(3000); + //pwm_ng_set(RIGHT_SPICKLE_PWM, -500); + wait_ms(1000); pwm_ng_set(LEFT_SPICKLE_PWM, 0); + pwm_ng_set(RIGHT_SPICKLE_PWM, 0); encoders_spi_set_value(LEFT_SPICKLE_ENCODER, 0); + encoders_spi_set_value(RIGHT_SPICKLE_ENCODER, 0); } -/* set CS command for spickle */ +/* Set CS command for spickle. Called by CS manager. */ void spickle_set(void *mot, int32_t cmd) { static int32_t oldpos_left, oldpos_right; @@ -86,12 +114,12 @@ void spickle_set(void *mot, int32_t cmd) speed = pos - oldpos; if (speed > 0 && cmd < 0) - maxcmd = spickle_k1; + maxcmd = spickle.k1; else if (speed < 0 && cmd > 0) - maxcmd = spickle_k1; + maxcmd = spickle.k1; else { speed = ABS(speed); - maxcmd = spickle_k1 + spickle_k2 * speed; + maxcmd = spickle.k1 + spickle.k2 * speed; } if (cmd > maxcmd) cmd = maxcmd; @@ -108,93 +136,62 @@ void spickle_set(void *mot, int32_t cmd) void spickle_set_coefs(uint32_t k1, uint32_t k2) { - spickle_k1 = k1; - spickle_k2 = k2; + spickle.k1 = k1; + spickle.k2 = k2; } -void spickle_set_delays(uint32_t delay_up, uint32_t delay_down) +void spickle_set_pos(uint8_t side, uint32_t pos_deployed, uint32_t pos_packed) { - spickle_delay_up = delay_up; - spickle_delay_down = delay_down; + spickle.pos_deployed[side] = pos_deployed; + spickle.pos_packed[side] = pos_packed; } -void spickle_set_pos(uint32_t pos_up, uint32_t pos_down) +void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed) { - spickle_pos_up = pos_up; - spickle_pos_down = pos_down; + spickle.delay_deployed[side] = delay_deployed; + spickle.delay_packed[side] = delay_packed; } void spickle_dump_params(void) { - printf_P(PSTR("coef %ld %ld\r\n"), spickle_k1, spickle_k2); - printf_P(PSTR("pos %ld %ld\r\n"), spickle_pos_up, spickle_pos_down); - printf_P(PSTR("delay %ld %ld\r\n"), spickle_delay_up, spickle_delay_down); + printf_P(PSTR("coef %ld %ld\r\n"), spickle.k1, spickle.k2); + printf_P(PSTR("left pos %ld %ld\r\n"), + spickle.pos_deployed[I2C_LEFT_SIDE], + spickle.pos_packed[I2C_LEFT_SIDE]); + printf_P(PSTR("left delay %ld %ld\r\n"), + spickle.delay_deployed[I2C_LEFT_SIDE], + spickle.delay_packed[I2C_LEFT_SIDE]); + printf_P(PSTR("right pos %ld %ld\r\n"), + spickle.pos_deployed[I2C_RIGHT_SIDE], + spickle.pos_packed[I2C_RIGHT_SIDE]); + printf_P(PSTR("right delay %ld %ld\r\n"), + spickle.delay_deployed[I2C_RIGHT_SIDE], + spickle.delay_packed[I2C_RIGHT_SIDE]); } -void spickle_up(void) +void spickle_deploy(uint8_t side) { - spickle_state = 0; - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]); } -void spickle_down(void) +void spickle_pack(uint8_t side) { - spickle_state = 0; - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down); + cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]); } -void spickle_stop(void) +uint16_t spickle_get_deploy_delay(uint8_t side) { - spickle_state = 0; + return spickle.delay_deployed[side]; } -void spickle_auto(void) +uint16_t spickle_get_pack_delay(uint8_t side) { - spickle_state = WAIT_SENSOR; - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); -} - -/* for spickle auto mode */ -static void spickle_cb(__attribute__((unused)) void *dummy) -{ - static uint8_t prev = 0; - uint8_t val; - - val = sensor_get(S_LCOB); - - switch (spickle_state) { - case OFF: - break; - case WAIT_SENSOR: - if (val && !prev) { - delay = spickle_delay_up; - spickle_state = SENSOR_OK; - } - break; - case SENSOR_OK: - if (delay-- == 0) { - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_down); - spickle_state = WAIT_DOWN; - delay = spickle_delay_down; - } - break; - case WAIT_DOWN: - if (delay-- == 0) { - cs_set_consign(&cobboard.left_spickle.cs, spickle_pos_up); - spickle_state = WAIT_SENSOR; - } - break; - default: - break; - } - prev = val; + return spickle.delay_packed[side]; } void spickle_init(void) { spickle_autopos(); - - scheduler_add_periodical_event_priority(spickle_cb, NULL, - 1000L / SCHEDULER_UNIT, - SPICKLE_PRIO); + cobboard.left_spickle.on = 1; + //cobboard.right_spickle.on = 1; } diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h index c2ffcf0..d36f80f 100644 --- a/projects/microb2010/cobboard/spickle.h +++ b/projects/microb2010/cobboard/spickle.h @@ -24,14 +24,17 @@ void spickle_set(void *dummy, int32_t cmd); void spickle_set_coefs(uint32_t k1, uint32_t k2); -void spickle_set_delays(uint32_t delay_up, uint32_t delay_down); -void spickle_set_pos(uint32_t pos_up, uint32_t pos_down); +void spickle_set_pos(uint8_t side, uint32_t pos_deploy, uint32_t pos_pack); +void spickle_set_delay(uint8_t side, uint32_t delay_deployed, uint32_t delay_packed); + void spickle_dump_params(void); -void spickle_left_manage(void); -void spickle_up(void); -void spickle_down(void); -void spickle_stop(void); -void spickle_auto(void); + +void spickle_deploy(uint8_t side); +void spickle_pack(uint8_t side); + +uint16_t spickle_get_deploy_delay(uint8_t side); +uint16_t spickle_get_pack_delay(uint8_t side); + void spickle_init(void); #endif diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c index f9aefa6..d02e544 100644 --- a/projects/microb2010/cobboard/state.c +++ b/projects/microb2010/cobboard/state.c @@ -48,31 +48,37 @@ #include "cmdline.h" #include "sensor.h" #include "actuator.h" +#include "spickle.h" +#include "shovel.h" #include "state.h" #define STMCH_DEBUG(args...) DEBUG(E_USER_ST_MACH, args) #define STMCH_NOTICE(args...) NOTICE(E_USER_ST_MACH, args) #define STMCH_ERROR(args...) ERROR(E_USER_ST_MACH, args) -/* shorter aliases for this file */ -#define INIT I2C_COBBOARD_MODE_INIT -#define MANUAL I2C_COBBOARD_MODE_MANUAL -#define HARVEST I2C_COBBOARD_MODE_HARVEST -#define EXIT I2C_COBBOARD_MODE_EXIT - -static struct i2c_cmd_cobboard_set_mode mainboard_command; static struct vt100 local_vt100; -static volatile uint8_t prev_state; -static volatile uint8_t changed = 0; +static volatile uint8_t state_mode; +static uint8_t cob_count; + +/* short aliases */ +#define L_DEPLOY(mode) (!!((mode) & I2C_COBBOARD_MODE_L_DEPLOY)) +#define R_DEPLOY(mode) (!!((mode) & I2C_COBBOARD_MODE_R_DEPLOY)) +#define DEPLOY(side, mode) ((side) == I2C_LEFT_SIDE ? L_DEPLOY(mode) : R_DEPLOY(mode)) +#define L_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_L_HARVEST)) +#define R_HARVEST(mode) (!!((mode) & I2C_COBBOARD_MODE_R_HARVEST)) +#define HARVEST(side, mode) ((side) == I2C_LEFT_SIDE ? L_HARVEST(mode) : R_HARVEST(mode)) +#define EJECT(mode) (!!((mode) & I2C_COBBOARD_MODE_EJECT)) uint8_t state_debug = 0; -void state_dump_sensors(void) +#if 0 +static void state_dump_sensors(void) { STMCH_DEBUG("TODO\n"); } +#endif -void state_debug_wait_key_pressed(void) +static void state_debug_wait_key_pressed(void) { if (!state_debug) return; @@ -80,84 +86,131 @@ void state_debug_wait_key_pressed(void) while(!cmdline_keypressed()); } +/* return true if cob is present */ +static uint8_t state_cob_present(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + return sensor_get(S_LCOB); + else + /* XXX */ + // return sensor_get(S_LCOB); + return 0; +} + +/* return the detected color of the cob (only valid if present) */ +static uint8_t state_cob_color(uint8_t side) +{ + if (side == I2C_LEFT_SIDE) + return I2C_COB_WHITE; + else + /* XXX */ + // return sensor_get(S_LCOB); + return 0; +} + /* set a new state, return 0 on success */ -int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd) +int8_t state_set_mode(uint8_t mode) { - changed = 1; - prev_state = mainboard_command.mode; - memcpy(&mainboard_command, cmd, sizeof(mainboard_command)); - STMCH_DEBUG("%s mode=%d", __FUNCTION__, mainboard_command.mode); + state_mode = mode; + /* XXX synchrounous pack here */ + STMCH_DEBUG("%s(): l_deploy=%d l_harvest=%d " + "r_deploy=%d r_harvest=%d eject=%d", + __FUNCTION__, L_DEPLOY(mode), L_HARVEST(mode), + R_DEPLOY(mode), R_HARVEST(mode), EJECT(mode)); return 0; } /* check that state is the one in parameter and that state did not * changed */ -uint8_t state_check(uint8_t mode) +static uint8_t state_want_exit(void) { int16_t c; - if (mode != mainboard_command.mode) - return 0; - - if (changed) - return 0; /* force quit when CTRL-C is typed */ c = cmdline_getchar(); if (c == -1) - return 1; - if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) { - mainboard_command.mode = EXIT; return 0; - } - return 1; + if (vt100_parser(&local_vt100, c) == KEY_CTRL_C) + return 1; + return 0; } uint8_t state_get_mode(void) { - return mainboard_command.mode; + return state_mode; } -/* manual mode, arm position is sent from mainboard */ -static void state_do_manual(void) +/* harvest cobs from area */ +static void state_do_harvest(uint8_t side) { - if (!state_check(MANUAL)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(MANUAL)); -} + uint16_t delay; -/* init mode */ -static void state_do_init(void) -{ - if (!state_check(INIT)) + /* if there is no cob, return */ + if (state_cob_present(side)) + return; + + /* if it is black, nothing to do */ + if (state_cob_color(side) == I2C_COB_BLACK) return; - state_init(); - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(INIT)); + + /* eat the cob */ + spickle_pack(side); + state_debug_wait_key_pressed(); + delay = spickle_get_pack_delay(side); + time_wait_ms(delay); + + /* redeploy the spickle */ + spickle_deploy(side); + state_debug_wait_key_pressed(); + + cob_count ++; + + /* store it */ + shovel_up(); + wait_ms(200); + state_debug_wait_key_pressed(); + shovel_down(); + state_debug_wait_key_pressed(); } -/* harvest columns elts from area */ -static void state_do_harvest(void) +/* eject cobs */ +static void state_do_eject(void) { - if (!state_check(HARVEST)) - return; - STMCH_DEBUG("%s mode=%d", __FUNCTION__, state_get_mode()); - while (state_check(HARVEST)); + cob_count = 0; + shovel_mid(); + time_wait_ms(2000); + shovel_down(); } + /* main state machine */ void state_machine(void) { - while (state_get_mode() != EXIT) { - changed = 0; - state_do_init(); - state_do_manual(); - state_do_harvest(); + while (state_want_exit() == 0) { + + /* pack spickles */ + if (!L_DEPLOY(state_mode)) + spickle_pack(I2C_LEFT_SIDE); + if (!R_DEPLOY(state_mode)) + spickle_pack(I2C_RIGHT_SIDE); + + /* harvest */ + if (L_DEPLOY(state_mode) && L_HARVEST(state_mode)) + state_do_harvest(I2C_LEFT_SIDE); + if (R_DEPLOY(state_mode) && R_HARVEST(state_mode)) + state_do_harvest(I2C_RIGHT_SIDE); + + /* eject */ + if (EJECT(state_mode)) + state_do_eject(); } } void state_init(void) { vt100_init(&local_vt100); - mainboard_command.mode = HARVEST; - cobboard.cob_count = 0; + shovel_down(); + spickle_pack(I2C_LEFT_SIDE); + spickle_pack(I2C_RIGHT_SIDE); + state_mode = 0; + cob_count = 0; } diff --git a/projects/microb2010/cobboard/state.h b/projects/microb2010/cobboard/state.h index 889970b..f3135ac 100644 --- a/projects/microb2010/cobboard/state.h +++ b/projects/microb2010/cobboard/state.h @@ -23,7 +23,7 @@ #define _STATE_H_ /* set a new state, return 0 on success */ -int8_t state_set_mode(struct i2c_cmd_cobboard_set_mode *cmd); +int8_t state_set_mode(uint8_t mode); /* get current state */ uint8_t state_get_mode(void); diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h index 9672d6a..43eed34 100644 --- a/projects/microb2010/common/i2c_commands.h +++ b/projects/microb2010/common/i2c_commands.h @@ -36,6 +36,9 @@ #define I2C_COLOR_RED 0 #define I2C_COLOR_GREEN 1 +#define I2C_COB_BLACK 0 +#define I2C_COB_WHITE 1 + struct i2c_cmd_hdr { uint8_t cmd; }; @@ -67,18 +70,13 @@ struct i2c_cmd_generic_color { struct i2c_cmd_cobboard_set_mode { struct i2c_cmd_hdr hdr; -#define I2C_COBBOARD_MODE_INIT 0x00 -#define I2C_COBBOARD_MODE_MANUAL 0x01 -#define I2C_COBBOARD_MODE_HARVEST 0x02 -#define I2C_COBBOARD_MODE_EXIT 0xFF - uint8_t mode; - union { - struct { - } manual; - struct { - } harvest; - }; +#define I2C_COBBOARD_MODE_L_DEPLOY 0x01 /* deploy the spickle */ +#define I2C_COBBOARD_MODE_L_HARVEST 0x02 /* auto harvest withe cobs */ +#define I2C_COBBOARD_MODE_R_DEPLOY 0x04 /* deploy the spickle */ +#define I2C_COBBOARD_MODE_R_HARVEST 0x08 /* auto harvest withe cobs */ +#define I2C_COBBOARD_MODE_EJECT 0x10 /* eject cobs */ + uint8_t mode; }; /****/ @@ -106,6 +104,9 @@ struct i2c_ans_cobboard_status { uint8_t status; uint8_t cob_count; + + int16_t left_cobroller_speed; + int16_t right_cobroller_speed; }; #define I2C_REQ_BALLBOARD_STATUS 0x82 -- 2.20.1