Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. fbc16a5651e122e72848efe649c9666aed0a78a5
|
From: Xavier L. <Ba...@us...> - 2012-01-31 15:03:37
|
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 fbc16a5651e122e72848efe649c9666aed0a78a5 (commit)
via 4526c9089a4329c2a0d2697c86750a707a3e9b7e (commit)
from 194a925f13cf0074d8da123604f1c5740efddd73 (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 fbc16a5651e122e72848efe649c9666aed0a78a5
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Jan 31 16:06:04 2012 +0100
[Controller_Motor_STM32/lib] Added some functionnalities and better initialization
of the arm2R_controller module.
commit 4526c9089a4329c2a0d2697c86750a707a3e9b7e
Author: Xavier Lagorce <Xav...@cr...>
Date: Tue Jan 31 16:05:18 2012 +0100
[Controller_Motor_STM32/lib] Added the possibility to force the position of a trajectory controller
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
index f2d8064..5c64043 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.c
@@ -10,8 +10,9 @@
#include "arm2R_controller.h"
typedef struct {
- uint8_t enabled, tc_ind[2];
+ uint8_t enabled, tc_ind[2], enc_theta1, enc_theta2;
command_generator_t theta1_s, theta2_s, theta1, theta2;
+ float l1, l2;
} a2Rc_state_t;
a2Rc_state_t ac_state[2];
@@ -34,6 +35,18 @@ void a2Rc_start(uint8_t arm,
uint8_t enc_theta1, uint8_t enc_theta2) {
uint8_t tc_x, tc_y;
+ float theta1, theta2, x, y;
+
+ // Record the parameters of the arm
+ ac_state[arm].l1 = l1;
+ ac_state[arm].l2 = l2;
+ ac_state[arm].enc_theta1 = enc_theta1;
+ ac_state[arm].enc_theta2 = enc_theta2;
+
+ // Get the current position of the arm
+ a2Rc_get_angles(arm, &theta1, &theta2);
+ x = l1*cos(theta1) + l2*cos(theta2);
+ y = l1*sin(theta1) + l2*sin(theta2);
// Read trajectory controllers identifiers
tc_x = ac_state[arm].tc_ind[0];
@@ -42,6 +55,9 @@ void a2Rc_start(uint8_t arm,
// Init Trajectory controllers
tc_new_controller(tc_x);
tc_new_controller(tc_y);
+ tc_set_position(tc_x, x);
+ tc_set_position(tc_y, y);
+ // Create the command generators
new_a2r_generator(&ac_state[arm].theta1_s,
tc_get_position_generator(tc_x),
tc_get_speed_generator(tc_x),
@@ -54,13 +70,18 @@ void a2Rc_start(uint8_t arm,
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);
+ // Create the reference generators
+ new_ramp2_generator(&ac_state[arm].theta1, theta1, &ac_state[arm].theta1_s);
+ new_ramp2_generator(&ac_state[arm].theta2, theta2, &ac_state[arm].theta2_s);
+ // Start the different references generators
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);
+
+ // Activate the 'enabled' flag
+ ac_state[arm].enabled = 1;
}
void a2Rc_goto_position_x(uint8_t arm, float position, float speed, float acc) {
@@ -76,3 +97,16 @@ command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm) {
command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm) {
return &ac_state[arm].theta2;
}
+
+void a2Rc_get_angles(uint8_t arm, float *theta1, float *theta2) {
+ *theta1 = getEncoderPosition_f(ac_state[arm].enc_theta1);
+ *theta2 = getEncoderPosition_f(ac_state[arm].enc_theta2);
+}
+
+void a2Rc_get_position(uint8_t arm, float *x, float *y) {
+ float theta1, theta2;
+
+ a2Rc_get_angles(arm, &theta1, &theta2);
+ *x = ac_state[arm].l1 * cos(theta1) + ac_state[arm].l2 * cos(theta2);
+ *y = ac_state[arm].l1 * sin(theta1) + ac_state[arm].l2 * sin(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
index 4620a47..fc884f5 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/arm2R_controller.h
@@ -67,4 +67,20 @@ command_generator_t* a2Rc_get_first_articulation_gen(uint8_t arm);
*/
command_generator_t* a2Rc_get_second_articulation_gen(uint8_t arm);
+/*
+ * Get the current angles of the articulations.
+ * - arm : ID of the arm of which to get the angles
+ * - theta1 : address of the variable to write the first angle into
+ * - theta2 : address of the variable to write the second angle into
+ */
+void a2Rc_get_angles(uint8_t arm, float *theta1, float *theta2);
+
+/*
+ * Computes the current position of the end of the arm
+ * - arm : ID of the arm of which to compute the end's position
+ * - x : address of the variable to wirte the X coordinate into
+ * - y : address of the variable to wirte the Y coordinate into
+ */
+void a2Rc_get_position(uint8_t arm, float *x, float *y);
+
#endif /* __ARM2R_CONTROLLER_H */
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.c
index ef59d93..d843f4e 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.c
@@ -316,6 +316,29 @@ void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration) {
cont->working = 1;
}
+void tc_set_position(uint8_t cntr_index, float position) {
+ trajectory_controller_t *cont;
+
+ // Get the controller and verifies it is enabled
+ if (cntr_index >= NUM_TC_MAX)
+ return;
+ cont = &controllers[cntr_index];
+ if (!cont->enabled)
+ return;
+
+ // Disable a possibly running profile
+ cont->aut.state = TRAPEZOID_STATE_STOP;
+ remove_callback(&cont->position);
+ remove_callback(&cont->speed);
+
+ // Set speed and acceleration
+ adjust_value(&cont->position, position);
+ adjust_value(&cont->speed, 0.0);
+ adjust_speed(&cont->speed, 0.0);
+
+ cont->working = 0;
+}
+
void tc_set_speed(uint8_t cntr_index, float speed) {
trajectory_controller_t *cont;
diff --git a/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.h b/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.h
index ba4df32..d0959e7 100644
--- a/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.h
+++ b/elec/boards/Controller_Motor_STM32/Firmwares/lib/trajectory_controller.h
@@ -84,6 +84,11 @@ void tc_move(uint8_t cntr_index, float angle, float speed, float acceleration);
void tc_goto_speed(uint8_t cntr_index, float speed, float acceleration);
/*
+ * Change the position of a controller without any concern about continuity
+ */
+void tc_set_position(uint8_t cntr_index, float position);
+
+/*
* Change the speed command of a controller without any concern about acceleration
*/
void tc_set_speed(uint8_t cntr_index, float speed);
hooks/post-receive
--
krobot
|
