From 6da4d0ba2001dda52ef1ac7917f0e88cec93bebc Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Thu, 17 Apr 2008 23:52:08 +0200 Subject: * digital/asserv/src/asserv: - added goto_angle command. - fixed: should always set pos_cons when use_pos is set. --- digital/asserv/src/asserv/eeprom.avr.c | 2 ++ digital/asserv/src/asserv/eeprom.h | 2 +- digital/asserv/src/asserv/main.c | 12 +++++++- digital/asserv/src/asserv/traj.c | 50 ++++++++++++++++++++++++++++++++++ digital/asserv/src/asserv/traj.h | 4 +++ digital/asserv/src/asserv/twi_proto.c | 5 ++++ 6 files changed, 73 insertions(+), 2 deletions(-) diff --git a/digital/asserv/src/asserv/eeprom.avr.c b/digital/asserv/src/asserv/eeprom.avr.c index aee320fb..a8c1371e 100644 --- a/digital/asserv/src/asserv/eeprom.avr.c +++ b/digital/asserv/src/asserv/eeprom.avr.c @@ -98,6 +98,7 @@ eeprom_read_params (void) pos_d_sat = eeprom_read_word (p16++); pos_blocked = eeprom_read_word (p16++); traj_eps = eeprom_read_word (p16++); + traj_aeps = eeprom_read_word (p16++); } /* Write parameters to eeprom. */ @@ -134,6 +135,7 @@ eeprom_write_params (void) eeprom_write_word (p16++, pos_d_sat); eeprom_write_word (p16++, pos_blocked); eeprom_write_word (p16++, traj_eps); + eeprom_write_word (p16++, traj_aeps); } /* Clear eeprom parameters. */ diff --git a/digital/asserv/src/asserv/eeprom.h b/digital/asserv/src/asserv/eeprom.h index b603942d..8c99fb5f 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 0x49 +#define EEPROM_KEY 0x4a void eeprom_read_params (void); diff --git a/digital/asserv/src/asserv/main.c b/digital/asserv/src/asserv/main.c index 99c0e369..79969a6b 100644 --- a/digital/asserv/src/asserv/main.c +++ b/digital/asserv/src/asserv/main.c @@ -359,6 +359,15 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) v8_to_v32 (args[4], args[5], args[6], args[7]), args[8]); break; + case c ('x', 5): + /* Go to angle. + * - d: a, f8.24. + * - b: sequence number. */ + if (args[4] == state_main.sequence) + break; + traj_goto_angle_start (v8_to_v32 (args[0], args[1], args[2], args[3]), + args[4]); + break; case c ('a', 2): /* Set both acknoledge. * - b: main ack sequence number. @@ -515,8 +524,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('b', 3): pos_blocked = v8_to_v16 (args[1], args[2]); break; - case c ('e', 3): + case c ('e', 5): traj_eps = v8_to_v16 (args[1], args[2]); + traj_aeps = v8_to_v16 (args[3], args[4]); break; case c ('w', 2): /* Set PWM direction. diff --git a/digital/asserv/src/asserv/traj.c b/digital/asserv/src/asserv/traj.c index bc95b39a..dc7c439b 100644 --- a/digital/asserv/src/asserv/traj.c +++ b/digital/asserv/src/asserv/traj.c @@ -53,6 +53,8 @@ enum TRAJ_GTD, /* Go to position. */ TRAJ_GOTO, + /* Go to angle. */ + TRAJ_GOTO_ANGLE, /* Everything done. */ TRAJ_DONE, }; @@ -63,9 +65,15 @@ uint8_t traj_mode; /** Epsilon, distance considered to be small enough. */ int16_t traj_eps = 500; +/** Angle epsilon, angle considered to be small enough (f0.16). */ +int16_t traj_aeps = 0x0100; + /** Go to position. */ static uint32_t traj_goto_x, traj_goto_y; +/** Go to angle. */ +static uint32_t traj_goto_a; + /** Angle offset. Directly handled to speed layer. */ void traj_angle_offset_start (int32_t angle, uint8_t seq) @@ -204,6 +212,45 @@ traj_goto_start (uint32_t x, uint32_t y, uint8_t seq) traj_goto_x = x; traj_goto_y = y; speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; + state_start (&state_main, seq); +} + +/** Go to angle mode. */ +static void +traj_goto_angle (void) +{ + /* There is some tricky parts to handle rotation direction. */ + int16_t da = (uint16_t) (traj_goto_a >> 8) - (uint16_t) (postrack_a >> 8); + if (UTILS_ABS (da) < traj_aeps) + { + /* Near enough, stop, let speed terminate the movement. */ + state_main.mode = MODE_SPEED; + traj_mode = TRAJ_DONE; + } + else + { + /* Compute arc length. */ + int32_t arad = fixed_mul_f824 (((int32_t) da) << 8, + 2 * M_PI * (1L << 24)); + int32_t arc = fixed_mul_f824 (arad, postrack_footing); + /* Compute consign. */ + speed_alpha.pos_cons = pos_alpha.cur; + speed_alpha.pos_cons += arc; + } +} + +/** Start go to angle mode (a: f8.24). */ +void +traj_goto_angle_start (uint32_t a, uint8_t seq) +{ + state_main.mode = MODE_TRAJ; + traj_mode = TRAJ_GOTO_ANGLE; + traj_goto_a = a; + speed_theta.use_pos = speed_alpha.use_pos = 1; + speed_theta.pos_cons = pos_theta.cons; + speed_alpha.pos_cons = pos_alpha.cons; state_start (&state_main, seq); } @@ -222,6 +269,9 @@ traj_update (void) case TRAJ_GOTO: traj_goto (); break; + case TRAJ_GOTO_ANGLE: + traj_goto_angle (); + break; case TRAJ_DONE: break; } diff --git a/digital/asserv/src/asserv/traj.h b/digital/asserv/src/asserv/traj.h index c13eab9d..fe9830c9 100644 --- a/digital/asserv/src/asserv/traj.h +++ b/digital/asserv/src/asserv/traj.h @@ -27,6 +27,7 @@ extern uint8_t traj_mode; extern int16_t traj_eps; +extern int16_t traj_aeps; void traj_update (void); @@ -43,4 +44,7 @@ traj_gtd_start (uint8_t seq); void traj_goto_start (uint32_t x, uint32_t y, uint8_t seq); +void +traj_goto_angle_start (uint32_t a, uint8_t seq); + #endif /* traj_h */ diff --git a/digital/asserv/src/asserv/twi_proto.c b/digital/asserv/src/asserv/twi_proto.c index 75a0a110..db6bac41 100644 --- a/digital/asserv/src/asserv/twi_proto.c +++ b/digital/asserv/src/asserv/twi_proto.c @@ -147,6 +147,11 @@ twi_proto_callback (u8 *buf, u8 size) v8_to_v32 (buf[5], buf[6], buf[7], 0), 0); break; + case c ('y', 0): + /* Go to angle. + * - w: angle. */ + traj_goto_angle_start (v8_to_v32 (0, buf[2], buf[3], 0), 0); + break; case c ('b', 3): /* Move the arm. * - w: new position. -- cgit v1.2.3