summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2008-04-30 23:06:37 +0200
committerNicolas Schodet2008-04-30 23:06:37 +0200
commite6ef3e6642c4966a59b176fe6032939451954564 (patch)
treee99291836407914e09ed624ade113af75b2bb4f9
parentb82634f57317dfba15ede759f5414ac5a22491ce (diff)
* digital/asserv/src/asserv:
- added angle limit setting.
-rw-r--r--digital/asserv/src/asserv/eeprom.avr.c2
-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/traj.c23
-rw-r--r--digital/asserv/src/asserv/traj.h7
-rw-r--r--digital/asserv/tools/asserv.py2
6 files changed, 40 insertions, 3 deletions
diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c
index dc379044..0c91fbc0 100644
--- a/digital/asserv/src/asserv/eeprom.avr.c
+++ b/digital/asserv/src/asserv/eeprom.avr.c
@@ -99,6 +99,7 @@ eeprom_read_params (void)
pos_blocked = eeprom_read_word (p16++);
traj_eps = eeprom_read_word (p16++);
traj_aeps = eeprom_read_word (p16++);
+ traj_set_angle_limit (eeprom_read_word (p16++));
}
/* Write parameters to eeprom. */
@@ -136,6 +137,7 @@ eeprom_write_params (void)
eeprom_write_word (p16++, pos_blocked);
eeprom_write_word (p16++, traj_eps);
eeprom_write_word (p16++, traj_aeps);
+ eeprom_write_word (p16++, traj_angle_limit);
}
/* Clear eeprom parameters. */
diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h
index 8c99fb5f..c9c0a158 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 0x4a
+#define EEPROM_KEY 0x4b
void
eeprom_read_params (void);
diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c
index 9516fd43..2ea3d95d 100644
--- a/digital/asserv/src/asserv/main.c
+++ b/digital/asserv/src/asserv/main.c
@@ -108,6 +108,7 @@ main (int argc, char **argv)
uart0_init ();
twi_proto_init ();
postrack_init ();
+ traj_init ();
eeprom_read_params ();
proto_send0 ('z');
sei ();
@@ -583,6 +584,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
traj_eps = v8_to_v16 (args[1], args[2]);
traj_aeps = v8_to_v16 (args[3], args[4]);
break;
+ case c ('l', 3):
+ traj_set_angle_limit (v8_to_v16 (args[1], args[2]));
+ break;
case c ('w', 2):
/* Set PWM direction.
* - b: bits: 0000[aux0][right][left]. */
@@ -616,7 +620,8 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args)
proto_send1w ('I', pos_i_sat);
proto_send1w ('D', pos_d_sat);
proto_send1w ('b', pos_blocked);
- proto_send1w ('e', traj_eps);
+ proto_send2w ('e', traj_eps, traj_aeps);
+ proto_send1w ('l', traj_angle_limit);
proto_send1b ('w', pwm_reverse);
break;
default:
diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c
index 2b754a26..e0194ba8 100644
--- a/digital/asserv/src/asserv/traj.c
+++ b/digital/asserv/src/asserv/traj.c
@@ -76,6 +76,12 @@ int16_t traj_eps = 500;
/** Angle epsilon, angle considered to be small enough (f0.16). */
int16_t traj_aeps = 0x0100;
+/** Angle at which to start going forward (f0.16). */
+uint16_t traj_angle_limit = 0x2000;
+
+/** Angle at which to start going forward (rad, f8.24). */
+int32_t traj_angle_limit_rad;
+
/** Go to position. */
static uint32_t traj_goto_x, traj_goto_y;
@@ -85,6 +91,13 @@ static uint32_t traj_goto_a;
/** Allow backward movements. */
static uint8_t traj_backward_ok;
+/** Initialise computed factors. */
+void
+traj_init (void)
+{
+ traj_set_angle_limit (traj_angle_limit);
+}
+
/** Angle offset. Directly handled to speed layer. */
void
traj_angle_offset_start (int32_t angle, uint8_t seq)
@@ -207,7 +220,7 @@ traj_goto (void)
/* Compute consign. */
speed_alpha.pos_cons = pos_alpha.cur;
speed_alpha.pos_cons += arc;
- if (UTILS_ABS (arad) < PI_F824 / 2)
+ if (UTILS_ABS (arad) < traj_angle_limit_rad)
{
speed_theta.pos_cons = pos_theta.cur;
speed_theta.pos_cons += dt >> 8;
@@ -330,3 +343,11 @@ traj_update (void)
}
}
+/* Set angle limit. */
+void
+traj_set_angle_limit (uint16_t a)
+{
+ traj_angle_limit = a;
+ traj_angle_limit_rad = (uint32_t) a * (uint32_t) ((1 << 8) * M_PI * 2);
+}
+
diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h
index 8619f0bd..2e5469c1 100644
--- a/digital/asserv/src/asserv/traj.h
+++ b/digital/asserv/src/asserv/traj.h
@@ -28,6 +28,10 @@
extern uint8_t traj_mode;
extern int16_t traj_eps;
extern int16_t traj_aeps;
+extern uint16_t traj_angle_limit;
+
+void
+traj_init (void);
void
traj_update (void);
@@ -51,4 +55,7 @@ void
traj_goto_xya_start (uint32_t x, uint32_t y, uint32_t a, uint8_t backward_ok,
uint8_t seq);
+void
+traj_set_angle_limit (uint16_t a);
+
#endif /* traj_h */
diff --git a/digital/asserv/tools/asserv.py b/digital/asserv/tools/asserv.py
index 8f2c65b7..55bdf125 100644
--- a/digital/asserv/tools/asserv.py
+++ b/digital/asserv/tools/asserv.py
@@ -52,6 +52,7 @@ class Asserv:
ta = 1, aa = 1, a0a = 1,
tsm = 0, asm = 0, tss = 0, ass = 0, a0sm = 0, a0ss = 0,
c = 1, f = 0x1000,
+ l = 0x2000,
)
self.param.update (param)
self.send_param ()
@@ -198,6 +199,7 @@ class Asserv:
p['tss'], p['ass'])
self.proto.send ('p', 'BL', ord ('c'), p['c'] * 256 * 256 * 256)
self.proto.send ('p', 'BH', ord ('f'), p['f'])
+ self.proto.send ('p', 'BH', ord ('l'), p['l'])
def write_eeprom (self):
self.proto.send ('p', 'BB', ord ('E'), 1)