/*
* 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>
#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>
#include "strat.h"
#include "strat_utils.h"
#include "strat_base.h"
+#include "strat_corn.h"
#include "i2c_protocol.h"
#include "actuator.h"
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;
}
}
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;
}
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);
.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,
},
};
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;
.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,
},
};
.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,
},
};
.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,
},
};
.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,
},
};
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),
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));
wait_ms(10);
continue;
}
-
+
if (cmd == -1) {
switch(c) {
case '1': print ^= PRINT_POS; break;
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);
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;
.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,
},
};
.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,
},
};
{
// 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));
.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,
},
};
.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,
},
};
.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,
},
};
/* 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();
else if (!strcmp_P(res->arg1, PSTR("eject")))
i2c_cobboard_mode_eject();
-#endif
}
prog_char str_cobboard_setmode1_arg0[] = "cobboard";
.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,
},
};
/* function called when cmd_cobboard_setmode2 is parsed successfully */
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;
i2c_cobboard_mode_harvest(side);
else if (!strcmp_P(res->arg1, PSTR("pack")))
i2c_cobboard_mode_pack(side);
-#endif
}
prog_char str_cobboard_setmode2_arg0[] = "cobboard";
.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,
},
};
/* 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";
.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,
},
};
.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,
},
};
/* 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")))
i2c_ballboard_set_mode(I2C_BALLBOARD_MODE_HARVEST);
/* other commands */
-#endif
}
prog_char str_ballboard_setmode1_arg0[] = "ballboard";
.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,
},
};
/* 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;
mode = I2C_BALLBOARD_MODE_TAKE_R_FORK;
}
i2c_ballboard_set_mode(mode);
-#endif
}
prog_char str_ballboard_setmode2_arg0[] = "ballboard";
.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,
},
};
/* 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";
.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,
},
};
.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,
},
};
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
+
+/* return 1 if there is a corn near, and fill the index ptr */
+static uint8_t corn_is_near(int8_t *corn_idx, uint8_t side)
+{
+#define SENSOR_CORN_DIST 225
+#define SENSOR_CORN_ANGLE 90
+ double x = position_get_x_double(&mainboard.pos);
+ double y = position_get_y_double(&mainboard.pos);
+ double a_rad = position_get_a_rad_double(&mainboard.pos);
+ double x_corn, y_corn;
+ int16_t x_corn_int, y_corn_int;
+
+ if (side == I2C_LEFT_SIDE) {
+ x_corn = x + cos(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+ y_corn = y + sin(a_rad + RAD(SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+ }
+ else {
+ x_corn = x + cos(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+ y_corn = y + sin(a_rad + RAD(-SENSOR_CORN_ANGLE)) * SENSOR_CORN_DIST;
+ }
+ x_corn_int = x_corn;
+ y_corn_int = y_corn;
+
+ *corn_idx = xycoord_to_corn_idx(&x_corn_int, &y_corn_int);
+ if (*corn_idx < 0)
+ return 0;
+ return 1;
+}
+
+/*
+ * - send the correct commands to the spickles
+ * - return 1 if we need to stop (cobboard is stucked)
+*/
+static uint8_t handle_spickles(void)
+{
+ int8_t corn_idx;
+
+ if (!corn_is_near(&corn_idx, I2C_LEFT_SIDE))
+ i2c_cobboard_mode_deploy(I2C_LEFT_SIDE);
+ else {
+ if (corn_table[corn_idx] == TYPE_WHITE_CORN)
+ i2c_cobboard_mode_harvest(I2C_LEFT_SIDE);
+ else
+ i2c_cobboard_mode_pack(I2C_LEFT_SIDE);
+ }
+/* printf("%d %d\n", corn_idx, corn_table[corn_idx]); */
+/* time_wait_ms(100); */
+
+ if (!corn_is_near(&corn_idx, I2C_RIGHT_SIDE))
+ i2c_cobboard_mode_deploy(I2C_RIGHT_SIDE);
+ else {
+ if (corn_table[corn_idx] == TYPE_WHITE_CORN)
+ i2c_cobboard_mode_harvest(I2C_RIGHT_SIDE);
+ else
+ i2c_cobboard_mode_pack(I2C_RIGHT_SIDE);
+ }
+
+ return 0;
+}
+
+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;
+ uint8_t err;
+
+ /* 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));
+ err = 0;
+ while (err == 0) {
+ err = WAIT_COND_OR_TRAJ_END(handle_spickles(), 0xFF);
+ if (err == 0) {
+ /* cobboard is stucked */
+ trajectory_hardstop(&mainboard.traj);
+ return; /* XXX do something */
+ }
+ err = test_traj_end(0xFF);
+ }
+ return;
+}
+
/* 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;
+ strat_set_speed(250, SPEED_ANGLE_FAST);
+#endif
+ init_corn_table(0, 0);
+ time_wait_ms(100);
+
+ line2line(LINE_UP, 0, LINE_R_DOWN, 2);
+ line2line(LINE_R_DOWN, 2, LINE_R_UP, 2);
+ line2line(LINE_R_UP, 2, LINE_UP, 5);
+
+ 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 = {
.help_str = help_test,
.tokens = { /* token list, NULL terminated */
(prog_void *)&cmd_test_arg0,
- (prog_void *)&cmd_test_arg1,
NULL,
},
};
git.droids-corp.org / aversive.git / blobdiff