Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 6745d46fc81891fd948daf587b9397fd8cb5dee4
|
From: Xavier L. <Ba...@us...> - 2011-03-30 13:52:47
|
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 6745d46fc81891fd948daf587b9397fd8cb5dee4 (commit)
via bc4e2c2e79146ce8fa9f83fac2e392f1e19d1f89 (commit)
from 4c596ea2f6a485e48aa77c844d9f0e3e6fa0eb98 (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 6745d46fc81891fd948daf587b9397fd8cb5dee4
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed Mar 30 15:52:02 2011 +0200
[Controller_Motor_STM32] Corrected parameters computation for speed profiles
commit bc4e2c2e79146ce8fa9f83fac2e392f1e19d1f89
Author: Xavier Lagorce <Xav...@cr...>
Date: Wed Mar 30 15:43:03 2011 +0200
[Controller_Motor_STM32] Do not use explicit brake when command is zero
Using explicit brake changes the command configuration of the bridge and
after this, inputting a non zero command doesn't changed back the configuration
to a proper state.
A command of zero should have the same behavior as explicit braking.
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor.c
index 3aefcbf..16bec21 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/motor.c
@@ -175,11 +175,6 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
uint8_t ind = 0;
- if (speed == 0) {
- motorStop(motor, MOTOR_BRAKE);
- return;
- }
-
if (speed >= MAX_PWM) {
speed = MAX_PWM;
ind = 1;
@@ -191,7 +186,7 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
}
if (motor & MOTOR1) {
- if(speed > 0) {
+ if(speed >= 0) {
if (currentSpeedSign[0] != 1) {
currentSpeedSign[0] = 1;
stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(4), 1);
@@ -220,7 +215,7 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
}
}
if (motor & MOTOR2) {
- if(speed > 0) {
+ if(speed >= 0) {
if (currentSpeedSign[1] != 1) {
currentSpeedSign[1] = 1;
stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(1), 1);
@@ -249,7 +244,7 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
}
}
if (motor & MOTOR3) {
- if(speed > 0) {
+ if(speed >= 0) {
if (currentSpeedSign[2] != 1) {
currentSpeedSign[2] = 1;
stm32_gpioPinWrite(((struct stm32_gpio *)GPIOC_BASE), BV(10), 1);
@@ -278,7 +273,7 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
}
}
if (motor & MOTOR4) {
- if(speed > 0) {
+ if(speed >= 0) {
if (currentSpeedSign[3] != 1) {
currentSpeedSign[3] = 1;
stm32_gpioPinWrite(((struct stm32_gpio *)GPIOB_BASE), BV(5), 1);
diff --git a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
index f09051b..407af94 100644
--- a/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
+++ b/elec/boards/Controller_Motor_STM32/Firmware/controller_motor_stm32/trajectory_controller.c
@@ -122,7 +122,7 @@ uint8_t tc_is_finished(void) {
if (right_trap.state == TRAPEZOID_STATE_STOP && left_trap.state == TRAPEZOID_STATE_STOP)
return 1;
- // No running process, last planified action should be finished
+ // One automaton is running, last planified action should be in progress
return 0;
}
@@ -147,7 +147,7 @@ void tc_move(float distance, float speed, float acceleration) {
// Is the trapezoidal speed profile posible ?
t_acc = speed / acceleration;
- t_end = (speed * speed + distance * acceleration) / (speed * acceleration);
+ t_end = (speed * speed + fabsf(distance) * acceleration) / (speed * acceleration);
if (t_end > (2. * t_acc)) {
// A trapezoidal speed profile is possible
@@ -162,7 +162,7 @@ void tc_move(float distance, float speed, float acceleration) {
left_trap.speed = left_trap.dir * speed / WHEEL_R * 180 / M_PI;
left_trap.state = TRAPEZOID_STATE_ACC;
- // This is distance during which the robot will accelerate
+ // This is the distance during which the robot will accelerate
acc_dist = SIGN(distance) * speed * speed / acceleration / 2.0 / WHEEL_R * 180.0 / M_PI;
// Set accelerations for the trapezoid's first phase and associated callbacks
@@ -178,19 +178,19 @@ void tc_move(float distance, float speed, float acceleration) {
left_trap.is_triangle = 1;
// Compute triangle parameters for right motor
- right_trap.speed = right_trap.dir * sqrt(right_trap.angle * right_trap.acceleration);
+ right_trap.speed = right_trap.dir * sqrt(distance / WHEEL_R * 180 / M_PI * right_trap.acceleration);
right_trap.state = TRAPEZOID_STATE_TRIANGLE;
// Compute triangle parameters for left motor
- left_trap.speed = left_trap.dir * sqrt(left_trap.angle * left_trap.acceleration);
+ left_trap.speed = left_trap.dir * sqrt(distance / WHEEL_R * 180 / M_PI * left_trap.acceleration);
left_trap.state = TRAPEZOID_STATE_TRIANGLE;
// Set accelerations for the triangle's first phase and associated callbacks
adjust_speed(&right_wheel_speed, right_trap.acceleration);
- add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + right_trap.angle/2.0, trapezoid_callback);
+ add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + distance / 2.0 / WHEEL_R * 180 / M_PI, trapezoid_callback);
add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
adjust_speed(&left_wheel_speed, left_trap.acceleration);
- add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + left_trap.angle/2.0, trapezoid_callback);
+ add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + distance / 2.0 / WHEEL_R * 180 / M_PI, trapezoid_callback);
add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
}
@@ -218,7 +218,7 @@ void tc_turn(float angle, float speed, float acceleration) {
// Is the trapezoidal speed profile posible ?
t_acc = speed / acceleration;
- t_end = (speed * speed + angle * acceleration) / (speed * acceleration);
+ t_end = (speed * speed + fabsf(angle) * acceleration) / (speed * acceleration);
if (t_end > (2. * t_acc)) {
// A trapezoidal speed profile is possible
@@ -249,19 +249,19 @@ void tc_turn(float angle, float speed, float acceleration) {
left_trap.is_triangle = 1;
// Compute triangle parameters for right motor
- right_trap.speed = right_trap.dir * sqrt(right_trap.angle * right_trap.acceleration);
+ right_trap.speed = right_trap.dir * sqrt(angle * STRUCT_B / 2.0 / WHEEL_R * right_trap.acceleration);
right_trap.state = TRAPEZOID_STATE_TRIANGLE;
// Compute triangle parameters for left motor
- left_trap.speed = left_trap.dir * sqrt(left_trap.angle * left_trap.acceleration);
+ left_trap.speed = left_trap.dir * sqrt(- angle * STRUCT_B / 2.0 / WHEEL_R * left_trap.acceleration);
left_trap.state = TRAPEZOID_STATE_TRIANGLE;
// Set accelerations for the triangle's first phase and associated callbacks
adjust_speed(&right_wheel_speed, right_trap.acceleration);
- add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + right_trap.angle/2.0, trapezoid_callback);
+ add_callback(&right_wheel, SELECT_THRESHOLD(right_trap.dir), right_trap.init_val + angle / 2.0 * STRUCT_B / 2.0 / WHEEL_R, trapezoid_callback);
add_callback(&right_wheel_speed, SELECT_THRESHOLD(right_trap.dir), right_trap.speed, trapezoid_callback);
adjust_speed(&left_wheel_speed, left_trap.acceleration);
- add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val + left_trap.angle/2.0, trapezoid_callback);
+ add_callback(&left_wheel, SELECT_THRESHOLD(left_trap.dir), left_trap.init_val - angle / 2.0 * STRUCT_B / 2.0 / WHEEL_R, trapezoid_callback);
add_callback(&left_wheel_speed, SELECT_THRESHOLD(left_trap.dir), left_trap.speed, trapezoid_callback);
}
hooks/post-receive
--
krobot
|
