summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--digital/ai/src/twi_master/asserv.c46
-rw-r--r--digital/ai/src/twi_master/asserv.h12
-rw-r--r--digital/ai/src/twi_master/mimot.c66
-rw-r--r--digital/ai/src/twi_master/mimot.h16
-rw-r--r--digital/asserv/src/asserv/aux.c8
-rw-r--r--digital/asserv/src/asserv/aux.h6
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c10
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/main.c53
-rw-r--r--digital/asserv/src/asserv/traj.c6
-rw-r--r--digital/asserv/src/asserv/twi_proto.c46
-rw-r--r--digital/asserv/tools/asserv/asserv.py10
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.c50
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.h20
-rw-r--r--digital/avr/modules/motor/speed_control/speed_control.txt2
-rw-r--r--digital/mimot/src/dirty/aux.c6
-rw-r--r--digital/mimot/src/dirty/aux.h6
-rw-r--r--digital/mimot/src/dirty/eeprom.avr.c10
-rw-r--r--digital/mimot/src/dirty/eeprom.h2
-rw-r--r--digital/mimot/src/dirty/main.c51
-rw-r--r--digital/mimot/src/dirty/twi_proto.c34
-rw-r--r--digital/mimot/tools/mimot/mimot.py10
22 files changed, 247 insertions, 225 deletions
diff --git a/digital/ai/src/twi_master/asserv.c b/digital/ai/src/twi_master/asserv.c
index 76b2eb15..153997b6 100644
--- a/digital/ai/src/twi_master/asserv.c
+++ b/digital/ai/src/twi_master/asserv.c
@@ -339,25 +339,27 @@ asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y,
#if AC_ASSERV_AUX_NB
void
-asserv_move_motor0_absolute (uint16_t position, uint8_t speed)
+asserv_move_motor0_absolute (uint16_t position, uint16_t speed)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
buffer[0] = 'b';
buffer[1] = v16_to_v8 (position, 1);
buffer[2] = v16_to_v8 (position, 0);
- buffer[3] = speed;
- twi_master_send_buffer (4);
+ buffer[3] = v16_to_v8 (speed, 1);
+ buffer[4] = v16_to_v8 (speed, 0);
+ twi_master_send_buffer (5);
}
void
-asserv_move_motor1_absolute (uint16_t position, uint8_t speed)
+asserv_move_motor1_absolute (uint16_t position, uint16_t speed)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
buffer[0] = 'c';
buffer[1] = v16_to_v8 (position, 1);
buffer[2] = v16_to_v8 (position, 0);
- buffer[3] = speed;
- twi_master_send_buffer (4);
+ buffer[3] = v16_to_v8 (speed, 1);
+ buffer[4] = v16_to_v8 (speed, 0);
+ twi_master_send_buffer (5);
}
#endif /* AC_ASSERV_AUX_NB */
@@ -400,17 +402,21 @@ asserv_set_angle_position (int16_t angle)
}
void
-asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
- uint8_t linear_low, uint8_t angular_low)
+asserv_set_speed (uint16_t linear_high, uint16_t angular_high,
+ uint16_t linear_low, uint16_t angular_low)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
buffer[0] = 'p';
buffer[1] = 's';
- buffer[2] = linear_high;
- buffer[3] = angular_high;
- buffer[4] = linear_low;
- buffer[5] = angular_low;
- twi_master_send_buffer (6);
+ buffer[2] = v16_to_v8 (linear_high, 1);
+ buffer[3] = v16_to_v8 (linear_high, 0);
+ buffer[4] = v16_to_v8 (angular_high, 1);
+ buffer[5] = v16_to_v8 (angular_high, 0);
+ buffer[6] = v16_to_v8 (linear_low, 1);
+ buffer[7] = v16_to_v8 (linear_low, 0);
+ buffer[8] = v16_to_v8 (angular_low, 1);
+ buffer[9] = v16_to_v8 (angular_low, 0);
+ twi_master_send_buffer (10);
}
void
@@ -454,21 +460,23 @@ asserv_goto (uint32_t x, uint32_t y, uint8_t backward)
#if AC_ASSERV_AUX_NB
void
-asserv_motor0_zero_position (int8_t speed)
+asserv_motor0_zero_position (int16_t speed)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
buffer[0] = 'B';
- buffer[1] = speed;
- twi_master_send_buffer (2);
+ buffer[1] = v16_to_v8 (speed, 1);
+ buffer[2] = v16_to_v8 (speed, 0);
+ twi_master_send_buffer (3);
}
void
-asserv_motor1_zero_position (int8_t speed)
+asserv_motor1_zero_position (int16_t speed)
{
uint8_t *buffer = twi_master_get_buffer (ASSERV_SLAVE);
buffer[0] = 'C';
- buffer[1] = speed;
- twi_master_send_buffer (2);
+ buffer[1] = v16_to_v8 (speed, 1);
+ buffer[2] = v16_to_v8 (speed, 0);
+ twi_master_send_buffer (3);
}
void
diff --git a/digital/ai/src/twi_master/asserv.h b/digital/ai/src/twi_master/asserv.h
index 90e3b785..ed6ed046 100644
--- a/digital/ai/src/twi_master/asserv.h
+++ b/digital/ai/src/twi_master/asserv.h
@@ -223,7 +223,7 @@ asserv_push_the_wall (uint8_t backward, uint32_t init_x, uint32_t init_y,
* @param speed speed of the movement.
*/
void
-asserv_move_motor0_absolute (uint16_t position, uint8_t speed);
+asserv_move_motor0_absolute (uint16_t position, uint16_t speed);
/**
* Move the motor1.
@@ -234,7 +234,7 @@ asserv_move_motor0_absolute (uint16_t position, uint8_t speed);
* @param speed speed of the movement.
*/
void
-asserv_move_motor1_absolute (uint16_t position, uint8_t speed);
+asserv_move_motor1_absolute (uint16_t position, uint16_t speed);
#endif /* AC_ASSERV_AUX_NB */
@@ -271,8 +271,8 @@ asserv_set_angle_position (int16_t angle);
* @param angular_low angular low speed
*/
void
-asserv_set_speed (uint8_t linear_high, uint8_t angular_high,
- uint8_t linear_low, uint8_t angular_low);
+asserv_set_speed (uint16_t linear_high, uint16_t angular_high,
+ uint16_t linear_low, uint16_t angular_low);
/**
* Set the complete position of the bot.
@@ -300,11 +300,11 @@ asserv_goto (uint32_t x, uint32_t y, uint8_t backward);
/** Reset the motor0 to the zero position. */
void
-asserv_motor0_zero_position (int8_t speed);
+asserv_motor0_zero_position (int16_t speed);
/** Reset the motor1 to the zero position. */
void
-asserv_motor1_zero_position (int8_t speed);
+asserv_motor1_zero_position (int16_t speed);
/** Set PWM to zero for motor0. */
void
diff --git a/digital/ai/src/twi_master/mimot.c b/digital/ai/src/twi_master/mimot.c
index 8df84538..cc66a280 100644
--- a/digital/ai/src/twi_master/mimot.c
+++ b/digital/ai/src/twi_master/mimot.c
@@ -148,89 +148,95 @@ mimot_set_motor1_position (uint16_t position)
}
void
-mimot_move_motor0_absolute (uint16_t position, uint8_t speed)
+mimot_move_motor0_absolute (uint16_t position, uint16_t speed)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
buffer[0] = 'b';
buffer[1] = v16_to_v8 (position, 1);
buffer[2] = v16_to_v8 (position, 0);
- buffer[3] = speed;
- twi_master_send_buffer (4);
+ buffer[3] = v16_to_v8 (speed, 1);
+ buffer[4] = v16_to_v8 (speed, 0);
+ twi_master_send_buffer (5);
}
void
-mimot_move_motor1_absolute (uint16_t position, uint8_t speed)
+mimot_move_motor1_absolute (uint16_t position, uint16_t speed)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
buffer[0] = 'c';
buffer[1] = v16_to_v8 (position, 1);
buffer[2] = v16_to_v8 (position, 0);
- buffer[3] = speed;
- twi_master_send_buffer (4);
+ buffer[3] = v16_to_v8 (speed, 1);
+ buffer[4] = v16_to_v8 (speed, 0);
+ twi_master_send_buffer (5);
}
void
-mimot_motor0_zero_position (int8_t speed)
+mimot_motor0_zero_position (int16_t speed)
{
mimot_motor0_find_zero (speed, 0, 0);
}
void
-mimot_motor1_zero_position (int8_t speed)
+mimot_motor1_zero_position (int16_t speed)
{
mimot_motor1_find_zero (speed, 0, 0);
}
void
-mimot_motor0_find_zero (int8_t speed, uint8_t use_switch,
+mimot_motor0_find_zero (int16_t speed, uint8_t use_switch,
uint16_t reset_position)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
buffer[0] = 'B';
buffer[1] = 0;
- buffer[2] = speed;
- buffer[3] = use_switch;
- buffer[4] = v16_to_v8 (reset_position, 1);
- buffer[5] = v16_to_v8 (reset_position, 0);
- twi_master_send_buffer (6);
+ buffer[2] = v16_to_v8 (speed, 1);
+ buffer[3] = v16_to_v8 (speed, 0);
+ buffer[4] = use_switch;
+ buffer[5] = v16_to_v8 (reset_position, 1);
+ buffer[6] = v16_to_v8 (reset_position, 0);
+ twi_master_send_buffer (7);
}
void
-mimot_motor1_find_zero (int8_t speed, uint8_t use_switch,
+mimot_motor1_find_zero (int16_t speed, uint8_t use_switch,
uint16_t reset_position)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
buffer[0] = 'B';
buffer[1] = 1;
- buffer[2] = speed;
- buffer[3] = use_switch;
- buffer[4] = v16_to_v8 (reset_position, 1);
- buffer[5] = v16_to_v8 (reset_position, 0);
- twi_master_send_buffer (6);
+ buffer[2] = v16_to_v8 (speed, 1);
+ buffer[3] = v16_to_v8 (speed, 0);
+ buffer[4] = use_switch;
+ buffer[5] = v16_to_v8 (reset_position, 1);
+ buffer[6] = v16_to_v8 (reset_position, 0);
+ twi_master_send_buffer (7);
}
void
-mimot_motor0_clamp (int8_t speed, int16_t pwm)
+mimot_motor0_clamp (int16_t speed, int16_t pwm)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
buffer[0] = 'l';
buffer[1] = 0;
- buffer[2] = speed;
- buffer[3] = v16_to_v8 (pwm, 1);
- buffer[4] = v16_to_v8 (pwm, 0);
- twi_master_send_buffer (5);
+ buffer[2] = v16_to_v8 (speed, 1);
+ buffer[3] = v16_to_v8 (speed, 0);
+ buffer[4] = v16_to_v8 (pwm, 1);
+ buffer[5] = v16_to_v8 (pwm, 0);
+ twi_master_send_buffer (6);
}
void
-mimot_motor1_clamp (int8_t speed, int16_t pwm)
+mimot_motor1_clamp (int16_t speed, int16_t pwm)
{
uint8_t *buffer = twi_master_get_buffer (MIMOT_SLAVE);
buffer[0] = 'l';
buffer[1] = 1;
- buffer[2] = speed;
- buffer[3] = v16_to_v8 (pwm, 1);
- buffer[4] = v16_to_v8 (pwm, 0);
- twi_master_send_buffer (5);
+ buffer[2] = v16_to_v8 (speed, 1);
+ buffer[3] = v16_to_v8 (speed, 0);
+ buffer[4] = v16_to_v8 (pwm, 1);
+ buffer[5] = v16_to_v8 (pwm, 0);
+ twi_master_send_buffer (6);
}
void
diff --git a/digital/ai/src/twi_master/mimot.h b/digital/ai/src/twi_master/mimot.h
index c9de7090..35b2dfbe 100644
--- a/digital/ai/src/twi_master/mimot.h
+++ b/digital/ai/src/twi_master/mimot.h
@@ -82,37 +82,37 @@ mimot_set_motor1_position (uint16_t position);
/** Move motor0 to absolute position in steps. */
void
-mimot_move_motor0_absolute (uint16_t position, uint8_t speed);
+mimot_move_motor0_absolute (uint16_t position, uint16_t speed);
/** Move motor1 to absolute position in steps. */
void
-mimot_move_motor1_absolute (uint16_t position, uint8_t speed);
+mimot_move_motor1_absolute (uint16_t position, uint16_t speed);
/** Reset motor0 to zero position. */
void
-mimot_motor0_zero_position (int8_t speed);
+mimot_motor0_zero_position (int16_t speed);
/** Reset motor1 to zero position. */
void
-mimot_motor1_zero_position (int8_t speed);
+mimot_motor1_zero_position (int16_t speed);
/** Find zero position. */
void
-mimot_motor0_find_zero (int8_t speed, uint8_t use_switch,
+mimot_motor0_find_zero (int16_t speed, uint8_t use_switch,
uint16_t reset_position);
/** Find zero position. */
void
-mimot_motor1_find_zero (int8_t speed, uint8_t use_switch,
+mimot_motor1_find_zero (int16_t speed, uint8_t use_switch,
uint16_t reset_position);
/** Clamp motor0. */
void
-mimot_motor0_clamp (int8_t speed, int16_t pwm);
+mimot_motor0_clamp (int16_t speed, int16_t pwm);
/** Clamp motor1. */
void
-mimot_motor1_clamp (int8_t speed, int16_t pwm);
+mimot_motor1_clamp (int16_t speed, int16_t pwm);
/** Free motor0. */
void
diff --git a/digital/asserv/src/asserv/aux.c b/digital/asserv/src/asserv/aux.c
index 204f3fac..5b942bd9 100644
--- a/digital/asserv/src/asserv/aux.c
+++ b/digital/asserv/src/asserv/aux.c
@@ -159,7 +159,7 @@ aux_traj_find_zero (struct aux_t *aux)
case AUX_TRAJ_FIND_ZERO_NOT_REVERSE:
if (zero)
{
- aux->cs->speed.cons = -aux->cs->speed.cons;
+ aux->cs->speed.cons_f = -aux->cs->speed.cons_f;
aux->traj_mode = AUX_TRAJ_FIND_ZERO;
}
break;
@@ -177,7 +177,7 @@ aux_traj_find_zero (struct aux_t *aux)
/** Start find zero mode. */
void
-aux_traj_find_zero_start (struct aux_t *aux, int8_t speed)
+aux_traj_find_zero_start (struct aux_t *aux, int16_t speed)
{
aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT;
speed_control_set_speed (&aux->cs->speed, speed);
@@ -186,7 +186,7 @@ aux_traj_find_zero_start (struct aux_t *aux, int8_t speed)
/** Start find zero reverse mode. */
void
-aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed)
+aux_traj_find_zero_reverse_start (struct aux_t *aux, int16_t speed)
{
aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT_REVERSE;
speed_control_set_speed (&aux->cs->speed, speed);
@@ -210,7 +210,7 @@ aux_traj_find_limit (struct aux_t *aux)
/** Start find limit mode. */
void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed)
+aux_traj_find_limit_start (struct aux_t *aux, int16_t speed)
{
aux->traj_mode = AUX_TRAJ_FIND_LIMIT;
speed_control_set_speed (&aux->cs->speed, speed);
diff --git a/digital/asserv/src/asserv/aux.h b/digital/asserv/src/asserv/aux.h
index cf12aa7d..6d2c583c 100644
--- a/digital/asserv/src/asserv/aux.h
+++ b/digital/asserv/src/asserv/aux.h
@@ -67,13 +67,13 @@ void
aux_traj_goto_start (struct aux_t *aux, uint16_t pos);
void
-aux_traj_find_zero_start (struct aux_t *aux, int8_t speed);
+aux_traj_find_zero_start (struct aux_t *aux, int16_t speed);
void
-aux_traj_find_zero_reverse_start (struct aux_t *aux, int8_t speed);
+aux_traj_find_zero_reverse_start (struct aux_t *aux, int16_t speed);
void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed);
+aux_traj_find_limit_start (struct aux_t *aux, int16_t speed);
void
aux_traj_update (void);
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 0d1753ee..c2a85050 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -48,9 +48,9 @@ struct eeprom_t
/** Saved parameters. */
uint32_t encoder_right_correction;
struct {
- uint8_t max;
- uint8_t slow;
- uint16_t acc;
+ uint16_t max;
+ uint16_t slow;
+ uint16_t acc_f;
} speed[EEPROM_INDEX_NB];
struct {
uint16_t kp;
@@ -99,7 +99,7 @@ eeprom_read_params_helper (struct eeprom_t *loaded, uint8_t index,
{
speed->max = loaded->speed[index].max;
speed->slow = loaded->speed[index].slow;
- speed->acc = loaded->speed[index].acc;
+ speed->acc_f = loaded->speed[index].acc_f;
pos->kp = loaded->pos[index].kp;
pos->ki = loaded->pos[index].ki;
pos->kd = loaded->pos[index].kd;
@@ -177,7 +177,7 @@ eeprom_write_params_helper (struct eeprom_t *param, uint8_t index,
{
param->speed[index].max = speed->max;
param->speed[index].slow = speed->slow;
- param->speed[index].acc = speed->acc;
+ param->speed[index].acc_f = speed->acc_f;
param->pos[index].kp = pos->kp;
param->pos[index].ki = pos->ki;
param->pos[index].kd = pos->kd;
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index 6da2a083..23744de1 100644
--- a/digital/asserv/src/asserv/eeprom.h
+++ b/digital/asserv/src/asserv/eeprom.h
@@ -26,7 +26,7 @@
* }}} */
/** Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x50
+#define EEPROM_KEY 0x51
extern int8_t eeprom_loaded;
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 36cd2219..97d0b1c7 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -181,10 +181,10 @@ main_loop (void)
#endif
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_sendb ('S', cs_main.speed_theta.cur >> 8,
- cs_main.speed_alpha.cur >> 8
- AUX_IF (, cs_aux[0].speed.cur >> 8,
- cs_aux[1].speed.cur >> 8));
+ proto_sendw ('S', cs_main.speed_theta.cur_f >> 8,
+ cs_main.speed_alpha.cur_f >> 8
+ AUX_IF (, cs_aux[0].speed.cur_f >> 8,
+ cs_aux[1].speed.cur_f >> 8));
main_stat_speed_cpt = main_stat_speed;
}
if (main_stat_pos && !--main_stat_pos_cpt)
@@ -327,21 +327,23 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_control_set_speed (&cs_main.speed_alpha, 0);
control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
- case c ('s', 2):
+ case c ('s', 4):
/* Set speed.
- * - b: theta speed.
- * - b: alpha speed. */
- speed_control_set_speed (&cs_main.speed_theta, args[0]);
- speed_control_set_speed (&cs_main.speed_alpha, args[1]);
+ * - w: theta speed.
+ * - w: alpha speed. */
+ speed_control_set_speed (&cs_main.speed_theta,
+ v8_to_v16 (args[0], args[1]));
+ speed_control_set_speed (&cs_main.speed_alpha,
+ v8_to_v16 (args[2], args[3]));
control_state_set_mode (&cs_main.state, CS_MODE_SPEED_CONTROL, 0);
break;
#if AC_ASSERV_AUX_NB
- case c ('S', 2):
+ case c ('S', 3):
/* Set auxiliary speed.
* - b: aux index.
- * - b: speed. */
+ * - w: speed. */
if (!auxp) { proto_send0 ('?'); return; }
- speed_control_set_speed (speed, args[1]);
+ speed_control_set_speed (speed, v8_to_v16 (args[1], args[2]));
control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0);
break;
#endif
@@ -484,18 +486,19 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]));
break;
- case c ('y', 3):
+ case c ('Y', 4):
/* Auxiliary find zero.
* - b: aux index.
- * - b: speed.
+ * - w: speed.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (!seq_start (seq, args[2]))
+ if (!seq_start (seq, args[3]))
break;
if (args[0] == 0)
- aux_traj_find_limit_start (auxp, args[1]);
+ aux_traj_find_limit_start (auxp, v8_to_v16 (args[1], args[2]));
else
- aux_traj_find_zero_reverse_start (auxp, args[1]);
+ aux_traj_find_zero_reverse_start (auxp,
+ v8_to_v16 (args[1], args[2]));
break;
case c ('a', 3):
/* Set all acknoledge.
@@ -634,16 +637,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: index.
* - w: acceleration. */
if (!speed) { proto_send0 ('?'); return; }
- speed->acc = v8_to_v16 (args[2], args[3]);
+ speed->acc_f = v8_to_v16 (args[2], args[3]);
break;
- case c ('s', 4):
+ case c ('s', 6):
/* Set maximum and slow speed.
* - b: index.
- * - b: max speed.
- * - b: slow speed. */
+ * - w: max speed.
+ * - w: slow speed. */
if (!speed) { proto_send0 ('?'); return; }
- speed->max = args[2];
- speed->slow = args[3];
+ speed->max = v8_to_v16 (args[2], args[3]);
+ speed->slow = v8_to_v16 (args[4], args[5]);
break;
case c ('p', 4):
/* Set proportional coefficient.
@@ -746,8 +749,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: index. */
if (!pos) { proto_send0 ('?'); return; }
proto_send2b ('E', EEPROM_KEY, eeprom_loaded);
- proto_send1w ('a', speed->acc);
- proto_send2b ('s', speed->max, speed->slow);
+ proto_send1w ('a', speed->acc_f);
+ proto_send2w ('s', speed->max, speed->slow);
proto_send3w ('b', bd->error_limit, bd->speed_limit,
bd->counter_limit);
proto_send1w ('p', pos->kp);
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index abf6c730..48802fcb 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -150,7 +150,7 @@ static void
traj_ftw (void)
{
uint8_t left, center, right;
- int8_t speed;
+ int16_t speed;
speed = cs_main.speed_theta.slow;
if (!traj_backward)
{
@@ -260,7 +260,7 @@ void
traj_ptw_start (uint8_t backward, int32_t init_x, int32_t init_y,
int32_t init_a)
{
- int8_t speed;
+ int16_t speed;
traj_mode = TRAJ_PTW;
traj_init_x = init_x;
traj_init_y = init_y;
@@ -281,7 +281,7 @@ static void
traj_gtd (void)
{
#ifdef CONTACT_CENTER_IO
- int8_t speed;
+ int16_t speed;
speed = cs_main.speed_theta.slow;
if (IO_GET (CONTACT_CENTER_IO))
{
diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c
index d466493f..190c3caf 100644
--- a/digital/asserv/src/asserv/twi_proto.c
+++ b/digital/asserv/src/asserv/twi_proto.c
@@ -79,8 +79,8 @@ twi_proto_update (void)
| (control_state_is_blocked (&cs_aux[0].state) ? (1 << 5) : 0)
| (control_state_is_finished (&cs_aux[0].state) ? (1 << 4) : 0)
#endif
- | (cs_main.speed_theta.cur < 0 ? (1 << 3) : 0)
- | (cs_main.speed_theta.cur > 0 ? (1 << 2) : 0)
+ | (cs_main.speed_theta.cur_f < 0 ? (1 << 3) : 0)
+ | (cs_main.speed_theta.cur_f > 0 ? (1 << 2) : 0)
| (control_state_is_blocked (&cs_main.state) ? (1 << 1) : 0)
| (control_state_is_finished (&cs_main.state) ? (1 << 0) : 0);
status[1] = PINC;
@@ -216,29 +216,29 @@ twi_proto_callback (u8 *buf, u8 size)
buf[10]);
break;
#if AC_ASSERV_AUX_NB
- case c ('b', 3):
+ case c ('b', 4):
/* Move the aux0.
* - w: new position.
- * - b: speed. */
- cs_aux[0].speed.max = buf[4];
+ * - w: speed. */
+ cs_aux[0].speed.max = v8_to_v16 (buf[4], buf[5]);
aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]));
break;
- case c ('B', 1):
+ case c ('B', 2):
/* Find the aux0 zero position.
- * - b: speed. */
- aux_traj_find_limit_start (&aux[0], buf[2]);
+ * - w: speed. */
+ aux_traj_find_limit_start (&aux[0], v8_to_v16 (buf[2], buf[3]));
break;
- case c ('c', 3):
+ case c ('c', 4):
/* Move the aux1.
* - w: new position.
- * - b: speed. */
- cs_aux[1].speed.max = buf[4];
+ * - w: speed. */
+ cs_aux[1].speed.max = v8_to_v16 (buf[4], buf[5]);
aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]));
break;
- case c ('C', 1):
+ case c ('C', 2):
/* Find the aux1 zero position.
- * - b: speed. */
- aux_traj_find_zero_reverse_start (&aux[1], buf[2]);
+ * - w: speed. */
+ aux_traj_find_zero_reverse_start (&aux[1], v8_to_v16 (buf[2], buf[3]));
break;
case c ('r', 1):
/* Set aux zero pwm.
@@ -302,16 +302,16 @@ twi_proto_params (u8 *buf, u8 size)
break;
case 's':
/* Set maximum and slow speed.
- * - b: theta max.
- * - b: alpha max.
- * - b: theta slow.
- * - b: alpha slow. */
- if (size < 4)
+ * - w: theta max.
+ * - w: alpha max.
+ * - w: theta slow.
+ * - w: alpha slow. */
+ if (size < 8)
return 1;
- cs_main.speed_theta.max = buf[0];
- cs_main.speed_alpha.max = buf[1];
- cs_main.speed_theta.slow = buf[2];
- cs_main.speed_alpha.slow = buf[3];
+ cs_main.speed_theta.max = v8_to_v16 (buf[0], buf[1]);
+ cs_main.speed_alpha.max = v8_to_v16 (buf[2], buf[3]);
+ cs_main.speed_theta.slow = v8_to_v16 (buf[4], buf[5]);
+ cs_main.speed_alpha.slow = v8_to_v16 (buf[6], buf[7]);
eat = 4;
break;
default:
diff --git a/digital/asserv/tools/asserv/asserv.py b/digital/asserv/tools/asserv/asserv.py
index b97174e0..fb45c633 100644
--- a/digital/asserv/tools/asserv/asserv.py
+++ b/digital/asserv/tools/asserv/asserv.py
@@ -36,7 +36,7 @@ class Proto:
stats_format = {
'C': 'HHHH',
- 'S': 'bbbb',
+ 'S': 'hhhh',
'P': 'hhhh',
'Q': 'hhhh',
'W': 'hhhh',
@@ -189,11 +189,11 @@ class Proto:
def speed (self, w, s):
"""Speed consign."""
if w == 't':
- self.proto.send ('s', 'bb', s, 0)
+ self.proto.send ('s', 'hh', s, 0)
elif w == 'a':
- self.proto.send ('s', 'bb', 0, s)
+ self.proto.send ('s', 'hh', 0, s)
else:
- self.proto.send ('S', 'Bb', self._index[w], s)
+ self.proto.send ('S', 'Bh', self._index[w], s)
def speed_pos (self, w, offset):
"""Speed controlled position consign."""
@@ -310,7 +310,7 @@ class Proto:
self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + '_ki']))
self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + '_kd']))
self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + '_acc']))
- self.proto.send ('p', 'cBBB', 's', index, p[m + '_speed_max'],
+ self.proto.send ('p', 'cBHH', 's', index, p[m + '_speed_max'],
p[m + '_speed_slow'])
self.proto.send ('p', 'cBHHB', 'b', index,
p[m + '_bd_error_limit'], p[m + '_bd_speed_limit'],
diff --git a/digital/avr/modules/motor/speed_control/speed_control.c b/digital/avr/modules/motor/speed_control/speed_control.c
index 2caf0937..c13b4614 100644
--- a/digital/avr/modules/motor/speed_control/speed_control.c
+++ b/digital/avr/modules/motor/speed_control/speed_control.c
@@ -33,44 +33,45 @@ static void
speed_control_update_by_speed (speed_control_t *speed_control)
{
/* Update current speed (be careful of overflow!). */
- if (speed_control->cons > speed_control->cur)
+ if (speed_control->cons_f > speed_control->cur_f)
{
- if ((uint16_t) (speed_control->cons - speed_control->cur)
- < (uint16_t) speed_control->acc)
- speed_control->cur = speed_control->cons;
+ if ((uint32_t) (speed_control->cons_f - speed_control->cur_f)
+ < (uint32_t) speed_control->acc_f)
+ speed_control->cur_f = speed_control->cons_f;
else
- speed_control->cur += speed_control->acc;
+ speed_control->cur_f += speed_control->acc_f;
}
else
{
- if ((uint16_t) (speed_control->cur - speed_control->cons)
- < (uint16_t) speed_control->acc)
- speed_control->cur = speed_control->cons;
+ if ((uint32_t) (speed_control->cur_f - speed_control->cons_f)
+ < (uint32_t) speed_control->acc_f)
+ speed_control->cur_f = speed_control->cons_f;
else
- speed_control->cur -= speed_control->acc;
+ speed_control->cur_f -= speed_control->acc_f;
}
}
/** Compute maximum allowed speed according to: distance left, maximum speed,
* current speed and acceleration. */
-static int16_t
-speed_control_compute_max_speed (int32_t d, int16_t cur, int16_t acc,
- int8_t max)
+static int32_t
+speed_control_compute_max_speed_f (int32_t d, int32_t cur_f, int16_t acc_f,
+ int16_t max)
{
int16_t s;
+ int32_t s_f;
/* Compute maximum speed in order to be able to brake in time.
* The "+ 0xff" is to ceil result.
* s = sqrt (2 * a * d) */
- s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc + 0xff) >> 8);
+ s = fixed_sqrt_ui32 ((2 * UTILS_ABS (d) * acc_f + 0xff) >> 8);
/* Apply consign. */
s = UTILS_MIN (max, s);
/* Apply sign. */
if (d < 0)
s = -s;
/* Convert to f8.8 and check acceleration. */
- s = s << 8;
- UTILS_BOUND (s, cur - acc, cur + acc);
- return s;
+ s_f = (int32_t) s << 8;
+ UTILS_BOUND (s_f, cur_f - acc_f, cur_f + acc_f);
+ return s_f;
}
/** Update current speed according to a position consign. */
@@ -79,8 +80,9 @@ speed_control_update_by_position (speed_control_t *speed_control,
pos_control_t *pos_control)
{
int32_t diff = speed_control->pos_cons - pos_control->cons;
- speed_control->cur = speed_control_compute_max_speed
- (diff, speed_control->cur, speed_control->acc, speed_control->max);
+ speed_control->cur_f = speed_control_compute_max_speed_f
+ (diff, speed_control->cur_f, speed_control->acc_f,
+ speed_control->max);
}
void
@@ -103,17 +105,17 @@ speed_control_update (speed_control_t *speed_control,
else
speed_control_update_by_speed (speed_control);
/* Update shaft position. */
- speed_control->pos_control->cons += speed_control->cur >> 8;
+ speed_control->pos_control->cons += speed_control->cur_f >> 8;
}
else
- speed_control->cur = 0;
+ speed_control->cur_f = 0;
}
void
-speed_control_set_speed (speed_control_t *speed_control, int8_t speed)
+speed_control_set_speed (speed_control_t *speed_control, int16_t speed)
{
speed_control->use_pos = 0;
- speed_control->cons = speed << 8;
+ speed_control->cons_f = speed << 8;
}
void
@@ -143,8 +145,8 @@ speed_control_hard_stop (speed_control_t *speed_control)
{
/* No future movement. */
speed_control->use_pos = 0;
- speed_control->cur = 0;
- speed_control->cons = 0;
+ speed_control->cur_f = 0;
+ speed_control->cons_f = 0;
/* Really stop right here, position control on the current point. */
speed_control->pos_control->cons = speed_control->pos_control->cur;
}
diff --git a/digital/avr/modules/motor/speed_control/speed_control.h b/digital/avr/modules/motor/speed_control/speed_control.h
index 14e1ef15..714f3d54 100644
--- a/digital/avr/modules/motor/speed_control/speed_control.h
+++ b/digital/avr/modules/motor/speed_control/speed_control.h
@@ -30,20 +30,20 @@
/** Speed control state. */
struct speed_control_t
{
- /** Current speed, f8.8. */
- int16_t cur;
+ /** Current speed, f16.8. */
+ int32_t cur_f;
/** Whether to use the consign position (1) or not (0). */
uint8_t use_pos;
- /** Consign speed, f8.8. */
- int16_t cons;
+ /** Consign speed, f16.8. */
+ int32_t cons_f;
/** Consign position. */
uint32_t pos_cons;
- /** Maximum speed for position consign, u7. */
- int8_t max;
- /** Slow speed, deprecated, u7. */
- int8_t slow;
+ /** Maximum speed for position consign, u15. */
+ int16_t max;
+ /** Slow speed, deprecated, u15. */
+ int16_t slow;
/** Acceleration, f8.8. */
- int16_t acc;
+ int16_t acc_f;
/** Associated position control, to simplify function prototypes. */
pos_control_t *pos_control;
};
@@ -61,7 +61,7 @@ speed_control_update (speed_control_t *speed_control,
/** Set speed consign. Accelerate to the given speed. */
void
-speed_control_set_speed (speed_control_t *speed_control, int8_t speed);
+speed_control_set_speed (speed_control_t *speed_control, int16_t speed);
/** Set position consign offset. Move to a position measured from the current
* controlled position (which may be different from the actual position),
diff --git a/digital/avr/modules/motor/speed_control/speed_control.txt b/digital/avr/modules/motor/speed_control/speed_control.txt
index 94b4d0b4..2cdebb46 100644
--- a/digital/avr/modules/motor/speed_control/speed_control.txt
+++ b/digital/avr/modules/motor/speed_control/speed_control.txt
@@ -13,7 +13,7 @@ too high.
Usage
=====
-The `max` and `acc` parameters should be set to maximum speed and maximum
+The `max` and `acc_f` parameters should be set to maximum speed and maximum
acceleration. Other fields should better be changed using the helper
functions.
diff --git a/digital/mimot/src/dirty/aux.c b/digital/mimot/src/dirty/aux.c
index 50bebc3a..d9110e11 100644
--- a/digital/mimot/src/dirty/aux.c
+++ b/digital/mimot/src/dirty/aux.c
@@ -162,7 +162,7 @@ aux_traj_clamp (struct aux_t *aux)
}
void
-aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm)
+aux_traj_clamp_start (struct aux_t *aux, int16_t speed, int16_t clampin_pwm)
{
aux->traj_mode = AUX_TRAJ_CLAMP;
aux->clampin_pwm = clampin_pwm;
@@ -196,7 +196,7 @@ aux_traj_find_zero (struct aux_t *aux)
/** Start find zero mode. */
void
-aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos)
+aux_traj_find_zero_start (struct aux_t *aux, int16_t speed, int16_t reset_pos)
{
aux->traj_mode = AUX_TRAJ_FIND_ZERO_NOT;
aux->reset_pos = reset_pos;
@@ -236,7 +236,7 @@ aux_traj_find_limit (struct aux_t *aux)
/** Start find limit mode. */
void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed, int16_t reset_pos)
+aux_traj_find_limit_start (struct aux_t *aux, int16_t speed, int16_t reset_pos)
{
aux->traj_mode = AUX_TRAJ_FIND_LIMIT;
speed_control_set_speed (&aux->cs->speed, speed);
diff --git a/digital/mimot/src/dirty/aux.h b/digital/mimot/src/dirty/aux.h
index 133b7d51..2716f438 100644
--- a/digital/mimot/src/dirty/aux.h
+++ b/digital/mimot/src/dirty/aux.h
@@ -66,13 +66,13 @@ void
aux_traj_goto_start (struct aux_t *aux, uint16_t pos);
void
-aux_traj_clamp_start (struct aux_t *aux, int8_t speed, int16_t clampin_pwm);
+aux_traj_clamp_start (struct aux_t *aux, int16_t speed, int16_t clampin_pwm);
void
-aux_traj_find_zero_start (struct aux_t *aux, int8_t speed, int16_t reset_pos);
+aux_traj_find_zero_start (struct aux_t *aux, int16_t speed, int16_t reset_pos);
void
-aux_traj_find_limit_start (struct aux_t *aux, int8_t speed,
+aux_traj_find_limit_start (struct aux_t *aux, int16_t speed,
int16_t reset_pos);
void
diff --git a/digital/mimot/src/dirty/eeprom.avr.c b/digital/mimot/src/dirty/eeprom.avr.c
index faf2c282..8a49a27d 100644
--- a/digital/mimot/src/dirty/eeprom.avr.c
+++ b/digital/mimot/src/dirty/eeprom.avr.c
@@ -45,9 +45,9 @@ struct eeprom_t
uint8_t key;
/** Saved parameters. */
struct {
- uint8_t max;
- uint8_t slow;
- uint16_t acc;
+ uint16_t max;
+ uint16_t slow;
+ uint16_t acc_f;
} speed[EEPROM_INDEX_NB];
struct {
uint16_t kp;
@@ -92,7 +92,7 @@ eeprom_read_params_helper (struct eeprom_t *loaded, uint8_t index,
{
speed->max = loaded->speed[index].max;
speed->slow = loaded->speed[index].slow;
- speed->acc = loaded->speed[index].acc;
+ speed->acc_f = loaded->speed[index].acc_f;
pos->kp = loaded->pos[index].kp;
pos->ki = loaded->pos[index].ki;
pos->kd = loaded->pos[index].kd;
@@ -154,7 +154,7 @@ eeprom_write_params_helper (struct eeprom_t *param, uint8_t index,
{
param->speed[index].max = speed->max;
param->speed[index].slow = speed->slow;
- param->speed[index].acc = speed->acc;
+ param->speed[index].acc_f = speed->acc_f;
param->pos[index].kp = pos->kp;
param->pos[index].ki = pos->ki;
param->pos[index].kd = pos->kd;
diff --git a/digital/mimot/src/dirty/eeprom.h b/digital/mimot/src/dirty/eeprom.h
index 6196cd99..0e9dc122 100644
--- a/digital/mimot/src/dirty/eeprom.h
+++ b/digital/mimot/src/dirty/eeprom.h
@@ -26,7 +26,7 @@
* }}} */
/** Change the eeprom key each time you change eeprom format. */
-#define EEPROM_KEY 0x03
+#define EEPROM_KEY 0x04
extern int8_t eeprom_loaded;
diff --git a/digital/mimot/src/dirty/main.c b/digital/mimot/src/dirty/main.c
index af61a5a3..03bae08a 100644
--- a/digital/mimot/src/dirty/main.c
+++ b/digital/mimot/src/dirty/main.c
@@ -147,8 +147,8 @@ main_loop (void)
}
if (main_stat_speed && !--main_stat_speed_cpt)
{
- proto_send2b ('S', cs_aux[0].speed.cur >> 8,
- cs_aux[1].speed.cur >> 8);
+ proto_send2w ('S', cs_aux[0].speed.cur_f >> 8,
+ cs_aux[1].speed.cur_f >> 8);
main_stat_speed_cpt = main_stat_speed;
}
if (main_stat_pos && !--main_stat_pos_cpt)
@@ -248,12 +248,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
speed_control_set_speed (speed, 0);
control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0);
break;
- case c ('s', 2):
+ case c ('s', 3):
/* Set auxiliary speed.
* - b: aux index.
- * - b: speed. */
+ * - w: speed. */
if (!auxp) { proto_send0 ('?'); return; }
- speed_control_set_speed (speed, args[1]);
+ speed_control_set_speed (speed, v8_to_v16 (args[1], args[2]));
control_state_set_mode (state, CS_MODE_SPEED_CONTROL, 0);
break;
case c ('s', 6):
@@ -279,33 +279,34 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
break;
aux_traj_goto_start (auxp, v8_to_v16 (args[1], args[2]));
break;
- case c ('y', 5):
+ case c ('y', 6):
/* Auxiliary clamp.
* - b: aux index.
- * - b: speed.
+ * - w: speed.
* - w: clamping PWM.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (!seq_start (seq, args[4]))
+ if (!seq_start (seq, args[5]))
break;
- aux_traj_clamp_start (auxp, args[1], v8_to_v16 (args[2], args[3]));
+ aux_traj_clamp_start (auxp, v8_to_v16 (args[1], args[2]),
+ v8_to_v16 (args[3], args[4]));
break;
- case c ('y', 6):
+ case c ('y', 7):
/* Auxiliary find zero.
* - b: aux index.
- * - b: speed.
+ * - w: speed.
* - b: use switch.
* - w: reset position.
* - b: sequence number. */
if (!auxp) { proto_send0 ('?'); return; }
- if (!seq_start (seq, args[5]))
+ if (!seq_start (seq, args[6]))
break;
- if (args[2])
- aux_traj_find_zero_start (auxp, args[1],
- v8_to_v16 (args[3], args[4]));
+ if (args[3])
+ aux_traj_find_zero_start (auxp, v8_to_v16 (args[1], args[2]),
+ v8_to_v16 (args[4], args[5]));
else
- aux_traj_find_limit_start (auxp, args[1],
- v8_to_v16 (args[3], args[4]));
+ aux_traj_find_limit_start (auxp, v8_to_v16 (args[1], args[2]),
+ v8_to_v16 (args[4], args[5]));
break;
case c ('a', 2):
/* Set all acknoledge.
@@ -374,16 +375,16 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: index.
* - w: acceleration. */
if (!speed) { proto_send0 ('?'); return; }
- speed->acc = v8_to_v16 (args[2], args[3]);
+ speed->acc_f = v8_to_v16 (args[2], args[3]);
break;
- case c ('s', 4):
+ case c ('s', 6):
/* Set maximum and slow speed.
* - b: index.
- * - b: max speed.
- * - b: slow speed. */
+ * - w: max speed.
+ * - w: slow speed. */
if (!speed) { proto_send0 ('?'); return; }
- speed->max = args[2];
- speed->slow = args[3];
+ speed->max = v8_to_v16 (args[2], args[3]);
+ speed->slow = v8_to_v16 (args[4], args[5]);
break;
case c ('p', 4):
/* Set proportional coefficient.
@@ -457,8 +458,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - b: index. */
if (!pos) { proto_send0 ('?'); return; }
proto_send2b ('E', EEPROM_KEY, eeprom_loaded);
- proto_send1w ('a', speed->acc);
- proto_send2b ('s', speed->max, speed->slow);
+ proto_send1w ('a', speed->acc_f);
+ proto_send2w ('s', speed->max, speed->slow);
proto_send3w ('b', bd->error_limit, bd->speed_limit,
bd->counter_limit);
proto_send1w ('p', pos->kp);
diff --git a/digital/mimot/src/dirty/twi_proto.c b/digital/mimot/src/dirty/twi_proto.c
index 5398815f..a61d3d6c 100644
--- a/digital/mimot/src/dirty/twi_proto.c
+++ b/digital/mimot/src/dirty/twi_proto.c
@@ -110,46 +110,48 @@ twi_proto_callback (u8 *buf, u8 size)
/* Reset. */
utils_reset ();
break;
- case c ('b', 3):
+ case c ('b', 4):
/* Move the aux0.
* - w: new position.
- * - b: speed. */
- cs_aux[0].speed.max = buf[4];
+ * - w: speed. */
+ cs_aux[0].speed.max = v8_to_v16 (buf[4], buf[5]);
aux_traj_goto_start (&aux[0], v8_to_v16 (buf[2], buf[3]));
break;
case c ('c', 3):
/* Move the aux1.
* - w: new position.
- * - b: speed. */
- cs_aux[1].speed.max = buf[4];
+ * - w: speed. */
+ cs_aux[1].speed.max = v8_to_v16 (buf[4], buf[5]);
aux_traj_goto_start (&aux[1], v8_to_v16 (buf[2], buf[3]));
break;
- case c ('B', 5):
+ case c ('B', 6):
/* Find the zero position.
* - b: aux index.
- * - b: speed.
+ * - w: speed.
* - b: use switch.
* - w: reset position. */
if (buf[2] < AC_ASSERV_AUX_NB)
{
- if (buf[4])
- aux_traj_find_zero_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[5], buf[6]));
+ if (buf[5])
+ aux_traj_find_zero_start (&aux[buf[2]],
+ v8_to_v16 (buf[3], buf[4]),
+ v8_to_v16 (buf[6], buf[7]));
else
- aux_traj_find_limit_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[5], buf[6]));
+ aux_traj_find_limit_start (&aux[buf[2]],
+ v8_to_v16 (buf[3], buf[4]),
+ v8_to_v16 (buf[6], buf[7]));
}
else
buf[0] = 0;
break;
- case c ('l', 4):
+ case c ('l', 5):
/* Clamp.
* - b: aux index.
- * - b: speed.
+ * - w: speed.
* - w: claming PWM. */
if (buf[2] < AC_ASSERV_AUX_NB)
- aux_traj_clamp_start (&aux[buf[2]], buf[3],
- v8_to_v16 (buf[4], buf[5]));
+ aux_traj_clamp_start (&aux[buf[2]], v8_to_v16 (buf[3], buf[4]),
+ v8_to_v16 (buf[5], buf[6]));
else
buf[0] = 0;
break;
diff --git a/digital/mimot/tools/mimot/mimot.py b/digital/mimot/tools/mimot/mimot.py
index 836479fb..cb4caace 100644
--- a/digital/mimot/tools/mimot/mimot.py
+++ b/digital/mimot/tools/mimot/mimot.py
@@ -36,7 +36,7 @@ class Proto:
stats_format = {
'C': 'HH',
- 'S': 'bb',
+ 'S': 'hh',
'P': 'hhhh',
'W': 'hh',
}
@@ -140,7 +140,7 @@ class Proto:
def speed (self, w, s):
"""Speed consign."""
- self.proto.send ('s', 'Bb', self._index[w], s)
+ self.proto.send ('s', 'Bh', self._index[w], s)
def speed_pos (self, w, offset):
"""Speed controlled position consign."""
@@ -160,14 +160,14 @@ class Proto:
"""Clamp (speed control, then open loop PWM)."""
i = self._index[w]
self.aseq[i] += 1
- self.proto.send ('y', 'BBhB', i, s, pwm, self.aseq[i])
+ self.proto.send ('y', 'BHhB', i, s, pwm, self.aseq[i])
self.wait (self.finished, auto = True)
def find_zero (self, w, s, use_switch = True, reset_pos = 0):
"""Find zero position."""
i = self._index[w]
self.aseq[i] += 1
- self.proto.send ('y', 'BBBhB', i, s, 1 if use_switch else 0,
+ self.proto.send ('y', 'BHBhB', i, s, 1 if use_switch else 0,
reset_pos, self.aseq[i])
self.wait (self.finished, auto = True)
@@ -182,7 +182,7 @@ class Proto:
self.proto.send ('p', 'cBH', 'i', index, f88 (p[m + '_ki']))
self.proto.send ('p', 'cBH', 'd', index, f88 (p[m + '_kd']))
self.proto.send ('p', 'cBH', 'a', index, f88 (p[m + '_acc']))
- self.proto.send ('p', 'cBBB', 's', index, p[m + '_speed_max'],
+ self.proto.send ('p', 'cBHH', 's', index, p[m + '_speed_max'],
p[m + '_speed_slow'])
self.proto.send ('p', 'cBHHB', 'b', index,
p[m + '_bd_error_limit'], p[m + '_bd_speed_limit'],