circuit strat, first version
[aversive.git] / projects / microb2010 / mainboard / commands_mainboard.c
index 9f39f86..52b6b92 100644 (file)
@@ -1,6 +1,6 @@
 /*
  *  Copyright Droids Corporation (2009)
- * 
+ *
  *  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
  *
  *  Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
  *
- *  Olivier MATZ <zer0@droids-corp.org> 
+ *  Olivier MATZ <zer0@droids-corp.org>
  */
 
 #include <stdio.h>
 #include <string.h>
+#include <math.h>
 
 #include <hostsim.h>
 #include <aversive/pgmspace.h>
@@ -40,6 +41,8 @@
 #include <quadramp.h>
 #include <control_system_manager.h>
 #include <trajectory_manager.h>
+#include <trajectory_manager_core.h>
+#include <trajectory_manager_utils.h>
 #include <vect_base.h>
 #include <lines.h>
 #include <polygon.h>
@@ -63,6 +66,8 @@
 #include "strat.h"
 #include "strat_utils.h"
 #include "strat_base.h"
+#include "strat_db.h"
+#include "strat_corn.h"
 #include "i2c_protocol.h"
 #include "actuator.h"
 
@@ -79,29 +84,30 @@ static void cmd_event_parsed(void *parsed_result, void *data)
        u08 bit=0;
 
        struct cmd_event_result * res = parsed_result;
-       
+
        if (!strcmp_P(res->arg1, PSTR("all"))) {
-               bit = DO_ENCODERS | DO_CS | DO_RS | DO_POS |
-                       DO_BD | DO_TIMER | DO_POWER;
+               bit = 0xFF;
                if (!strcmp_P(res->arg2, PSTR("on")))
                        mainboard.flags |= bit;
                else if (!strcmp_P(res->arg2, PSTR("off")))
                        mainboard.flags &= bit;
                else { /* show */
-                       printf_P(PSTR("encoders is %s\r\n"), 
+                       printf_P(PSTR("encoders is %s\r\n"),
                                 (DO_ENCODERS & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("cs is %s\r\n"), 
+                       printf_P(PSTR("cs is %s\r\n"),
                                 (DO_CS & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("rs is %s\r\n"), 
+                       printf_P(PSTR("rs is %s\r\n"),
                                 (DO_RS & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("pos is %s\r\n"), 
+                       printf_P(PSTR("pos is %s\r\n"),
                                 (DO_POS & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("bd is %s\r\n"), 
+                       printf_P(PSTR("bd is %s\r\n"),
                                 (DO_BD & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("timer is %s\r\n"), 
+                       printf_P(PSTR("timer is %s\r\n"),
                                 (DO_TIMER & mainboard.flags) ? "on":"off");
-                       printf_P(PSTR("power is %s\r\n"), 
+                       printf_P(PSTR("power is %s\r\n"),
                                 (DO_POWER & mainboard.flags) ? "on":"off");
+                       printf_P(PSTR("errblock is %s\r\n"),
+                                (DO_ERRBLOCKING & mainboard.flags) ? "on":"off");
                }
                return;
        }
@@ -124,6 +130,8 @@ static void cmd_event_parsed(void *parsed_result, void *data)
        }
        else if (!strcmp_P(res->arg1, PSTR("power")))
                bit = DO_POWER;
+       else if (!strcmp_P(res->arg1, PSTR("errblock")))
+               bit = DO_ERRBLOCKING;
 
        if (!strcmp_P(res->arg2, PSTR("on")))
                mainboard.flags |= bit;
@@ -139,13 +147,13 @@ static void cmd_event_parsed(void *parsed_result, void *data)
                }
                mainboard.flags &= (~bit);
        }
-       printf_P(PSTR("%s is %s\r\n"), res->arg1, 
+       printf_P(PSTR("%s is %s\r\n"), res->arg1,
                      (bit & mainboard.flags) ? "on":"off");
 }
 
 prog_char str_event_arg0[] = "event";
 parse_pgm_token_string_t cmd_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg0, str_event_arg0);
-prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power";
+prog_char str_event_arg1[] = "all#encoders#cs#rs#pos#bd#timer#power#errblock";
 parse_pgm_token_string_t cmd_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg1, str_event_arg1);
 prog_char str_event_arg2[] = "on#off#show";
 parse_pgm_token_string_t cmd_event_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_event_result, arg2, str_event_arg2);
@@ -156,9 +164,9 @@ parse_pgm_inst_t cmd_event = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_event,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_event_arg0, 
-               (prog_void *)&cmd_event_arg1, 
-               (prog_void *)&cmd_event_arg2, 
+               (prog_void *)&cmd_event_arg0,
+               (prog_void *)&cmd_event_arg1,
+               (prog_void *)&cmd_event_arg2,
                NULL,
        },
 };
@@ -179,7 +187,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data)
        printf("not implemented\n");
 #else
        uint16_t i = 0, ret = 0, ret2 = 0;
-       
+
        if (mainboard.flags & DO_ENCODERS) {
                printf_P(PSTR("Disable encoder event first\r\n"));
                return;
@@ -209,7 +217,7 @@ parse_pgm_inst_t cmd_spi_test = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_spi_test,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_spi_test_arg0, 
+               (prog_void *)&cmd_spi_test_arg0,
                NULL,
        },
 };
@@ -249,8 +257,8 @@ parse_pgm_inst_t cmd_opponent = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_opponent,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_opponent_arg0, 
-               (prog_void *)&cmd_opponent_arg1, 
+               (prog_void *)&cmd_opponent_arg0,
+               (prog_void *)&cmd_opponent_arg1,
                NULL,
        },
 };
