From: zer0 <zer0@carbon.local>
Date: Fri, 30 Apr 2010 21:30:51 +0000 (+0200)
Subject: cobboard no move mode
X-Git-Url: http://git.droids-corp.org/?p=aversive.git;a=commitdiff_plain;h=38196781643cba8e1d681aafb28cdadc51fe20ec

cobboard no move mode
---

diff --git a/projects/microb2010/cobboard/commands_cobboard.c b/projects/microb2010/cobboard/commands_cobboard.c
index 515f3d5..669c4c8 100644
--- a/projects/microb2010/cobboard/commands_cobboard.c
+++ b/projects/microb2010/cobboard/commands_cobboard.c
@@ -253,11 +253,22 @@ static void cmd_state2_parsed(void *parsed_result,
 		state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
 				  I2C_COBBOARD_SPK_AUTOHARVEST);
 	}
+	else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+		state_set_mode(I2C_COBBOARD_MODE_HARVEST);
+		state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+				  I2C_COBBOARD_SPK_NO_MOVE);
+	}
+	else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+		state_set_mode(I2C_COBBOARD_MODE_HARVEST);
+		state_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+				  I2C_COBBOARD_SPK_AUTOHARVEST |
+				  I2C_COBBOARD_SPK_NO_MOVE);
+	}
 }
 
 prog_char str_state2_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_state2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg0, str_state2_arg0);
-prog_char str_state2_arg1[] = "harvest#deploy#pack";
+prog_char str_state2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
 parse_pgm_token_string_t cmd_state2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg1, str_state2_arg1);
 prog_char str_state2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_state2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_state2_result, arg2, str_state2_arg2);
diff --git a/projects/microb2010/cobboard/spickle.c b/projects/microb2010/cobboard/spickle.c
index f8e1613..7296d8e 100644
--- a/projects/microb2010/cobboard/spickle.c
+++ b/projects/microb2010/cobboard/spickle.c
@@ -137,7 +137,6 @@ void spickle_set_coefs(uint32_t k1, uint32_t k2)
 	spickle.k2 = k2;
 }
 
-
 void spickle_set_pos(uint8_t side, int32_t pos_packed,
 		     int32_t pos_mid, int32_t pos_deployed)
 {
@@ -159,6 +158,32 @@ void spickle_dump_params(void)
 		 spickle.pos_deployed[I2C_RIGHT_SIDE]);
 }
 
