Commit from zer0 on branch b_zer0 (2008-03-31 00:00 CEST)
=================================
remove old style's blocking detection
aversive
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
1.1.2.3
aversive
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.h
1.1.2.5
======================================================================================
aversive/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
(1.1.2.2 -> 1.1.2.3)
======================================================================================
@@ -15,38 +15,25 @@
* 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: blocking_detection_manager.c,v 1.1.2.2 2008-03-27 19:20:49
zer0 Exp $
+ * Revision : $Id: blocking_detection_manager.c,v 1.1.2.3 2008-03-30 22:00:56
zer0 Exp $
*
* Olivier MATZ <[EMAIL PROTECTED]>
*/
/* blocking detection manager */
+#include <stdio.h>
#include <string.h>
#include <aversive/error.h>
#include <blocking_detection_manager.h>
/** init module, give the robot system to use as a parameter */
-void bd_init(struct blocking_detection * bd, struct cs *cs)
+void bd_init(struct blocking_detection * bd)
{
uint8_t flags;
IRQ_LOCK(flags);
memset(bd, 0, sizeof(*bd));
- bd->cs = cs;
- IRQ_UNLOCK(flags);
-}
-
-/* thresholds */
-void bd_set_speed_err_thresholds(struct blocking_detection * bd, uint32_t
speed_thres,
- uint32_t err_thres, uint16_t cpt_thres)
-{
- uint8_t flags;
- IRQ_LOCK(flags);
- bd->speed_thres = speed_thres;
- bd->err_thres = err_thres;
- bd->cpt1_thres = cpt_thres;
- bd->cpt1 = 0;
IRQ_UNLOCK(flags);
}
@@ -60,8 +47,8 @@
bd->k1 = k1;
bd->k2 = k2;
bd->i_thres = i_thres;
- bd->cpt2_thres = cpt_thres;
- bd->cpt2 = 0;
+ bd->cpt_thres = cpt_thres;
+ bd->cpt = 0;
IRQ_UNLOCK(flags);
}
@@ -70,67 +57,55 @@
{
uint8_t flags;
IRQ_LOCK(flags);
- bd->cpt1 = 0;
- bd->cpt2 = 0;
+ bd->cpt = 0;
IRQ_UNLOCK(flags);
}
+
+
/** function to be called periodically */
-void bd_manage(struct blocking_detection * bd)
+void bd_manage_from_speed_cmd(struct blocking_detection * bd,
+ int32_t speed, int32_t cmd)
{
- int32_t err=0;
- int32_t pos;
- int32_t cmd;
int32_t i=0;
- int32_t speed;
-
- static uint8_t a=0;
-
- pos = cs_get_filtered_feedback(bd->cs);
- speed = (pos - bd->prev_pos);
-
- /* if blocking detection enabled */
- if ( bd->cpt1_thres ) {
- err = cs_get_error(bd->cs);
- if ((ABS(err) > bd->err_thres &&
- ABS(speed) < bd->speed_thres)) {
- if (bd->cpt1 == bd->cpt1_thres - 1)
- DEBUG(E_BLOCKING_DETECTION_MANAGER,
- "A: err=%ld speed=%ld",
- ABS(err), ABS(speed));
-
- if(bd->cpt1 < bd->cpt1_thres)
- bd->cpt1++;
- }
- else {
- bd->cpt2=0;
- }
- }
-
/* if current-based blocking_detection enabled */
- if ( bd->cpt2_thres ) {
- cmd = cs_get_out(bd->cs);
- i = bd->k1 * cmd - bd->k2*speed;
+ if ( bd->cpt_thres ) {
+ i = bd->k1 * cmd - bd->k2 * speed;
if (ABS(i) > bd->i_thres) {
- if (bd->cpt2 == bd->cpt2_thres - 1)
+ if (bd->cpt == bd->cpt_thres - 1)
DEBUG(E_BLOCKING_DETECTION_MANAGER,
- "A: i=%ld", i);
- if(bd->cpt2 < bd->cpt2_thres)
- bd->cpt2++;
+ "BLOCKING cmd=%ld, speed=%ld i=%ld",
+ cmd, speed, i);
+ if(bd->cpt < bd->cpt_thres)
+ bd->cpt++;
}
else {
- bd->cpt2=0;
+ bd->cpt=0;
+ }
+#if BD_DEBUG
+ if (bd->debug_cpt++ == BD_DEBUG) {
+ DEBUG(E_BLOCKING_DETECTION_MANAGER, "cmd=%ld, speed=%ld
i=%ld",
+ cmd, speed, i);
+ bd->debug_cpt = 0;
}
}
+#endif
+}
- if (a++ == 0)
- DEBUG(E_BLOCKING_DETECTION_MANAGER,
- "test err=%ld speed=%ld i=%ld",
- ABS(err), ABS(speed), i);
-
+/** function to be called periodically */
+void bd_manage_from_pos_cmd(struct blocking_detection * bd,
+ int32_t pos, int32_t cmd)
+{
+ int32_t speed = (pos - bd->prev_pos);
+ bd_manage_from_speed_cmd(bd, speed, cmd);
bd->prev_pos = pos;
- bd->speed = speed;
+}
+
+/** function to be called periodically */
+void bd_manage_from_cs(struct blocking_detection * bd, struct cs *cs)
+{
+ bd_manage_from_pos_cmd(bd, cs_get_filtered_feedback(cs),
cs_get_out(cs));
}
/** get value of blocking detection */
@@ -138,8 +113,7 @@
{
uint8_t ret, flags;
IRQ_LOCK(flags);
- ret = (bd->cpt1 && (bd->cpt1 == bd->cpt1_thres)) ||
- ( bd->cpt2 && (bd->cpt2 == bd->cpt2_thres));
+ ret = (bd->cpt && (bd->cpt == bd->cpt_thres));
IRQ_UNLOCK(flags);
return ret;
}
======================================================================================
aversive/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.h
(1.1.2.4 -> 1.1.2.5)
======================================================================================
@@ -15,7 +15,7 @@
* 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: blocking_detection_manager.h,v 1.1.2.4 2008-03-29 18:48:34
zer0 Exp $
+ * Revision : $Id: blocking_detection_manager.h,v 1.1.2.5 2008-03-30 22:00:56
zer0 Exp $
*
* Olivier MATZ <[EMAIL PROTECTED]>
*/
@@ -25,15 +25,15 @@
#ifndef BLOCKING_DETECTION_MANAGER_H_
#define BLOCKING_DETECTION_MANAGER_H_
+/* display debug every 128 calls of manage if defined */
+#define BD_DEBUG 128
+
#include <control_system_manager.h>
-/* detect blocking :
- *
- * 1/ if the speed is below a threshold and the error is above another
- * threshold during n tests, trigger the blocking detection.
+/* detect blocking based on motor current. if the current in the motor
+ * is a above a threshold suring n tests, trigger the blocking
+ * detection.
*
- * 2/ based on motor current. if the current in the motor is a above a
- * threshold suring n tests, trigger the blocking detection.
* We suppose that i = k1.V - k2.w
* (V is the voltage applied on the motor, and w the current speed
* of the motor)
@@ -41,28 +41,22 @@
struct blocking_detection {
struct cs *cs;
- uint32_t speed_thres;
- uint32_t err_thres;
- uint16_t cpt1_thres;
- uint16_t cpt1;
uint32_t i_thres;
int32_t k1;
int32_t k2;
- uint16_t cpt2_thres;
- uint16_t cpt2;
+ uint16_t cpt_thres;
+ uint16_t cpt;
+#ifdef BD_DEBUG
+ uint16_t debug_cpt;
+#endif
int32_t prev_pos;
int32_t speed;
};
/** init module, give the cs as parameter */
-void bd_init(struct blocking_detection * bd, struct cs *cs);
-
-/** thresholds. Be carreful, speed threshold depends on the period of
- * bd_manage() call. If cpt_thres is 0, disable it. */
-void bd_set_speed_err_thresholds(struct blocking_detection * bd,
- uint32_t speed_thres, uint32_t err_thres, uint16_t
cpt_thres);
+void bd_init(struct blocking_detection * bd);
/* thresholds for current-based blocking detection. If cpt_thres
* is 0, disable it. */
@@ -73,8 +67,16 @@
/** reset the blocking */
void bd_reset(struct blocking_detection * bd);
-/** function to be called periodically */
-void bd_manage(struct blocking_detection * bd);
+/** function to be called periodicallyn, when we use cs structure */
+void bd_manage_from_cs(struct blocking_detection * bd, struct cs *cs);
+
+/** function to be called periodically, when we use values directly */
+void bd_manage_from_pos_cmd(struct blocking_detection * bd,
+ int32_t pos, int32_t cmd);
+
+/** function to be called periodically, when we use values directly */
+void bd_manage_from_speed_cmd(struct blocking_detection * bd,
+ int32_t speed, int32_t cmd);
/** get value of blocking detection */
uint8_t bd_get(struct blocking_detection * bd);
Commit from zer0 (2008-03-31 00:01 CEST)
================
- blocking detection
- small wait before drop
aversive_projects microb2008/extension/barrel.c 1.6
aversive_projects microb2008/extension/commands.c 1.5
aversive_projects microb2008/extension/main.c 1.17
aversive_projects microb2008/extension/state.c 1.6
===============================================
aversive_projects/microb2008/extension/barrel.c (1.5 -> 1.6)
===============================================
@@ -15,7 +15,7 @@
* 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: barrel.c,v 1.5 2008-03-29 18:53:12 zer0 Exp $
+ * Revision : $Id: barrel.c,v 1.6 2008-03-30 22:01:43 zer0 Exp $
*
*/
@@ -84,10 +84,7 @@
cs_set_process_out(&ext.cs_b, encoders_microb_get_value,
BARREL_ENCODER);
barrel_set_consign(0);
- bd_init(&ext.bd_b, &ext.cs_b);
-
- /* speed, err, cpt */
- //bd_set_speed_err_thresholds(&ext.bd_b, 300, 150, 20);
+ bd_init(&ext.bd_b);
/* with this constants, current is in uA */
bd_set_current_thresholds(&ext.bd_b, 870, 667, 500000, 20);
@@ -97,7 +94,7 @@
{
static uint8_t limit=0;
cs_set_consign(&ext.cs_b, ext.barrel_consign);
- bd_manage(&ext.bd_b);
+ bd_manage_from_cs(&ext.bd_b, &ext.cs_b);
/* current and speed limitation */
if (bd_get(&ext.bd_b))
=================================================
aversive_projects/microb2008/extension/commands.c (1.4 -> 1.5)
=================================================
@@ -530,77 +530,6 @@
/**********************************************************/
-/* Blocking_Se for control system */
-
-/* this structure is filled when cmd_blocking_se is parsed successfully */
-struct cmd_blocking_se_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint32_t s;
- uint32_t e;
- uint16_t cpt;
-};
-
-/* function called when cmd_blocking_se is parsed successfully */
-static void cmd_blocking_se_parsed(void * parsed_result, void * data)
-{
- struct cmd_blocking_se_result * res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("barrel"))) {
- bd_set_speed_err_thresholds(&ext.bd_b, res->s, res->e,
res->cpt);
- }
- else if (!strcmp_P(res->arg1, PSTR("catapult"))) {
- printf_P(PSTR("no bd for catapult\r\n"));
- }
- /* else it's a "show" */
-
- printf_P(PSTR("blocking_se barrel %ld %ld %d\r\n"),
ext.bd_b.speed_thres,
- ext.bd_b.err_thres, ext.bd_b.cpt1_thres);
-/* printf_P(PSTR("blocking_se catapult %ld %ld %d\r\n"),
ext.bd_c.speed_thres, */
-/* ext.bd_c.err_thres, ext.bd_c.cpt_thres); */
-}
-
-prog_char str_blocking_se_arg0[] = "blocking_se";
-parse_pgm_token_string_t cmd_blocking_se_arg0 =
TOKEN_STRING_INITIALIZER(struct cmd_blocking_se_result, arg0,
str_blocking_se_arg0);
-prog_char str_blocking_se_arg1[] = "barrel#catapult";
-parse_pgm_token_string_t cmd_blocking_se_arg1 =
TOKEN_STRING_INITIALIZER(struct cmd_blocking_se_result, arg1,
str_blocking_se_arg1);
-parse_pgm_token_num_t cmd_blocking_se_s = TOKEN_NUM_INITIALIZER(struct
cmd_blocking_se_result, s, UINT32);
-parse_pgm_token_num_t cmd_blocking_se_e = TOKEN_NUM_INITIALIZER(struct
cmd_blocking_se_result, e, UINT32);
-parse_pgm_token_num_t cmd_blocking_se_cpt = TOKEN_NUM_INITIALIZER(struct
cmd_blocking_se_result, cpt, UINT16);
-
-prog_char help_blocking_se[] = "Set blocking detection values (speed, error,
cpt)";
-parse_pgm_inst_t cmd_blocking_se = {
- .f = cmd_blocking_se_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_blocking_se,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_blocking_se_arg0,
- (prog_void *)&cmd_blocking_se_arg1,
- (prog_void *)&cmd_blocking_se_s,
- (prog_void *)&cmd_blocking_se_e,
- (prog_void *)&cmd_blocking_se_cpt,
- NULL,
- },
-};
-
-/* show */
-
-prog_char str_blocking_se_show_arg[] = "show";
-parse_pgm_token_string_t cmd_blocking_se_show_arg =
TOKEN_STRING_INITIALIZER(struct cmd_blocking_se_result, arg1,
str_blocking_se_show_arg);
-
-prog_char help_blocking_se_show[] = "Show blocking_se detection values";
-parse_pgm_inst_t cmd_blocking_se_show = {
- .f = cmd_blocking_se_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_blocking_se_show,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_blocking_se_arg0,
- (prog_void *)&cmd_blocking_se_show_arg,
- NULL,
- },
-};
-
-/**********************************************************/
/* Blocking_I for control system */
/* this structure is filled when cmd_blocking_i is parsed successfully */
@@ -627,12 +556,10 @@
/* else it's a "show" */
printf_P(PSTR("blocking_i barrel %ld %ld %ld %d\r\n"),
- ext.bd_b.k1, ext.bd_b.k2, ext.bd_b.i_thres,
ext.bd_b.cpt2_thres);
-/* printf_P(PSTR("blocking_i catapult %ld %ld %d\r\n"),
ext.bd_c.speed_thres, */
-/* ext.bd_c.err_thres, ext.bd_c.cpt_thres); */
+ ext.bd_b.k1, ext.bd_b.k2, ext.bd_b.i_thres,
ext.bd_b.cpt_thres);
}
-prog_char str_blocking_i_arg0[] = "blocking_i";
+prog_char str_blocking_i_arg0[] = "blocking";
parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_blocking_i_result, arg0, str_blocking_i_arg0);
prog_char str_blocking_i_arg1[] = "barrel#catapult";
parse_pgm_token_string_t cmd_blocking_i_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_blocking_i_result, arg1, str_blocking_i_arg1);
@@ -662,7 +589,7 @@
prog_char str_blocking_i_show_arg[] = "show";
parse_pgm_token_string_t cmd_blocking_i_show_arg =
TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, arg1,
str_blocking_i_show_arg);
-prog_char help_blocking_i_show[] = "Show blocking_i detection values";
+prog_char help_blocking_i_show[] = "Show blocking detection values";
parse_pgm_inst_t cmd_blocking_i_show = {
.f = cmd_blocking_i_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
@@ -1428,8 +1355,6 @@
(parse_pgm_inst_t *)&cmd_pwm,
(parse_pgm_inst_t *)&cmd_gain,
(parse_pgm_inst_t *)&cmd_gain_show,
- (parse_pgm_inst_t *)&cmd_blocking_se,
- (parse_pgm_inst_t *)&cmd_blocking_se_show,
(parse_pgm_inst_t *)&cmd_blocking_i,
(parse_pgm_inst_t *)&cmd_blocking_i_show,
(parse_pgm_inst_t *)&cmd_speed,
=============================================
aversive_projects/microb2008/extension/main.c (1.16 -> 1.17)
=============================================
@@ -15,7 +15,7 @@
* 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: main.c,v 1.16 2008-03-29 18:53:12 zer0 Exp $
+ * Revision : $Id: main.c,v 1.17 2008-03-30 22:01:43 zer0 Exp $
*
*/
@@ -84,7 +84,6 @@
cs_set_consign(&ext.cs_c, ext.catapult_consign);
cs_manage(&ext.cs_c);
}
- /* bd_manage ? */
if (ext.flags & BARREL_CS_ON) {
barrel_cs_manage();
==============================================
aversive_projects/microb2008/extension/state.c (1.5 -> 1.6)
==============================================
@@ -15,7 +15,7 @@
* 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: state.c,v 1.5 2008-03-29 18:53:12 zer0 Exp $
+ * Revision : $Id: state.c,v 1.6 2008-03-30 22:01:43 zer0 Exp $
*
*/
@@ -177,6 +177,7 @@
}
DEBUG(E_USER_ST_MACH, "%s: ready to get balls", __FUNCTION__);
+ arm_roller_default();
/* pickup the balls */
action_servo_set(SERVO_LEFT_ARM_NUM, SERVO_LEFT_ARM_POS_PICKUP);
@@ -287,7 +288,7 @@
return 0;
}
- wait_ms(50); /* a little wait to be sure that the barrel is
+ wait_ms(150); /* a little wait to be sure that the barrel is
not moving anymore */
DEBUG(E_USER_ST_MACH, "%s: prepared !", __FUNCTION__);
Commit from zer0 (2008-03-31 00:02 CEST)
================
- blocking detection is now ready
- enhance strats... we still have some pbs
- add commands for derivate filter
aversive_projects microb2008/main/commands.c 1.17
aversive_projects microb2008/main/main.c 1.25
aversive_projects microb2008/main/main.h 1.14
aversive_projects microb2008/main/strat.c 1.7
aversive_projects microb2008/main/strat.h 1.4
aversive_projects microb2008/main/strat_base.c 1.9
aversive_projects microb2008/main/strat_base.h 1.7
============================================
aversive_projects/microb2008/main/commands.c (1.16 -> 1.17)
============================================
@@ -127,7 +127,7 @@
i2c_pwm(1, res->arg2);
if (pwm_ptr)
- pwm_set(pwm_ptr, res->arg2);
+ pwm_set_and_save(pwm_ptr, res->arg2);
printf_P(PSTR("done\r\n"));
}
@@ -551,11 +551,11 @@
}
/* else it is a "show" */
- printf_P(PSTR("angle %lu %lu %lu\r\n"),
+ printf_P(PSTR("maximum angle %lu %lu %lu\r\n"),
pid_get_max_in(&robot.pid_a),
pid_get_max_I(&robot.pid_a),
pid_get_max_out(&robot.pid_a));
- printf_P(PSTR("distance %lu %lu %lu\r\n"),
+ printf_P(PSTR("maximum distance %lu %lu %lu\r\n"),
pid_get_max_in(&robot.pid_d),
pid_get_max_I(&robot.pid_d),
pid_get_max_out(&robot.pid_d));
@@ -601,6 +601,71 @@
},
};
+/**********************************************************/
+/* Derivate_Filters for control system */
+
+/* this structure is filled when cmd_derivate_filter is parsed successfully */
+struct cmd_derivate_filter_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t size;
+};
+
+/* function called when cmd_derivate_filter is parsed successfully */
+static void cmd_derivate_filter_parsed(void * parsed_result, void * data)
+{
+ struct cmd_derivate_filter_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("angle"))) {
+ pid_set_derivate_filter(&robot.pid_a, res->size);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("distance"))) {
+ pid_set_derivate_filter(&robot.pid_d, res->size);
+ }
+ /* else it is a "show" */
+
+ printf_P(PSTR("derivate_filter angle %u\r\n"),
+ pid_get_derivate_filter(&robot.pid_a));
+ printf_P(PSTR("derivate_filter distance %u\r\n"),
+ pid_get_derivate_filter(&robot.pid_d));
+}
+
+prog_char str_derivate_filter_arg0[] = "derivate_filter";
+parse_pgm_token_string_t cmd_derivate_filter_arg0 =
TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, arg0,
str_derivate_filter_arg0);
+prog_char str_derivate_filter_arg1[] = "angle#distance";
+parse_pgm_token_string_t cmd_derivate_filter_arg1 =
TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, arg1,
str_derivate_filter_arg1);
+parse_pgm_token_num_t cmd_derivate_filter_size = TOKEN_NUM_INITIALIZER(struct
cmd_derivate_filter_result, size, UINT32);
+
+prog_char help_derivate_filter[] = "Set derivate_filter values for PID (in, I,
out)";
+parse_pgm_inst_t cmd_derivate_filter = {
+ .f = cmd_derivate_filter_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_derivate_filter,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_derivate_filter_arg1,
+ (prog_void *)&cmd_derivate_filter_size,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_derivate_filter_show_arg[] = "show";
+parse_pgm_token_string_t cmd_derivate_filter_show_arg =
TOKEN_STRING_INITIALIZER(struct cmd_derivate_filter_result, arg1,
str_derivate_filter_show_arg);
+
+prog_char help_derivate_filter_show[] = "Show derivate_filter values for PID";
+parse_pgm_inst_t cmd_derivate_filter_show = {
+ .f = cmd_derivate_filter_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_derivate_filter_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_derivate_filter_arg0,
+ (prog_void *)&cmd_derivate_filter_show_arg,
+ NULL,
+ },
+};
+
@@ -688,78 +753,6 @@
},
};
-
-/**********************************************************/
-/* Blocking_Se for control system */
-
-/* this structure is filled when cmd_blocking_se is parsed successfully */
-struct cmd_blocking_se_result {
- fixed_string_t arg0;
- fixed_string_t arg1;
- uint32_t s;
- uint32_t e;
- uint16_t cpt;
-};
-
-/* function called when cmd_blocking_se is parsed successfully */
-static void cmd_blocking_se_parsed(void * parsed_result, void * data)
-{
- struct cmd_blocking_se_result * res = parsed_result;
-
- if (!strcmp_P(res->arg1, PSTR("angle"))) {
- bd_set_speed_err_thresholds(&robot.bd_a, res->s, res->e,
res->cpt);
- }
- else if (!strcmp_P(res->arg1, PSTR("distance"))) {
- bd_set_speed_err_thresholds(&robot.bd_d, res->s, res->e,
res->cpt);
- }
- /* else it's a "show" */
-
- printf_P(PSTR("blocking_se angle %ld %ld %d\r\n"),
robot.bd_a.speed_thres,
- robot.bd_a.err_thres, robot.bd_a.cpt1_thres);
- printf_P(PSTR("blocking_se distance %ld %ld %d\r\n"),
robot.bd_d.speed_thres,
- robot.bd_d.err_thres, robot.bd_d.cpt1_thres);
-}
-
-prog_char str_blocking_se_arg0[] = "blocking_se";
-parse_pgm_token_string_t cmd_blocking_se_arg0 =
TOKEN_STRING_INITIALIZER(struct cmd_blocking_se_result, arg0,
str_blocking_se_arg0);
-prog_char str_blocking_se_arg1[] = "angle#distance";
-parse_pgm_token_string_t cmd_blocking_se_arg1 =
TOKEN_STRING_INITIALIZER(struct cmd_blocking_se_result, arg1,
str_blocking_se_arg1);
-parse_pgm_token_num_t cmd_blocking_se_s = TOKEN_NUM_INITIALIZER(struct
cmd_blocking_se_result, s, UINT32);
-parse_pgm_token_num_t cmd_blocking_se_e = TOKEN_NUM_INITIALIZER(struct
cmd_blocking_se_result, e, UINT32);
-parse_pgm_token_num_t cmd_blocking_se_cpt = TOKEN_NUM_INITIALIZER(struct
cmd_blocking_se_result, cpt, UINT16);
-
-prog_char help_blocking_se[] = "Set blocking detection values (speed, error,
cpt)";
-parse_pgm_inst_t cmd_blocking_se = {
- .f = cmd_blocking_se_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_blocking_se,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_blocking_se_arg0,
- (prog_void *)&cmd_blocking_se_arg1,
- (prog_void *)&cmd_blocking_se_s,
- (prog_void *)&cmd_blocking_se_e,
- (prog_void *)&cmd_blocking_se_cpt,
- NULL,
- },
-};
-
-/* show */
-
-prog_char str_blocking_se_show_arg[] = "show";
-parse_pgm_token_string_t cmd_blocking_se_show_arg =
TOKEN_STRING_INITIALIZER(struct cmd_blocking_se_result, arg1,
str_blocking_se_show_arg);
-
-prog_char help_blocking_se_show[] = "Show blocking_se detection values";
-parse_pgm_inst_t cmd_blocking_se_show = {
- .f = cmd_blocking_se_parsed, /* function to call */
- .data = NULL, /* 2nd arg of func */
- .help_str = help_blocking_se_show,
- .tokens = { /* token list, NULL terminated */
- (prog_void *)&cmd_blocking_se_arg0,
- (prog_void *)&cmd_blocking_se_show_arg,
- NULL,
- },
-};
-
/**********************************************************/
/* Blocking_I for control system */
@@ -778,23 +771,23 @@
{
struct cmd_blocking_i_result * res = parsed_result;
- if (!strcmp_P(res->arg1, PSTR("angle"))) {
- bd_set_current_thresholds(&robot.bd_a, res->k1, res->k2,
res->i, res->cpt);
+ if (!strcmp_P(res->arg1, PSTR("left"))) {
+ bd_set_current_thresholds(&robot.bd_l, res->k1, res->k2,
res->i, res->cpt);
}
- else if (!strcmp_P(res->arg1, PSTR("distance"))) {
- bd_set_current_thresholds(&robot.bd_d, res->k1, res->k2,
res->i, res->cpt);
+ else if (!strcmp_P(res->arg1, PSTR("right"))) {
+ bd_set_current_thresholds(&robot.bd_r, res->k1, res->k2,
res->i, res->cpt);
}
/* else it's a "show" */
- printf_P(PSTR("blocking_i angle %ld %ld %ld %d\r\n"),
- robot.bd_a.k1, robot.bd_a.k2, robot.bd_a.i_thres,
robot.bd_a.cpt2_thres);
- printf_P(PSTR("blocking_i distance %ld %ld %ld %d\r\n"),
- robot.bd_d.k1, robot.bd_d.k2, robot.bd_d.i_thres,
robot.bd_d.cpt2_thres);
+ printf_P(PSTR("blocking left %ld %ld %ld %d\r\n"),
+ robot.bd_l.k1, robot.bd_l.k2, robot.bd_l.i_thres,
robot.bd_l.cpt_thres);
+ printf_P(PSTR("blocking right %ld %ld %ld %d\r\n"),
+ robot.bd_r.k1, robot.bd_r.k2, robot.bd_r.i_thres,
robot.bd_r.cpt_thres);
}
-prog_char str_blocking_i_arg0[] = "blocking_i";
+prog_char str_blocking_i_arg0[] = "blocking";
parse_pgm_token_string_t cmd_blocking_i_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_blocking_i_result, arg0, str_blocking_i_arg0);
-prog_char str_blocking_i_arg1[] = "angle#distance";
+prog_char str_blocking_i_arg1[] = "left#right";
parse_pgm_token_string_t cmd_blocking_i_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_blocking_i_result, arg1, str_blocking_i_arg1);
parse_pgm_token_num_t cmd_blocking_i_k1 = TOKEN_NUM_INITIALIZER(struct
cmd_blocking_i_result, k1, INT32);
parse_pgm_token_num_t cmd_blocking_i_k2 = TOKEN_NUM_INITIALIZER(struct
cmd_blocking_i_result, k2, INT32);
@@ -822,7 +815,7 @@
prog_char str_blocking_i_show_arg[] = "show";
parse_pgm_token_string_t cmd_blocking_i_show_arg =
TOKEN_STRING_INITIALIZER(struct cmd_blocking_i_result, arg1,
str_blocking_i_show_arg);
-prog_char help_blocking_i_show[] = "Show blocking_i detection values";
+prog_char help_blocking_i_show[] = "Show blocking detection values";
parse_pgm_inst_t cmd_blocking_i_show = {
.f = cmd_blocking_i_parsed, /* function to call */
.data = NULL, /* 2nd arg of func */
@@ -1608,11 +1601,14 @@
else if (!strcmp_P(res->name, PSTR("eject"))) {
strat_eject_balls();
}
+ if (!strcmp_P(res->name, PSTR("position"))) {
+ strat_position();
+ }
}
prog_char str_substrat_arg0[] = "substrat";
parse_pgm_token_string_t cmd_substrat_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_substrat_result, arg0, str_substrat_arg0);
-prog_char str_substrat_name[] = "shot#eject";
+prog_char str_substrat_name[] = "shot#eject#position";
parse_pgm_token_string_t cmd_substrat_name = TOKEN_STRING_INITIALIZER(struct
cmd_substrat_result, name, str_substrat_name);
prog_char help_substrat[] = "Test sub-strats";
@@ -1650,6 +1646,7 @@
static const prog_char i2cproto_log[] = "i2cproto";
static const prog_char ext_log[] = "ext";
static const prog_char sensor_log[] = "sensor";
+static const prog_char block_log[] = "bd";
struct log_name_and_num {
const prog_char * name;
@@ -1667,6 +1664,7 @@
{ i2cproto_log, E_USER_I2C_PROTO },
{ ext_log, E_USER_FROM_EXT_BOARD },
{ sensor_log, E_USER_SENSOR },
+ { block_log, E_BLOCKING_DETECTION_MANAGER },
};
static uint8_t
@@ -1815,7 +1813,7 @@
prog_char str_log_arg1_type[] = "type";
parse_pgm_token_string_t cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct
cmd_log_type_result, arg1, str_log_arg1_type);
/* keep it sync with log_name_and_num above */
-prog_char str_log_arg2_type[] =
"uart#rs#servo#traj#i2c#oa#strat#i2cproto#ext#sensor";
+prog_char str_log_arg2_type[] =
"uart#rs#servo#traj#i2c#oa#strat#i2cproto#ext#sensor#bd";
parse_pgm_token_string_t cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct
cmd_log_type_result, arg2, str_log_arg2_type);
prog_char str_log_arg3[] = "on#off";
parse_pgm_token_string_t cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct
cmd_log_type_result, arg3, str_log_arg3);
@@ -2023,8 +2021,8 @@
/* stop motors */
robot.cs_events &= (~DO_CS);
- pwm_set(LEFT_PWM, 0);
- pwm_set(RIGHT_PWM, 00);
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
while(1) {
if (print & PRINT_POS) {
@@ -2075,12 +2073,12 @@
case 'q':
trajectory_hardstop(&robot.traj);
- pwm_set(LEFT_PWM, 0);
- pwm_set(RIGHT_PWM, 0);
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
return;
case ' ':
- pwm_set(LEFT_PWM, 0);
- pwm_set(RIGHT_PWM, 0);
+ pwm_set_and_save(LEFT_PWM, 0);
+ pwm_set_and_save(RIGHT_PWM, 0);
break;
default:
break;
@@ -2089,20 +2087,20 @@
else {
switch(cmd) {
case KEY_UP_ARR:
- pwm_set(LEFT_PWM, 1200);
- pwm_set(RIGHT_PWM, 1200);
+ pwm_set_and_save(LEFT_PWM, 1200);
+ pwm_set_and_save(RIGHT_PWM, 1200);
break;
case KEY_LEFT_ARR:
- pwm_set(LEFT_PWM, -1200);
- pwm_set(RIGHT_PWM, 1200);
+ pwm_set_and_save(LEFT_PWM, -1200);
+ pwm_set_and_save(RIGHT_PWM, 1200);
break;
case KEY_DOWN_ARR:
- pwm_set(LEFT_PWM, -1200);
- pwm_set(RIGHT_PWM, -1200);
+ pwm_set_and_save(LEFT_PWM, -1200);
+ pwm_set_and_save(RIGHT_PWM, -1200);
break;
case KEY_RIGHT_ARR:
- pwm_set(LEFT_PWM, 1200);
- pwm_set(RIGHT_PWM, -1200);
+ pwm_set_and_save(LEFT_PWM, 1200);
+ pwm_set_and_save(RIGHT_PWM, -1200);
break;
}
}
@@ -2459,6 +2457,8 @@
(parse_pgm_inst_t *)&cmd_gain_show,
(parse_pgm_inst_t *)&cmd_maximum,
(parse_pgm_inst_t *)&cmd_maximum_show,
+ (parse_pgm_inst_t *)&cmd_derivate_filter,
+ (parse_pgm_inst_t *)&cmd_derivate_filter_show,
(parse_pgm_inst_t *)&cmd_quadramp,
(parse_pgm_inst_t *)&cmd_quadramp_show,
(parse_pgm_inst_t *)&cmd_encoders,
@@ -2485,8 +2485,6 @@
(parse_pgm_inst_t *)&cmd_goto3,
(parse_pgm_inst_t *)&cmd_speed,
(parse_pgm_inst_t *)&cmd_speed_show,
- (parse_pgm_inst_t *)&cmd_blocking_se,
- (parse_pgm_inst_t *)&cmd_blocking_se_show,
(parse_pgm_inst_t *)&cmd_blocking_i,
(parse_pgm_inst_t *)&cmd_blocking_i_show,
(parse_pgm_inst_t *)&cmd_pt_list,
========================================
aversive_projects/microb2008/main/main.c (1.24 -> 1.25)
========================================
@@ -15,7 +15,7 @@
* 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: main.c,v 1.24 2008-03-29 18:53:12 zer0 Exp $
+ * Revision : $Id: main.c,v 1.25 2008-03-30 22:02:53 zer0 Exp $
*
*/
@@ -141,8 +141,13 @@
}
if(robot.cs_events & DO_BD) {
- bd_manage(&robot.bd_a);
- bd_manage(&robot.bd_d);
+ bd_manage_from_pos_cmd(&robot.bd_l,
+
encoders_microb_get_value(LEFT_ENCODER_EXT),
+ robot.pwm_l);
+ /* right encoder is inverted */
+ bd_manage_from_pos_cmd(&robot.bd_r,
+
-encoders_microb_get_value(RIGHT_ENCODER_EXT),
+ robot.pwm_r);
}
#if 0
@@ -161,6 +166,16 @@
cpt++;
}
+/* used to process blocking detection on wheels motors */
+void pwm_set_and_save(void *enc, int32_t val)
+{
+ if (enc == LEFT_PWM)
+ robot.pwm_l = val;
+ else if (enc == RIGHT_PWM)
+ robot.pwm_r = val;
+ pwm_set(enc, val);
+}
+
void main_timer_interrupt(void)
{
static uint8_t cpt = 0;
@@ -401,16 +416,16 @@
/* ROBOT_SYSTEM */
rs_init(&robot.rs);
- rs_set_left_pwm(&robot.rs, pwm_set, LEFT_PWM);
- rs_set_right_pwm(&robot.rs, pwm_set, RIGHT_PWM);
+ rs_set_left_pwm(&robot.rs, pwm_set_and_save, LEFT_PWM);
+ rs_set_right_pwm(&robot.rs, pwm_set_and_save, RIGHT_PWM);
/* increase gain to decrease dist */
/* increase left and it will turn more left */
/* EXT encoders */
- rs_set_right_ext_encoder(&robot.rs, encoders_microb_get_value,
- RIGHT_ENCODER_EXT, -1.0); //en augmentant on
tourne à gauche
rs_set_left_ext_encoder(&robot.rs, encoders_microb_get_value,
LEFT_ENCODER_EXT, 1.0); // en augmentant on
tourne à droite
+ rs_set_right_ext_encoder(&robot.rs, encoders_microb_get_value,
+ RIGHT_ENCODER_EXT, -1.0); //en augmentant on
tourne à gauche
/* MOTORS encoders, nothing: we only use external encoders */
rs_set_flags(&robot.rs, 1); /* rs use ext encoders */
@@ -423,7 +438,7 @@
/* CS DISTANCE */
pid_init(&robot.pid_d);
- pid_set_gains(&robot.pid_d, 500, 7, 13000);
+ pid_set_gains(&robot.pid_d, 400, 5, 11000);
pid_set_maximums(&robot.pid_d, 0, 50000, 4095);
pid_set_out_shift(&robot.pid_d, 6);
pid_set_derivate_filter(&robot.pid_d, 6);
@@ -441,7 +456,7 @@
/* CS ANGLE */
pid_init(&robot.pid_a);
- pid_set_gains(&robot.pid_a, 800, 5, 13000);
+ pid_set_gains(&robot.pid_a, 500, 5, 7000);
pid_set_maximums(&robot.pid_a, 0, 100000, 4095);
pid_set_out_shift(&robot.pid_a, 6);
pid_set_derivate_filter(&robot.pid_a, 6);
@@ -466,10 +481,10 @@
trajectory_set_windows(&robot.traj, 20.0, 10.0, 50.0);
/* Blocking detection */
- bd_init(&robot.bd_a, &robot.cs_a);
- bd_set_speed_err_thresholds(&robot.bd_a, 100, 10000, 20);
- bd_init(&robot.bd_d, &robot.cs_d);
- bd_set_speed_err_thresholds(&robot.bd_d, 100, 10000, 20);
+ bd_init(&robot.bd_l);
+ bd_set_current_thresholds(&robot.bd_l, 500, 3000, 1200000, 60);
+ bd_init(&robot.bd_r);
+ bd_set_current_thresholds(&robot.bd_r, 500, 3000, 1200000, 60);
/* CS EVENT */
scheduler_add_periodical_event_priority(do_cs, NULL,
========================================
aversive_projects/microb2008/main/main.h (1.13 -> 1.14)
========================================
@@ -15,7 +15,7 @@
* 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: main.h,v 1.13 2008-03-27 09:55:49 zer0 Exp $
+ * Revision : $Id: main.h,v 1.14 2008-03-30 22:02:53 zer0 Exp $
*
*/
@@ -96,8 +96,10 @@
struct quadramp_filter qr_d;
struct trajectory traj;
- struct blocking_detection bd_a;
- struct blocking_detection bd_d;
+ struct blocking_detection bd_l;
+ struct blocking_detection bd_r;
+ int32_t pwm_l;
+ int32_t pwm_r;
int16_t opponent_x;
int16_t opponent_y;
@@ -137,6 +139,7 @@
void do_cs(void * dummy);
+void pwm_set_and_save(void * enc, int32_t val);
/* LEDS */
=========================================
aversive_projects/microb2008/main/strat.c (1.6 -> 1.7)
=========================================
@@ -15,7 +15,7 @@
* 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.c,v 1.6 2008-03-29 18:53:12 zer0 Exp $
+ * Revision : $Id: strat.c,v 1.7 2008-03-30 22:02:53 zer0 Exp $
*
*/
@@ -43,10 +43,19 @@
struct v_dispenser {
int8_t count;
uint8_t tries;
+ uint8_t (*ball_count)(void);
};
-static struct v_dispenser colored_disp;
-static struct v_dispenser white_disp;
+static struct v_dispenser colored_disp = {
+ .count = 5,
+ .tries = 0,
+ .ball_count = barrel_colored_count,
+};
+static struct v_dispenser white_disp = {
+ .count = 5,
+ .tries = 0,
+ .ball_count = barrel_white_count,
+};
static uint8_t h_disp_enabled;
@@ -54,7 +63,7 @@
void strat_init(void)
{
/* we consider that the color is correctly set */
- position_set(&robot.pos, START_X, START_Y, START_A);
+ //position_set(&robot.pos, START_X, START_Y, START_A);
i2c_set_roller(1);
i2c_set_arm(I2C_EXTENSION_ARM_HARVEST);
trajectory_set_speed(&robot.traj, 300, 300);
@@ -73,79 +82,180 @@
robot.cs_events &= DO_TIMER;
}
+void strat_position(void)
+{
+ trajectory_set_speed(&robot.traj, 100, 100);
+
+ trajectory_d_rel(&robot.traj, -20);
+ wait_traj_end(END_TRAJ|END_BLOCKING);
+ wait_ms(100);
+
+ position_set(&robot.pos, 16, 0, 0);
+
+ trajectory_d_rel(&robot.traj, 10);
+ wait_traj_end(END_TRAJ);
+
+ trajectory_a_rel(&robot.traj, 90);
+ wait_traj_end(END_TRAJ);
+
+ trajectory_d_rel(&robot.traj, -20);
+ wait_traj_end(END_TRAJ|END_BLOCKING);
+ wait_ms(100);
+
+ position_set(&robot.pos, position_get_x_s16(&robot.pos), 16, 90);
+
+ trajectory_d_rel(&robot.traj, 10);
+ wait_traj_end(END_TRAJ);
+ wait_ms(500);
+
+ trajectory_a_abs(&robot.traj, 45);
+ wait_traj_end(END_TRAJ);
+ wait_ms(500);
+
+ trajectory_d_rel(&robot.traj, -4);
+ wait_traj_end(END_TRAJ);
+}
+
+uint8_t strat_unblock(void)
+{
+ uint8_t flags;
+ int32_t pwm_l, pwm_r;
+
+ DEBUG(E_USER_STRAT, "%s", __FUNCTION__);
+ IRQ_LOCK(flags);
+ pwm_l = robot.pwm_l;
+ pwm_r = robot.pwm_r;
+ IRQ_UNLOCK(flags);
+
+ if (pwm_l > 0 || pwm_r > 0) {
+ trajectory_d_rel(&robot.traj, -10);
+ wait_ms(500);
+ }
+ else {
+ trajectory_d_rel(&robot.traj, 10);
+ wait_ms(500);
+ }
+ trajectory_hardstop(&robot.traj);
+ wait_ms(1000);
+ return END_TRAJ;
+}
+
/* get static balls on the game area
* Return values:
* - END_TRAJ: we browsed the area successfully,
* - ERROR: Cannot finish the trajectory */
static uint8_t strat_get_static_balls(void)
{
+ uint8_t err;
DEBUG(E_USER_STRAT, "%s", __FUNCTION__);
trajectory_goto_forward_xy_abs(&robot.traj, BALL7_X, BALL7_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
trajectory_goto_forward_xy_abs(&robot.traj, BALL11_X, BALL11_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
trajectory_goto_forward_xy_abs(&robot.traj, BALL9_X, BALL9_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
trajectory_goto_forward_xy_abs(&robot.traj, BALL5_X, BALL5_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
trajectory_goto_forward_xy_abs(&robot.traj, BALL3_X, BALL3_Y);
- wait_traj_end(END_TRAJ);
- trajectory_goto_forward_xy_abs(&robot.traj, BALL1_X, BALL1_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
+ trajectory_goto_forward_xy_abs(&robot.traj, BALL1_X,
+ BALL1_Y + COL_SIGN(10));
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
trajectory_goto_forward_xy_abs(&robot.traj, BALL4_X, BALL4_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
return END_TRAJ;
}
-#define PICKUP_TIMEOUT (2000000L) /* 2 seconds */
+#define PICKUP_TIMEOUT (3000000L) /* 3 seconds */
/* get balls from dispenser */
static uint8_t strat_pickup_v_disp(struct v_dispenser *disp)
{
- uint8_t prev_count;
+ uint8_t initial_count;
+ uint8_t prev_retrieved_balls = 0;
+ uint8_t retrieved_balls = 0;
microseconds us;
if (disp->tries >= DISP_MAX_TRIES || disp->count == 0)
return END_TRAJ;
disp->tries++;
- prev_count = barrel_colored_count();
+ initial_count = disp->ball_count();
+ retrieved_balls = disp->ball_count() - initial_count;
+ prev_retrieved_balls = retrieved_balls;
i2c_pickup();
/* retrieve balls */
us = time_get_us2();
- while (!barrel_full()) {
+ while (1) {
+
+ retrieved_balls = disp->ball_count() - initial_count;
/* we got a ball */
- if (barrel_colored_count() != prev_count) {
+ if (retrieved_balls != prev_retrieved_balls) {
us = time_get_us2(); /* reinit timeout */
- disp->count --;
- prev_count = barrel_colored_count();
+ prev_retrieved_balls = retrieved_balls;
}
/* no ball left */
- if (disp->count == 0) {
+ if (retrieved_balls == disp->count) {
DEBUG(E_USER_STRAT, "%s: all balls have been
retrieved");
- return END_TRAJ;
+ disp->count = 0;
+ break;
}
/* cannot get any ball, ok we will try later */
if ((time_get_us2() - us) > PICKUP_TIMEOUT) {
+ DEBUG(E_USER_STRAT, "%s: timeout");
+ break;
+ }
+
+ if (barrel_full()) {
DEBUG(E_USER_STRAT, "%s: barrel full");
- return END_TRAJ;
+ break;
}
}
- DEBUG(E_USER_STRAT, "%s: barrel full");
+ retrieved_balls = disp->ball_count() - initial_count;
+ disp->count -= retrieved_balls;
+ if (disp->count < 0)
+ disp->count = 0;
+
+ trajectory_d_rel(&robot.traj, -10);
+ wait_traj_end(END_TRAJ|END_BLOCKING);
+
return END_TRAJ;
}
/* */
static uint8_t strat_get_white_v_disp(void)
{
+ uint8_t err;
+
if (white_disp.count <= 0)
return END_TRAJ;
@@ -155,13 +265,24 @@
DEBUG(E_USER_STRAT, "%s, %d balls remaining", __FUNCTION__,
white_disp.count);
- /* XXX check traj failures */
- trajectory_goto_xy_abs(&robot.traj, V_WHI_DISP_X, PREPARE_V_WHI_DISP_Y);
- wait_traj_end(END_TRAJ);
+ trajectory_goto_xy_abs(&robot.traj, PREPARE_V_WHI_DISP_X, V_WHI_DISP_Y);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
trajectory_a_abs(&robot.traj, V_WHI_DISP_A);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
+ trajectory_set_speed(&robot.traj, 100, 300);
+
trajectory_goto_xy_abs(&robot.traj, V_WHI_DISP_X, V_WHI_DISP_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ DEBUG(E_USER_STRAT, "%s end=%d", __FUNCTION__, err);
+
+ trajectory_d_rel(&robot.traj, -1);
+ trajectory_set_speed(&robot.traj, 300, 300);
strat_pickup_v_disp(&white_disp);
@@ -171,6 +292,8 @@
/* */
static uint8_t strat_get_colored_v_disp(void)
{
+ uint8_t err;
+
if (colored_disp.count <= 0)
return END_TRAJ;
@@ -180,14 +303,25 @@
DEBUG(E_USER_STRAT, "%s, %d balls remaining", __FUNCTION__,
colored_disp.count);
- /* XXX check traj failures */
trajectory_goto_xy_abs(&robot.traj, V_COL_DISP_X, PREPARE_V_COL_DISP_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
trajectory_a_abs(&robot.traj, V_COL_DISP_A);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
+ trajectory_set_speed(&robot.traj, 100, 300);
+
trajectory_goto_xy_abs(&robot.traj, V_COL_DISP_X, V_COL_DISP_Y);
- wait_traj_end(END_TRAJ);
-
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ DEBUG(E_USER_STRAT, "%s end=%d", __FUNCTION__, err);
+
+ trajectory_d_rel(&robot.traj, -1);
+ trajectory_set_speed(&robot.traj, 300, 300);
+
strat_pickup_v_disp(&colored_disp);
return END_TRAJ;
@@ -196,6 +330,8 @@
/* shot balls */
uint8_t strat_shot_balls(void)
{
+ uint8_t err;
+
/* XXX improve it here, use force and time */
if (barrel_colored_count() == 0)
return END_TRAJ;
@@ -208,12 +344,15 @@
if (!can_shot()) {
/* go to the center of area */
trajectory_goto_xy_abs(&robot.traj, CENTER_X, CENTER_Y);
- while(test_traj_end(END_TRAJ) == 0 && !can_shot());
+ while(test_traj_end(END_TRAJ|END_BLOCKING) == 0 && !can_shot());
/* XXX process blocking */
}
trajectory_turnto_xy_behind(&robot.traj, GOAL_X, GOAL_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
while (barrel_colored_count()) {
while(extension_state() != I2C_EXTENSION_STATE_DROP_READY);
@@ -229,6 +368,7 @@
/* eject balls */
uint8_t strat_eject_balls(void)
{
+ uint8_t err;
uint8_t col;
uint8_t side;
@@ -252,11 +392,20 @@
/* XXX go forward ? and check blocking ... */
trajectory_goto_xy_abs(&robot.traj, STD_CONT_X_MID, PREPARE_STD_CONT_Y);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
trajectory_goto_xy_abs(&robot.traj, STD_CONT_X_MID, STD_CONT_Y);
- wait_traj_end(END_TRAJ);
+ wait_traj_end(END_TRAJ|END_BLOCKING);
+ trajectory_d_rel(&robot.traj, -5);
+ wait_traj_end(END_TRAJ|END_BLOCKING);
+
trajectory_a_abs(&robot.traj, STD_CONT_A);
- wait_traj_end(END_TRAJ);
+ err = wait_traj_end(END_TRAJ|END_BLOCKING);
+ if (err == END_BLOCKING)
+ return strat_unblock();
+
/* XXX do something better for order */
while (col != I2C_BALL_TYPE_EMPTY) {
@@ -312,6 +461,33 @@
*
*/
+ /* not much time left ! */
+ if (time_get_s() > MATCH_TIME - 10) {
+ if (barrel_white_count() != 0) {
+ DEBUG(E_USER_STRAT, "%s not much time, eject balls",
__FUNCTION__);
+ return strat_eject_balls();
+ }
+ else if (barrel_colored_count() != 0) {
+ DEBUG(E_USER_STRAT, "%s not much time, shot balls",
__FUNCTION__);
+ return strat_shot_balls();
+ }
+ }
+
+ /* if we are close of std container, eject balls in it */
+ if (distance_from_robot(STD_CONT_X_MID, STD_CONT_Y) < 60) {
+ DEBUG(E_USER_STRAT, "%s we are close of std cont, eject",
__FUNCTION__);
+ return strat_eject_balls();
+ }
+
+#if 0
+ /* if we are close of col disp, */
+ if (distance_from_robot(V_COL_DISP_X, V_COL_DISP_Y) < 60) {
+ if (barrel_empty_count()*2 + barrel_colored_count() >= 5) {
+
+ }
+ }
+#endif
+
/* XXX currently very bof */
if (barrel_white_count() < 3)
return strat_shot_balls();
@@ -390,13 +566,15 @@
strat_drop_balls();
/* si on a une chance s'y retourner sans encombres, on retente
*/
- if (err == END_TRAJ && colored_disp.count != 0)
+ if (err == END_TRAJ && colored_disp.count != 0 &&
+ colored_disp.tries < DISP_MAX_TRIES)
continue;
err = strat_get_white_v_disp();
strat_drop_balls();
- if (white_disp.count != 0)
+ if (white_disp.count != 0 &&
+ white_disp.tries < DISP_MAX_TRIES)
continue;
/* faut-il le faire tout le temps ? ca peut dependre
=========================================
aversive_projects/microb2008/main/strat.h (1.3 -> 1.4)
=========================================
@@ -15,7 +15,7 @@
* 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.h,v 1.3 2008-03-29 18:53:12 zer0 Exp $
+ * Revision : $Id: strat.h,v 1.4 2008-03-30 22:02:53 zer0 Exp $
*
*/
@@ -34,13 +34,13 @@
#define V_COL_DISP_X 70
#define PREPARE_V_COL_DISP_Y COL_COORD_Y(45)
-#define V_COL_DISP_Y COL_COORD_Y(25)
+#define V_COL_DISP_Y COL_COORD_Y(20)
#define V_COL_DISP_A COL_ANGLE(-90)
-#define V_WHI_DISP_X 70
-#define PREPARE_V_WHI_DISP_Y COL_COORD_Y(45)
-#define V_WHI_DISP_Y COL_COORD_Y(25)
-#define V_WHI_DISP_A COL_ANGLE(-90)
+#define V_WHI_DISP_X 20
+#define PREPARE_V_WHI_DISP_X 45
+#define V_WHI_DISP_Y COL_COORD_Y(75)
+#define V_WHI_DISP_A COL_ANGLE(180)
#define GOAL_X 300
#define GOAL_Y COL_COORD_Y(185)
@@ -51,7 +51,7 @@
#define STD_CONT_X_MAX 300
#define STD_CONT_X_MID ((STD_CONT_X_MIN+STD_CONT_X_MAX)/2)
#define PREPARE_STD_CONT_Y COL_COORD_Y(160)
-#define STD_CONT_Y COL_COORD_Y(190)
+#define STD_CONT_Y COL_COORD_Y(200)
#define STD_CONT_A COL_ANGLE(0)
/* from left to right, and from up to down */
@@ -98,6 +98,7 @@
void strat_uninit(void);
uint8_t strat_main(void);
+void strat_position(void);
uint8_t strat_shot_balls(void);
uint8_t strat_eject_balls(void);
==============================================
aversive_projects/microb2008/main/strat_base.c (1.8 -> 1.9)
==============================================
@@ -15,7 +15,7 @@
* 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_base.c,v 1.8 2008-03-29 18:53:12 zer0 Exp $
+ * Revision : $Id: strat_base.c,v 1.9 2008-03-30 22:02:53 zer0 Exp $
*
*/
#include <stdio.h>
@@ -77,7 +77,7 @@
{ .cm=160, .speed=825 },
{ .cm=200, .speed=850 },
{ .cm=250, .speed=925 },
- { .cm=275, .speed=950 },
+ // { .cm=275, .speed=925 },
};
/* get the best shot parameters depending on distance.
@@ -276,6 +276,12 @@
{
return robot.white_ball_count;
}
+
+uint8_t barrel_empty_count(void)
+{
+ return 5-robot.white_ball_count-robot.colored_ball_count;
+}
+
uint8_t extension_state(void)
{
return robot.extension_state;
@@ -360,10 +366,10 @@
if ((why & END_NEAR) && trajectory_in_window(&robot.traj))
return END_NEAR;
- if( (why & END_BLOCKING) && bd_get(&robot.bd_a) )
+ if( (why & END_BLOCKING) && bd_get(&robot.bd_l) )
return END_BLOCKING;
- if( (why & END_BLOCKING) && bd_get(&robot.bd_d) )
+ if( (why & END_BLOCKING) && bd_get(&robot.bd_r) )
return END_BLOCKING;
if( (why & END_OBSTACLE) && sensor_obstacle() ) {
==============================================
aversive_projects/microb2008/main/strat_base.h (1.6 -> 1.7)
==============================================
@@ -15,7 +15,7 @@
* 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_base.h,v 1.6 2008-03-29 18:53:12 zer0 Exp $
+ * Revision : $Id: strat_base.h,v 1.7 2008-03-30 22:02:53 zer0 Exp $
*
*/
@@ -36,6 +36,7 @@
#define COL_COORD_Y(y) ((robot.our_color==RED)? (y) : (AREA_Y-y))
#define COL_ANGLE(a) ((robot.our_color==RED)? (a) : (-a))
+#define COL_SIGN(x) ((robot.our_color==RED)? (x) : (-x))
void set_opponent_pos(int16_t x, int16_t y);
int8_t goto_and_avoid(int16_t x, int16_t y);
@@ -69,6 +70,7 @@
uint8_t barrel_full(void);
uint8_t barrel_colored_count(void);
uint8_t barrel_white_count(void);
+uint8_t barrel_empty_count(void);
uint8_t extension_state(void);
int16_t get_catapult_speed(int32_t cm);
_______________________________________________
Avr-list mailing list
Avr-list@droids-corp.org
CVSWEB : http://cvsweb.droids-corp.org/cgi-bin/viewcvs.cgi/aversive
WIKI : http://wiki.droids-corp.org/index.php/Aversive
DOXYGEN : http://zer0.droids-corp.org/doxygen_aversive/html/
BUGZILLA : http://bugzilla.droids-corp.org
COMMIT LOGS : http://zer0.droids-corp.org/aversive_commitlog
