Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 1c622c29659ace246c78926491a29bd98d75d13e
|
From: Xavier L. <Ba...@us...> - 2010-05-22 11:58:23
|
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 1c622c29659ace246c78926491a29bd98d75d13e (commit)
via 1e0363fd488565fe530f0163a57d8312dc79d6f7 (commit)
from 54d1ed6481b23cc83fc0168205a60a50e3f1e3e3 (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 1c622c29659ace246c78926491a29bd98d75d13e
Author: Xavier Lagorce <Xav...@cr...>
Date: Sat May 22 13:57:04 2010 +0200
Minor corrections to GPIO init
and there was a remaining statement from the past which was making motor control foobar
commit 1e0363fd488565fe530f0163a57d8312dc79d6f7
Author: Xavier Lagorce <Xav...@cr...>
Date: Fri May 21 22:55:31 2010 +0200
Changed controller computation method.
This one works better and offers better control over the integrator saturation
Controller constants are tuned for KrobotJr
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/motor.c b/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
index 7972948..00c2f1f 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
@@ -57,6 +57,7 @@ void motorsInit(void) {
GPIO_InitTypeDef GPIO_InitStructure;
//Enable GPIOB and GPIOC clock
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
@@ -79,8 +80,8 @@ void motorsInit(void) {
// IN2 PC8
// PWM PA3
// IND PC9
- GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_0 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5 | GPIO_Pin_6
- | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_13);
+ GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5 | GPIO_Pin_6
+ | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
@@ -94,8 +95,8 @@ void motorsInit(void) {
// Default value of H-Bridge configuration
GPIO_ResetBits(GPIOB, GPIO_Pin_0 | GPIO_Pin_1);
- GPIO_ResetBits(GPIOC, GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_10
- | GPIO_Pin_3 | GPIO_Pin_9 | GPIO_Pin_13);
+ GPIO_ResetBits(GPIOC, GPIO_Pin_0 | GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_6 | GPIO_Pin_7
+ | GPIO_Pin_8 | GPIO_Pin_10);
// TimeBase configuration
TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t) (72000000 / 72000000) - 1;;
@@ -258,7 +259,7 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
GPIO_SetBits(GPIOC, GPIO_Pin_7);
GPIO_ResetBits(GPIOC, GPIO_Pin_8);
}
- TIM_SetCompare3(TIM2, (uint16_t)(-speed));
+ TIM_OCInitStructure.TIM_Pulse = (uint16_t)(-speed);
}
TIM_OC4Init(TIM2, &TIM_OCInitStructure);
TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
index b689068..10f186a 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
@@ -22,8 +22,8 @@ static msg_t ThreadSpeedController(void *arg) {
int32_t errors[3] = {0, 0, 0};
uint16_t positions[3] = {0, 0, 0};
int32_t last_errors[3] = {0, 0, 0};
- int32_t last_commands[3] = {0, 0, 0};
uint16_t prev_positions[3] = {0, 0, 0};
+ int32_t integ[3] = {0, 0, 0};
(void)arg;
@@ -51,35 +51,29 @@ static msg_t ThreadSpeedController(void *arg) {
errors[1] = ref_speeds[1] - cur_speeds[1];
errors[2] = ref_speeds[2] - cur_speeds[2];
+ integ[0] += (errors[0] + last_errors[0])*Te/2;
+ integ[1] += (errors[1] + last_errors[1])*Te/2;
+ integ[2] += (errors[2] + last_errors[2])*Te/2;
+
+ if (integ[0] > INTEG_MAX)
+ integ[0] = INTEG_MAX;
+ else if (integ[0] < -INTEG_MAX)
+ integ[0] = -INTEG_MAX;
+ if (integ[1] > INTEG_MAX)
+ integ[1] = INTEG_MAX;
+ else if (integ[1] < -INTEG_MAX)
+ integ[1] = -INTEG_MAX;
+ if (integ[2] > INTEG_MAX)
+ integ[2] = INTEG_MAX;
+ else if (integ[2] < -INTEG_MAX)
+ integ[2] = -INTEG_MAX;
+
//--> Command computation
- commands[0] = ((K_Pn + K_In)*errors[0] - K_Pn*last_errors[0] + 100*last_commands[0])/100;
- commands[1] = ((K_Pn + K_In)*errors[1] - K_Pn*last_errors[1] + 100*last_commands[1])/100;
- commands[2] = ((K_Pn + K_In)*errors[2] - K_Pn*last_errors[2] + 100*last_commands[2])/100;
+ commands[0] = (K_P*errors[0] + K_I*integ[0])/100;
+ commands[1] = (K_P*errors[1] + K_I*integ[1])/100;
+ commands[2] = (K_P*errors[2] + K_I*integ[2])/100;
//--> End of command computation
- if (commands[0] >= 0)
- commands[0] = MIN(MAX_COMMAND, commands[0]);
- else
- commands[0] = MAX(-MAX_COMMAND, commands[0]);
- if (commands[1] >= 0)
- commands[1] = MIN(MAX_COMMAND, commands[1]);
- else
- commands[1] = MAX(-MAX_COMMAND, commands[1]);
- if (commands[2] >= 0)
- commands[2] = MIN(MAX_COMMAND, commands[2]);
- else
- commands[2] = MAX(-MAX_COMMAND, commands[2]);
-
- last_commands[0] = commands[0];
- last_commands[1] = commands[1];
- last_commands[2] = commands[2];
-
- if (commands[0] >= -DEAD_ZONE && commands[0] <= DEAD_ZONE)
- commands[0] = 0;
- if (commands[1] >= -DEAD_ZONE && commands[1] <= DEAD_ZONE)
- commands[1] = 0;
- if (commands[2] >= -DEAD_ZONE && commands[2] <= DEAD_ZONE)
- commands[2] = 0;
last_errors[0] = errors[0];
last_errors[1] = errors[1];
last_errors[2] = errors[2];
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.h b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.h
index 9354b1b..3017066 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.h
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.h
@@ -6,17 +6,14 @@
#ifndef HEADER__SPEEDCONTROL
#define HEADER__SPEEDCONTROL
-#define K_P 80
-#define K_I 1
-#define Tcomp 100 // speed will be computed on a Tcomp timesample
-#define Te 110
+#define K_P 150
+#define K_I 3
+#define Tcomp 40 // speed will be computed on a Tcomp timesample
+#define Te 50
#define K_v (1000/Tcomp)
-#define K_Pn K_P
-#define K_In 10//(K_I)*Te
-#define DEAD_ZONE 0
-#define MAX_COMMAND 3600
+#define INTEG_MAX 20000
#include "ch.h"
#include "motor.h"
hooks/post-receive
--
krobot
|
