summaryrefslogtreecommitdiffhomepage
path: root/digital
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
parenta2075772c77f87aae93c5766ef697019aa77ff51 (diff)
* digital/asserv/src/asserv:
- added counter correction to handle wheels with different size.
Diffstat (limited to 'digital')
-rw-r--r--digital/asserv/src/asserv/counter.h1
-rw-r--r--digital/asserv/src/asserv/counter_ext.avr.c16
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c25
-rw-r--r--digital/asserv/src/asserv/eeprom.h2
-rw-r--r--digital/asserv/src/asserv/main.c7
-rw-r--r--digital/asserv/src/asserv/simu.host.c13
6 files changed, 59 insertions, 5 deletions
diff --git a/digital/asserv/src/asserv/counter.h b/digital/asserv/src/asserv/counter.h
index 2e17dbfb..c5eae18b 100644
--- a/digital/asserv/src/asserv/counter.h
+++ b/digital/asserv/src/asserv/counter.h
@@ -26,6 +26,7 @@
* }}} */
extern uint16_t counter_left, counter_right, counter_aux0;
+extern uint32_t counter_right_correction;
extern int16_t counter_left_diff, counter_right_diff, counter_aux0_diff;
void
diff --git a/digital/asserv/src/asserv/counter_ext.avr.c b/digital/asserv/src/asserv/counter_ext.avr.c
index f0d1622d..4f8cad3b 100644
--- a/digital/asserv/src/asserv/counter_ext.avr.c
+++ b/digital/asserv/src/asserv/counter_ext.avr.c
@@ -26,12 +26,15 @@
#include "counter.h"
#include "modules/utils/utils.h"
+#include "modules/math/fixed/fixed.h"
#include "io.h"
/**
* This file add support for an external counter like the hdlcounter or
* avrcounter project. This can be better in order not to loose steps and
* support more counters.
+ *
+ * There is additionnal support for error correction on the right counter.
*/
/** Define the left counter address. */
@@ -56,8 +59,12 @@
static uint8_t counter_left_old, counter_right_old, counter_aux0_old;
/** Overall counter values. */
uint16_t counter_left, counter_right, counter_aux0;
+/** Overall uncorrected counter values. */
+static int32_t counter_right_raw;
+/** Correction factor (f8.24). */
+uint32_t counter_right_correction = 1L << 24;
/** 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;
#if !COUNTER_USE_XMEM
@@ -137,7 +144,12 @@ counter_update (void)
counter_right_diff = (int8_t) (counter_right_old - right);
#endif
counter_right_old = right;
- counter_right += counter_right_diff;
+ /* Fix right counter. */
+ 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;
/* First auxiliary counter. */
#if !COUNTER_AUX0_REVERSE
counter_aux0_diff = (int8_t) (aux0 - counter_aux0_old);
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index 9f4920a4..4277ceac 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -25,8 +25,11 @@
#include "common.h"
#include "eeprom.h"
+#include "modules/utils/byte.h"
+
#include <avr/eeprom.h>
+#include "counter.h"
#include "pwm.h"
#include "pos.h"
#include "speed.h"
@@ -40,6 +43,26 @@
* your new format is not compatible with the old one or you will load
* garbages in parameters. */
+static uint32_t
+eeprom_read_dword (uint8_t *addr)
+{
+ uint8_t dw[4];
+ dw[0] = eeprom_read_byte (addr++);
+ dw[1] = eeprom_read_byte (addr++);
+ dw[2] = eeprom_read_byte (addr++);
+ dw[3] = eeprom_read_byte (addr++);
+ return v8_to_v32 (dw[3], dw[2], dw[1], dw[0]);
+}
+
+static void
+eeprom_write_dword (uint8_t *addr, uint32_t dw)
+{
+ eeprom_write_byte (addr++, v32_to_v8 (dw, 0));
+ eeprom_write_byte (addr++, v32_to_v8 (dw, 1));
+ eeprom_write_byte (addr++, v32_to_v8 (dw, 2));
+ eeprom_write_byte (addr++, v32_to_v8 (dw, 3));
+}
+
/* Read parameters from eeprom. */
void
eeprom_read_params (void)
@@ -55,6 +78,7 @@ eeprom_read_params (void)
speed_alpha.slow = eeprom_read_byte (p8++);
speed_aux0.slow = eeprom_read_byte (p8++);
pwm_set_reverse (eeprom_read_byte (p8++));
+ counter_right_correction = eeprom_read_dword (p8); p8 += 4;
p16 = (uint16_t *) p8;
postrack_set_footing (eeprom_read_word (p16++));
speed_theta.acc = eeprom_read_word (p16++);
@@ -89,6 +113,7 @@ eeprom_write_params (void)
eeprom_write_byte (p8++, speed_alpha.slow);
eeprom_write_byte (p8++, speed_aux0.slow);
eeprom_write_byte (p8++, pwm_reverse);
+ eeprom_write_dword (p8, counter_right_correction); p8 += 4;
p16 = (uint16_t *) p8;
eeprom_write_word (p16++, postrack_footing);
eeprom_write_word (p16++, speed_theta.acc);
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index 2f3d5c74..c3a57075 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 0x47
+#define EEPROM_KEY 0x48
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 1e242a1d..5913c107 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -441,6 +441,12 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
* - d: angle. */
postrack_a = v8_to_v32 (args[1], args[2], args[3], args[4]);
break;
+ case c ('c', 5):
+ /* Set right counter correction factor.
+ * - d: factor (f8.24). */
+ counter_right_correction = v8_to_v32 (args[1], args[2],
+ args[3], args[4]);
+ break;
case c ('f', 3):
/* Set footing.
* - w: footing. */
@@ -525,6 +531,7 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
case c ('P', 1):
/* Print current settings. */
proto_send1b ('E', EEPROM_KEY);
+ proto_send1d ('c', counter_right_correction);
proto_send1w ('f', postrack_footing);
proto_send2w ('a', speed_theta.acc, speed_alpha.acc);
proto_send4b ('s', speed_theta.max, speed_alpha.max,
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)