Commit from zer0 on branch b_zer0 (2008-04-06 19:33 CEST)
=================================
fix some bugs in trajectory, pwm, position_manager
aversive config/Configure.help
1.13.4.14
aversive config/config.in
1.42.4.19
aversive config/prog_fuses.sh
1.3.4.5
aversive mk/aversive_project.mk
1.32.4.13
aversive
modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
1.1.2.4
aversive modules/devices/robot/position_manager/position_manager.c
1.6.4.4
aversive modules/devices/robot/robot_system/robot_system.c
1.6.4.6
aversive modules/devices/robot/robot_system/robot_system.h
1.5.4.3
aversive modules/devices/robot/trajectory_manager/trajectory_manager.c
1.4.4.11
aversive modules/hardware/pwm/pwm.c
1.8.4.8
aversive modules/hardware/pwm/pwm.h
1.7.4.7
==============================
aversive/config/Configure.help (1.13.4.13 -> 1.13.4.14)
==============================
@@ -216,6 +216,10 @@
wheels. It provides a virtual angle/distance PWM and a virtual
angle/distance encoder.
+CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
+ If the robot has external and motor encoder, you can use both
+ by defining this option.
+
CONFIG_MODULE_POSITION_MANAGER
This module processes the position of the robot, depending of the
value returned by the associated robot system, and some physical
=========================
aversive/config/config.in (1.42.4.18 -> 1.42.4.19)
=========================
@@ -318,6 +318,9 @@
dep_bool 'Robot System' CONFIG_MODULE_ROBOT_SYSTEM \
$CONFIG_MODULE_FIXED_POINT
+dep_bool ' |-- Allow motor and external encoders'
CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT \
+ $CONFIG_MODULE_ROBOT_SYSTEM
+
#### POSITION_MANAGER
dep_bool 'Position manager' CONFIG_MODULE_POSITION_MANAGER \
$CONFIG_MODULE_ROBOT_SYSTEM
=============================
aversive/config/prog_fuses.sh (1.3.4.4 -> 1.3.4.5)
=============================
@@ -140,7 +140,7 @@
do
rm -f $f 2> /dev/null
echo 0x00 > ${f}_new
- ${AVRDUDE} -p ${MCU} -P `echo ${AVRDUDE_PORT} | sed 's,",,g'` -c
${AVRDUDE_PROGRAMMER} -U ${f}:r:${f}:i ${DELAY}
+ ${AVRDUDE} ${DELAY} -p ${MCU} -P `echo ${AVRDUDE_PORT} | sed 's,",,g'` -c
${AVRDUDE_PROGRAMMER} -U ${f}:r:${f}:i
if [ ! -f $f ]; then
CANNOT_READ=1
fi
===============================
aversive/mk/aversive_project.mk (1.32.4.12 -> 1.32.4.13)
===============================
@@ -111,7 +111,7 @@
AVRDUDE_WRITE_FLASH = -U flash:w:$(TARGET).$(FORMAT_EXTENSION)
#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
-AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER) -q
+AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)
ifneq ($(AVRDUDE_DELAY),)
AVRDUDE_FLAGS += -i $(AVRDUDE_DELAY)
======================================================================================
aversive/modules/devices/robot/blocking_detection_manager/blocking_detection_manager.c
(1.1.2.3 -> 1.1.2.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: blocking_detection_manager.c,v 1.1.2.3 2008-03-30 22:00:56
zer0 Exp $
+ * Revision : $Id: blocking_detection_manager.c,v 1.1.2.4 2008-04-06 17:33:57
zer0 Exp $
*
* Olivier MATZ <[EMAIL PROTECTED]>
*/
@@ -74,7 +74,7 @@
i = bd->k1 * cmd - bd->k2 * speed;
if (ABS(i) > bd->i_thres) {
if (bd->cpt == bd->cpt_thres - 1)
- DEBUG(E_BLOCKING_DETECTION_MANAGER,
+ WARNING(E_BLOCKING_DETECTION_MANAGER,
"BLOCKING cmd=%ld, speed=%ld i=%ld",
cmd, speed, i);
if(bd->cpt < bd->cpt_thres)
==================================================================
aversive/modules/devices/robot/position_manager/position_manager.c (1.6.4.3 ->
1.6.4.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: position_manager.c,v 1.6.4.3 2008-03-31 16:51:27 zer0 Exp $
+ * Revision : $Id: position_manager.c,v 1.6.4.4 2008-04-06 17:33:57 zer0 Exp $
*
*/
@@ -89,6 +89,7 @@
IRQ_UNLOCK(flags);
}
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
void position_use_mot(struct robot_position * pos)
{
struct rs_polar encoders;
@@ -101,6 +102,9 @@
pos->use_ext = 0;
IRQ_UNLOCK(flags);
}
+#endif
+
+extern int8_t toto;
/**
* Process the absolute position (x,y,a) depending on the delta on
@@ -125,6 +129,7 @@
IRQ_UNLOCK(flags);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
if(pos->use_ext) {
encoders.distance = rs_get_ext_distance(rs);
encoders.angle = rs_get_ext_angle(rs);
@@ -133,6 +138,10 @@
encoders.distance = rs_get_mot_distance(rs);
encoders.angle = rs_get_mot_angle(rs);
}
+#else
+ encoders.distance = rs_get_ext_distance(rs);
+ encoders.angle = rs_get_ext_angle(rs);
+#endif
/* process difference between 2 measures.
* No lock for prev_encoders since we are the only one to use
@@ -149,23 +158,29 @@
y = pos->pos_d.y;
IRQ_UNLOCK(flags);
-
- if (delta.angle == 0) {
- /* we go straight */
- x = x + cos(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_cm)) ;
- y = y + sin(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_cm)) ;
- }
- else {
- /* r the radius of the circle arc */
- r = (double)delta.distance * pos->phys.track_cm / (double)
delta.angle;
- arc_angle = (double) delta.angle / (pos->phys.track_cm *
pos->phys.distance_imp_per_cm);
-
- x += r * (-sin(a) + sin(a+arc_angle));
- y += r * (cos(a) - cos(a+arc_angle));
- a += arc_angle;
+ if (toto) {
+ a += 2 * (double) delta.angle / (pos->phys.track_cm *
pos->phys.distance_imp_per_cm);
+ x += cos(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_cm)) ;
+ y += sin(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_cm)) ;
+ } else {
+
+ if (delta.angle == 0) {
+ /* we go straight */
+ x = x + cos(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_cm)) ;
+ y = y + sin(a) * ((double) delta.distance /
(pos->phys.distance_imp_per_cm)) ;
+ }
+ else {
+ /* r the radius of the circle arc */
+ r = (double)delta.distance * pos->phys.track_cm /
((double) delta.angle * 2);
+ arc_angle = 2 * (double) delta.angle /
(pos->phys.track_cm * pos->phys.distance_imp_per_cm);
+
+ x += r * (-sin(a) + sin(a+arc_angle));
+ y += r * (cos(a) - cos(a+arc_angle));
+ a += arc_angle;
+ }
}
- if(a < -M_PI)
+ if (a < -M_PI)
a += (M_PI*2);
else if (a > (M_PI))
a -= (M_PI*2);
==========================================================
aversive/modules/devices/robot/robot_system/robot_system.c (1.6.4.5 -> 1.6.4.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: robot_system.c,v 1.6.4.5 2007-12-31 16:25:01 zer0 Exp $
+ * Revision : $Id: robot_system.c,v 1.6.4.6 2008-04-06 17:33:57 zer0 Exp $
*
*/
@@ -90,10 +90,13 @@
IRQ_LOCK(flags);
memset(rs, 0, sizeof(struct robot_system));
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
rs_set_ratio(rs, 1.0);
+#endif
IRQ_UNLOCK(flags);
}
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
/** define ratio between mot and ext track. (track_mot / track_ext) */
void rs_set_ratio(struct robot_system * rs, double ratio)
{
@@ -103,6 +106,7 @@
rs->ratio_mot_ext = f64_from_double(ratio);
IRQ_UNLOCK(flags);
}
+#endif
/** define left PWM function and param */
void rs_set_left_pwm(struct robot_system * rs, void (*left_pwm)(void *,
int32_t), void *left_pwm_param)
@@ -126,6 +130,7 @@
IRQ_UNLOCK(flags);
}
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
/** define left motor encoder function and param */
void rs_set_left_mot_encoder(struct robot_system * rs, int32_t
(*left_mot_encoder)(void *),
void *left_mot_encoder_param, double gain)
@@ -151,6 +156,7 @@
rs->right_mot_gain = f64_from_double(gain);
IRQ_UNLOCK(flags);
}
+#endif
/** define left external encoder function and param */
void rs_set_left_ext_encoder(struct robot_system * rs, int32_t
(*left_ext_encoder)(void *),
@@ -280,6 +286,7 @@
return distance;
}
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
int32_t rs_get_mot_angle(void * data)
{
struct robot_system * rs = data;
@@ -303,6 +310,7 @@
IRQ_UNLOCK(flags);
return distance;
}
+#endif
int32_t rs_get_ext_left(void * data)
{
@@ -328,6 +336,7 @@
return right;
}
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
int32_t rs_get_mot_left(void * data)
{
struct robot_system * rs = data;
@@ -351,6 +360,7 @@
IRQ_UNLOCK(flags);
return right;
}
+#endif
void rs_set_flags(struct robot_system * rs, uint8_t flags)
{
@@ -370,8 +380,12 @@
void rs_update(void * data)
{
struct robot_system * rs = data;
- struct rs_wheels wext, wmot;
- struct rs_polar pext, pmot;
+ struct rs_wheels wext;
+ struct rs_polar pext;
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
+ struct rs_wheels wmot;
+ struct rs_polar pmot;
+#endif
int32_t delta_angle, delta_distance;
uint8_t flags;
@@ -379,36 +393,49 @@
wext.left = safe_getencoder(rs->left_ext_encoder,
rs->left_ext_encoder_param);
wext.right = safe_getencoder(rs->right_ext_encoder,
rs->right_ext_encoder_param);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
wmot.left = safe_getencoder(rs->left_mot_encoder,
rs->left_mot_encoder_param);
wmot.right = safe_getencoder(rs->right_mot_encoder,
rs->right_mot_encoder_param);
+#endif
- DEBUG(E_ROBOT_SYSTEM, "ENCODERS : %ld %ld %ld %ld", wmot.left,
wmot.right, wext.left, wext.right);
-
/* apply gains to each wheel */
- wext.left = f64_msb_mul(f64_from_lsb(wext.left), rs->left_ext_gain);
- wext.right = f64_msb_mul(f64_from_lsb(wext.right), rs->right_ext_gain);
+ if( ! (rs->flags & RS_IGNORE_EXT_GAIN ) ) {
+ wext.left = f64_msb_mul(f64_from_lsb(wext.left),
rs->left_ext_gain);
+ wext.right = f64_msb_mul(f64_from_lsb(wext.right),
rs->right_ext_gain);
+ }
- wmot.left = f64_msb_mul(f64_from_lsb(wmot.left), rs->left_mot_gain);
- wmot.right = f64_msb_mul(f64_from_lsb(wmot.right), rs->right_mot_gain);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
+ if( ! (rs->flags & RS_IGNORE_MOT_GAIN ) ) {
+ wmot.left = f64_msb_mul(f64_from_lsb(wmot.left),
rs->left_mot_gain);
+ wmot.right = f64_msb_mul(f64_from_lsb(wmot.right),
rs->right_mot_gain);
+ }
+#endif
rs_get_polar_from_wheels(&pext, &wext);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
rs_get_polar_from_wheels(&pmot, &wmot);
/* apply ratio to polar and reupdate wheels for ext coders */
pext.angle = f64_msb_mul(f64_from_lsb(pext.angle), rs->ratio_mot_ext);
+#endif
rs_get_wheels_from_polar(&wext, &pext);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
/* update from external encoders */
if( rs->flags & RS_USE_EXT ) {
- delta_angle = pext.angle - rs->pext_prev.angle;
+ delta_angle = pext.angle - rs->pext_prev.angle;
delta_distance = pext.distance - rs->pext_prev.distance;
}
/* update from motor encoders */
else {
- delta_angle = pmot.angle - rs->pmot_prev.angle;
+ delta_angle = pmot.angle - rs->pmot_prev.angle;
delta_distance = pmot.distance - rs->pmot_prev.distance;
}
+#else
+ delta_angle = pext.angle - rs->pext_prev.angle;
+ delta_distance = pext.distance - rs->pext_prev.distance;
+#endif
IRQ_LOCK(flags);
rs->virtual_encoders.angle += delta_angle;
@@ -419,11 +446,13 @@
IRQ_LOCK(flags);
rs->pext_prev = pext;
- rs->pmot_prev = pmot;
+ rs->wext_prev = wext;
IRQ_UNLOCK(flags);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
IRQ_LOCK(flags);
- rs->wext_prev = wext;
+ rs->pmot_prev = pmot;
rs->wmot_prev = wmot;
IRQ_UNLOCK(flags);
+#endif
}
==========================================================
aversive/modules/devices/robot/robot_system/robot_system.h (1.5.4.2 -> 1.5.4.3)
==========================================================
@@ -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: robot_system.h,v 1.5.4.2 2007-05-23 17:18:14 zer0 Exp $
+ * Revision : $Id: robot_system.h,v 1.5.4.3 2008-04-06 17:33:57 zer0 Exp $
*
*/
@@ -36,46 +36,45 @@
/* flags */
#define RS_USE_EXT 1
+#define RS_IGNORE_EXT_GAIN 2
+#define RS_IGNORE_MOT_GAIN 4
struct robot_system
{
uint8_t flags;
- struct rs_polar pmot_prev;
- struct rs_polar pext_prev;
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
+ struct rs_polar pmot_prev;
struct rs_wheels wmot_prev;
- struct rs_wheels wext_prev;
- struct rs_polar virtual_pwm;
- struct rs_polar virtual_encoders;
f64 ratio_mot_ext;
- /* blocking */
- uint16_t nb_blocking;
- uint16_t nb_blocking_max;
- uint16_t nb_blocking_detect;
- int32_t blocking_min_speed;
- double blocking_ext_mot_ratio;
-
/* Motor encoders */
int32_t (*left_mot_encoder)(void *);
void* left_mot_encoder_param;
- f64 left_ext_gain;
+ f64 left_mot_gain;
int32_t (*right_mot_encoder)(void *);
void* right_mot_encoder_param;
- f64 right_ext_gain;
-
+ f64 right_mot_gain;
+#endif
+
+ struct rs_polar virtual_pwm;
+ struct rs_polar virtual_encoders;
+
+ struct rs_polar pext_prev;
+ struct rs_wheels wext_prev;
+
/* External encoders */
int32_t (*left_ext_encoder)(void *);
void* left_ext_encoder_param;
- f64 left_mot_gain;
+ f64 left_ext_gain;
int32_t (*right_ext_encoder)(void *);
void* right_ext_encoder_param;
- f64 right_mot_gain;
-
+ f64 right_ext_gain;
+
/* PWM */
void (*left_pwm)(void *, int32_t);
void *left_pwm_param;
@@ -88,24 +87,10 @@
/**** ACCESSORS */
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
/** define ratio between mot and ext track. (track_mot / track_ext) */
void rs_set_ratio(struct robot_system * rs, double ratio);
-
-/** define blocking detection params */
-void rs_set_blocking(struct robot_system * rs,
- uint16_t nb_blocking_max,
- uint16_t nb_blocking_detect,
- int32_t blocking_min_speed,
- double blocking_ext_mot_ratio);
-
-/** reset blocking counter */
-void rs_reset_blocking(struct robot_system * rs);
-
-/** return blocking counter */
-uint16_t rs_get_blocking(struct robot_system * rs);
-
-/** true if robot is blocked */
-uint8_t rs_is_blocked(struct robot_system * rs);
+#endif
/** define left PWM function and param */
void rs_set_left_pwm(struct robot_system * rs, void (*left_pwm)(void *,
int32_t), void *left_pwm_param);
@@ -113,6 +98,7 @@
/** define right PWM function and param */
void rs_set_right_pwm(struct robot_system * rs, void (*right_pwm)(void *,
int32_t), void *right_pwm_param);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
/** define left motor encoder function and param */
void rs_set_left_mot_encoder(struct robot_system * rs, int32_t
(*left_mot_encoder)(void *),
void *left_mot_encoder_param, double gain);
@@ -120,6 +106,7 @@
/** define right motor encoder function and param */
void rs_set_right_mot_encoder(struct robot_system * rs, int32_t
(*right_mot_encoder)(void *),
void *right_mot_encoder_param, double gain);
+#endif
/** define left external encoder function and param */
void rs_set_left_ext_encoder(struct robot_system * rs, int32_t
(*left_ext_encoder)(void *),
@@ -163,6 +150,7 @@
*/
int32_t rs_get_ext_distance(void * rs);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
/**
* get the angle according to mot encoders value.
*/
@@ -172,12 +160,15 @@
* get the distance according to mot encoders value.
*/
int32_t rs_get_mot_distance(void * rs);
+#endif
/* same for left/right */
int32_t rs_get_ext_left(void * rs);
int32_t rs_get_ext_right(void * rs);
+#ifdef CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT
int32_t rs_get_mot_left(void * rs);
int32_t rs_get_mot_right(void * rs);
+#endif
/**
======================================================================
aversive/modules/devices/robot/trajectory_manager/trajectory_manager.c
(1.4.4.10 -> 1.4.4.11)
======================================================================
@@ -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: trajectory_manager.c,v 1.4.4.10 2008-04-01 22:45:09 zer0
Exp $
+ * Revision : $Id: trajectory_manager.c,v 1.4.4.11 2008-04-06 17:33:57 zer0
Exp $
*
*/
@@ -243,7 +243,7 @@
}
else {
a_consign = (int32_t)(a_rad *
(traj->position->phys.distance_imp_per_cm) *
- (traj->position->phys.track_cm) /
2.0);
+ (traj->position->phys.track_cm) /
2);
traj->target.pol.angle = a_consign;
}
cs_set_consign(traj->csm_angle, a_consign +
rs_get_angle(traj->robot));
@@ -532,10 +532,10 @@
d_consign += rs_get_distance(traj->robot);
/* angle consign */
- /* XXX here we specify 1.1 instead of 1.0 to avoid oscillations
*/
+ /* XXX here we specify 2.2 instead of 2.0 to avoid oscillations
*/
a_consign = (int32_t)(v2pol_target.theta *
-
(traj->position->phys.distance_imp_per_cm)*
- (traj->position->phys.track_cm) / 1.1);
+
(traj->position->phys.distance_imp_per_cm) *
+ (traj->position->phys.track_cm) / 2.2);
a_consign += rs_get_angle(traj->robot);
break;
@@ -591,13 +591,13 @@
/* step 3 : send the processed commands to cs */
- DEBUG(E_TRAJECTORY, "EVENT XY cart=(%f,%f) pol=(%f,%f)",
- v2cart_pos.x, v2cart_pos.y, v2pol_target.r, v2pol_target.theta);
+ DEBUG(E_TRAJECTORY, "EVENT XY cur=(%f,%f,%f) cart=(%f,%f) pol=(%f,%f)",
+ x, y, a, v2cart_pos.x, v2cart_pos.y, v2pol_target.r,
v2pol_target.theta);
- DEBUG(E_TRAJECTORY,"d_consign=%" PRIi32 ", d_speed=%" PRIi32 ", "
- "a_consign=%" PRIi32 ", a_speed=%" PRIi32,
- d_consign, get_quadramp_distance_speed(traj),
- a_consign, get_quadramp_angle_speed(traj));
+ DEBUG(E_TRAJECTORY,"d_cur=%" PRIi32 ", d_consign=%" PRIi32 ",
d_speed=%" PRIi32 ", "
+ "a_cur=%" PRIi32 ", a_consign=%" PRIi32 ", a_speed=%" PRIi32,
+ rs_get_distance(traj->robot), d_consign,
get_quadramp_distance_speed(traj),
+ rs_get_angle(traj->robot), a_consign,
get_quadramp_angle_speed(traj));
cs_set_consign(traj->csm_angle, a_consign);
cs_set_consign(traj->csm_distance, d_consign);
===================================
aversive/modules/hardware/pwm/pwm.c (1.8.4.7 -> 1.8.4.8)
===================================
@@ -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: pwm.c,v 1.8.4.7 2007-11-15 11:15:37 zer0 Exp $
+ * Revision : $Id: pwm.c,v 1.8.4.8 2008-04-06 17:33:57 zer0 Exp $
*
*/
@@ -286,7 +286,7 @@
pwm_invert_value(mode, value); \
} \
else { \
- pwm##n##_sign_reset(); \
+ pwm##n##_sign_reset(); \
} \
} \
else { \
===================================
aversive/modules/hardware/pwm/pwm.h (1.7.4.6 -> 1.7.4.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: pwm.h,v 1.7.4.6 2007-09-06 08:18:22 zer0 Exp $
+ * Revision : $Id: pwm.h,v 1.7.4.7 2008-04-06 17:33:57 zer0 Exp $
*
*/
@@ -51,7 +51,7 @@
/** value to be used for limiting inputs */
#define PWM_MAX ((1<< PWM_SIGNIFICANT_BITS)-1)
-#define PWM_MIN (-(1<< PWM_SIGNIFICANT_BITS))
+#define PWM_MIN (-PWM_MAX)
/** global functions*/
Commit from zer0 (2008-04-06 19:33 CEST)
================
fix some bugs in trajectory, pwm, position_manager
aversive_projects microb2008/main/.config 1.15
aversive_projects microb2008/main/Makefile 1.6
aversive_projects microb2008/main/commands.c 1.22
aversive_projects microb2008/main/main.c 1.30
aversive_projects microb2008/main/main.h 1.18
aversive_projects microb2008/main/sensor.c 1.5
aversive_projects microb2008/main/strat.c 1.14
=========================================
aversive_projects/microb2008/main/.config (1.14 -> 1.15)
=========================================
@@ -147,6 +147,7 @@
# Robot specific modules
#
CONFIG_MODULE_ROBOT_SYSTEM=y
+# CONFIG_MODULE_ROBOT_SYSTEM_MOT_AND_EXT is not set
CONFIG_MODULE_POSITION_MANAGER=y
CONFIG_MODULE_TRAJECTORY_MANAGER=y
CONFIG_MODULE_BLOCKING_DETECTION_MANAGER=y
==========================================
aversive_projects/microb2008/main/Makefile (1.5 -> 1.6)
==========================================
@@ -18,6 +18,8 @@
# care about how the name is spelled on its command-line.
ASRC =
+AVRDUDE_DELAY=5
+
########################################
-include .aversive_conf
============================================
aversive_projects/microb2008/main/commands.c (1.21 -> 1.22)
============================================
@@ -361,6 +361,73 @@
},
};
+/**********************************************************/
+/* rs_gains window configuration */
+
+/* this structure is filled when cmd_rs_gains is parsed successfully */
+struct cmd_rs_gains_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ float left;
+ float right;
+};
+
+/* function called when cmd_rs_gains is parsed successfully */
+static void cmd_rs_gains_parsed(void * parsed_result, void * data)
+{
+ struct cmd_rs_gains_result * res = parsed_result;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ rs_set_left_ext_encoder(&robot.rs, encoders_microb_get_value,
+ LEFT_ENCODER_EXT, res->left); // en
augmentant on tourne à gauche
+ rs_set_right_ext_encoder(&robot.rs, encoders_microb_get_value,
+ RIGHT_ENCODER_EXT, res->right); //en
augmentant on tourne à droite
+ }
+ printf_P(PSTR("rs_gains set "));
+ f64_print(robot.rs.left_ext_gain);
+ printf_P(PSTR(" "));
+ f64_print(robot.rs.right_ext_gain);
+ printf_P(PSTR("\r\n"));
+}
+
+prog_char str_rs_gains_arg0[] = "rs_gains";
+parse_pgm_token_string_t cmd_rs_gains_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_rs_gains_result, arg0, str_rs_gains_arg0);
+prog_char str_rs_gains_arg1[] = "set";
+parse_pgm_token_string_t cmd_rs_gains_arg1 = TOKEN_STRING_INITIALIZER(struct
cmd_rs_gains_result, arg1, str_rs_gains_arg1);
+parse_pgm_token_num_t cmd_rs_gains_l = TOKEN_NUM_INITIALIZER(struct
cmd_rs_gains_result, left, FLOAT);
+parse_pgm_token_num_t cmd_rs_gains_r = TOKEN_NUM_INITIALIZER(struct
cmd_rs_gains_result, right, FLOAT);
+
+prog_char help_rs_gains[] = "Set rs_gains (left, right)";
+parse_pgm_inst_t cmd_rs_gains = {
+ .f = cmd_rs_gains_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rs_gains,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_rs_gains_arg0,
+ (prog_void *)&cmd_rs_gains_arg1,
+ (prog_void *)&cmd_rs_gains_l,
+ (prog_void *)&cmd_rs_gains_r,
+ NULL,
+ },
+};
+
+/* show */
+
+prog_char str_rs_gains_show_arg[] = "show";
+parse_pgm_token_string_t cmd_rs_gains_show_arg =
TOKEN_STRING_INITIALIZER(struct cmd_rs_gains_result, arg1,
str_rs_gains_show_arg);
+
+prog_char help_rs_gains_show[] = "Show rs_gains";
+parse_pgm_inst_t cmd_rs_gains_show = {
+ .f = cmd_rs_gains_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rs_gains_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_rs_gains_arg0,
+ (prog_void *)&cmd_rs_gains_show_arg,
+ NULL,
+ },
+};
+
/**********************************************************/
/* Pt_Lists for testing traj */
@@ -926,7 +993,8 @@
static void cmd_goto_parsed(void * parsed_result, void * data)
{
struct cmd_goto_result * res = parsed_result;
-
+ uint8_t err;
+
interrupt_traj_reset();
if (!strcmp_P(res->arg1, PSTR("a_rel"))) {
trajectory_a_rel(&robot.traj, res->arg2);
@@ -962,8 +1030,9 @@
printf_P(PSTR("not implemented\r\n"));
//trajectory_goto_xya(&robot.traj, res->arg2, res->arg3,
res->arg4);
}
- wait_traj_end(0xFF);
- trajectory_stop(&robot.traj);
+ err = wait_traj_end(0xFF);
+ if (err != END_TRAJ && err != END_NEAR)
+ trajectory_stop(&robot.traj);
}
prog_char str_goto_arg0[] = "goto";
@@ -1057,22 +1126,16 @@
/* display encoders from robot_system left/right */
else if (!strcmp_P(res->arg1, PSTR("rs_lr"))) {
while(uart0_recv_nowait() == -1) {
- struct rs_wheels wext, wmot, wvir;
- struct rs_polar pext, pmot, pvir;
+ struct rs_wheels wext;
+ struct rs_polar pext;
pext.distance = rs_get_ext_distance(&robot.rs);
pext.angle = rs_get_ext_angle(&robot.rs);
- pmot.distance = rs_get_mot_distance(&robot.rs);
- pmot.angle = rs_get_mot_angle(&robot.rs);
- pvir.distance = rs_get_distance(&robot.rs);
- pvir.angle = rs_get_angle(&robot.rs);
rs_get_wheels_from_polar(&wext, &pext);
- rs_get_wheels_from_polar(&wmot, &pmot);
- rs_get_wheels_from_polar(&wvir, &pvir);
- printf_P(PSTR("ext l=% .8ld r=% .8ld | mot l=% .8ld r=%
.8ld | vir l=% .8ld r=% .8ld\r\n"),
- wext.left, wext.right, wmot.left, wmot.right,
wvir.left, wvir.right);
+ printf_P(PSTR("ext l=% .8ld r=% .8ld\r\n"),
+ wext.left, wext.right);
wait_ms(100);
}
}
@@ -1080,13 +1143,9 @@
/* display encoders from robot_system distance/angle */
else if (!strcmp_P(res->arg1, PSTR("rs_da"))) {
while(uart0_recv_nowait() == -1) {
- printf_P(PSTR("ext d=% .8ld a=% .8ld | mot d=% .8ld a=%
.8ld | vir d=% .8ld a=% .8ld\r\n"),
+ printf_P(PSTR("ext d=% .8ld a=% .8ld\r\n"),
rs_get_ext_distance(&robot.rs),
- rs_get_ext_angle(&robot.rs),
- rs_get_mot_distance(&robot.rs),
- rs_get_mot_angle(&robot.rs),
- rs_get_distance(&robot.rs),
- rs_get_angle(&robot.rs));
+ rs_get_ext_angle(&robot.rs));
}
}
}
@@ -1190,7 +1249,7 @@
static void auto_position(void)
{
- trajectory_set_speed(&robot.traj, 100, 100);
+ trajectory_set_speed(&robot.traj, 400, 400);
trajectory_d_rel(&robot.traj, -20);
wait_traj_end(END_TRAJ|END_BLOCKING);
@@ -1326,48 +1385,30 @@
/**********************************************************/
/* Test */
+int8_t toto = 0;
+int8_t modulo = 2;
+
/* this structure is filled when cmd_test is parsed successfully */
struct cmd_test_result {
fixed_string_t arg0;
+ int8_t arg1;
+ int8_t arg2;
};
/* function called when cmd_test is parsed successfully */
-static void cmd_test_parsed(void * parsed_result, void * data)
+static void cmd_test_parsed(void *parsed_result, void *data)
{
-#define MIN_SHOT_DIST 60
-#define MAX_SHOT_DIST 275
- double posx = position_get_x_double(&robot.pos);
- double posy = position_get_y_double(&robot.pos);
- uint16_t dist_to_container;
- double goal_angle_width;
-
- dist_to_container = distance_from_robot(GOAL_X, GOAL_Y);
-
- if (dist_to_container < MIN_SHOT_DIST) {
- printf("too close\n");
- return;
- }
+ struct cmd_test_result *res = parsed_result;
- if (dist_to_container > MAX_SHOT_DIST) {
- printf("too far\n");
- return;
- }
-
- goal_angle_width = atan2(GOAL_Y+GOAL_HALF_WIDTH - posy, GOAL_X - posx) -
- atan2(GOAL_Y-GOAL_HALF_WIDTH - posy, GOAL_X - posx);
- goal_angle_width = (180. / M_PI) * ABS(goal_angle_width);
-
- /* we want at least 10° */
- if (goal_angle_width < 10.) {
- printf("bad angle %2.2f\n", goal_angle_width);
- return;
- }
-
- printf("can shot\n");
+ toto = res->arg1;
+ modulo = res->arg2;
+ printf_P(PSTR("use_seg=%d, modulo=%d, %d\r\n"), toto, modulo,
res->arg2);
}
prog_char str_test_arg0[] = "test";
parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_test_result, arg0, str_test_arg0);
+parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct
cmd_position_result, arg1, INT8);
+parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct
cmd_position_result, arg2, INT8);
prog_char help_test[] = "Test function";
parse_pgm_inst_t cmd_test = {
@@ -1376,6 +1417,8 @@
.help_str = help_test,
.tokens = { /* token list, NULL terminated */
(prog_void *)&cmd_test_arg0,
+ (prog_void *)&cmd_test_arg1,
+ (prog_void *)&cmd_test_arg2,
NULL,
},
};
@@ -1650,6 +1693,7 @@
{
struct cmd_cam_result *res = parsed_result;
int8_t n;
+ uint8_t err;
if (!strcmp_P(res->type, PSTR("init"))) {
cam_init();
@@ -1674,8 +1718,9 @@
trajectory_goto_d_a_rel(&robot.traj,
cam_ball_tab[0].distance,
cam_ball_tab[0].angle);
- wait_traj_end(0xFF);
- trajectory_stop(&robot.traj);
+ err = wait_traj_end(0xFF);
+ if (err != END_TRAJ && err != END_NEAR)
+ trajectory_stop(&robot.traj);
}
}
}
@@ -1706,42 +1751,46 @@
fixed_string_t name;
};
-void test_square(void)
+void test_square(uint8_t flags)
{
+ uint8_t i = 0;
position_set(&robot.pos, 0, 0, 0);
- while(uart0_recv_nowait() == -1) {
+ while(uart0_recv_nowait() == -1 && i<10) {
trajectory_goto_forward_xy_abs(&robot.traj, 0, 60);
- wait_traj_end(0xFF);
+ wait_traj_end(flags);
trajectory_goto_forward_xy_abs(&robot.traj, 60, 60);
- wait_traj_end(0xFF);
+ wait_traj_end(flags);
trajectory_goto_forward_xy_abs(&robot.traj, 60, 0);
- wait_traj_end(0xFF);
+ wait_traj_end(flags);
trajectory_goto_forward_xy_abs(&robot.traj, 0, 0);
- wait_traj_end(0xFF);
+ wait_traj_end(flags);
+ i++;
}
}
-void test_hourglass(void)
+void test_hourglass(uint8_t flags)
{
+ uint8_t i = 0;
position_set(&robot.pos, 0, 0, 0);
- while(uart0_recv_nowait() == -1) {
+ while(uart0_recv_nowait() == -1 && i<10) {
trajectory_goto_forward_xy_abs(&robot.traj, 60, 60);
- wait_traj_end(0xFF);
+ wait_traj_end(flags);
trajectory_goto_forward_xy_abs(&robot.traj, 0, 60);
- wait_traj_end(0xFF);
+ wait_traj_end(flags);
trajectory_goto_forward_xy_abs(&robot.traj, 60, 0);
- wait_traj_end(0xFF);
+ wait_traj_end(flags);
trajectory_goto_forward_xy_abs(&robot.traj, 0, 0);
- wait_traj_end(0xFF);
- }
+ wait_traj_end(flags);
+ i++;
+ }
}
@@ -1759,16 +1808,22 @@
strat_eject_balls();
}
else if (!strcmp_P(res->name, PSTR("square"))) {
- test_square();
+ test_square(END_TRAJ|END_BLOCKING|END_INTR);
}
else if (!strcmp_P(res->name, PSTR("hourglass"))) {
- test_hourglass();
+ test_hourglass(END_TRAJ|END_BLOCKING|END_INTR);
+ }
+ else if (!strcmp_P(res->name, PSTR("square_near"))) {
+ test_square(0xFF);
+ }
+ else if (!strcmp_P(res->name, PSTR("hourglass_near"))) {
+ test_hourglass(0xFF);
}
}
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#square#hourglass";
+prog_char str_substrat_name[] =
"shot#eject#square#hourglass#square_near#hourglass_near";
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";
@@ -1992,6 +2047,61 @@
},
};
+
+/**********************************************************/
+/* Strat_Params */
+
+
+/* this structure is filled when cmd_strat_params is parsed successfully */
+struct cmd_strat_params_type_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+};
+
+/* function called when cmd_strat_params is parsed successfully */
+static void cmd_strat_params_type_parsed(void * parsed_result, void * data)
+{
+ // struct cmd_strat_params_type_result *res = parsed_result;
+
+}
+
+prog_char str_strat_params_arg0[] = "strat_params";
+parse_pgm_token_string_t cmd_strat_params_arg0 =
TOKEN_STRING_INITIALIZER(struct cmd_strat_params_type_result, arg0,
str_strat_params_arg0);
+prog_char str_strat_params_arg1_type[] =
"h_disp#static_balls#col_v_disp#white_v_disp";
+parse_pgm_token_string_t cmd_strat_params_arg1_type =
TOKEN_STRING_INITIALIZER(struct cmd_strat_params_type_result, arg1,
str_strat_params_arg1_type);
+prog_char str_strat_params_arg2[] = "on#off";
+parse_pgm_token_string_t cmd_strat_params_arg2 =
TOKEN_STRING_INITIALIZER(struct cmd_strat_params_type_result, arg2,
str_strat_params_arg2);
+
+prog_char help_strat_params_type[] = "Set strat_params";
+parse_pgm_inst_t cmd_strat_params_type = {
+ .f = cmd_strat_params_type_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_params_type,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_params_arg0,
+ (prog_void *)&cmd_strat_params_arg1_type,
+ (prog_void *)&cmd_strat_params_arg2,
+ NULL,
+ },
+};
+
+prog_char str_strat_params_arg1_show[] = "show";
+parse_pgm_token_string_t cmd_strat_params_arg1_show =
TOKEN_STRING_INITIALIZER(struct cmd_strat_params_type_result, arg1,
str_strat_params_arg1_type);
+
+prog_char help_strat_params_show[] = "Show strat_params";
+parse_pgm_inst_t cmd_strat_params_show = {
+ .f = cmd_strat_params_type_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_strat_params_show,
+ .tokens = { /* token list, NULL terminated */
+ (prog_void *)&cmd_strat_params_arg0,
+ (prog_void *)&cmd_strat_params_arg1_show,
+ NULL,
+ },
+};
+
+
/**********************************************************/
/* Debug */
@@ -2636,6 +2746,8 @@
(parse_pgm_inst_t *)&cmd_log,
(parse_pgm_inst_t *)&cmd_log_show,
(parse_pgm_inst_t *)&cmd_log_type,
+ (parse_pgm_inst_t *)&cmd_strat_params_type,
+ (parse_pgm_inst_t *)&cmd_strat_params_show,
(parse_pgm_inst_t *)&cmd_debug,
(parse_pgm_inst_t *)&cmd_start,
(parse_pgm_inst_t *)&cmd_cam,
@@ -2654,6 +2766,8 @@
(parse_pgm_inst_t *)&cmd_pt_list_show,
(parse_pgm_inst_t *)&cmd_trajectory,
(parse_pgm_inst_t *)&cmd_trajectory_show,
+ (parse_pgm_inst_t *)&cmd_rs_gains,
+ (parse_pgm_inst_t *)&cmd_rs_gains_show,
(parse_pgm_inst_t *)&cmd_opponent,
(parse_pgm_inst_t *)&cmd_opponent_set,
(parse_pgm_inst_t *)&cmd_extension,
========================================
aversive_projects/microb2008/main/main.c (1.29 -> 1.30)
========================================
@@ -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.29 2008-04-04 08:24:15 zer0 Exp $
+ * Revision : $Id: main.c,v 1.30 2008-04-06 17:33:57 zer0 Exp $
*
*/
@@ -83,6 +83,7 @@
a = !a;
#endif
}
+extern int8_t modulo;
/* called every 5 ms */
void do_cs(void * dummy)
@@ -92,6 +93,10 @@
/* used for cs debugging */
static uint32_t debug_cs_cpt = 0;
uint8_t flags;
+#if DEBUG_TIME == 1
+ uint8_t old_time;
+ uint8_t rs_time=0, cs_time=0, pos_time=0;
+#endif
if (robot.cs_events & DO_POWER)
BRAKE_OFF();
@@ -99,11 +104,25 @@
BRAKE_ON();
if(robot.cs_events & DO_RS) {
+#if DEBUG_TIME == 1
+ old_time = TCNT2;
+#endif
+ /* about 0.5ms */
rs_update(&robot.rs);
+#if DEBUG_TIME == 1
+ rs_time = TCNT2 - old_time;
+#endif
}
if(robot.cs_events & DO_CS) {
+#if DEBUG_TIME == 1
+ old_time = TCNT2;
+#endif
+ /* about 2ms in worst case for both */
cs_manage(&robot.cs_a);
cs_manage(&robot.cs_d);
+#if DEBUG_TIME == 1
+ cs_time = TCNT2 - old_time;
+#endif
if (robot.debug & DEBUG_CS) {
debug_cs_cpt++;
@@ -136,8 +155,15 @@
}
- if((cpt % 4 == 0) && robot.cs_events & DO_POS) {
+ if(((cpt % modulo) == 0) && robot.cs_events & DO_POS) {
+#if DEBUG_TIME == 1
+ old_time = TCNT2;
+#endif
+ /* about 1.5ms in worst case */
position_manage(&robot.pos);
+#if DEBUG_TIME == 1
+ pos_time = TCNT2 - old_time;
+#endif
}
if(robot.cs_events & DO_BD) {
@@ -151,7 +177,11 @@
}
if (cpt % 512 == 0) {
+#if DEBUG_TIME == 1
+ printf("rs=%d, cs=%d, pos=%d\n", rs_time, cs_time, pos_time);
+#endif
DEBUG(E_USER_STRAT, "time %d, position=%d,%d,%d",
+ (int16_t)time_get_s(),
position_get_x_s16(&robot.pos),
position_get_y_s16(&robot.pos),
position_get_a_deg_s16(&robot.pos));
@@ -176,6 +206,11 @@
/* used to process blocking detection on wheels motors */
void pwm_set_and_save(void *enc, int32_t val)
{
+ if (val > 4095)
+ val = 4095;
+ if (val < -4095)
+ val = -4095;
+
if (enc == LEFT_PWM)
robot.pwm_l = val;
else if (enc == RIGHT_PWM)
@@ -212,7 +247,7 @@
u16 stream_flags = stdout->flags;
uint8_t i;
- if (e->severity < ERROR_SEVERITY_ERROR) {
+ if (e->severity > ERROR_SEVERITY_ERROR) {
if (robot.log_level < e->severity)
return;
@@ -376,10 +411,10 @@
const char * history;
/* check eeprom to avoid to run the bad program */
- if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
+ if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
EEPROM_MAGIC_MAIN)
while(1);
-
+
sbi(DDR(LED1PORT), LED1BIT);
sbi(DDR(LED2PORT), LED2BIT);
sbi(DDR(LED3PORT), LED3BIT);
@@ -437,12 +472,11 @@
/* increase left and it will turn more left */
/* EXT encoders */
rs_set_left_ext_encoder(&robot.rs, encoders_microb_get_value,
- LEFT_ENCODER_EXT, 1.0); // en augmentant on
tourne à droite
+ LEFT_ENCODER_EXT, 9.9909); // en augmentant on
tourne à gauche
rs_set_right_ext_encoder(&robot.rs, encoders_microb_get_value,
- RIGHT_ENCODER_EXT, -1.0); //en augmentant on
tourne à gauche
+ RIGHT_ENCODER_EXT, -10.0361); //en augmentant
on tourne à droite
- /* MOTORS encoders, nothing: we only use external encoders */
- rs_set_flags(&robot.rs, 1); /* rs use ext encoders */
+ rs_set_flags(&robot.rs, RS_USE_EXT); /* rs use ext encoders */
/* POSITION MANAGER */
position_init(&robot.pos);
@@ -452,14 +486,14 @@
/* CS DISTANCE */
pid_init(&robot.pid_d);
- pid_set_gains(&robot.pid_d, 400, 5, 11000);
+ pid_set_gains(&robot.pid_d, 20, 2, 800);
pid_set_maximums(&robot.pid_d, 0, 50000, 0);
pid_set_out_shift(&robot.pid_d, 6);
pid_set_derivate_filter(&robot.pid_d, 6);
quadramp_init(&robot.qr_d);
- quadramp_set_1st_order_vars(&robot.qr_d, 300, 300); /* set speed */
- quadramp_set_2nd_order_vars(&robot.qr_d, 10, 10); /* set accel */
+ quadramp_set_1st_order_vars(&robot.qr_d, 2500, 2500); /* set speed */
+ quadramp_set_2nd_order_vars(&robot.qr_d, 20, 20); /* set accel */
cs_init(&robot.cs_d);
cs_set_consign_filter(&robot.cs_d, quadramp_do_filter, &robot.qr_d);
@@ -470,14 +504,14 @@
/* CS ANGLE */
pid_init(&robot.pid_a);
- pid_set_gains(&robot.pid_a, 500, 5, 7000);
+ pid_set_gains(&robot.pid_a, 20, 2, 1000);
pid_set_maximums(&robot.pid_a, 0, 100000, 0);
pid_set_out_shift(&robot.pid_a, 6);
pid_set_derivate_filter(&robot.pid_a, 6);
quadramp_init(&robot.qr_a);
- quadramp_set_1st_order_vars(&robot.qr_a, 300, 300); /* set speed */
- quadramp_set_2nd_order_vars(&robot.qr_a, 3, 3); /* set accel */
+ quadramp_set_1st_order_vars(&robot.qr_a, 2000, 2000); /* set speed */
+ quadramp_set_2nd_order_vars(&robot.qr_a, 20, 20); /* set accel */
cs_init(&robot.cs_a);
cs_set_consign_filter(&robot.cs_a, quadramp_do_filter, &robot.qr_a);
@@ -490,15 +524,15 @@
trajectory_init(&robot.traj);
trajectory_set_cs(&robot.traj, &robot.cs_d, &robot.cs_a);
trajectory_set_robot_params(&robot.traj, &robot.rs, &robot.pos);
- trajectory_set_speed(&robot.traj, 300, 300);
+ trajectory_set_speed(&robot.traj, 2000, 2000); /* d, a */
/* distance window, angle window, angle start */
- trajectory_set_windows(&robot.traj, 20.0, 10.0, 50.0);
+ trajectory_set_windows(&robot.traj, 10.0, 5.0, 50.0);
/* Blocking detection */
bd_init(&robot.bd_l);
- bd_set_current_thresholds(&robot.bd_l, 500, 3000, 1200000, 60);
+ bd_set_current_thresholds(&robot.bd_l, 500, 8000, 1800000, 60);
bd_init(&robot.bd_r);
- bd_set_current_thresholds(&robot.bd_r, 500, 3000, 1200000, 60);
+ bd_set_current_thresholds(&robot.bd_r, 500, 8000, 1800000, 60);
/* CS EVENT */
scheduler_add_periodical_event_priority(do_cs, NULL,
@@ -524,6 +558,9 @@
TCNT2 = 0;
TCCR2 = 4;
sbi(TIMSK, OCIE2);
+#else /* else we use timer 2 for debug time measuring */
+ TCNT2 = 0;
+ TCCR2 = 5;
#endif
sei();
========================================
aversive_projects/microb2008/main/main.h (1.17 -> 1.18)
========================================
@@ -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.17 2008-04-01 11:47:19 zer0 Exp $
+ * Revision : $Id: main.h,v 1.18 2008-04-06 17:33:57 zer0 Exp $
*
*/
@@ -39,14 +39,14 @@
#define MATCH_TIME 90
/* decrease track to decrease angle */
-#define EXT_TRACK_CM 29.9
+#define EXT_TRACK_CM 29.858
#define VIRTUAL_TRACK_CM EXT_TRACK_CM
/* it is a 2000 imps -> 8000 because we see 1/4 period
* and diameter: 40.5mm -> perimeter 127.23mm
* 8000/12.723 -> 628.78 */
/* increase it to go further */
-#define DIST_IMP_CM 635.0
+#define DIST_IMP_CM 6350.0 /* rapport 10. [voir main()] */
#define LEFT_ENCODER_MOT ((void *)1)
#define RIGHT_ENCODER_MOT ((void *)0)
@@ -78,7 +78,9 @@
#define I2C_POLL_PRIO 20
/* debug flags */
-#define DEBUG_CS 1
+#define DEBUG_CS 0
+#define DEBUG_TIME 0
+
#define NB_LOGS 4
struct robot {
==========================================
aversive_projects/microb2008/main/sensor.c (1.4 -> 1.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: sensor.c,v 1.4 2008-03-31 22:22:52 zer0 Exp $
+ * Revision : $Id: sensor.c,v 1.5 2008-04-06 17:33:57 zer0 Exp $
*
*/
@@ -99,8 +99,8 @@
* can call the same func during
* limitation... in this case the restoration
* will be wrong */
- trajectory_set_speed(&robot.traj, LIMITED_SPEED,
- robot.traj.a_speed);
+/* trajectory_set_speed(&robot.traj, LIMITED_SPEED, */
+/* robot.traj.a_speed); */
}
}
else {
@@ -109,8 +109,8 @@
}
else if (saved_speed != -1) {
DEBUG(E_USER_SENSOR, "Restoring speed from %d to %d",
LIMITED_SPEED, saved_speed);
- trajectory_set_speed(&robot.traj, saved_speed,
- robot.traj.a_speed);
+/* trajectory_set_speed(&robot.traj, saved_speed, */
+/* robot.traj.a_speed); */
saved_speed = -1;
}
}
@@ -137,7 +137,8 @@
uint8_t
sensor_obstacle(void)
{
- return !disable && (filter_us > (FILTER_US_CPT_MAX/2));
+ return 0;
+ // return !disable && (filter_us > (FILTER_US_CPT_MAX/2));
}
uint8_t
=========================================
aversive_projects/microb2008/main/strat.c (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: strat.c,v 1.13 2008-04-04 15:21:28 zer0 Exp $
+ * Revision : $Id: strat.c,v 1.14 2008-04-06 17:33:58 zer0 Exp $
*
*/
@@ -38,6 +38,9 @@
#define CATAPULT_SPEED_H_DISP 200
#define CATAPULT_POS_H_DISP 8000
+#define SPEED_SLOW 1000
+#define SPEED_FAST 2500
+
#define DISP_MAX_TRIES 2
#define TRAJ_SUCCESS(f) (f & (END_TRAJ|END_NEAR))
@@ -72,7 +75,7 @@
//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);
+ trajectory_set_speed(&robot.traj, SPEED_FAST, 3000);
time_reset();
interrupt_traj_reset();
colored_disp.count = 5;
@@ -256,14 +259,18 @@
if (!TRAJ_SUCCESS(err))
return err;
- trajectory_set_speed(&robot.traj, 100, 300);
+ trajectory_set_speed(&robot.traj, SPEED_SLOW, 3000);
trajectory_goto_forward_xy_abs(&robot.traj, V_WHI_DISP_X, V_WHI_DISP_Y);
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
DEBUG(E_USER_STRAT, "%s end=%d", __FUNCTION__, err);
+ if (err == END_BLOCKING) {
+ bd_reset(&robot.bd_l);
+ bd_reset(&robot.bd_r);
+ }
trajectory_d_rel(&robot.traj, -1);
- trajectory_set_speed(&robot.traj, 300, 300);
+ trajectory_set_speed(&robot.traj, SPEED_FAST, 3000);
strat_pickup_v_disp(&white_disp);
@@ -285,23 +292,27 @@
colored_disp.count);
trajectory_goto_forward_xy_abs(&robot.traj, V_COL_DISP_X,
PREPARE_V_COL_DISP_Y);
- err = wait_traj_end(TRAJ_STD_FLAGS);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
return err;
trajectory_a_abs(&robot.traj, V_COL_DISP_A);
- err = wait_traj_end(TRAJ_STD_FLAGS);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
if (!TRAJ_SUCCESS(err))
return err;
- trajectory_set_speed(&robot.traj, 100, 300);
+ trajectory_set_speed(&robot.traj, SPEED_SLOW, 3000);
trajectory_goto_forward_xy_abs(&robot.traj, V_COL_DISP_X, V_COL_DISP_Y);
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
DEBUG(E_USER_STRAT, "%s end=%d", __FUNCTION__, err);
+ if (err == END_BLOCKING) {
+ bd_reset(&robot.bd_l);
+ bd_reset(&robot.bd_r);
+ }
trajectory_d_rel(&robot.traj, -1);
- trajectory_set_speed(&robot.traj, 300, 300);
+ trajectory_set_speed(&robot.traj, SPEED_FAST, 3000);
strat_pickup_v_disp(&colored_disp);
@@ -342,6 +353,8 @@
i2c_prepare_ball(I2C_BALL_TYPE_COLORED,
I2C_DST_DROP_SHOT);
}
+ /* XXX en cas d'echec, si le barillet est plein, il faut reessayer */
+
return END_TRAJ;
}
@@ -376,10 +389,19 @@
if (!TRAJ_SUCCESS(err) && err != END_TIMER)
return err;
+ trajectory_set_speed(&robot.traj, SPEED_SLOW, 3000);
trajectory_goto_forward_xy_abs(&robot.traj, STD_CONT_X_MID, STD_CONT_Y);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ /* we should be blocked on the border */
+ if (err == END_BLOCKING) {
+ DEBUG(E_USER_STRAT, "%s, reset blocking");
+ bd_reset(&robot.bd_l);
+ bd_reset(&robot.bd_r);
+ }
+ trajectory_set_speed(&robot.traj, SPEED_FAST, 3000);
+
trajectory_d_rel(&robot.traj, -5);
- wait_traj_end(TRAJ_FLAGS_NO_NEAR);
+ err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
trajectory_a_abs(&robot.traj, STD_CONT_A);
err = wait_traj_end(TRAJ_FLAGS_NO_NEAR);
@@ -408,13 +430,15 @@
if (col != I2C_BALL_TYPE_EMPTY)
i2c_prepare_ball(col, side);
}
+
+ /* XXX en cas d'echec, si le barillet est plein, il faut reessayer */
+
return END_TRAJ;
}
/* drop (shot or eject) balls if necessary */
static uint8_t strat_drop_balls(void)
{
- /* XXX improve it here, use force and time */
if (barrel_colored_count() == 0 && barrel_white_count() == 0)
return END_TRAJ;
@@ -440,7 +464,7 @@
return strat_eject_balls();
}
- /* if we are close of col disp, */
+ /* if we are close of col disp, XXX wrong if colored_disp==0 */
if (distance_from_robot(V_COL_DISP_X, V_COL_DISP_Y) < 60) {
/* if there is some balls in container, just shot our
* colored balls */
@@ -454,13 +478,18 @@
if (barrel_white_count() + barrel_colored_count() >= 4) {
/* if we don't have lots of white */
- if (barrel_white_count() <= 1)
+ if (barrel_white_count() <= 1) {
+ DEBUG(E_USER_STRAT, "%s lot of balls, shot",
__FUNCTION__);
return strat_shot_balls();
+ }
/* else eject */
+ DEBUG(E_USER_STRAT, "%s lot of balls, eject", __FUNCTION__);
return strat_eject_balls();
}
+ DEBUG(E_USER_STRAT, "%s do it later", __FUNCTION__);
+
/* else do nothing... we will be called later */
return END_TRAJ;
}
@@ -500,7 +529,7 @@
if (!TRAJ_SUCCESS(err))
goto err;
- trajectory_set_speed(&robot.traj, 100, 300);
+ trajectory_set_speed(&robot.traj, SPEED_SLOW, 3000);
i2c_catapult_out();
/* backwards */
@@ -524,7 +553,7 @@
if (!TRAJ_SUCCESS(err))
goto err;
- trajectory_set_speed(&robot.traj, 100, 300);
+ trajectory_set_speed(&robot.traj, SPEED_SLOW, 3000);
i2c_catapult_out();
/* backwards */
@@ -535,14 +564,14 @@
}
i2c_harvest();
- trajectory_set_speed(&robot.traj, 300, 300);
+ trajectory_set_speed(&robot.traj, SPEED_FAST, 3000);
h_disp_enabled = 1;
return END_TRAJ;
err_restore_params:
i2c_harvest();
- trajectory_set_speed(&robot.traj, 300, 300);
+ trajectory_set_speed(&robot.traj, SPEED_FAST, 3000);
err:
return err;
}
Commit from zer0 (2008-04-07 00:33 CEST)
================
fix bug in 'test' command
aversive_projects microb2008/main/commands.c 1.23
============================================
aversive_projects/microb2008/main/commands.c (1.22 -> 1.23)
============================================
@@ -1407,8 +1407,8 @@
prog_char str_test_arg0[] = "test";
parse_pgm_token_string_t cmd_test_arg0 = TOKEN_STRING_INITIALIZER(struct
cmd_test_result, arg0, str_test_arg0);
-parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct
cmd_position_result, arg1, INT8);
-parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct
cmd_position_result, arg2, INT8);
+parse_pgm_token_num_t cmd_test_arg1 = TOKEN_NUM_INITIALIZER(struct
cmd_test_result, arg1, INT8);
+parse_pgm_token_num_t cmd_test_arg2 = TOKEN_NUM_INITIALIZER(struct
cmd_test_result, arg2, INT8);
prog_char help_test[] = "Test function";
parse_pgm_inst_t cmd_test = {
_______________________________________________
Avr-list mailing list
[email protected]
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
