summaryrefslogtreecommitdiff
path: root/n/asserv/src/asserv/main.c
diff options
context:
space:
mode:
Diffstat (limited to 'n/asserv/src/asserv/main.c')
-rw-r--r--n/asserv/src/asserv/main.c42
1 files changed, 25 insertions, 17 deletions
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);