+static uint8_t spickle_is_at_pos(uint8_t side, int32_t pos)
+{
+	int32_t diff;
+	int32_t enc;
+	if (side == I2C_LEFT_SIDE)
+		enc = encoders_spi_get_value(LEFT_SPICKLE_ENCODER);
+	else
+		enc = encoders_spi_get_value(RIGHT_SPICKLE_ENCODER);
+	diff = pos - enc;
+	if (diff < 0)
+		diff = -diff;
+	if (diff < 500)
+		return 1;
+	return 0;
+}
+
+uint8_t spickle_is_packed(uint8_t side)
+{
+	return spickle_is_at_pos(side, spickle.pos_packed[side]);
+}
+
+uint8_t spickle_is_deployed(uint8_t side)
+{
+	return spickle_is_at_pos(side, spickle.pos_deployed[side]);
+}
+
 void spickle_deploy(uint8_t side)
 {
 	cs_set_consign(&spickle.csb[side]->cs, spickle.pos_deployed[side]);
diff --git a/projects/microb2010/cobboard/spickle.h b/projects/microb2010/cobboard/spickle.h
index 81c74d8..f34894a 100644
--- a/projects/microb2010/cobboard/spickle.h
+++ b/projects/microb2010/cobboard/spickle.h
@@ -1,6 +1,6 @@
-/*  
+/*
  *  Copyright Droids Corporation (2010)
- * 
+ *
  *  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
@@ -29,6 +29,9 @@ void spickle_set_pos(uint8_t side, int32_t pos_pack,
 
 void spickle_dump_params(void);
 
+uint8_t spickle_is_packed(void);
+uint8_t spickle_is_deployed(void);
+
 void spickle_deploy(uint8_t side);
 void spickle_mid(uint8_t side);
 void spickle_pack(uint8_t side);
diff --git a/projects/microb2010/cobboard/state.c b/projects/microb2010/cobboard/state.c
index 86afce9..e315da0 100644
--- a/projects/microb2010/cobboard/state.c
+++ b/projects/microb2010/cobboard/state.c
@@ -96,7 +96,7 @@ static uint8_t state_no_cob_inside(void)
 		!sensor_get(S_COB_INSIDE_R);
 }
 
-static uint8_t is_deployed(uint8_t side)
+static uint8_t state_spicklemode_deployed(uint8_t side)
 {
 	if (side == I2C_LEFT_SIDE)
 		return lspickle & I2C_COBBOARD_SPK_DEPLOY;
@@ -104,7 +104,7 @@ static uint8_t is_deployed(uint8_t side)
 		return rspickle & I2C_COBBOARD_SPK_DEPLOY;
 }
 
-static uint8_t is_autoharvest(uint8_t side)
+static uint8_t state_spicklemode_autoharvest(uint8_t side)
 {
 	if (side == I2C_LEFT_SIDE)
 		return lspickle & I2C_COBBOARD_SPK_AUTOHARVEST;
@@ -112,14 +112,22 @@ static uint8_t is_autoharvest(uint8_t side)
 		return rspickle & I2C_COBBOARD_SPK_AUTOHARVEST;
 }
 
+static uint8_t state_spicklemode_nomove(uint8_t side)
+{
+	if (side == I2C_LEFT_SIDE)
+		return lspickle & I2C_COBBOARD_SPK_NO_MOVE;
+	else
+		return rspickle & I2C_COBBOARD_SPK_NO_MOVE;
+}
+
 /* pack/deploy spickles depending on mode */
 static void spickle_prepare(uint8_t side)
 {
-	if (cob_count >= 5)
-		spickle_pack(side);
-	else if (is_deployed(side) && !is_autoharvest(side))
-		spickle_deploy(side); /*spickle_mid(side);*/
-	else if (is_deployed(side) && is_autoharvest(side))
+	/* we do nothing in mode no out */
+	if (state_spicklemode_nomove(side))
+		return;
+
+	if (state_spicklemode_deployed(side))
 		spickle_deploy(side);
 	else
 		spickle_pack(side);
@@ -247,12 +255,15 @@ static void state_do_harvest(uint8_t side)
 	cobroller_off(side);
 	cob_count ++;
 
+	state_debug_wait_key_pressed();
+
 	/* last cob, nothing to do */
 	if (cob_count == 5)
 		return;
 
-	/* redeploy the spickle */
-	spickle_deploy(side);
+	/* redeploy the spickle if there is a black cob */
+	if (!state_spicklemode_nomove(side))
+		spickle_deploy(side);
 	state_debug_wait_key_pressed();
 
 	/* let the loaded cobs go */
@@ -326,11 +337,13 @@ void state_machine(void)
 
 		/* harvest */
 		if (cob_count < 5) {
-			if ((lspickle & I2C_COBBOARD_SPK_DEPLOY) &&
-			    (lspickle & I2C_COBBOARD_SPK_AUTOHARVEST))
+			if (state_spicklemode_deployed(I2C_LEFT_SIDE) &&
+			    state_spicklemode_autoharvest(I2C_LEFT_SIDE) &&
+			    !state_spicklemode_nomove(I2C_LEFT_SIDE))
 				state_do_harvest(I2C_LEFT_SIDE);
-			if ((rspickle & I2C_COBBOARD_SPK_DEPLOY) &&
-			    (rspickle & I2C_COBBOARD_SPK_AUTOHARVEST))
+			if (state_spicklemode_deployed(I2C_RIGHT_SIDE) &&
+			    state_spicklemode_autoharvest(I2C_RIGHT_SIDE) &&
+			    !state_spicklemode_nomove(I2C_RIGHT_SIDE))
 				state_do_harvest(I2C_RIGHT_SIDE);
 		}
 
diff --git a/projects/microb2010/common/avr6.x b/projects/microb2010/common/avr6.x
index a758e96..3ad392f 100644
--- a/projects/microb2010/common/avr6.x
+++ b/projects/microb2010/common/avr6.x
@@ -123,6 +123,18 @@ SECTIONS
     KEEP (*(.init9))
     *(.text.*) /* trucs de gcc ? */
     . = ALIGN(2048);
+    /* some libc stuff */
+    strc*(.text)
+    mem*(.text)
+    printf*(.text)
+    vfprintf*(.text)
+    sprintf*(.text)
+    snprintf*(.text)
+    malloc*(.text)
+    free*(.text)
+    fdevopen*(.text)
+    fputc*(.text)
+    . = ALIGN(2048);
     uart*(.text)
     pwm*(.text)
     parse*(.text)
@@ -151,18 +163,6 @@ SECTIONS
     spi*(.text)
     ax12*(.text)
     . = ALIGN(2048);
-    /* some libc stuff */
-    str*(.text)
-    mem*(.text)
-    printf*(.text)
-    vfprintf*(.text)
-    sprintf*(.text)
-    snprintf*(.text)
-    malloc*(.text)
-    free*(.text)
-    fdevopen*(.text)
-    fputc*(.text)
-    . = ALIGN(2048);
     *(.text)
     . = ALIGN(2);
     *(.fini9)  /* _exit() starts here.  */
diff --git a/projects/microb2010/common/i2c_commands.h b/projects/microb2010/common/i2c_commands.h
index fa17fca..d64ed8b 100644
--- a/projects/microb2010/common/i2c_commands.h
+++ b/projects/microb2010/common/i2c_commands.h
@@ -108,6 +108,7 @@ struct i2c_req_cobboard_status {
 
 #define I2C_COBBOARD_SPK_DEPLOY  0x01 /* deploy the spickle */
 #define I2C_COBBOARD_SPK_AUTOHARVEST 0x02 /* auto harvest the cobs */
+#define I2C_COBBOARD_SPK_NO_MOVE 0x04 /* if enabled, don't change state */
 	uint8_t lspickle;
 	uint8_t rspickle;
 };
diff --git a/projects/microb2010/mainboard/commands.c b/projects/microb2010/mainboard/commands.c
index 255f26b..e79562b 100644
--- a/projects/microb2010/mainboard/commands.c
+++ b/projects/microb2010/mainboard/commands.c
@@ -82,6 +82,7 @@ extern parse_pgm_inst_t cmd_beacon_start;
 extern parse_pgm_inst_t cmd_servo_balls;
 extern parse_pgm_inst_t cmd_clitoid;
 extern parse_pgm_inst_t cmd_time_monitor;
+extern parse_pgm_inst_t cmd_strat_event;
 extern parse_pgm_inst_t cmd_test;
 
 /* commands_traj.c */
@@ -173,6 +174,7 @@ parse_pgm_ctx_t main_ctx[] = {
 	(parse_pgm_inst_t *)&cmd_servo_balls,
 	(parse_pgm_inst_t *)&cmd_clitoid,
 	(parse_pgm_inst_t *)&cmd_time_monitor,
+	(parse_pgm_inst_t *)&cmd_strat_event,
 	(parse_pgm_inst_t *)&cmd_test,
 
 	/* commands_traj.c */
diff --git a/projects/microb2010/mainboard/commands_mainboard.c b/projects/microb2010/mainboard/commands_mainboard.c
index ee0ffa4..13a327a 100644
--- a/projects/microb2010/mainboard/commands_mainboard.c
+++ b/projects/microb2010/mainboard/commands_mainboard.c
@@ -785,11 +785,19 @@ static void cmd_cobboard_setmode2_parsed(void *parsed_result, void *data)
 		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
 		i2c_cobboard_pack(side);
 	}
+	else if (!strcmp_P(res->arg1, PSTR("deploy_nomove"))) {
+		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+		i2c_cobboard_deploy_nomove(side);
+	}
+	else if (!strcmp_P(res->arg1, PSTR("harvest_nomove"))) {
+		i2c_cobboard_set_mode(I2C_COBBOARD_MODE_HARVEST);
+		i2c_cobboard_autoharvest_nomove(side);
+	}
 }
 
 prog_char str_cobboard_setmode2_arg0[] = "cobboard";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg0, str_cobboard_setmode2_arg0);
-prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack";
+prog_char str_cobboard_setmode2_arg1[] = "harvest#deploy#pack#harvest_nomove#deploy_nomove";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg1, str_cobboard_setmode2_arg1);
 prog_char str_cobboard_setmode2_arg2[] = "left#right";
 parse_pgm_token_string_t cmd_cobboard_setmode2_arg2 = TOKEN_STRING_INITIALIZER(struct cmd_cobboard_setmode2_result, arg2, str_cobboard_setmode2_arg2);
@@ -1164,6 +1172,44 @@ parse_pgm_inst_t cmd_time_monitor = {
 	},
 };
 
+
+/**********************************************************/
+/* Strat_Event */
+
+/* this structure is filled when cmd_strat_event is parsed successfully */
+struct cmd_strat_event_result {
+	fixed_string_t arg0;
+	fixed_string_t arg1;
+};
+
+/* function called when cmd_strat_event is parsed successfully */
+static void cmd_strat_event_parsed(void *parsed_result, void *data)
+{
+	struct cmd_strat_event_result *res = parsed_result;
+
+	if (!strcmp_P(res->arg1, PSTR("on")))
+		strat_event_enable();
+	else
+		strat_event_disable();
+}
+
+prog_char str_strat_event_arg0[] = "strat_event";
+parse_pgm_token_string_t cmd_strat_event_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg0, str_strat_event_arg0);
+prog_char str_strat_event_arg1[] = "on#off";
+parse_pgm_token_string_t cmd_strat_event_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_strat_event_result, arg1, str_strat_event_arg1);
+
+prog_char help_strat_event[] = "Enable/disable strat_event callback";
+parse_pgm_inst_t cmd_strat_event = {
+	.f = cmd_strat_event_parsed,  /* function to call */
+	.data = NULL,      /* 2nd arg of func */
+	.help_str = help_strat_event,
+	.tokens = {        /* token list, NULL terminated */
+		(prog_void *)&cmd_strat_event_arg0,
+		(prog_void *)&cmd_strat_event_arg1,
+		NULL,
+	},
+};
+
 /**********************************************************/
 /* Test */
 
