#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"
/* 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;
}
gen.logs[NB_LOGS] = 0;
gen.log_level = old_level;
-#endif
}
prog_char str_start_arg0[] = "start";
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";
git.droids-corp.org / aversive.git / blobdiff