@@ -267,10 +275,10 @@ parse_pgm_inst_t cmd_opponent_set = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_opponent_set,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_opponent_arg0, 
+               (prog_void *)&cmd_opponent_arg0,
                (prog_void *)&cmd_opponent_arg1_set,
-               (prog_void *)&cmd_opponent_arg2, 
-               (prog_void *)&cmd_opponent_arg3, 
+               (prog_void *)&cmd_opponent_arg2,
+               (prog_void *)&cmd_opponent_arg3,
                NULL,
        },
 };
@@ -289,19 +297,16 @@ struct cmd_start_result {
 /* function called when cmd_start is parsed successfully */
 static void cmd_start_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_start_result *res = parsed_result;
        uint8_t old_level = gen.log_level;
 
        gen.logs[NB_LOGS] = E_USER_STRAT;
        if (!strcmp_P(res->debug, PSTR("debug"))) {
-               strat_infos.dump_enabled = 1;
+               strat_db.dump_enabled = 1;
                gen.log_level = 5;
        }
        else {
-               strat_infos.dump_enabled = 0;
+               strat_db.dump_enabled = 0;
                gen.log_level = 0;
        }
 
@@ -320,7 +325,6 @@ static void cmd_start_parsed(void *parsed_result, void *data)
 
        gen.logs[NB_LOGS] = 0;
        gen.log_level = old_level;
-#endif
 }
 
 prog_char str_start_arg0[] = "start";
@@ -336,9 +340,9 @@ parse_pgm_inst_t cmd_start = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_start,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_start_arg0, 
-               (prog_void *)&cmd_start_color, 
-               (prog_void *)&cmd_start_debug, 
+               (prog_void *)&cmd_start_arg0,
+               (prog_void *)&cmd_start_color,
+               (prog_void *)&cmd_start_debug,
                NULL,
        },
 };