diff --git a/projects/microb2010/mainboard/i2c_protocol.c b/projects/microb2010/mainboard/i2c_protocol.c
index 9908f1a..7a2c378 100644
--- a/projects/microb2010/mainboard/i2c_protocol.c
+++ b/projects/microb2010/mainboard/i2c_protocol.c
@@ -438,6 +438,20 @@ int8_t i2c_cobboard_deploy(uint8_t side)
 	return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY);
 }
 
+int8_t i2c_cobboard_autoharvest_nomove(uint8_t side)
+{
+	return i2c_cobboard_set_spickle(side,
+					I2C_COBBOARD_SPK_DEPLOY |
+					I2C_COBBOARD_SPK_AUTOHARVEST |
+					I2C_COBBOARD_SPK_NO_MOVE);
+}
+
+int8_t i2c_cobboard_deploy_nomove(uint8_t side)
+{
+	return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY |
+					I2C_COBBOARD_SPK_NO_MOVE);
+}
+
 int8_t i2c_ballboard_set_mode(uint8_t mode)
 {
 	struct i2c_cmd_ballboard_set_mode buf;
diff --git a/projects/microb2010/mainboard/i2c_protocol.h b/projects/microb2010/mainboard/i2c_protocol.h
index b107c44..06cd47e 100644
--- a/projects/microb2010/mainboard/i2c_protocol.h
+++ b/projects/microb2010/mainboard/i2c_protocol.h
@@ -39,6 +39,9 @@ int8_t i2c_cobboard_set_mode(uint8_t mode);
 int8_t i2c_cobboard_pack(uint8_t side);
 int8_t i2c_cobboard_autoharvest(uint8_t side);
 int8_t i2c_cobboard_deploy(uint8_t side);
+int8_t i2c_cobboard_autoharvest_nomove(uint8_t side);
+int8_t i2c_cobboard_deploy_nomove(uint8_t side);
+
 int8_t i2c_ballboard_set_mode(uint8_t mode);
 
 #endif
diff --git a/projects/microb2010/mainboard/strat.c b/projects/microb2010/mainboard/strat.c
index 423b0f7..94b9b6f 100644
--- a/projects/microb2010/mainboard/strat.c
+++ b/projects/microb2010/mainboard/strat.c
@@ -67,7 +67,7 @@
 #define COL_DISP_MARGIN 400 /* stop 40 cm in front of dispenser */
 #define COL_SCAN_PRE_MARGIN 250
 
-static uint8_t strat_running = 0;
+static volatile uint8_t strat_running = 0;
 struct strat_conf strat_conf;
 
 /*************************************************************/
@@ -98,6 +98,16 @@ void strat_conf_dump(const char *caller)
 
 }
 
