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.c30
1 files changed, 30 insertions, 0 deletions
diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c
index 866d784..11ef09a 100644
--- a/n/asserv/src/asserv/main.c
+++ b/n/asserv/src/asserv/main.c
@@ -27,6 +27,7 @@
#include "modules/proto/proto.h"
#include "modules/utils/utils.h"
#include "modules/utils/byte.h"
+#include "modules/math/fixed/fixed.h"
#include "io.h"
#include "misc.h"
@@ -55,6 +56,9 @@ int8_t main_mode;
/** Report of counters. */
uint8_t main_stat_counter, main_stat_counter_cpt;
+/** Statistics about speed control. */
+uint8_t main_stat_speed, main_stat_speed_cpt;
+
/** Statistics about shaft position control. */
uint8_t main_stat_pos, main_stat_pos_cpt;
@@ -127,6 +131,11 @@ main_loop (void)
proto_send2w ('C', counter_left, counter_right);
main_stat_counter_cpt = main_stat_counter;
}
+ if (main_stat_speed && !--main_stat_speed_cpt)
+ {
+ proto_send2b ('S', speed_theta_cur >> 8, speed_alpha_cur >> 8);
+ main_stat_speed_cpt = main_stat_speed;
+ }
if (main_stat_pos && !--main_stat_pos_cpt)
{
proto_send4w ('P', pos_theta_e_old, pos_theta_int,
@@ -208,12 +217,33 @@ 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):
+ /* 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! */
+ 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;
+ break;
/* Stats.
* - b: interval between stats. */
case c ('C', 1):
/* Counter stats. */
main_stat_counter_cpt = main_stat_counter = args[0];
break;
+ case c ('S', 1):
+ /* Motor speed control stats. */
+ main_stat_speed_cpt = main_stat_speed = args[0];
+ break;
case c ('P', 1):
/* Motor position control stats. */
main_stat_pos_cpt = main_stat_pos = args[0];