@@ -356,7 +360,7 @@ struct cmd_interact_result {
 static void print_cs(void)
 {
        printf_P(PSTR("cons_d=% .8"PRIi32" cons_a=% .8"PRIi32" fil_d=% .8"PRIi32" fil_a=% .8"PRIi32" "
-                     "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"), 
+                     "err_d=% .8"PRIi32" err_a=% .8"PRIi32" out_d=% .8"PRIi32" out_a=% .8"PRIi32"\r\n"),
                 cs_get_consign(&mainboard.distance.cs),
                 cs_get_consign(&mainboard.angle.cs),
                 cs_get_filtered_consign(&mainboard.distance.cs),
@@ -369,7 +373,7 @@ static void print_cs(void)
 
 static void print_pos(void)
 {
-       printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"), 
+       printf_P(PSTR("x=% .8d y=% .8d a=% .8d\r\n"),
                 position_get_x_s16(&mainboard.pos),
                 position_get_y_s16(&mainboard.pos),
                 position_get_a_deg_s16(&mainboard.pos));
@@ -482,7 +486,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data)
                        wait_ms(10);
                        continue;
                }
-               
+
                if (cmd == -1) {
                        switch(c) {
                        case '1': print ^= PRINT_POS; break;
@@ -492,7 +496,7 @@ static void cmd_interact_parsed(void * parsed_result, void * data)
                        case '5': print ^= PRINT_TIME; break;
                        case '6': print ^= PRINT_BLOCKING; break;
 
-                       case 'q': 
+                       case 'q':
                                if (mainboard.flags & DO_CS)
                                        strat_hardstop();
                                pwm_set_and_save(LEFT_PWM, 0);
@@ -502,21 +506,21 @@ static void cmd_interact_parsed(void * parsed_result, void * data)
                                pwm_set_and_save(LEFT_PWM, 0);
                                pwm_set_and_save(RIGHT_PWM, 0);
                                break;
-                       default: 
+                       default:
                                break;
                        }
                }
                else {
                        switch(cmd) {
-                       case KEY_UP_ARR: 
+                       case KEY_UP_ARR:
                                pwm_set_and_save(LEFT_PWM, 1200);
                                pwm_set_and_save(RIGHT_PWM, 1200);
                                break;
-                       case KEY_LEFT_ARR: 
+                       case KEY_LEFT_ARR:
                                pwm_set_and_save(LEFT_PWM, -1200);
                                pwm_set_and_save(RIGHT_PWM, 1200);
                                break;
-                       case KEY_DOWN_ARR: 
+                       case KEY_DOWN_ARR:
                                pwm_set_and_save(LEFT_PWM, -1200);
                                pwm_set_and_save(RIGHT_PWM, -1200);
                                break;
@@ -539,7 +543,7 @@ parse_pgm_inst_t cmd_interact = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_interact,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_interact_arg0, 
+               (prog_void *)&cmd_interact_arg0,
                NULL,
        },
 };
@@ -586,8 +590,8 @@ parse_pgm_inst_t cmd_color = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_color,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_color_arg0, 
-               (prog_void *)&cmd_color_color, 
+               (prog_void *)&cmd_color_arg0,
+               (prog_void *)&cmd_color_color,
                NULL,
        },
 };
@@ -607,11 +611,11 @@ static void cmd_rs_parsed(void *parsed_result, void *data)
 {
        //      struct cmd_rs_result *res = parsed_result;
        do {
-               printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), 
+               printf_P(PSTR("angle cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
                         cs_get_consign(&mainboard.angle.cs),
                         cs_get_filtered_feedback(&mainboard.angle.cs),
                         cs_get_out(&mainboard.angle.cs));
-               printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "), 
+               printf_P(PSTR("distance cons=% .6"PRIi32" in=% .6"PRIi32" out=% .6"PRIi32" / "),
                         cs_get_consign(&mainboard.distance.cs),
                         cs_get_filtered_feedback(&mainboard.distance.cs),
                         cs_get_out(&mainboard.distance.cs));
@@ -632,8 +636,8 @@ parse_pgm_inst_t cmd_rs = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_rs,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_rs_arg0, 
-               (prog_void *)&cmd_rs_arg1, 
+               (prog_void *)&cmd_rs_arg0,
+               (prog_void *)&cmd_rs_arg1,
                NULL,
        },
 };
@@ -666,7 +670,7 @@ parse_pgm_inst_t cmd_i2cdebug = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_i2cdebug,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_i2cdebug_arg0, 
+               (prog_void *)&cmd_i2cdebug_arg0,
                NULL,
        },
 };
@@ -705,8 +709,8 @@ parse_pgm_inst_t cmd_cobboard_show = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobboard_show,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobboard_show_arg0, 
-               (prog_void *)&cmd_cobboard_show_arg1, 
+               (prog_void *)&cmd_cobboard_show_arg0,
+               (prog_void *)&cmd_cobboard_show_arg1,
                NULL,
        },
 };