+void strat_event_enable(void)
+{
+	strat_running = 1;
+}
+
+void strat_event_disable(void)
+{
+	strat_running = 0;
+}
+
 /* call it just before launching the strat */
 void strat_init(void)
 {
@@ -173,11 +183,9 @@ void strat_event(void *dummy)
 
 	/* detect cob on left side */
 	if (corn_is_near(&lidx, I2C_LEFT_SIDE)) {
-		if (lcob != I2C_COB_NONE) {
+		if (lcob != I2C_COB_NONE)
 			corn_set_color(strat_db.corn_table[lidx], lcob);
-			DEBUG(E_USER_STRAT, "lcob %s %d",
-			      lcob == I2C_COB_WHITE ? "white" : "black", lidx);
-		}
+
 		if (strat_db.corn_table[lidx]->corn.color == I2C_COB_WHITE)
 			i2c_cobboard_autoharvest(I2C_LEFT_SIDE);
 		else
@@ -189,11 +197,9 @@ void strat_event(void *dummy)
 
 	/* detect cob on right side */
 	if (corn_is_near(&ridx, I2C_RIGHT_SIDE)) {
-		if (rcob != I2C_COB_NONE) {
+		if (rcob != I2C_COB_NONE)
 			corn_set_color(strat_db.corn_table[ridx], rcob);
-			DEBUG(E_USER_STRAT, "rcob %s %d",
-			      rcob == I2C_COB_WHITE ? "white" : "black", ridx);
-		}
+
 		if (strat_db.corn_table[ridx]->corn.color == I2C_COB_WHITE)
 			i2c_cobboard_autoharvest(I2C_RIGHT_SIDE);
 		else
@@ -216,12 +222,18 @@ static uint8_t strat_harvest(void)
 
 static uint8_t strat_eject(void)
 {
+	uint8_t err;
+
+	//XXX return vals
+	strat_set_speed(600, SPEED_ANGLE_SLOW);
+
 	trajectory_goto_xy_abs(&mainboard.traj, 2625, COLOR_Y(1847));
-	err = wait_traj_end(END_INTR|END_TRAJ);
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+	trajectory_a_abs(&mainboard.traj, COLOR_A(70));
+	err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
 
 	DEBUG(E_USER_STRAT, "%s():%d", __FUNCTION__, __LINE__);
 	strat_hardstop();
-	strat_set_speed(600, SPEED_ANGLE_FAST);
 
 	/* ball ejection */
 	trajectory_a_abs(&mainboard.traj, COLOR_A(90));
@@ -263,6 +275,7 @@ static uint8_t strat_beginning(void)
 				    TRAJ_FLAGS_STD);
 
 	strat_set_acc(ACC_DIST, ACC_ANGLE);
+	strat_set_speed(250, SPEED_ANGLE_SLOW);
 
 #if 1
  l1:
diff --git a/projects/microb2010/mainboard/strat.h b/projects/microb2010/mainboard/strat.h
index 9fa720a..daf607c 100644
--- a/projects/microb2010/mainboard/strat.h
+++ b/projects/microb2010/mainboard/strat.h
@@ -162,5 +162,7 @@ void strat_dump_flags(void);
 void strat_goto_near(int16_t x, int16_t y, uint16_t dist);
 uint8_t strat_main(void);
 void strat_event(void *dummy);
+void strat_event_enable(void);
+void strat_event_disable(void);
 
 #endif
diff --git a/projects/microb2010/mainboard/strat_corn.c b/projects/microb2010/mainboard/strat_corn.c
index f8a9666..8d3f839 100644
--- a/projects/microb2010/mainboard/strat_corn.c
+++ b/projects/microb2010/mainboard/strat_corn.c
@@ -218,6 +218,7 @@ uint8_t line2line(uint8_t dir1, uint8_t num1,
 	strat_get_speed(&d_speed, &a_speed);
 
 	/* XXX 600 -> cste */
+	/* XXX does not work, do better */
 /* 	if (err == 0 && d_speed < 600 && */
 /* 	    mainboard.traj.state == RUNNING_CLITOID_LINE) */
 /* 		strat_set_speed(600, SPEED_ANGLE_FAST); */