summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorschodet2006-04-30 17:48:31 +0000
committerschodet2006-04-30 17:48:31 +0000
commiteb001ccd562cba753fc593608f80f3ee2bb971ab (patch)
treea6c7a306d8584bdbc56c6643eb7413b0ccad0254
parentebaf738ea722eec1ac327a5128e159ace50559f2 (diff)
Ajout de l'idée géniale de Djerem pour l'acquitement.
Changement des défauts pour les sens de rotation/codage.
-rw-r--r--n/asserv/src/asserv/counter.avr.c4
-rw-r--r--n/asserv/src/asserv/eeprom.avr.c12
-rw-r--r--n/asserv/src/asserv/main.c42
-rw-r--r--n/asserv/src/asserv/pwm.avr.c2
-rw-r--r--n/asserv/src/asserv/speed.c20
5 files changed, 47 insertions, 33 deletions
diff --git a/n/asserv/src/asserv/counter.avr.c b/n/asserv/src/asserv/counter.avr.c
index b0d14ee..c2b5878 100644
--- a/n/asserv/src/asserv/counter.avr.c
+++ b/n/asserv/src/asserv/counter.avr.c
@@ -24,9 +24,9 @@
* }}} */
/** Define to 1 to reverse the left counter. */
-#define COUNTER_REVERSE_LEFT 1
+#define COUNTER_REVERSE_LEFT 0
/** Define to 1 to reverse the right counter. */
-#define COUNTER_REVERSE_RIGHT 1
+#define COUNTER_REVERSE_RIGHT 0
/** Forward and reverse counter values. */
static uint8_t counter_left_frw, counter_left_rev,
diff --git a/n/asserv/src/asserv/eeprom.avr.c b/n/asserv/src/asserv/eeprom.avr.c
index 671e0e6..0ef63a0 100644
--- a/n/asserv/src/asserv/eeprom.avr.c
+++ b/n/asserv/src/asserv/eeprom.avr.c
@@ -24,7 +24,7 @@
* }}} */
#include <avr/eeprom.h>
-#define EEPROM_KEY 0x42
+#define EEPROM_KEY 0x43
#define EEPROM_START 256
/* +AutoDec */
@@ -34,10 +34,12 @@
static void
eeprom_read_params (void)
{
- uint8_t *p8 = EEPROM_START;
+ uint8_t *p8 = (uint8_t *) EEPROM_START;
uint16_t *p16;
if (eeprom_read_byte (p8++) != EEPROM_KEY)
return;
+ speed_theta_max = eeprom_read_byte (p8++);
+ speed_alpha_max = eeprom_read_byte (p8++);
pwm_dir = eeprom_read_byte (p8++);
p16 = (uint16_t *) p8;
postrack_set_footing (eeprom_read_word (p16++));
@@ -57,9 +59,11 @@ eeprom_read_params (void)
static void
eeprom_write_params (void)
{
- uint8_t *p8 = EEPROM_START;
+ uint8_t *p8 = (uint8_t *) EEPROM_START;
uint16_t *p16;
eeprom_write_byte (p8++, EEPROM_KEY);
+ eeprom_write_byte (p8++, speed_theta_max);
+ eeprom_write_byte (p8++, speed_alpha_max);
eeprom_write_byte (p8++, pwm_dir);
p16 = (uint16_t *) p8;
eeprom_write_word (p16++, postrack_footing);
@@ -79,7 +83,7 @@ eeprom_write_params (void)
static void
eeprom_clear_params (void)
{
- uint8_t *p = EEPROM_START;
+ uint8_t *p = (uint8_t *) EEPROM_START;
eeprom_write_byte (p, 0xff);
}
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index c74f786..2a19877 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -54,6 +54,9 @@
* 3: trajectory control. */
int8_t main_mode;
+/** Motor command sequence. */
+uint8_t main_sequence;
+
/** Report of counters. */
uint8_t main_stat_counter, main_stat_counter_cpt;
@@ -77,8 +80,7 @@ uint8_t main_print_pin, main_print_pin_cpt;
/** Record timer value at different stage of computing. Used for performance
* analisys. */
-uint8_t main_timer_0, main_timer_1, main_timer_2, main_timer_3, main_timer_4,
- main_timer_5;
+uint8_t main_timer[6];
/* +AutoDec */
@@ -113,24 +115,24 @@ main (int argc, char **argv)
static void
main_loop (void)
{
- main_timer_5 = timer_read ();
+ main_timer[5] = timer_read ();
timer_wait ();
/* Counter update. */
counter_update ();
- main_timer_0 = timer_read ();
+ main_timer[0] = timer_read ();
/* Postion control. */
if (main_mode >= 1)
pos_update ();
- main_timer_1 = timer_read ();
+ main_timer[1] = timer_read ();
/* Pwm setup. */
pwm_update ();
- main_timer_2 = timer_read ();
+ main_timer[2] = timer_read ();
/* Compute absolute position. */
postrack_update ();
/* Speed control. */
if (main_mode >= 2)
speed_update ();
- main_timer_3 = timer_read ();
+ main_timer[3] = timer_read ();
/* Stats. */
if (main_stat_counter && !--main_stat_counter_cpt)
{
@@ -160,8 +162,8 @@ main_loop (void)
}
if (main_stat_timer && !--main_stat_timer_cpt)
{
- proto_send6b ('T', main_timer_0, main_timer_2, main_timer_3,
- main_timer_4, main_timer_4, main_timer_5);
+ proto_send6b ('T', main_timer[0], main_timer[2], main_timer[3],
+ main_timer[4], main_timer[4], main_timer[5]);
main_stat_timer_cpt = main_stat_timer;
}
if (main_print_pin && !--main_print_pin_cpt)
@@ -172,7 +174,7 @@ main_loop (void)
/* Misc. */
while (uart0_poll ())
proto_accept (uart0_getc ());
- main_timer_4 = timer_read ();
+ main_timer[4] = timer_read ();
}
/** Handle incoming messages. */
@@ -229,22 +231,20 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta_cons = args[0] << 8;
speed_alpha_cons = args[1] << 8;
break;
- case c ('s', 6):
+ case c ('s', 5):
/* Set speed controlled position consign.
* - w: theta consign offset.
* - w: alpha consign offset.
- * - b: theta max speed.
- * - b: alpha max speed. */
- /* WARNING! This is not safe regarding proto! If the sender miss the
- * acknoledgement it will send this commande twice! */
+ * - b: sequence number. */
+ if (args[4] == main_sequence)
+ break;
main_mode = 2;
speed_pos = 1;
speed_theta_pos_cons = pos_theta_cons;
speed_theta_pos_cons += v8_to_v16 (args[0], args[1]);
speed_alpha_pos_cons = pos_alpha_cons;
speed_alpha_pos_cons += v8_to_v16 (args[2], args[3]);
- speed_theta_cons = args[4] << 8;
- speed_alpha_cons = args[5] << 8;
+ main_sequence = args[4];
break;
/* Stats.
* - b: interval between stats. */
@@ -315,6 +315,13 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_theta_acc = v8_to_v16 (args[1], args[2]);
speed_alpha_acc = v8_to_v16 (args[3], args[4]);
break;
+ case c ('s', 3):
+ /* Set maximum speed.
+ * - b: theta.
+ * - b: alpha. */
+ speed_theta_max = args[1];
+ speed_alpha_max = args[2];
+ break;
case c ('p', 3):
pos_theta_kp = pos_alpha_kp = v8_to_v16 (args[1], args[2]);
break;
@@ -363,6 +370,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1b ('E', EEPROM_KEY);
proto_send1w ('f', postrack_footing);
proto_send2w ('a', speed_theta_acc, speed_alpha_acc);
+ proto_send2b ('s', speed_theta_max, speed_alpha_max);
proto_send2w ('p', pos_theta_kp, pos_alpha_kp);
proto_send2w ('i', pos_theta_ki, pos_alpha_ki);
proto_send2w ('d', pos_theta_kd, pos_alpha_kd);
diff --git a/n/asserv/src/asserv/pwm.avr.c b/n/asserv/src/asserv/pwm.avr.c
index b0db836..773cfef 100644
--- a/n/asserv/src/asserv/pwm.avr.c
+++ b/n/asserv/src/asserv/pwm.avr.c
@@ -40,7 +40,7 @@
int16_t pwm_left, pwm_right;
/** PWM reverse direction, only set pwm dir bits or you will get weird results
* on port B. */
-uint8_t pwm_dir = _BV (PWM_LEFT_DIR) | _BV (PWM_RIGHT_DIR);
+uint8_t pwm_dir = _BV (PWM_LEFT_DIR);
/* +AutoDec */
diff --git a/n/asserv/src/asserv/speed.c b/n/asserv/src/asserv/speed.c
index 324e231..eb9b568 100644
--- a/n/asserv/src/asserv/speed.c
+++ b/n/asserv/src/asserv/speed.c
@@ -31,8 +31,10 @@
/** Current speed, f8.8. */
int16_t speed_theta_cur, speed_alpha_cur;
-/** Consign speed or maximum speed for position consign, f8.8. */
+/** Consign speed, f8.8. */
int16_t speed_theta_cons, speed_alpha_cons;
+/** Maximum speed for position consign, u8. */
+int8_t speed_theta_max, speed_alpha_max;
/** Consign position. */
uint32_t speed_theta_pos_cons, speed_alpha_pos_cons;
/** Weither to use the consign position (1) or not (0). */
@@ -63,17 +65,17 @@ speed_update_by_speed (void)
speed_alpha_cur -= speed_alpha_acc;
}
-/** Compute maximum allowed speed according to: distance left, maximum speed
- * consign, current speed and acceleration. */
+/** Compute maximum allowed speed according to: distance left, maximum speed,
+ * current speed and acceleration. */
static int16_t
-speed_compute_max_speed (int32_t d, int16_t cons, int16_t cur, int16_t acc)
+speed_compute_max_speed (int32_t d, int16_t cur, int16_t acc, int8_t max)
{
int16_t s;
/* Compute maximum speed in order to be able to brake in time.
* s = sqrt (2 * a * d) */
s = fixed_sqrt_ui32 (2 * (acc >> 8) * UTILS_ABS (d));
/* Apply consign. */
- s = UTILS_MIN (cons >> 8, s);
+ s = UTILS_MIN (max, s);
/* Apply sign. */
if (d < 0)
s = -s;
@@ -89,12 +91,12 @@ speed_update_by_position (void)
{
speed_theta_cur =
speed_compute_max_speed (speed_theta_pos_cons - pos_theta_cons,
- speed_theta_cons, speed_theta_cur,
- speed_theta_acc);
+ speed_theta_cur, speed_theta_acc,
+ speed_theta_max);
speed_alpha_cur =
speed_compute_max_speed (speed_alpha_pos_cons - pos_alpha_cons,
- speed_alpha_cons, speed_alpha_cur,
- speed_alpha_acc);
+ speed_alpha_cur, speed_alpha_acc,
+ speed_alpha_max);
}
/** Update shaft position consign according to consign. */