Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 194a925f13cf0074d8da123604f1c5740efddd73
|
From: Xavier L. <Ba...@us...> - 2012-01-28 16:09:20
|
This is an automated email from the git hooks/post-receive script. It was
generated because a ref change was pushed to the repository containing
the project "krobot".
The branch, master has been updated
via 194a925f13cf0074d8da123604f1c5740efddd73 (commit)
from 356ea449946b76d194a653473fe99df01c6b6b1a (commit)
Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.
- Log -----------------------------------------------------------------
commit 194a925f13cf0074d8da123604f1c5740efddd73
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat Jan 28 17:11:10 2012 +0100
[Controller_Motor_STM32/lib] Added some support for some 2R arms.
The associated functions and generators enable the control of a 2R arm
in effector space (X,Y of the end of the arm) instead of controlling its
two articulations directly.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
index 523bdbb..6092393 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Effectors/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -24,6 +24,7 @@ controller_motor_stm32_USER_CSRC = \
$(controller_motor_stm32_LIB_PATH)/bezier_utils.c \
$(controller_motor_stm32_LIB_PATH)/differential_drive.c \
$(controller_motor_stm32_LIB_PATH)/lift_controller.c \
+ $(controller_motor_stm32_LIB_PATH)/arm2R_controller.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32_user.mk b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32_user.mk
index 523bdbb..6092393 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32_user.mk
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/Propulsion_Drive/controller_motor_stm32/controller_motor_stm32_user.mk
@@ -24,6 +24,7 @@ controller_motor_stm32_USER_CSRC = \
$(controller_motor_stm32_LIB_PATH)/bezier_utils.c \
$(controller_motor_stm32_LIB_PATH)/differential_drive.c \
$(controller_motor_stm32_LIB_PATH)/lift_controller.c \
+ $(controller_motor_stm32_LIB_PATH)/arm2R_controller.c \
$(controller_motor_stm32_SRC_PATH)/can_monitor.c \
$(controller_motor_stm32_SRC_PATH)/main.c \
#
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
new file mode 100644
index 0000000..f2d8064
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
@@ -0,0 +1,78 @@
+/*
+ * arm2R_controller.c
+ * -----------------
+ * Copyright : (c) 2012, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#include "arm2R_controller.h"
+
+typedef struct {
+ uint8_t enabled, tc_ind[2];
+ command_generator_t theta1_s, theta2_s, theta1, theta2;
+} a2Rc_state_t;
+
+a2Rc_state_t ac_state[2];
+
+void a2Rc_init(void) {
+ int i;
+
+ // Init state
+ for (i=0; i<2; i++) {
+ ac_state[i].enabled = 0;
+ }
+ ac_state[ARM1].tc_ind[0] = A2R_ARM1_TC + A2R_TC_X;
+ ac_state[ARM1].tc_ind[1] = A2R_ARM1_TC + A2R_TC_Y;
+ ac_state[ARM2].tc_ind[0] = A2R_ARM2_TC + A2R_TC_X;
+ ac_state[ARM2].tc_ind[1] = A2R_ARM2_TC + A2R_TC_Y;
+}
+
+void a2Rc_start(uint8_t arm,
+ float l1, float l2,
+ uint8_t enc_theta1, uint8_t enc_theta2) {
+
+ uint8_t tc_x, tc_y;
+
+ // Read trajectory controllers identifiers
+ tc_x = ac_state[arm].tc_ind[0];
+ tc_y = ac_state[arm].tc_ind[1];
+
+ // Init Trajectory controllers
+ tc_new_controller(tc_x);
+ tc_new_controller(tc_y);
+ new_a2r_generator(&ac_state[arm].theta1_s,
+ tc_get_position_generator(tc_x),
+ tc_get_speed_generator(tc_x),
+ tc_get_position_generator(tc_y),
+ tc_get_speed_generator(tc_y),
+ l1, l2, enc_theta1, enc_theta2, 1);
+ new_a2r_generator(&ac_state[arm].theta2_s,
+ tc_get_position_generator(tc_x),
+ tc_get_speed_generator(tc_x),
+ tc_get_position_generator(tc_y),
+ tc_get_speed_generator(tc_y),
+ l1, l2, enc_theta1, enc_theta2, 2);
+ new_ramp2_generator(&ac_state[arm].theta1, 0.0, &ac_state[arm].theta1_s);
+ new_ramp2_generator(&ac_state[arm].theta2, 0.0, &ac_state[arm].theta2_s);
+
+ start_generator(&ac_state[arm].theta1_s);
+ start_generator(&ac_state[arm].theta2_s);
+ start_generator(&ac_state[arm].theta1);
+ start_generator(&ac_state[arm].theta2);
+}
+
+void a2Rc_goto_position_x(uint8_t arm, float position, float speed, float acc) {
+ tc_goto(arm+A2R_TC_X, position, speed, acc);
+}
+void a2Rc_goto_position_y(uint8_t arm, float position, float speed, float acc) {
+ tc_goto(arm+A2R_TC_Y, position, speed, acc);
+}
+
+command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm) {
+ return &ac_state[arm].theta1;
+}
+command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm) {
+ return &ac_state[arm].theta2;
+}
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
new file mode 100644
index 0000000..4620a47
--- /dev/null
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
@@ -0,0 +1,70 @@
+/*
+ * arm2R_controller.h
+ * -----------------
+ * Copyright : (c) 2012, Xavier Lagorce <Xav...@cr...>
+ * Licence : BSD3
+ *
+ * This file is a part of [kro]bot.
+ */
+
+#ifndef __ARM2R_CONTROLLER_H
+#define __ARM2R_CONTROLLER_H
+
+#include "motor_controller.h"
+#include "trajectory_controller.h"
+#include "command_generator.h"
+
+#define ARM1 0
+#define ARM2 1
+
+#ifndef A2R_ARM1_TC
+ #define A2R_ARM1_TC 0
+#endif
+#ifndef A2R_ARM2_TC
+ #define A2R_ARM2_TC 2
+#endif
+
+#define A2R_TC_X 0
+#define A2R_TC_Y 1
+
+/*
+ * Initializes data structures associated with the arm 2R controller
+ */
+void a2Rc_init(void);
+
+/*
+ * Initializes a particular arm controller and configure the associated
+ * generators
+ * - arm : ID of the arm controller to start
+ * - l1, l2 : length of the two segments of the arm
+ * - enc_theta1, enc_theta2 : IDs of the encoders measuring the position
+ * of the two articulations
+ */
+void a2Rc_start(uint8_t arm,
+ float l1, float l2,
+ uint8_t enc_theta1, uint8_t enc_theta2);
+
+/*
+ * Moves an arm2R controller along an axis.
+ * - arm : ID of the arm to move
+ * - position : position on the axis to go to
+ * - speed : movement speed along the axis
+ * - acc : movement acceleration along the axis
+ */
+void a2Rc_goto_position_x(uint8_t arm, float position, float speed, float acc);
+void a2Rc_goto_position_y(uint8_t arm, float position, float speed, float acc);
+
+/*
+ * Get the command generator outputing the reference position for
+ * the first articulation
+ * - arm : ID of the arm of which to get the generator
+ */
+command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm);
+/*
+ * Get the command generator outputing the reference position for
+ * the second articulation
+ * - arm : ID of the arm of which to get the generator
+ */
+command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm);
+
+#endif /* __ARM2R_CONTROLLER_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
index 0d1145a..3b36e70 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.c
@@ -98,12 +98,45 @@ command_generator_t* new_hd_generator(command_generator_t *generator,
return generator;
}
+command_generator_t* new_a2r_generator(command_generator_t *generator,
+ command_generator_t *linear_pos_x,
+ command_generator_t *linear_speed_x,
+ command_generator_t *linear_pos_y,
+ command_generator_t *linear_speed_y,
+ float l1, float l2,
+ uint8_t enc_theta1, uint8_t enc_theta2,
+ uint8_t type) {
+ switch (type) {
+ case 1: // Back wheel
+ generator->type.t = GEN_AC_T1;
+ break;
+ case 2: // Right-front wheel
+ generator->type.t = GEN_AC_T2;
+ break;
+ default:
+ return NULL;
+ break;
+ }
+ generator->type.callback.type = GEN_CALLBACK_NONE;
+ generator->a2r.linear_pos_x = linear_pos_x;
+ generator->a2r.linear_speed_x = linear_speed_x;
+ generator->a2r.linear_pos_x = linear_pos_y;
+ generator->a2r.linear_speed_x = linear_speed_y;
+ generator->a2r.l1 = l1;
+ generator->a2r.l2 = l2;
+ generator->a2r.enc_theta1 = enc_theta1;
+ generator->a2r.enc_theta2 = enc_theta2;
+
+ return generator;
+}
+
command_generator_t* adjust_value(command_generator_t *generator, float value) {
uint8_t type = generator->type.t;
if (type != GEN_DD_RIGHT && type != GEN_DD_LEFT
- && type != GEN_HD_B && type != GEN_HD_RF && type != GEN_HD_LF) {
+ && type != GEN_HD_B && type != GEN_HD_RF && type != GEN_HD_LF
+ && type != GEN_AC_T1 && type != GEN_AC_T2) {
generator->type.last_output = value;
return generator;
} else {
@@ -160,7 +193,8 @@ command_generator_t* remove_callback(command_generator_t *generator) {
float get_output_value(command_generator_t *generator) {
int32_t cur_time;
- float speed, dt, u1, u2, u_x, u_y, w;
+ float speed, dt, u1, u2, u_x, u_y, w, theta1, theta2;
+ float l1, l2, s1, c1, s2, c2;
if (generator->type.state != GEN_STATE_RUNNING)
return generator->type.last_output;
@@ -235,6 +269,32 @@ float get_output_value(command_generator_t *generator) {
generator->type.last_output = MAX(generator->type.last_output, -generator->hd.max_speed);
}
break;
+ case GEN_AC_T1:
+ case GEN_AC_T2:
+ // Update position generators to allow callbacks
+ get_output_value(generator->a2r.linear_pos_x);
+ get_output_value(generator->a2r.linear_pos_y);
+ // Compute output
+ u_x = get_output_value(generator->a2r.linear_speed_x);
+ u_y = get_output_value(generator->a2r.linear_speed_y);
+ l1 = generator->a2r.l1;
+ l2 = generator->a2r.l2;
+ theta1 = getEncoderPosition_f(generator->a2r.enc_theta1);
+ theta2 = getEncoderPosition_f(generator->a2r.enc_theta2);
+ c1 = cos(theta1);
+ c2 = cos(theta2);
+ s1 = sin(theta1);
+ s2 = sin(theta2);
+ w = -l1*l2*s1*c2+l1*l2*s2*c1;
+ switch (generator->type.t) {
+ case GEN_AC_T1:
+ generator->type.last_output = (-l1*s1*u_x+l2*s2*u_y)/w;
+ break;
+ case GEN_AC_T2:
+ generator->type.last_output = (-l1*c1*u_x+l2*c2*u_y)/w;
+ break;
+ }
+ break;
}
switch (generator->type.callback.type) {
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.h
index 3fa2b6a..bd5ae8e 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/command_generator.h
@@ -15,6 +15,7 @@
#include <drv/timer.h>
#include <math.h>
+#include "encoder.h"
// Generator types
#define GEN_NONE 0 // No type, the generator is not initialized.
@@ -26,9 +27,12 @@
// drive.
#define GEN_DD_RIGHT 5 // Outputs the right wheel speed for a differential
// drive.
-#define GEN_HD_B 6 // Outpus the back wheel speed for an holonomic drive.
-#define GEN_HD_RF 7 // Outpus the right-front wheel speed for an holonomic drive.
-#define GEN_HD_LF 8 // Outpus the left-front wheel speed for an holonomic drive.
+#define GEN_HD_B 6 // Outputs the back wheel speed for an holonomic drive.
+#define GEN_HD_RF 7 // Outputs the right-front wheel speed for an holonomic drive.
+#define GEN_HD_LF 8 // Outputs the left-front wheel speed for an holonomic drive.
+#define GEN_AC_T1 9 // Outputs the angle of the first articulation of a RR arm
+#define GEN_AC_T2 10 // Outputs the angle of the second articulation of a RR arm
+
// Generator states
#define GEN_STATE_PAUSE 0 // The output is freezed.
@@ -96,6 +100,18 @@ typedef struct {
float max_speed;
} hd_generator_t;
+typedef struct {
+ placeholder_generator_t gen;
+ command_generator_t *linear_pos_x;
+ command_generator_t *linear_speed_x;
+ command_generator_t *linear_pos_y;
+ command_generator_t *linear_speed_y;
+ float l1;
+ float l2;
+ uint8_t enc_theta1;
+ uint8_t enc_theta2;
+} a2r_generator_t;
+
// Usable generator meta-type
union _command_generator_t {
placeholder_generator_t type;
@@ -104,6 +120,7 @@ union _command_generator_t {
ramp2_generator_t ramp2;
dd_generator_t dd;
hd_generator_t hd;
+ a2r_generator_t a2r;
};
/* Initializes a new Constant Generator.
@@ -187,6 +204,35 @@ command_generator_t* new_hd_generator(command_generator_t *generator,
float wheel_radius, float struct_radius, float max_speed,
uint8_t type);
+/* Initializes a new 2R arm controller generator.
+ * - generator : pointer to the generator to initialize
+ * - linear_pos_x : pointer to the generator giving the integrates of linear_speed_x. This
+ * generator will be called at each computation to allow update in parallel
+ * with linear_speed.
+ * - linear_speed_x : pointer to the generator giving the linear speed along the x axis of
+ * the arm end.
+ * - linear_pos_y : pointer to the generator giving the integrates of linear_speed_y. This
+ * generator will be called at each computation to allow update in parallel
+ * with linear_speed.
+ * - linear_speed_y : pointer to the generator giving the linear speed along the y axis of
+ * the arm end.
+ * - l1 : length of the first segment of the arm.
+ * - l2 : length of the second segment of the arm.
+ * - enc_theta1 : ID of the encoder measuring the first articulation position
+ * - enc_theta2 : ID of the encoder measuring the second articulation position
+ * - type :
+ * o 1 is the first articulation
+ * o 2 is the second articulation
+ */
+command_generator_t* new_a2r_generator(command_generator_t *generator,
+ command_generator_t *linear_pos_x,
+ command_generator_t *linear_speed_x,
+ command_generator_t *linear_pos_y,
+ command_generator_t *linear_speed_y,
+ float l1, float l2,
+ uint8_t enc_theta1, uint8_t enc_theta2,
+ uint8_t type);
+
/*
* Adjusts the current output value of 'generator' to 'value'.
*/
hooks/post-receive
--
krobot
|
