--- /dev/null
+/*
+ * Copyright Droids, Microb Technology (2008)
+ *
+ * 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
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: strat_static_columns.c,v 1.5 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <ax12.h>
+#include <uart.h>
+#include <pwm_ng.h>
+#include <time.h>
+#include <spi.h>
+
+#include <pid.h>
+#include <quadramp.h>
+#include <control_system_manager.h>
+#include <trajectory_manager.h>
+#include <vect_base.h>
+#include <lines.h>
+#include <polygon.h>
+#include <obstacle_avoidance.h>
+#include <blocking_detection_manager.h>
+#include <robot_system.h>
+#include <position_manager.h>
+
+#include <rdline.h>
+#include <parse.h>
+
+#include "../common/i2c_commands.h"
+#include "main.h"
+#include "strat.h"
+#include "strat_base.h"
+#include "strat_utils.h"
+#include "strat_avoid.h"
+#include "sensor.h"
+#include "i2c_protocol.h"
+
+#define ERROUT(e) do { \
+ err = e; \
+ goto end; \
+ } while(0)
+
+#define BIG_DIST 5000
+
+/*
+ * must be called from start area.
+ * get 4 static columns and build a temple on the disc
+ */
+uint8_t strat_static_columns(uint8_t configuration)
+{
+ uint8_t err;
+ uint8_t col1_present = 0, col4_present = 0;
+ uint16_t old_spdd, old_spda;
+
+ DEBUG(E_USER_STRAT, "%s(%d)", __FUNCTION__, configuration);
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ /* calibrate scanner */
+ i2c_sensorboard_scanner_calib();
+
+ i2c_mechboard_mode_harvest();
+
+ /* go straight. total distance is less than 5 meters */
+ strat_set_speed(1000, 1000);
+ trajectory_d_rel(&mainboard.traj, BIG_DIST);
+
+ /* when y > 50, break */
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(500), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+
+ /* turn to 90° abs while going forward */
+ DEBUG(E_USER_STRAT, "turn now");
+ strat_set_speed(1000, 350);
+ trajectory_only_a_abs(&mainboard.traj, COLOR_A(90));
+
+ /* when y > 100, check the presence of column 4 */
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1000), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
+ col4_present = 1;
+ if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
+ col4_present = 1;
+
+ /* when y > 120, check the presence of column 1 */
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1200), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ if (get_color() == I2C_COLOR_RED && sensor_get(S_COLUMN_RIGHT))
+ col1_present = 1;
+ if (get_color() == I2C_COLOR_GREEN && sensor_get(S_COLUMN_LEFT))
+ col1_present = 1;
+
+ /* when y > 130, break */
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1300), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+
+ strat_infos.s_cols.flags |= STATIC_COL_LINE0_DONE;
+
+ DEBUG(E_USER_STRAT, "col4=%d col1=%d", col4_present, col1_present);
+ DEBUG(E_USER_STRAT, "have %d cols", get_column_count());
+
+ if (configuration == 0) {
+ if (get_column_count() > 2) {
+ configuration = 1;
+ if (col4_present || col1_present) {
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE2_DONE;
+ }
+ else {
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE1_DONE;
+ }
+ }
+
+ /* only 2 colums on the first line */
+ else {
+ /* all other colums are on line 1 */
+ if (col4_present && col1_present) {
+ configuration = 2;
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE2_DONE;
+ }
+
+ /* only 2 columns on line 1, so there are also
+ * 2 on line 2 */
+ else if (col4_present || col1_present) {
+ configuration = 4;
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE2_DONE;
+ }
+
+ /* all other columns are on line 2 */
+ else {
+ configuration = 3;
+ strat_infos.s_cols.flags |=
+ STATIC_COL_LINE1_DONE;
+ }
+ }
+ }
+
+ strat_infos.s_cols.configuration = configuration;
+ DEBUG(E_USER_STRAT, "use configuration %d", configuration);
+
+ if (configuration == 1) {
+ /* we already got 4 columns, go to the disc directly */
+
+ strat_set_speed(1500, 900);
+ trajectory_only_a_abs(&mainboard.traj, COLOR_A(0));
+ err = WAIT_COND_OR_TRAJ_END(x_is_more_than(1100), TRAJ_FLAGS_STD);
+
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ }
+ else if (configuration == 2 /* go from line 0 to line 1 */) {
+ strat_set_speed(800, 1000);
+ /* relative is needed here */
+ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
+ err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1300), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ strat_set_speed(1000, 600);
+ err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1100),
+ TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ }
+ else if (configuration == 3 /* go from line 0 to line 2 and there is 4 columns
+ on line 2*/) {
+ strat_set_speed(1000, 600);
+ /* relative is needed here */
+ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
+ err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(1110), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ }
+ else if (configuration == 4 /* go from line 0 to line 2 and there is 2 columns
+ on line 2 */) {
+ strat_set_speed(1000, 600);
+ /* relative is needed here */
+ trajectory_only_a_rel(&mainboard.traj, COLOR_A(-180));
+ err = WAIT_COND_OR_TRAJ_END(!y_is_more_than(600), TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+ }
+ else {
+ trajectory_stop(&mainboard.traj);
+ }
+
+ ERROUT(END_TRAJ);
+
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
+
+/*
+ * get last 2 columns
+ * must be called after the first temple building
+ */
+uint8_t strat_static_columns_pass2(void)
+{
+ uint16_t old_spdd, old_spda;
+ uint8_t side, err, next_mode;
+
+ DEBUG(E_USER_STRAT, "%s()", __FUNCTION__);
+
+ strat_get_speed(&old_spdd, &old_spda);
+
+ if (get_color() == I2C_COLOR_RED)
+ side = I2C_RIGHT_SIDE;
+ else
+ side = I2C_LEFT_SIDE;
+
+ if (strat_infos.conf.flags & STRAT_CONF_STORE_STATIC2)
+ next_mode = I2C_MECHBOARD_MODE_STORE;
+ else
+ next_mode = I2C_MECHBOARD_MODE_HARVEST;
+
+ switch (strat_infos.s_cols.configuration) {
+
+ /* configuration 1: 4 cols on line 0 */
+ case 1:
+ if (strat_infos.s_cols.flags & STATIC_COL_LINE1_DONE) {
+ /* go on line 2 */
+
+ strat_set_speed(2000, 700);
+ trajectory_d_a_rel(&mainboard.traj, -450, COLOR_A(35));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ trajectory_goto_forward_xy_abs(&mainboard.traj,
+ LINE2_X,
+ COLOR_Y(400));
+ err = WAIT_COND_OR_TRAJ_END(get_column_count() == 2,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+ }
+ else {
+ /* go on line 1 */
+ strat_set_speed(2000, 700);
+ trajectory_d_a_rel(&mainboard.traj, -650, COLOR_A(55));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+
+ err = goto_and_avoid_forward(LINE1_X,
+ COLOR_Y(400),
+ TRAJ_FLAGS_NO_NEAR,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+ }
+
+ ERROUT(END_TRAJ);
+ break;
+
+ /* configuration 2: 2 cols on line 0,
+ all other colums are on line 1 */
+ case 2:
+ /* go on line 1 */
+ strat_set_speed(2000, 700);
+ trajectory_d_a_rel(&mainboard.traj, -410, COLOR_A(-20));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+
+ err = goto_and_avoid_forward(COL10_X, COLOR_Y(400),
+ TRAJ_FLAGS_NO_NEAR,
+ TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ ERROUT(END_TRAJ);
+ break;
+
+ /* configuration 3: 2 cols on line 0,
+ all other colums are on line 2 */
+ case 3:
+ /* go on line 2 */
+ strat_set_speed(2000, 700);
+ trajectory_d_a_rel(&mainboard.traj, -150, COLOR_A(-30));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+
+ trajectory_goto_forward_xy_abs(&mainboard.traj,
+ LINE2_X,
+ COLOR_Y(400));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ ERROUT(END_TRAJ);
+ break;
+
+ /* configuration 4: 2 cols on line 0,
+ 2 on line 1, 2 on line 2 */
+ case 4:
+ /* go on line 1 */
+ strat_set_speed(600, 2000);
+ trajectory_d_a_rel(&mainboard.traj, -BIG_DIST,
+ COLOR_A(-135));
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(900),
+ TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+
+ DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ i2c_mechboard_mode_prepare_pickup_next(side,
+ next_mode);
+
+ strat_set_speed(2000, 2000);
+ trajectory_d_rel(&mainboard.traj, -BIG_DIST);
+ err = WAIT_COND_OR_TRAJ_END(y_is_more_than(1100),
+ TRAJ_FLAGS_STD);
+ if (TRAJ_SUCCESS(err)) /* we should not reach end */
+ ERROUT(END_ERROR);
+ else if (err)
+ ERROUT(err);
+
+ DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ trajectory_d_a_rel(&mainboard.traj, -600, COLOR_A(40));
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ if (!TRAJ_SUCCESS(err))
+ ERROUT(err);
+
+ DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
+ strat_set_speed(SPEED_DIST_SLOW, SPEED_ANGLE_FAST);
+ err = goto_and_avoid_forward(LINE1_X,
+ COLOR_Y(400),
+ TRAJ_FLAGS_NO_NEAR,
+ TRAJ_FLAGS_NO_NEAR);
+ ERROUT(END_TRAJ);
+ break;
+
+ default:
+ break;
+ }
+
+ /* should not reach this point */
+ ERROUT(END_ERROR);
+
+ end:
+ strat_set_speed(old_spdd, old_spda);
+ return err;
+}
git.droids-corp.org / aversive.git / blobdiff