summaryrefslogtreecommitdiffhomepage
path: root/digital/asserv/src/asserv/simu.host.c
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-17 00:32:45 +0200
committerNicolas Schodet2008-04-17 00:32:45 +0200
commit0db1380594157e8b15c62b46bdd75ebd010dc9dc (patch)
treece1e190a3a75c47bc3dc2d157b2e75712b9b9c0f /digital/asserv/src/asserv/simu.host.c
parenta2075772c77f87aae93c5766ef697019aa77ff51 (diff)
* digital/asserv/src/asserv:
- added counter correction to handle wheels with different size.
Diffstat (limited to 'digital/asserv/src/asserv/simu.host.c')
-rw-r--r--digital/asserv/src/asserv/simu.host.c13
1 files changed, 11 insertions, 2 deletions
diff --git a/digital/asserv/src/asserv/simu.host.c b/digital/asserv/src/asserv/simu.host.c
index 803142dd..8b499123 100644
--- a/digital/asserv/src/asserv/simu.host.c
+++ b/digital/asserv/src/asserv/simu.host.c
@@ -28,6 +28,7 @@
#include "modules/host/host.h"
#include "modules/host/mex.h"
#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
#include <math.h>
#include <stdlib.h>
@@ -45,8 +46,12 @@ uint8_t DDRF, PORTC, PORTD, PORTE, PORTF, PORTG, PINC;
/** Overall counter values. */
uint16_t counter_left, counter_right, counter_aux0;
/** Counter differences since last update.
- * Maximum of 7 significant bits, sign included. */
+ * Maximum of 9 significant bits, sign included. */
int16_t counter_left_diff, counter_right_diff, counter_aux0_diff;
+/** Overall uncorrected counter values. */
+static int32_t counter_right_raw;
+/** Correction factor (f8.24). */
+uint32_t counter_right_correction = 1L << 24;
/** PWM values, this is an error if absolute value is greater than the
* maximum. */
@@ -207,7 +212,11 @@ simu_step (void)
counter_left += counter_left_diff;
simu_counter_left = counter_left_new;
counter_right_diff = counter_right_new - simu_counter_right;
- counter_right += counter_right_diff;
+ counter_right_raw += counter_right_diff;
+ uint16_t right_new = fixed_mul_f824 (counter_right_raw,
+ counter_right_correction);
+ counter_right_diff = (int16_t) (right_new - counter_right);
+ counter_right = right_new;
simu_counter_right = counter_right_new;
/* Update auxiliary counter. */
if (simu_robot->aux0_motor)