@@ -723,16 +727,12 @@ struct cmd_cobboard_setmode1_result {
 /* function called when cmd_cobboard_setmode1 is parsed successfully */
 static void cmd_cobboard_setmode1_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_cobboard_setmode1_result *res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("init")))
-               i2c_cobboard_mode_init();
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_INIT);
        else if (!strcmp_P(res->arg1, PSTR("eject")))
-               i2c_cobboard_mode_eject();
-#endif
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_EJECT);
 }
 
 prog_char str_cobboard_setmode1_arg0[] = "cobboard";
@@ -746,8 +746,8 @@ parse_pgm_inst_t cmd_cobboard_setmode1 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobboard_setmode1,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobboard_setmode1_arg0, 
-               (prog_void *)&cmd_cobboard_setmode1_arg1, 
+               (prog_void *)&cmd_cobboard_setmode1_arg0,
+               (prog_void *)&cmd_cobboard_setmode1_arg1,
                NULL,
        },
 };
@@ -763,11 +763,8 @@ struct cmd_cobboard_setmode2_result {
 };
 
 /* function called when cmd_cobboard_setmode2 is parsed successfully */
-static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
+static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_cobboard_setmode2_result *res = parsed_result;
        uint8_t side = I2C_LEFT_SIDE;
 
@@ -776,18 +773,31 @@ static void cmd_cobboard_setmode2_parsed(void * parsed_result, void * data)
        else if (!strcmp_P(res->arg2, PSTR("right")))
                side = I2C_RIGHT_SIDE;
 
-       if (!strcmp_P(res->arg1, PSTR("deploy")))
-               i2c_cobboard_mode_deploy(side);
-       else if (!strcmp_P(res->arg1, PSTR("harvest")))
-               i2c_cobboard_mode_harvest(side);
-       else if (!strcmp_P(res->arg1, PSTR("pack")))
-               i2c_cobboard_mode_pack(side);
-#endif
+       if (!strcmp_P(res->arg1, PSTR("deploy"))) {
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               i2c_cobboard_deploy(side);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("harvest"))) {
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               i2c_cobboard_autoharvest(side);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("pack"))) {
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               i2c_cobboard_pack(side);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               i2c_cobboard_deploy_nomove(side);
+       }
+       else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+               i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+               i2c_cobboard_autoharvest_nomove(side);
+       }
 }
 
 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
-prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
 prog_char str_cobboard_setmode2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
@@ -798,9 +808,9 @@ parse_pgm_inst_t cmd_cobboard_setmode2 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobboard_setmode2,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobboard_setmode2_arg0, 
-               (prog_void *)&cmd_cobboard_setmode2_arg1, 
-               (prog_void *)&cmd_cobboard_setmode2_arg2, 
+               (prog_void *)&cmd_cobboard_setmode2_arg0,
+               (prog_void *)&cmd_cobboard_setmode2_arg1,
+               (prog_void *)&cmd_cobboard_setmode2_arg2,
                NULL,
        },
 };
@@ -818,13 +828,9 @@ struct cmd_cobboard_setmode3_result {
 /* function called when cmd_cobboard_setmode3 is parsed successfully */
 static void cmd_cobboard_setmode3_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_cobboard_setmode3_result *res = parsed_result;
        if (!strcmp_P(res->arg1, PSTR("xxx")))
                printf("faux\r\n");
-#endif
 }
 
 prog_char str_cobboard_setmode3_arg0[] = "cobboard";
@@ -839,9 +845,9 @@ parse_pgm_inst_t cmd_cobboard_setmode3 = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_cobboard_setmode3,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_cobboard_setmode3_arg0, 
-               (prog_void *)&cmd_cobboard_setmode3_arg1, 
-               (prog_void *)&cmd_cobboard_setmode3_arg2, 
+               (prog_void *)&cmd_cobboard_setmode3_arg0,
+               (prog_void *)&cmd_cobboard_setmode3_arg1,
+               (prog_void *)&cmd_cobboard_setmode3_arg2,
                NULL,
        },
 };
@@ -864,6 +870,8 @@ 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);
+       printf_P(PSTR("lcob = %d\r\n"), ballboard.lcob);
+       printf_P(PSTR("rcob = %d\r\n"), ballboard.rcob);
 #endif
 }
 
