X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=blobdiff_plain;f=projects%2Fmicrob2010%2Fmainboard%2Fcommands_mainboard.c;h=8a79c35f3f95b589e4d39ff998e8d3090146523f;hp=fcadd07ff5bfb28fbfa2e4abe4ac9027a87ea0eb;hb=aed049ab6be4e1916457743d53f6b610a21b4854;hpb=09e0cfb842943982e9fa3c4792c097bf4be25360 diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c index fcadd07..8a79c35 100644 --- a/projects/microb2010/mainboard/commands_mainboard.c +++ b/projects/microb2010/mainboard/commands_mainboard.c @@ -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 @@ -17,12 +17,14 @@ * * Revision : $Id: commands_mainboard.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $ * - * Olivier MATZ + * Olivier MATZ */ #include #include +#include +#include #include #include #include @@ -39,6 +41,8 @@ #include #include #include +#include +#include #include #include #include @@ -56,6 +60,7 @@ #include "../common/eeprom_mapping.h" #include "main.h" +#include "robotsim.h" #include "sensor.h" #include "cmdline.h" #include "strat.h" @@ -77,29 +82,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; } @@ -122,23 +128,30 @@ 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; else if (!strcmp_P(res->arg2, PSTR("off"))) { if (!strcmp_P(res->arg1, PSTR("cs"))) { +#ifdef HOST_VERSION + robotsim_pwm(LEFT_PWM, 0); + robotsim_pwm(RIGHT_PWM, 0); +#else pwm_ng_set(LEFT_PWM, 0); pwm_ng_set(RIGHT_PWM, 0); +#endif } 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); @@ -149,9 +162,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, }, }; @@ -168,8 +181,11 @@ struct cmd_spi_test_result { /* function called when cmd_spi_test is parsed successfully */ static void cmd_spi_test_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + 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; @@ -187,6 +203,7 @@ static void cmd_spi_test_parsed(void * parsed_result, void * data) i++; } while(!cmdline_keypressed()); +#endif } prog_char str_spi_test_arg0[] = "spi_test"; @@ -198,7 +215,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, }, }; @@ -238,8 +255,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, }, }; @@ -256,10 +273,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, }, }; @@ -278,6 +295,9 @@ 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; @@ -306,6 +326,7 @@ 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"; @@ -321,9 +342,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, }, }; @@ -340,8 +361,8 @@ struct cmd_interact_result { static void print_cs(void) { - printf_P(PSTR("cons_d=% .8ld cons_a=% .8ld fil_d=% .8ld fil_a=% .8ld " - "err_d=% .8ld err_a=% .8ld out_d=% .8ld out_a=% .8ld\r\n"), + 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"), cs_get_consign(&mainboard.distance.cs), cs_get_consign(&mainboard.angle.cs), cs_get_filtered_consign(&mainboard.distance.cs), @@ -354,7 +375,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)); @@ -385,8 +406,8 @@ static void print_sensors(void) static void print_pid(void) { - printf_P(PSTR("P=% .8ld I=% .8ld D=% .8ld out=% .8ld | " - "P=% .8ld I=% .8ld D=% .8ld out=% .8ld\r\n"), + printf_P(PSTR("P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32" | " + "P=% .8"PRIi32" I=% .8"PRIi32" D=% .8"PRIi32" out=% .8"PRIi32"\r\n"), pid_get_value_in(&mainboard.distance.pid) * pid_get_gain_P(&mainboard.distance.pid), pid_get_value_I(&mainboard.distance.pid) * pid_get_gain_I(&mainboard.distance.pid), pid_get_value_D(&mainboard.distance.pid) * pid_get_gain_D(&mainboard.distance.pid), @@ -467,7 +488,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; @@ -477,7 +498,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); @@ -487,21 +508,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; @@ -524,7 +545,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, }, }; @@ -542,6 +563,9 @@ struct cmd_color_result { /* function called when cmd_color is parsed successfully */ static void cmd_color_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else struct cmd_color_result *res = (struct cmd_color_result *) parsed_result; if (!strcmp_P(res->color, PSTR("yellow"))) { mainboard.our_color = I2C_COLOR_YELLOW; @@ -554,6 +578,7 @@ static void cmd_color_parsed(void *parsed_result, void *data) i2c_set_color(I2C_BALLBOARD_ADDR, I2C_COLOR_BLUE); } printf_P(PSTR("Done\r\n")); +#endif } prog_char str_color_arg0[] = "color"; @@ -567,8 +592,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, }, }; @@ -588,15 +613,15 @@ static void cmd_rs_parsed(void *parsed_result, void *data) { // struct cmd_rs_result *res = parsed_result; do { - printf_P(PSTR("angle cons=% .6ld in=% .6ld out=% .6ld / "), + 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=% .6ld in=% .6ld out=% .6ld / "), + 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)); - printf_P(PSTR("l=% .4ld r=% .4ld\r\n"), mainboard.pwm_l, + printf_P(PSTR("l=% .4"PRIi32" r=% .4"PRIi32"\r\n"), mainboard.pwm_l, mainboard.pwm_r); wait_ms(100); } while(!cmdline_keypressed()); @@ -613,8 +638,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, }, }; @@ -630,8 +655,12 @@ struct cmd_i2cdebug_result { /* function called when cmd_i2cdebug is parsed successfully */ static void cmd_i2cdebug_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else i2c_debug(); i2c_protocol_debug(); +#endif } prog_char str_i2cdebug_arg0[] = "i2cdebug"; @@ -643,7 +672,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, }, }; @@ -660,11 +689,15 @@ struct cmd_cobboard_show_result { /* function called when cmd_cobboard_show is parsed successfully */ static void cmd_cobboard_show_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else 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); +#endif } prog_char str_cobboard_show_arg0[] = "cobboard"; @@ -678,8 +711,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, }, }; @@ -715,8 +748,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,9 +796,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, }, }; @@ -800,9 +833,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, }, }; @@ -819,9 +852,13 @@ struct cmd_ballboard_show_result { /* function called when cmd_ballboard_show is parsed successfully */ static void cmd_ballboard_show_parsed(void * parsed_result, void * data) { +#ifdef HOST_VERSION + printf("not implemented\n"); +#else 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); +#endif } prog_char str_ballboard_show_arg0[] = "ballboard"; @@ -835,8 +872,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, }, }; @@ -878,8 +915,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, }, }; @@ -928,9 +965,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, }, }; @@ -965,9 +1002,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, }, }; @@ -1006,8 +1043,78 @@ 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, }, }; @@ -1019,16 +1126,162 @@ parse_pgm_inst_t cmd_servo_balls = { struct cmd_test_result { fixed_string_t arg0; int32_t radius; + int32_t dist; +}; + +#define LINE_UP 0 +#define LINE_DOWN 1 +#define LINE_R_UP 2 +#define LINE_L_DOWN 3 +#define LINE_L_UP 4 +#define LINE_R_DOWN 5 + +struct line_2pts { + point_t p1; + point_t p2; }; +static void num2line(struct line_2pts *l, uint8_t dir, uint8_t num) +{ + float n = num; + + switch (dir) { + + case LINE_UP: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(0); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(2100); + break; + case LINE_DOWN: + l->p1.x = n * 450 + 375; + l->p1.y = COLOR_Y(2100); + l->p2.x = n * 450 + 375; + l->p2.y = COLOR_Y(0); + break; + case LINE_R_UP: + l->p1.x = 150; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 2850; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_L_DOWN: + l->p1.x = 2850; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 150; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + case LINE_L_UP: + l->p1.x = 2850; + l->p1.y = COLOR_Y(-n * 500 + 1472); + l->p2.x = 150; + l->p2.y = COLOR_Y((-n + 4) * 500 + 972); + break; + case LINE_R_DOWN: + l->p1.x = 150; + l->p1.y = COLOR_Y((-n + 4) * 500 + 972); + l->p2.x = 2850; + l->p2.y = COLOR_Y(-n * 500 + 1472); + break; + default: + break; + } +} + +#if 0 +static void reverse_line(struct line_2pts *l) +{ + point_t tmp; + + tmp.x = l->p1.x; + tmp.y = l->p1.y; + l->p1.x = l->p2.x; + l->p1.y = l->p2.y; + l->p2.x = tmp.x; + l->p2.y = tmp.y; +} +#endif + +static void line2line(uint8_t dir1, uint8_t num1, + uint8_t dir2, uint8_t num2) +{ + double line1_a_rad, line1_a_deg, line2_a_rad; + double diff_a_deg, diff_a_deg_abs, beta_deg; + double radius; + struct line_2pts l1, l2; + line_t ll1, ll2; + point_t p; + + /* convert to 2 points */ + num2line(&l1, dir1, num1); + num2line(&l2, dir2, num2); + + printf_P(PSTR("A2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), + l1.p1.x, l1.p1.y, l1.p2.x, l1.p2.y); + printf_P(PSTR("B2 (%2.2f, %2.2f) -> (%2.2f, %2.2f)\r\n"), + l2.p1.x, l2.p1.y, l2.p2.x, l2.p2.y); + + /* convert to line eq and find intersection */ + pts2line(&l1.p1, &l1.p2, &ll1); + pts2line(&l2.p1, &l2.p2, &ll2); + intersect_line(&ll1, &ll2, &p); + + line1_a_rad = atan2(l1.p2.y - l1.p1.y, + l1.p2.x - l1.p1.x); + line1_a_deg = DEG(line1_a_rad); + line2_a_rad = atan2(l2.p2.y - l2.p1.y, + l2.p2.x - l2.p1.x); + diff_a_deg = DEG(line2_a_rad - line1_a_rad); + diff_a_deg_abs = fabs(diff_a_deg); + + if (diff_a_deg_abs < 70.) { + radius = 200; + if (diff_a_deg > 0) + beta_deg = 40; + else + beta_deg = -40; + } + else if (diff_a_deg_abs < 100.) { + radius = 100; + if (diff_a_deg > 0) + beta_deg = 40; + else + beta_deg = -40; + } + else { + radius = 120; + if (diff_a_deg > 0) + beta_deg = 60; + else + beta_deg = -60; + } + trajectory_clitoid(&mainboard.traj, l1.p1.x, l1.p1.y, + line1_a_deg, 150., diff_a_deg, beta_deg, + radius, xy_norm(l1.p1.x, l1.p1.y, + p.x, p.y)); + wait_traj_end(0xFF); +} + /* function called when cmd_test is parsed successfully */ static void cmd_test_parsed(void *parsed_result, void *data) { +#ifdef HOST_VERSION + strat_reset_pos(298.48, 309.21, 70.02); + mainboard.angle.on = 1; + mainboard.distance.on = 1; +#endif + time_wait_ms(100); + + line2line(LINE_UP, 0, LINE_R_DOWN, 2); + line2line(LINE_R_DOWN, 2, LINE_R_UP, 1); + + trajectory_hardstop(&mainboard.traj); } 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 = { @@ -1037,7 +1290,6 @@ 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, }, };