Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 4791b8a737302d52223f36b111ecbf4b21ddda29
|
From: Xavier L. <Ba...@us...> - 2010-05-22 12:54:34
|
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 4791b8a737302d52223f36b111ecbf4b21ddda29 (commit)
via 0eae3c85c0ee88fdc2a26d9cf1d0c4f7de5d3c95 (commit)
from 1c622c29659ace246c78926491a29bd98d75d13e (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 4791b8a737302d52223f36b111ecbf4b21ddda29
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat May 22 14:53:43 2010 +0200
Improvments to speed controller
* Some clearer code
* We MUST consider counter wrapper or terrible things happen
commit 0eae3c85c0ee88fdc2a26d9cf1d0c4f7de5d3c95
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat May 22 14:48:21 2010 +0200
Cleaning
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/motor.c b/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
index 00c2f1f..5d71d92 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
@@ -10,45 +10,6 @@ signed char currentSpeedSign[] = {0, 0, 0};
TIM_OCInitTypeDef TIM_OCInitStructure;
/*
- * Interrupt routine for PWM generation
- */
-/*void VectorB0(void) {
-
- if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET)
- {
- // Clear TIM2 Capture Compare1 interrupt pending bit
- TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
-
- // End of motor1 pulse
- GPIO_ResetBits(GPIOA, GPIO_Pin_1);
- }
- else if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)
- {
- // Clear TIM2 Capture Compare2 interrupt pending bit
- TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
-
- // End of motor2 pulse
- GPIO_ResetBits(GPIOA, GPIO_Pin_2);
- }
- else if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET)
- {
- // Clear TIM2 Capture Compare3 interrupt pending bit
- TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
-
- // End of motor3 pulse
- GPIO_ResetBits(GPIOA, GPIO_Pin_3);
- }
- else {
- // TIM_IT_Update
-
- // Clear TIM2 Update interrupt pending bit
- TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
-
- GPIO_SetBits(GPIOA, GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3);
- }
- }*/
-
-/*
* Initialises TIM2 for PWM generation and associated GPIOs
*/
void motorsInit(void) {
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
index 10f186a..418f2fe 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
@@ -20,9 +20,9 @@ static msg_t ThreadSpeedController(void *arg) {
systime_t time;
int32_t commands[3] = {0, 0, 0};
int32_t errors[3] = {0, 0, 0};
- uint16_t positions[3] = {0, 0, 0};
+ int32_t positions[3] = {0, 0, 0};
int32_t last_errors[3] = {0, 0, 0};
- uint16_t prev_positions[3] = {0, 0, 0};
+ int32_t prev_positions[3] = {0, 0, 0};
int32_t integ[3] = {0, 0, 0};
@@ -32,20 +32,36 @@ static msg_t ThreadSpeedController(void *arg) {
while (TRUE) {
time += MS2ST(Tcomp);
- prev_positions[0] = getEncoderPosition(ENCODER1);
- prev_positions[1] = getEncoderPosition(ENCODER2);
- prev_positions[2] = getEncoderPosition(ENCODER3);
+ prev_positions[0] = (int32_t)getEncoderPosition(ENCODER1);
+ prev_positions[1] = (int32_t)getEncoderPosition(ENCODER2);
+ prev_positions[2] = (int32_t)getEncoderPosition(ENCODER3);
- time += MS2ST(Te-Tcomp);
chThdSleepUntil(time);
+ time += MS2ST(Te-Tcomp);
- positions[0] = getEncoderPosition(ENCODER1);
- positions[1] = getEncoderPosition(ENCODER2);
- positions[2] = getEncoderPosition(ENCODER3);
-
- cur_speeds[0] = (K_v*((int32_t)positions[0] - (int32_t)prev_positions[0]));
- cur_speeds[1] = (K_v*((int32_t)positions[1] - (int32_t)prev_positions[1]));
- cur_speeds[2] = (K_v*((int32_t)positions[2] - (int32_t)prev_positions[2]));
+ positions[0] = (int32_t)getEncoderPosition(ENCODER1);
+ positions[1] = (int32_t)getEncoderPosition(ENCODER2);
+ positions[2] = (int32_t)getEncoderPosition(ENCODER3);
+
+ // we MUST consider counter wrapping
+ cur_speeds[0] = positions[0] - prev_positions[0];
+ if (cur_speeds[0] >= 10000)
+ cur_speeds[0] -= 65536;
+ else if (cur_speeds[0] <= -10000)
+ cur_speeds[0] += 65536;
+ cur_speeds[0] *= K_v;
+ cur_speeds[1] = positions[1] - prev_positions[1];
+ if (cur_speeds[1] >= 10000)
+ cur_speeds[1] -= 65536;
+ else if (cur_speeds[1] <= -10000)
+ cur_speeds[1] += 65536;
+ cur_speeds[1] *= K_v;
+ cur_speeds[2] = positions[2] - prev_positions[2];
+ if (cur_speeds[2] >= 10000)
+ cur_speeds[2] -= 65536;
+ else if (cur_speeds[2] <= -10000)
+ cur_speeds[2] += 65536;
+ cur_speeds[2] *= K_v;
errors[0] = ref_speeds[0] - cur_speeds[0];
errors[1] = ref_speeds[1] - cur_speeds[1];
@@ -68,6 +84,8 @@ static msg_t ThreadSpeedController(void *arg) {
else if (integ[2] < -INTEG_MAX)
integ[2] = -INTEG_MAX;
+ chThdSleepUntil(time);
+
//--> Command computation
commands[0] = (K_P*errors[0] + K_I*integ[0])/100;
commands[1] = (K_P*errors[1] + K_I*integ[1])/100;
@@ -81,12 +99,6 @@ static msg_t ThreadSpeedController(void *arg) {
motorSetSpeed(MOTOR1, commands[0]);
motorSetSpeed(MOTOR2, commands[1]);
motorSetSpeed(MOTOR3, commands[2]);
-
- prev_positions[0] = positions[0];
- prev_positions[1] = positions[1];
- prev_positions[2] = positions[2];
-
- chThdSleepUntil(time);
}
return 0;
}
@@ -99,7 +111,9 @@ void speedControlInit(void) {
enableMotor(MOTOR1 | MOTOR2 | MOTOR3);
motorSetSpeed(MOTOR1 | MOTOR2 | MOTOR3, 0);
- resetEncoderPosition(ENCODER1 | ENCODER2 | ENCODER3);
+ resetEncoderPosition(ENCODER1);
+ resetEncoderPosition(ENCODER2);
+ resetEncoderPosition(ENCODER3);
chThdCreateStatic(waThreadSC, sizeof(waThreadSC), HIGHPRIO, ThreadSpeedController, NULL);
}
hooks/post-receive
--
krobot
|