@@ -878,8 +886,8 @@ parse_pgm_inst_t cmd_ballboard_show = {
        .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, 
+               (prog_void *)&cmd_ballboard_show_arg0,
+               (prog_void *)&cmd_ballboard_show_arg1,
                NULL,
        },
 };
@@ -896,9 +904,6 @@ struct cmd_ballboard_setmode1_result {
 /* function called when cmd_ballboard_setmode1 is parsed successfully */
 static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_ballboard_setmode1_result *res = parsed_result;
 
        if (!strcmp_P(res->arg1, PSTR("init")))
@@ -911,7 +916,6 @@ static void cmd_ballboard_setmode1_parsed(void *parsed_result, void *data)
                i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
 
        /* other commands */
-#endif
 }
 
 prog_char str_ballboard_setmode1_arg0[] = "ballboard";
@@ -925,8 +929,8 @@ parse_pgm_inst_t cmd_ballboard_setmode1 = {
        .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, 
+               (prog_void *)&cmd_ballboard_setmode1_arg0,
+               (prog_void *)&cmd_ballboard_setmode1_arg1,
                NULL,
        },
 };
@@ -944,9 +948,6 @@ struct cmd_ballboard_setmode2_result {
 /* function called when cmd_ballboard_setmode2 is parsed successfully */
 static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_ballboard_setmode2_result *res = parsed_result;
        uint8_t mode = I2C_BALLBOARD_MODE_INIT;
 
@@ -963,7 +964,6 @@ static void cmd_ballboard_setmode2_parsed(void * parsed_result, void * data)
                        mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
        }
        i2c_ballboard_set_mode(mode);
-#endif
 }
 
 prog_char str_ballboard_setmode2_arg0[] = "ballboard";
@@ -979,9 +979,9 @@ parse_pgm_inst_t cmd_ballboard_setmode2 = {
        .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, 
+               (prog_void *)&cmd_ballboard_setmode2_arg0,
+               (prog_void *)&cmd_ballboard_setmode2_arg1,
+               (prog_void *)&cmd_ballboard_setmode2_arg2,
                NULL,
        },
 };
@@ -999,13 +999,9 @@ struct cmd_ballboard_setmode3_result {
 /* function called when cmd_ballboard_setmode3 is parsed successfully */
 static void cmd_ballboard_setmode3_parsed(void *parsed_result, void *data)
 {
-#ifdef HOST_VERSION
-       printf("not implemented\n");
-#else
        struct cmd_ballboard_setmode3_result *res = parsed_result;
        if (!strcmp_P(res->arg1, PSTR("xxx")))
                printf("faux\r\n");
-#endif
 }
 
 prog_char str_ballboard_setmode3_arg0[] = "ballboard";
@@ -1020,9 +1016,9 @@ parse_pgm_inst_t cmd_ballboard_setmode3 = {
        .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, 
+               (prog_void *)&cmd_ballboard_setmode3_arg0,
+               (prog_void *)&cmd_ballboard_setmode3_arg1,
+               (prog_void *)&cmd_ballboard_setmode3_arg2,
                NULL,
        },
 };
