summaryrefslogtreecommitdiff
path: root/digital/mimot/src/dirty/pos.c
diff options
context:
space:
mode:
authorNicolas Schodet2011-04-19 21:26:15 +0200
committerNicolas Schodet2011-04-19 21:26:15 +0200
commit4cd9c4458eaa49c269171145f4bc2a184948eae1 (patch)
tree780cdc9493bfcfd2907a61cce62ff8a6f64a0c45 /digital/mimot/src/dirty/pos.c
parente2594ccdadae65d7e7473e51fb8bfbbd49c04a24 (diff)
digital/mimot: switch to differential drive
Diffstat (limited to 'digital/mimot/src/dirty/pos.c')
-rw-r--r--digital/mimot/src/dirty/pos.c75
1 files changed, 45 insertions, 30 deletions
diff --git a/digital/mimot/src/dirty/pos.c b/digital/mimot/src/dirty/pos.c
index 28684414..6304bb5c 100644
--- a/digital/mimot/src/dirty/pos.c
+++ b/digital/mimot/src/dirty/pos.c
@@ -40,8 +40,8 @@
* 16 bits are enough as long as there is no long blocking (see 2005 cup!).
*/
-/** Auxiliaries control states. */
-struct pos_t pos_aux[AC_ASSERV_AUX_NB];
+/** Theta/alpha control states. */
+struct pos_t pos_theta, pos_alpha;
/** Error saturation. */
int32_t pos_e_sat = 1023;
@@ -84,43 +84,59 @@ pos_compute_pid (int32_t e, struct pos_t *pos)
return pid >> 8;
}
-/** Update PWM for a single motor system. */
-static void
-pos_update_single (struct state_t *state, struct pos_t *pos,
- int16_t counter_diff, struct pwm_t *pwm)
+/** Update PWM for a polar motor system. */
+void
+pos_update_polar (struct state_t *state,
+ struct pos_t *pos_theta, struct pos_t *pos_alpha,
+ int16_t counter_left_diff, int16_t counter_right_diff,
+ struct pwm_t *pwm_left, struct pwm_t *pwm_right)
{
if (state->mode >= MODE_POS)
{
- int16_t pid;
- int32_t error;
- /* Update current shaft position. */
- pos->cur += counter_diff;
+ int16_t pid_theta, pid_alpha;
+ int32_t error_theta, error_alpha;
+ int16_t cur_speed_theta, cur_speed_alpha;
+ /* Update current shaft positions. */
+ cur_speed_theta = counter_left_diff + counter_right_diff;
+ cur_speed_alpha = counter_right_diff - counter_left_diff;
+ pos_theta->cur += cur_speed_theta;
+ pos_alpha->cur += cur_speed_alpha;
+ if (state->variant & 1)
+ pos_reset (pos_theta);
+ if (state->variant & 2)
+ pos_reset (pos_alpha);
/* Compute error. */
- error = pos->cons - pos->cur;
- /* Test or blocking. */
- if (UTILS_ABS (error) > pos->blocked_error_limit
- && UTILS_ABS (counter_diff) < pos->blocked_speed_limit)
- pos->blocked_counter++;
+ error_theta = pos_theta->cons - pos_theta->cur;
+ error_alpha = pos_alpha->cons - pos_alpha->cur;
+ /* Test for blocking. */
+ if (UTILS_ABS (error_theta) > pos_theta->blocked_error_limit
+ && UTILS_ABS (cur_speed_theta) < pos_theta->blocked_speed_limit)
+ pos_theta->blocked_counter++;
else
- pos->blocked_counter = 0;
- if (state->variant & 1)
- {
- pos_reset (pos);
- }
- else if (!(state->variant & 4)
- && pos->blocked_counter > pos->blocked_counter_limit)
+ pos_theta->blocked_counter = 0;
+ if (UTILS_ABS (error_alpha) > pos_alpha->blocked_error_limit
+ && UTILS_ABS (cur_speed_alpha) < pos_alpha->blocked_speed_limit)
+ pos_alpha->blocked_counter++;
+ else
+ pos_alpha->blocked_counter = 0;
+ if (pos_theta->blocked_counter > pos_theta->blocked_counter_limit
+ || pos_alpha->blocked_counter > pos_alpha->blocked_counter_limit)
{
/* Blocked. */
- pos_reset (pos);
+ pos_reset (pos_theta);
+ pos_reset (pos_alpha);
state_blocked (state);
- pwm_set (pwm, 0);
+ pwm_set (pwm_left, 0);
+ pwm_set (pwm_right, 0);
}
else
{
/* Compute PID. */
- pid = pos_compute_pid (error, pos);
+ pid_theta = pos_compute_pid (error_theta, pos_theta);
+ pid_alpha = pos_compute_pid (error_alpha, pos_alpha);
/* Update PWM. */
- pwm_set (pwm, pid);
+ pwm_set (pwm_left, pid_theta - pid_alpha);
+ pwm_set (pwm_right, pid_theta + pid_alpha);
}
}
}
@@ -129,10 +145,9 @@ pos_update_single (struct state_t *state, struct pos_t *pos,
void
pos_update (void)
{
- uint8_t i;
- for (i = 0; i < AC_ASSERV_AUX_NB; i++)
- pos_update_single (&state_aux[i], &pos_aux[i], counter_aux_diff[i],
- &pwm_aux[i]);
+ pos_update_polar (&state_main, &pos_theta, &pos_alpha,
+ counter_left_diff, counter_right_diff,
+ &pwm_left, &pwm_right);
}
/** Reset position control state. To be called when the position control is