From e6ef3e6642c4966a59b176fe6032939451954564 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Wed, 30 Apr 2008 23:06:37 +0200 Subject: * digital/asserv/src/asserv: - added angle limit setting. --- digital/asserv/src/asserv/eeprom.avr.c | 2 ++ digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 7 ++++++- digital/asserv/src/asserv/traj.c | 23 ++++++++++++++++++++++- digital/asserv/src/asserv/traj.h | 7 +++++++ digital/asserv/tools/asserv.py | 2 ++ 6 files changed, 40 insertions(+), 3 deletions(-) (limited to 'digital') 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) -- cgit v1.2.3