@@ -1061,8 +1057,157 @@ parse_pgm_inst_t cmd_servo_balls = {
        .data = NULL,      /* 2nd arg of func */
        .help_str = help_servo_balls,
        .tokens = {        /* token list, NULL terminated */
-               (prog_void *)&cmd_servo_balls_arg0, 
-               (prog_void *)&cmd_servo_balls_arg1, 
+               (prog_void *)&cmd_servo_balls_arg0,
+               (prog_void *)&cmd_servo_balls_arg1,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Clitoid */
+
+/* this structure is filled when cmd_clitoid is parsed successfully */
+struct cmd_clitoid_result {
+       fixed_string_t arg0;
+       float alpha_deg;
+       float beta_deg;
+       float R_mm;
+       float Vd;
+       float Amax;
+       float d_inter_mm;
+};
+
+/* function called when cmd_test is parsed successfully */
+static void cmd_clitoid_parsed(void *parsed_result, void *data)
+{
+       struct cmd_clitoid_result *res = parsed_result;
+/*     clitoid(res->alpha_deg, res->beta_deg, res->R_mm, */
+/*             res->Vd, res->Amax, res->d_inter_mm); */
+       double x = position_get_x_double(&mainboard.pos);
+       double y = position_get_y_double(&mainboard.pos);
+       double a = position_get_a_rad_double(&mainboard.pos);
+
+       strat_set_speed(res->Vd, SPEED_ANGLE_FAST);
+       trajectory_clitoid(&mainboard.traj, x, y, a, 150.,
+                          res->alpha_deg, res->beta_deg, res->R_mm,
+                          res->d_inter_mm);
+}
+
+prog_char str_clitoid_arg0[] = "clitoid";
+parse_pgm_token_string_t cmd_clitoid_arg0 =
+       TOKEN_STRING_INITIALIZER(struct cmd_clitoid_result,
+                                arg0, str_clitoid_arg0);
+parse_pgm_token_num_t cmd_clitoid_alpha_deg =
+       TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+                             alpha_deg, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_beta_deg =
+       TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+                             beta_deg, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_R_mm =
+       TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+                             R_mm, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_Vd =
+       TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+                             Vd, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_Amax =
+       TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+                             Amax, FLOAT);
+parse_pgm_token_num_t cmd_clitoid_d_inter_mm =
+       TOKEN_NUM_INITIALIZER(struct cmd_clitoid_result,
+                             d_inter_mm, FLOAT);
+
+prog_char help_clitoid[] = "do a clitoid (alpha, beta, R, Vd, Amax, d_inter)";
+parse_pgm_inst_t cmd_clitoid = {
+       .f = cmd_clitoid_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_clitoid,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_clitoid_arg0,
+               (prog_void *)&cmd_clitoid_alpha_deg,
+               (prog_void *)&cmd_clitoid_beta_deg,
+               (prog_void *)&cmd_clitoid_R_mm,
+               (prog_void *)&cmd_clitoid_Vd,
+               (prog_void *)&cmd_clitoid_Amax,
+               (prog_void *)&cmd_clitoid_d_inter_mm,
+               NULL,
+       },
+};
+
+/**********************************************************/
+/* Time_Monitor */
+
+/* this structure is filled when cmd_time_monitor is parsed successfully */
+struct cmd_time_monitor_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_time_monitor is parsed successfully */
+static void cmd_time_monitor_parsed(void *parsed_result, void *data)
+{
+#ifndef HOST_VERSION
+       struct cmd_time_monitor_result *res = parsed_result;
+       uint16_t seconds;
+
+       if (!strcmp_P(res->arg1, PSTR("reset"))) {
+               eeprom_write_word(EEPROM_TIME_ADDRESS, 0);
+       }
+       seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
+       printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
+#endif
+}
+
+prog_char str_time_monitor_arg0[] = "time_monitor";
+parse_pgm_token_string_t cmd_time_monitor_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg0, str_time_monitor_arg0);
+prog_char str_time_monitor_arg1[] = "show#reset";
+parse_pgm_token_string_t cmd_time_monitor_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_time_monitor_result, arg1, str_time_monitor_arg1);
+
+prog_char help_time_monitor[] = "Show since how long we are running";
+parse_pgm_inst_t cmd_time_monitor = {
+       .f = cmd_time_monitor_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_time_monitor,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_time_monitor_arg0,
+               (prog_void *)&cmd_time_monitor_arg1,
+               NULL,
+       },
+};
+
+
+/**********************************************************/
+/* Strat_Event */
+
+/* this structure is filled when cmd_strat_event is parsed successfully */
+struct cmd_strat_event_result {
+       fixed_string_t arg0;
+       fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_event is parsed successfully */
+static void cmd_strat_event_parsed(void *parsed_result, void *data)
+{
+       struct cmd_strat_event_result *res = parsed_result;
+
+       if (!strcmp_P(res->arg1, PSTR("on")))
+               strat_event_enable();
+       else
+               strat_event_disable();
+}
+
+prog_char str_strat_event_arg0[] = "strat_event";
+parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
+prog_char str_strat_event_arg1[] = "on#off";
+parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
+
+prog_char help_strat_event[] = "Enable/disable strat_event callback";
+parse_pgm_inst_t cmd_strat_event = {
+       .f = cmd_strat_event_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_strat_event,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_strat_event_arg0,
+               (prog_void *)&cmd_strat_event_arg1,
                NULL,
        },
 };
