+//////////////////////
+
+// 500 -- 5
+// 400 -- 3
+#define TEST_SPEED 400
+#define TEST_ACC 3
+
+static void line2line(double line1x1, double line1y1,
+ double line1x2, double line1y2,
+ double line2x1, double line2y1,
+ double line2x2, double line2y2,
+ double radius, double dist)
+{
+ uint8_t err;
+ double speed_d, speed_a;
+ double distance, angle;
+ double line1_angle = atan2(line1y2-line1y1, line1x2-line1x1);
+ double line2_angle = atan2(line2y2-line2y1, line2x2-line2x1);
+
+ printf_P(PSTR("%s()\r\n"), __FUNCTION__);
+
+ strat_set_speed(TEST_SPEED, TEST_SPEED);
+ quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
+
+ circle_get_da_speed_from_radius(&mainboard.traj, radius,
+ &speed_d, &speed_a);
+ trajectory_line_abs(&mainboard.traj,
+ line1x1, line1y1,
+ line1x2, line1y2, 150.);
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
+ dist, TRAJ_FLAGS_NO_NEAR);
+ /* circle */
+ strat_set_speed(speed_d, speed_a);
+ angle = line2_angle - line1_angle;
+ distance = angle * radius;
+ if (distance < 0)
+ distance = -distance;
+ angle = simple_modulo_2pi(angle);
+ angle = DEG(angle);
+ printf_P(PSTR("(%d,%d,%d) "),
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+ printf_P(PSTR("circle distance=%2.2f angle=%2.2f\r\n"),
+ distance, angle);
+
+ /* take some margin on dist to avoid deceleration */
+ trajectory_d_a_rel(&mainboard.traj, distance + 250, angle);
+
+ /* circle exit condition */
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+ TRAJ_FLAGS_NO_NEAR);
+
+ strat_set_speed(500, 500);
+ printf_P(PSTR("(%d,%d,%d) "),
+ position_get_x_s16(&mainboard.pos),
+ position_get_y_s16(&mainboard.pos),
+ position_get_a_deg_s16(&mainboard.pos));
+ printf_P(PSTR("line\r\n"));
+ trajectory_line_abs(&mainboard.traj,
+ line2x1, line2y1,
+ line2x2, line2y2, 150.);
+}
+
+static void halfturn(double line1x1, double line1y1,
+ double line1x2, double line1y2,
+ double line2x1, double line2y1,
+ double line2x2, double line2y2,
+ double radius, double dist, double dir)
+{
+ uint8_t err;
+ double speed_d, speed_a;
+ double distance, angle;
+
+ printf_P(PSTR("%s()\r\n"), __FUNCTION__);
+
+ strat_set_speed(TEST_SPEED, TEST_SPEED);
+ quadramp_set_2nd_order_vars(&mainboard.angle.qr, TEST_ACC, TEST_ACC);
+
+ circle_get_da_speed_from_radius(&mainboard.traj, radius,
+ &speed_d, &speed_a);
+ trajectory_line_abs(&mainboard.traj,
+ line1x1, line1y1,
+ line1x2, line1y2, 150.);
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line1x2, line1y2) <
+ dist, TRAJ_FLAGS_NO_NEAR);
+ /* circle */
+ strat_set_speed(speed_d, speed_a);
+ angle = dir * M_PI/2.;
+ distance = angle * radius;
+ if (distance < 0)
+ distance = -distance;
+ angle = simple_modulo_2pi(angle);
+ angle = DEG(angle);
+
+ /* take some margin on dist to avoid deceleration */
+ DEBUG(E_USER_STRAT, "circle1 distance=%2.2f angle=%2.2f",
+ distance, angle);
+ trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
+
+ /* circle exit condition */
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+ TRAJ_FLAGS_NO_NEAR);
+
+ DEBUG(E_USER_STRAT, "miniline");
+ err = WAIT_COND_OR_TRAJ_END(distance_from_robot(line2x1, line2y1) <
+ dist, TRAJ_FLAGS_NO_NEAR);
+ DEBUG(E_USER_STRAT, "circle2");
+ /* take some margin on dist to avoid deceleration */
+ trajectory_d_a_rel(&mainboard.traj, distance + 500, angle);
+
+ err = WAIT_COND_OR_TRAJ_END(trajectory_angle_finished(&mainboard.traj),
+ TRAJ_FLAGS_NO_NEAR);
+
+ strat_set_speed(500, 500);
+ DEBUG(E_USER_STRAT, "line");
+ trajectory_line_abs(&mainboard.traj,
+ line2x1, line2y1,
+ line2x2, line2y2, 150.);
+}
+
+
+/* function called when cmd_test is parsed successfully */
+static void subtraj_test(void)
+{
+#ifdef HOST_VERSION
+ strat_reset_pos(400, 400, 90);
+ mainboard.angle.on = 1;
+ mainboard.distance.on = 1;
+#endif
+ printf_P(PSTR("%s()\r\n"), __FUNCTION__);
+ while (!cmdline_keypressed()) {
+ /****** PASS1 */
+
+#define DIST_HARD_TURN 260
+#define RADIUS_HARD_TURN 100
+#define DIST_EASY_TURN 190
+#define RADIUS_EASY_TURN 190
+#define DIST_HALF_TURN 225
+#define RADIUS_HALF_TURN 130
+
+ /* hard turn */
+ line2line(375, 597, 375, 1847,
+ 375, 1847, 1050, 1472,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /* easy left and easy right !*/
+ line2line(825, 1596, 1050, 1472,
+ 1050, 1472, 1500, 1722,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+ line2line(1050, 1472, 1500, 1722,
+ 1500, 1722, 2175, 1347,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+ line2line(1500, 1722, 2175, 1347,
+ 2175, 1347, 2175, 847,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+ /* half turns */
+ halfturn(2175, 1347, 2175, 722,
+ 2625, 722, 2625, 1597,
+ RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
+ halfturn(2625, 847, 2625, 1722,
+ 2175, 1722, 2175, 1097,
+ RADIUS_HALF_TURN, DIST_HALF_TURN, 1.);
+
+ /* easy turns */
+ line2line(2175, 1597, 2175, 1097,
+ 2175, 1097, 1500, 722,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+ line2line(2175, 1097, 1500, 722,
+ 1500, 722, 1050, 972,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+ line2line(1500, 722, 1050, 972,
+ 1050, 972, 375, 597,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+ /* hard turn */
+ line2line(1050, 972, 375, 597,
+ 375, 597, 375, 1097,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /****** PASS2 */
+
+ /* easy turn */
+ line2line(375, 597, 375, 1097,
+ 375, 1097, 1050, 1472,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+ /* hard turn */
+ line2line(375, 1097, 1050, 1472,
+ 1050, 1472, 375, 1847,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /* hard turn */
+ line2line(1050, 1472, 375, 1847,
+ 375, 1847, 375, 1347,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /* easy turn */
+ line2line(375, 1847, 375, 1347,
+ 375, 1347, 1050, 972,
+ RADIUS_EASY_TURN, DIST_EASY_TURN);
+
+ /* hard turn */
+ line2line(375, 1347, 1050, 972,
+ 1050, 972, 375, 597,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ /* hard turn */
+ line2line(1050, 972, 375, 597,
+ 375, 597, 375, 1847,
+ RADIUS_HARD_TURN, DIST_HARD_TURN);
+
+ }
+ trajectory_hardstop(&mainboard.traj);
+}
+
+
git.droids-corp.org / aversive.git / commitdiff