summaryrefslogtreecommitdiff
path: root/n/asserv
diff options
context:
space:
mode:
authorschodet2005-04-09 21:30:07 +0000
committerschodet2005-04-09 21:30:07 +0000
commitcbdfded9991dc3e01da3f70cd81b854694c0f769 (patch)
treef2d6a92f3b3ae7fbec28f9f86e698c0452181f0e /n/asserv
parentd2f36b53cd72dc6e2d627faeea21be01507b758a (diff)
footing pour Taz, 0.05/256 d'erreur d'angle !!
Diffstat (limited to 'n/asserv')
-rw-r--r--n/asserv/src/avrconfig.h4
-rw-r--r--n/asserv/src/goto.c57
-rw-r--r--n/asserv/src/main.c22
-rw-r--r--n/asserv/src/postrack.c6
-rw-r--r--n/asserv/src/pwm.c2
-rw-r--r--n/asserv/src/speed.c8
6 files changed, 95 insertions, 4 deletions
diff --git a/n/asserv/src/avrconfig.h b/n/asserv/src/avrconfig.h
index be35d22..a96d7ef 100644
--- a/n/asserv/src/avrconfig.h
+++ b/n/asserv/src/avrconfig.h
@@ -37,9 +37,9 @@
/** Send mode :
* - POLLING : no interrupts;
* - RING : interrupts, ring buffer. */
-#define AC_RS232_SEND_MODE POLLING
+#define AC_RS232_SEND_MODE RING
/** Recv mode, same as send mode. */
-#define AC_RS232_RECV_MODE POLLING
+#define AC_RS232_RECV_MODE RING
/** Character size : 5, 6, 7, 8, 9 (only 8 implemented). */
#define AC_RS232_CHAR_SIZE 8
/** Parity : ODD, EVEN, NONE. */
diff --git a/n/asserv/src/goto.c b/n/asserv/src/goto.c
index bd57d85..8aa5e91 100644
--- a/n/asserv/src/goto.c
+++ b/n/asserv/src/goto.c
@@ -27,6 +27,7 @@
* 0: linear move.
* 1: angular move.
* 2: position move.
+ * 10: we fuck the wall.
*/
uint8_t goto_mode;
/** Distance for mode 0, ui16. */
@@ -37,6 +38,8 @@ uint8_t goto_sign;
int32_t goto_x, goto_y;
/** Destination angle, f0.8. */
int8_t goto_a;
+/** Travel speed for fixed speed movements, i8. */
+int8_t goto_s;
/** Destination epsillon. */
int32_t goto_eps = 20L << 8;
/** Debug values. */
@@ -110,10 +113,61 @@ goto_position_mode (void)
goto_da = dsp_mul_f824 (goto_dy, c) - dsp_mul_f824 (goto_dx, s);
/* Convert da into a arc. This is a rough aproximation. */
goto_da = goto_da * (postrack_footing / 2) / (goto_dl >> 8);
+ speed_distance (goto_dl / 2, goto_da * 2);
+ }
+}
+
+/** Position control mode, `exact' method. */
+static inline void
+goto_position_exact_mode (void)
+{
+ int32_t c, s;
+ /* Project in the robot base. */
+ goto_dx = goto_x - postrack_x; /* f24.8 */
+ goto_dy = goto_y - postrack_y;
+ if (goto_dx < goto_eps && goto_dx > -goto_eps
+ && goto_dy < goto_eps && goto_dy > -goto_eps)
+ {
+ speed_left_aim = 0;
+ speed_right_aim = 0;
+ }
+ else
+ {
+ c = dsp_cos_f824 (postrack_a);
+ s = dsp_sin_f824 (postrack_a);
+ goto_dl = dsp_mul_f824 (goto_dx, c) + dsp_mul_f824 (goto_dy, s);
+ goto_da = dsp_mul_f824 (goto_dy, c) - dsp_mul_f824 (goto_dx, s);
+ /* Convert da into a arc. This is a rough aproximation. */
+ goto_da = goto_da * (postrack_footing / 2) / (goto_dl >> 8);
speed_distance (goto_dl, goto_da);
}
}
+/** We fuck the wall mode. */
+static inline void
+goto_ftw_mode (void)
+{
+ /* Change speed. */
+ if (PINC & _BV (0))
+ {
+ speed_left_aim = goto_s;
+ }
+ else
+ {
+ speed_left = 0;
+ speed_left_aim = 0;
+ }
+ if (PINC & _BV (7))
+ {
+ speed_right_aim = goto_s;
+ }
+ else
+ {
+ speed_right = 0;
+ speed_right_aim = 0;
+ }
+}
+
/** Update the speed according to the desired destination. */
static inline void
goto_update (void)
@@ -129,6 +183,9 @@ goto_update (void)
case 2:
goto_position_mode ();
break;
+ case 10:
+ goto_ftw_mode ();
+ break;
}
}
diff --git a/n/asserv/src/main.c b/n/asserv/src/main.c
index d119e4b..fb6b2d4 100644
--- a/n/asserv/src/main.c
+++ b/n/asserv/src/main.c
@@ -64,6 +64,9 @@ uint8_t motor_stat_pwm, motor_stat_pwm_cpt;
/** Report of timer. */
uint8_t motor_stat_timer, motor_stat_timer_cpt;
+/** Print PINC. */
+uint8_t motor_print_pinc, motor_print_pinc_cpt;
+
/** Record timer value at different stage of computing. Used for performance
* analisys. */
uint8_t motor_timer_0, motor_timer_1, motor_timer_2, motor_timer_3,
@@ -159,6 +162,11 @@ main_loop (void)
motor_timer_1, motor_timer_0);
motor_stat_timer_cpt = motor_stat_timer;
}
+ if (motor_print_pinc && !--motor_print_pinc_cpt)
+ {
+ proto_send1b ('P', PINC);
+ motor_print_pinc_cpt = motor_print_pinc;
+ }
/* Misc. */
if ((motor_loop_cpt & 7) == 0)
{
@@ -200,6 +208,11 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
goto_x = v8_to_v32 (args[0], args[1], args[2], args[3]);
goto_y = v8_to_v32 (args[4], args[5], args[6], args[7]);
break;
+ case c ('f', 1):
+ motor_mode = 2;
+ goto_mode = 10;
+ goto_s = args[0];
+ break;
case c ('s', 0):
motor_mode = 1;
speed_left_aim = 0;
@@ -246,6 +259,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('T', 1):
motor_stat_timer_cpt = motor_stat_timer = args[0];
break;
+ case c ('P', 1):
+ motor_print_pinc_cpt = motor_print_pinc = args[0];
+ break;
default:
/* Params. */
if (cmd == 'p')
@@ -273,6 +289,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('i', 3):
speed_ki = v8_to_v16 (args[1], args[2]);
break;
+ case c ('d', 3):
+ speed_dead_zone = v8_to_v16 (args[1], args[2]);
+ break;
+ case c ('I', 3):
+ speed_int_max = v8_to_v16 (args[1], args[2]);
+ break;
case c ('a', 2):
speed_acc_cpt = speed_acc = args[1];
break;
diff --git a/n/asserv/src/postrack.c b/n/asserv/src/postrack.c
index ae0d0c3..3cca141 100644
--- a/n/asserv/src/postrack.c
+++ b/n/asserv/src/postrack.c
@@ -60,7 +60,11 @@ postrack_set_footing (uint16_t footing);
static inline void
postrack_init (void)
{
- postrack_set_footing (5142);
+ // Mesured : 2667.
+ // Correction : / 1.00708 -> 6223 (184F).
+ // Correction : / 1.00210 -> 6209.9107 (1842).
+ // Correction : -> 6216 (1848).
+ postrack_set_footing (0x1842);
}
#define M_PI 3.14159265358979323846 /* pi */
diff --git a/n/asserv/src/pwm.c b/n/asserv/src/pwm.c
index 6e90e65..9452077 100644
--- a/n/asserv/src/pwm.c
+++ b/n/asserv/src/pwm.c
@@ -34,7 +34,7 @@
/** Define the forward direction logic for left motor. */
#define PWM_LEFT_DIR_FRW 0
/** Define the forward direction logic for right motor. */
-#define PWM_RIGHT_DIR_FRW 0
+#define PWM_RIGHT_DIR_FRW 1
/** PWM values. */
int16_t pwm_left, pwm_right;
diff --git a/n/asserv/src/speed.c b/n/asserv/src/speed.c
index 38842a1..7fdb0dc 100644
--- a/n/asserv/src/speed.c
+++ b/n/asserv/src/speed.c
@@ -43,6 +43,8 @@ int16_t speed_left_e_old, speed_right_e_old;
uint16_t speed_kp = 5 * 255;
/** I coeficients. 4.8 fixed point format. */
uint16_t speed_ki = 1 * 255;
+/** Dead zone. */
+uint16_t speed_dead_zone = 0;
/* +AutoDec */
@@ -114,6 +116,9 @@ speed_compute_left_pwm (void)
int16_t e;
int16_t pwm;
e = speed_left - counter_left_diff; /* 10b = 8b + 9b */
+ /* Dead zone. */
+ if (speed_dead_zone > e && e > -speed_dead_zone)
+ e = 0;
/* Integral update. */
speed_left_int += e; /* 12b = 11b + 10b */
if (speed_left_int > speed_int_max) /* 11b */
@@ -135,6 +140,9 @@ speed_compute_right_pwm (void)
int16_t e;
int16_t pwm;
e = speed_right - counter_right_diff;
+ /* Dead zone. */
+ if (speed_dead_zone > e && e > -speed_dead_zone)
+ e = 0;
/* Integral update. */
speed_right_int += e;
if (speed_right_int > speed_int_max)