add ballboard commands on mainboard

diff --git a/projects/microb2010/mainboard/actuator.c b/projects/microb2010/mainboard/actuator.c
index 24ad07f2f3e8926edfe5072f3bedec79370f4777..1d134bbfadff31e4ee413021e86ba7870b339944 100644 (file)
--- a/projects/microb2010/mainboard/actuator.c
+++ b/projects/microb2010/mainboard/actuator.c
@@ -68,12 +68,12 @@ void pwm_set_and_save(void *pwm, int32_t val)
 void support_balls_deploy(void)
 {
        pwm_ng_set(SUPPORT_BALLS_R_SERVO, 510);
-       pwm_ng_set(SUPPORT_BALLS_L_SERVO, 240);
+       pwm_ng_set(SUPPORT_BALLS_L_SERVO, 205);
 }
 
 void support_balls_pack(void)
 {
-       pwm_ng_set(SUPPORT_BALLS_R_SERVO, 200);
+       pwm_ng_set(SUPPORT_BALLS_R_SERVO, 250);
        pwm_ng_set(SUPPORT_BALLS_L_SERVO, 480);
 }
 

diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index 29ecd3d7774eee3b83e9db52f961825be90a0f4e..fcadd07ff5bfb28fbfa2e4abe4ac9027a87ea0eb 100644 (file)
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -662,6 +662,7 @@ static void cmd_cobboard_show_parsed(void * parsed_result, void * data)
 {
        printf_P(PSTR("mode = %x\r\n"), cobboard.mode);
        printf_P(PSTR("status = %x\r\n"), cobboard.status);
+       printf_P(PSTR("cob_count = %x\r\n"), cobboard.cob_count);
        printf_P(PSTR("left_cobroller_speed = %d\r\n"), cobboard.left_cobroller_speed);
        printf_P(PSTR("right_cobroller_speed = %d\r\n"), cobboard.right_cobroller_speed);
 }
@@ -806,6 +807,171 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = {
        },
 };
 
+/**********************************************************/
+/* Ballboard_Show */
+
+/* this structure is filled when cmd_ballboard_show is parsed successfully */
+struct cmd_ballboard_show_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_ballboard_show is parsed successfully */
+static void cmd_ballboard_show_parsed(void * parsed_result, void * data)
+{
+       printf_P(PSTR("mode = %x\r\n"), ballboard.mode);
+       printf_P(PSTR("status = %x\r\n"), ballboard.status);
+       printf_P(PSTR("ball_count = %d\r\n"), ballboard.ball_count);
+}
+
+prog_char str_ballboard_show_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_show_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg0, str_ballboard_show_arg0);
+prog_char str_ballboard_show_arg1[] = "show";
+parse_pgm_token_string_t cmd_ballboard_show_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_show_result, arg1, str_ballboard_show_arg1);
+
+prog_char help_ballboard_show[] = "show ballboard status";
+parse_pgm_inst_t cmd_ballboard_show = {
+       .f = cmd_ballboard_show_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_ballboard_show,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_ballboard_show_arg0, 
+               (prog_void *)&cmd_ballboard_show_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode1 */
+
+/* this structure is filled when cmd_ballboard_setmode1 is parsed successfully */
+struct cmd_ballboard_setmode1_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_ballboard_setmode1 is parsed successfully */
+static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
+{
+       struct cmd_ballboard_setmode1_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("init")))
+               i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_INIT);
+       else if (!strcmp_P(res->arg1, PSTR("off")))
+               i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_OFF);
+       else if (!strcmp_P(res->arg1, PSTR("eject")))
+               i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_EJECT);
+       else if (!strcmp_P(res->arg1, PSTR("harvest")))
+               i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
+
+       /* other commands */
+}
+
+prog_char str_ballboard_setmode1_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_setmode1_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg0, str_ballboard_setmode1_arg0);
+prog_char str_ballboard_setmode1_arg1[] = "init#eject#harvest#off";
+parse_pgm_token_string_t cmd_ballboard_setmode1_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode1_result, arg1, str_ballboard_setmode1_arg1);
+
+prog_char help_ballboard_setmode1[] = "set ballboard mode (mode)";
+parse_pgm_inst_t cmd_ballboard_setmode1 = {
+       .f = cmd_ballboard_setmode1_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_ballboard_setmode1,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_ballboard_setmode1_arg0, 
+               (prog_void *)&cmd_ballboard_setmode1_arg1, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode2 */
+
+/* this structure is filled when cmd_ballboard_setmode2 is parsed successfully */
+struct cmd_ballboard_setmode2_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       fixed_string_t arg2;
+};
+
+/* function called when cmd_ballboard_setmode2 is parsed successfully */
+static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
+{
+       struct cmd_ballboard_setmode2_result *res = parsed_result;
+       uint8_t mode = I2C_BALLBOARD_MODE_INIT;
+
+       if (!strcmp_P(res->arg2, PSTR("left"))) {
+               if (!strcmp_P(res->arg1, PSTR("prepare")))
+                       mode = I2C_BALLBOARD_MODE_PREP_L_FORK;
+               else if (!strcmp_P(res->arg1, PSTR("take")))
+                       mode = I2C_BALLBOARD_MODE_TAKE_L_FORK;
+       }
+       else {
+               if (!strcmp_P(res->arg1, PSTR("prepare")))
+                       mode = I2C_BALLBOARD_MODE_PREP_R_FORK;
+               else if (!strcmp_P(res->arg1, PSTR("take")))
+                       mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
+       }
+       i2c_ballboard_set_mode(mode);
+}
+
+prog_char str_ballboard_setmode2_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg0, str_ballboard_setmode2_arg0);
+prog_char str_ballboard_setmode2_arg1[] = "prepare#take";
+parse_pgm_token_string_t cmd_ballboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg1, str_ballboard_setmode2_arg1);
+prog_char str_ballboard_setmode2_arg2[] = "left#right";
+parse_pgm_token_string_t cmd_ballboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode2_result, arg2, str_ballboard_setmode2_arg2);
+
+prog_char help_ballboard_setmode2[] = "set ballboard mode (mode, side)";
+parse_pgm_inst_t cmd_ballboard_setmode2 = {
+       .f = cmd_ballboard_setmode2_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_ballboard_setmode2,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_ballboard_setmode2_arg0, 
+               (prog_void *)&cmd_ballboard_setmode2_arg1, 
+               (prog_void *)&cmd_ballboard_setmode2_arg2, 
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Ballboard_Setmode3 */
+
+/* this structure is filled when cmd_ballboard_setmode3 is parsed successfully */
+struct cmd_ballboard_setmode3_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+       uint8_t level;
+};
+
+/* function called when cmd_ballboard_setmode3 is parsed successfully */
+static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
+{
+       struct cmd_ballboard_setmode3_result *res = parsed_result;
+       if (!strcmp_P(res->arg1, PSTR("xxx")))
+               printf("faux\r\n");
+}
+
+prog_char str_ballboard_setmode3_arg0[] = "ballboard";
+parse_pgm_token_string_t cmd_ballboard_setmode3_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg0, str_ballboard_setmode3_arg0);
+prog_char str_ballboard_setmode3_arg1[] = "xxx";
+parse_pgm_token_string_t cmd_ballboard_setmode3_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_ballboard_setmode3_result, arg1, str_ballboard_setmode3_arg1);
+parse_pgm_token_num_t cmd_ballboard_setmode3_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_ballboard_setmode3_result, level, UINT8);
+
+prog_char help_ballboard_setmode3[] = "set ballboard mode (mode, level)";
+parse_pgm_inst_t cmd_ballboard_setmode3 = {
+       .f = cmd_ballboard_setmode3_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_ballboard_setmode3,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_ballboard_setmode3_arg0, 
+               (prog_void *)&cmd_ballboard_setmode3_arg1, 
+               (prog_void *)&cmd_ballboard_setmode3_arg2, 
+               NULL,
+       },
+};
+
 /**********************************************************/
 /* Servo_Balls */
 

diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index d0726cb8ee9a8ea4bb5ebc6886f8de80dc6a3e12..403decd0cc63d56f77bb8730ade7adcb7e24cecb 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -270,14 +270,16 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
 
                break;
        }
-               
+
        case I2C_ANS_BALLBOARD_STATUS: {
                struct i2c_ans_ballboard_status * ans = 
                        (struct i2c_ans_ballboard_status *)buf;
-               
+
                if (size != sizeof (*ans))
                        goto error;
+               ballboard.mode = ans->mode;
                ballboard.status = ans->status;
+               ballboard.ball_count = ans->ball_count;
                break;
        }
 
@@ -296,13 +298,11 @@ void i2c_recvevent(uint8_t * buf, int8_t size)
                i2c_errors = 0;
        }
 }
-       
+
 void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
 {
 }
 
-
-
 /* ******** ******** ******** ******** */
 /* commands */
 /* ******** ******** ******** ******** */
@@ -438,3 +438,11 @@ int8_t i2c_cobboard_mode_init(void)
        return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 }
 
+int8_t i2c_ballboard_set_mode(uint8_t mode)
+{
+       struct i2c_cmd_ballboard_set_mode buf;
+       buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE;
+       buf.mode = mode;
+       return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
+}
+

diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h
index d788ed737e70e591709746daaed3512806f6517d..7ae7283c7fb2ae0374232f3d0cf41d6593101b47 100644 (file)
--- a/projects/microb2010/mainboard/i2c_protocol.h
+++ b/projects/microb2010/mainboard/i2c_protocol.h
@@ -41,4 +41,6 @@ int8_t i2c_cobboard_mode_harvest(uint8_t side);
 int8_t i2c_cobboard_mode_deploy(uint8_t side);
 int8_t i2c_cobboard_mode_pack(uint8_t side);
 
+int8_t i2c_ballboard_set_mode(uint8_t mode);
+
 #endif

diff --git a/projects/microb2010/mainboard/main.h b/projects/microb2010/mainboard/main.h
index 2fdd212c2551b74e165f534c51d8503518be411d..c248644f85e356ba22dff3c2f7e20fa6c7833ebc 100755 (executable)
--- a/projects/microb2010/mainboard/main.h
+++ b/projects/microb2010/mainboard/main.h
@@ -115,7 +115,7 @@ struct genboard {
        struct pwm_ng servo2;
        struct pwm_ng servo3;
        struct pwm_ng servo4;
-       
+
        /* ax12 interface */
        AX12 ax12;
 
@@ -165,7 +165,7 @@ struct mainboard {
 
 /* state of cobboard, synchronized through i2c */
 struct cobboard {
-       uint8_t mode;   
+       uint8_t mode;
        uint8_t status;
        int16_t left_cobroller_speed;
        int16_t right_cobroller_speed;
@@ -174,7 +174,9 @@ struct cobboard {
 
 /* state of ballboard, synchronized through i2c */
 struct ballboard {
+       uint8_t mode;
        uint8_t status;
+       uint8_t ball_count;
 };
 
 extern struct genboard gen;