@@ -1074,16 +1219,30 @@ parse_pgm_inst_t cmd_servo_balls = {
 struct cmd_test_result {
        fixed_string_t arg0;
        int32_t radius;
+       int32_t dist;
 };
 
 /* function called when cmd_test is parsed successfully */
 static void cmd_test_parsed(void *parsed_result, void *data)
 {
+       strat_db.dump_enabled = 1;
+       strat_db_dump(__FUNCTION__);
+
+       corn_set_color(strat_db.corn_table[0], I2C_COB_BLACK);
+       strat_db_dump(__FUNCTION__);
+
+       corn_set_color(strat_db.corn_table[3], I2C_COB_WHITE);
+       strat_db_dump(__FUNCTION__);
+       corn_set_color(strat_db.corn_table[4], I2C_COB_WHITE);
+       strat_db_dump(__FUNCTION__);
+       corn_set_color(strat_db.corn_table[5], I2C_COB_WHITE);
+       strat_db_dump(__FUNCTION__);
 }
 
 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_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, radius, INT32);
+parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_test_result, dist, INT32);
 
 prog_char help_test[] = "Test function";
 parse_pgm_inst_t cmd_test = {
@@ -1092,7 +1251,78 @@ parse_pgm_inst_t cmd_test = {
        .help_str = help_test,
        .tokens = {        /* token list, NULL terminated */
                (prog_void *)&cmd_test_arg0,
-               (prog_void *)&cmd_test_arg1,
+               NULL,
+       },
+};
+
+
+
+
+/**********************************************************/
+/* Climb */
+
+/* this structure is filled when cmd_climb is parsed successfully */
+struct cmd_climb_result {
+       fixed_string_t arg0;
+       int32_t radius;
+       int32_t dist;
+};
+
+
+//#define DIM_BALLS_DOWN 620
+//#define DIM_BALLS_UP 700
+
+#define DIM_BALLS_DOWN 530
+#define DIM_BALLS_UP 700
+
+/* function called when cmd_climb is parsed successfully */
+static void cmd_climb_parsed(void *parsed_result, void *data)
+{
+       uint8_t err;
+
+       printf_P(PSTR("starting Clifenger\r\n"));
+       strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_SLOW );
+
+       trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
+       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+       trajectory_a_abs(&mainboard.traj, 0);
+       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+
+
+       trajectory_goto_xy_abs(&mainboard.traj, 1250, 250);
+       err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_DOWN, 0xFF);
+       printf_P(PSTR("ball_pack\r\n"));
+       support_balls_pack();
+       err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)>DIM_BALLS_UP, 0xFF);
+       printf_P(PSTR("ball_deploy\r\n"));
+       support_balls_deploy();
+       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+
+       /* reach top, go down */
+       trajectory_goto_xy_abs(&mainboard.traj, 250, 250);
+       err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_UP, 0xFF);
+       printf_P(PSTR("ball_pack\r\n"));
+       support_balls_pack();
+       err = WAIT_COND_OR_TRAJ_END(position_get_x_s16(&mainboard.pos)<DIM_BALLS_DOWN, 0xFF);
+       printf_P(PSTR("ball_deploy\r\n"));
+       support_balls_deploy();
+
+       err = wait_traj_end(TRAJ_FLAGS_SMALL_DIST);
+}
+
+prog_char str_climb_arg0[] = "climb";
+parse_pgm_token_string_t cmd_climb_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_climb_result, arg0, str_climb_arg0);
+parse_pgm_token_num_t cmd_climb_arg1 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, radius, INT32);
+parse_pgm_token_num_t cmd_climb_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_climb_result, dist, INT32);
+
+prog_char help_climb[] = "Climb function";
+parse_pgm_inst_t cmd_climb = {
+       .f = cmd_climb_parsed,  /* function to call */
+       .data = NULL,      /* 2nd arg of func */
+       .help_str = help_climb,
+       .tokens = {        /* token list, NULL terminated */
+               (prog_void *)&cmd_climb_arg0,
                NULL,
        },
 };