Menu
▾
▴
[Krobot-commits] [SCM] krobot branch, master, updated. 01c6c4d6b738786e36dce17f38467fc1e0c72fd7
|
From: Xavier L. <Ba...@us...> - 2010-05-20 19:26:41
|
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 01c6c4d6b738786e36dce17f38467fc1e0c72fd7 (commit)
via 16ac54f3285ee3f4036e1b4650ea6ba007855e80 (commit)
via 3c3cfb29da4be2f565aea48827a885140fbe5656 (commit)
from 9ab5f4a67923e98473a1685942ecf176e8c06fc4 (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 01c6c4d6b738786e36dce17f38467fc1e0c72fd7
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 20 21:25:46 2010 +0200
Better constants, the PID controller parameters need to be better tuned
commit 16ac54f3285ee3f4036e1b4650ea6ba007855e80
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 20 21:25:12 2010 +0200
Better controller computation
commit 3c3cfb29da4be2f565aea48827a885140fbe5656
Author: Xavier Lagorce <Xav...@cr...>
Date: Thu May 20 21:23:59 2010 +0200
Use hardware PWM instead of semi-software PWM generation
Also use the indicators on the motor boards to show that the commands is at saturation point
-----------------------------------------------------------------------
Changes:
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/motor.c b/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
index 748356f..7972948 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/motor.c
@@ -5,13 +5,14 @@
#include "motor.h"
-uint8_t enabledMotors = 0;
+uint8_t enabledMotors = 0, indMotors = 0;
signed char currentSpeedSign[] = {0, 0, 0};
+TIM_OCInitTypeDef TIM_OCInitStructure;
/*
* Interrupt routine for PWM generation
*/
-void VectorB0(void) {
+/*void VectorB0(void) {
if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET)
{
@@ -45,7 +46,7 @@ void VectorB0(void) {
GPIO_SetBits(GPIOA, GPIO_Pin_1 | GPIO_Pin_2 | GPIO_Pin_3);
}
-}
+ }*/
/*
* Initialises TIM2 for PWM generation and associated GPIOs
@@ -53,7 +54,6 @@ void VectorB0(void) {
void motorsInit(void) {
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_OCInitTypeDef TIM_OCInitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
//Enable GPIOB and GPIOC clock
@@ -68,16 +68,19 @@ void motorsInit(void) {
// IN1 PB0
// IN2 PB1
// PWM PA1
+ // IND PC3
//Motor2 : STBY PC0
// IN1 PC1
// IN2 PC2
// PWM PA2
+ // IND PC5
//Motor3 : STBY PC6
// IN1 PC7
// IN2 PC8
// PWM PA3
- GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_0 | GPIO_Pin_2 | GPIO_Pin_6
- | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_10 | GPIO_Pin_13);
+ // 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_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
@@ -86,6 +89,7 @@ void motorsInit(void) {
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Default value of H-Bridge configuration
@@ -94,37 +98,32 @@ void motorsInit(void) {
| GPIO_Pin_3 | GPIO_Pin_9 | GPIO_Pin_13);
// TimeBase configuration
- TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t) (72000000 / 72000000) - 1;;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseStructure.TIM_Period = 36000; // 2 kHz
+ TIM_TimeBaseStructure.TIM_Period = 3600; // 20 kHz
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
- // Output Compare Active Mode configuration: Channel1
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Inactive;
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
- TIM_OCInitStructure.TIM_Pulse = 18000;
+ // PWM1 Mode configuration: Channel1
+ TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+ TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+ TIM_OCInitStructure.TIM_Pulse = 0;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- TIM_OC1Init(TIM2, &TIM_OCInitStructure);
TIM_OC2Init(TIM2, &TIM_OCInitStructure);
+ TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_OC3Init(TIM2, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Disable);
- TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Disable);
- TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Disable);
-
- // Enable interrupt vector
- NVICEnableVector(TIM2_IRQn, 0);
-
- // TIM IT enable
- TIM_ITConfig(TIM2, TIM_IT_CC1 | TIM_IT_CC2 | TIM_IT_CC3 | TIM_IT_Update, ENABLE);
+ TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
+ TIM_OC4Init(TIM2, &TIM_OCInitStructure);
+ TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
// All motors are disabled
enabledMotors = 0;
//Enable timer Peripherals
TIM_Cmd(TIM2,ENABLE);
+
+ GPIO_ResetBits(GPIOC, GPIO_Pin_3 | GPIO_Pin_5 | GPIO_Pin_9);
}
/*
@@ -170,15 +169,22 @@ void disableMotor(uint8_t motor) {
*/
void motorSetSpeed(uint8_t motor, int32_t speed) {
+ uint8_t ind = 0;
+
if (speed == 0) {
motorStop(motor, MOTOR_BRAKE);
return;
}
- if (speed >= MAX_PWM)
+ if (speed >= MAX_PWM) {
speed = MAX_PWM;
- if (speed <= -MAX_PWM)
+ ind = 1;
+ } else if (speed <= -MAX_PWM) {
speed = -MAX_PWM;
+ ind = 1;
+ } else {
+ ind = 0;
+ }
if (motor & MOTOR1) {
if(speed > 0) {
@@ -187,14 +193,26 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
GPIO_SetBits(GPIOB, GPIO_Pin_1);
GPIO_ResetBits(GPIOB, GPIO_Pin_0);
}
- TIM_SetCompare1(TIM2, (uint16_t)speed);
+ TIM_OCInitStructure.TIM_Pulse = (uint16_t)speed;
} else {
if (currentSpeedSign[0] != -1) {
currentSpeedSign[0] = -1;
GPIO_SetBits(GPIOB, GPIO_Pin_0);
GPIO_ResetBits(GPIOB, GPIO_Pin_1);
}
- TIM_SetCompare1(TIM2, (uint16_t)(-speed));
+ TIM_OCInitStructure.TIM_Pulse = (uint16_t)(-speed);
+ }
+ TIM_OC2Init(TIM2, &TIM_OCInitStructure);
+ TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
+ TIM_ARRPreloadConfig(TIM2, ENABLE);
+ if (ind) {
+ if ((indMotors & MOTOR1) == 0) {
+ GPIO_SetBits(GPIOC, GPIO_Pin_3);
+ indMotors |= MOTOR1;
+ }
+ } else if ((indMotors & MOTOR1) != 0) {
+ GPIO_ResetBits(GPIOC, GPIO_Pin_3);
+ indMotors &= ~MOTOR1;
}
}
if (motor & MOTOR2) {
@@ -204,14 +222,26 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
GPIO_SetBits(GPIOC, GPIO_Pin_2);
GPIO_ResetBits(GPIOC, GPIO_Pin_1);
}
- TIM_SetCompare2(TIM2, (uint16_t)speed);
+ TIM_OCInitStructure.TIM_Pulse = (uint16_t)speed;
} else {
if (currentSpeedSign[1] != -1) {
currentSpeedSign[1] = -1;
GPIO_SetBits(GPIOC, GPIO_Pin_1);
GPIO_ResetBits(GPIOC, GPIO_Pin_2);
}
- TIM_SetCompare2(TIM2, (uint16_t)(-speed));
+ TIM_OCInitStructure.TIM_Pulse = (uint16_t)(-speed);
+ }
+ TIM_OC3Init(TIM2, &TIM_OCInitStructure);
+ TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
+ TIM_ARRPreloadConfig(TIM2, ENABLE);
+ if (ind) {
+ if ((indMotors & MOTOR2) == 0) {
+ GPIO_SetBits(GPIOC, GPIO_Pin_5);
+ indMotors |= MOTOR2;
+ }
+ } else if ((indMotors & MOTOR2) != 0) {
+ GPIO_ResetBits(GPIOC, GPIO_Pin_5);
+ indMotors &= ~MOTOR2;
}
}
if (motor & MOTOR3) {
@@ -221,7 +251,7 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
GPIO_SetBits(GPIOC, GPIO_Pin_8);
GPIO_ResetBits(GPIOC, GPIO_Pin_7);
}
- TIM_SetCompare3(TIM2, (uint16_t)speed);
+ TIM_OCInitStructure.TIM_Pulse = (uint16_t)speed;
} else {
if (currentSpeedSign[2] != -1) {
currentSpeedSign[2] = -1;
@@ -230,6 +260,18 @@ void motorSetSpeed(uint8_t motor, int32_t speed) {
}
TIM_SetCompare3(TIM2, (uint16_t)(-speed));
}
+ TIM_OC4Init(TIM2, &TIM_OCInitStructure);
+ TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
+ TIM_ARRPreloadConfig(TIM2, ENABLE);
+ if (ind) {
+ if ((indMotors & MOTOR3) == 0) {
+ GPIO_SetBits(GPIOC, GPIO_Pin_9);
+ indMotors |= MOTOR3;
+ }
+ } else if ((indMotors & MOTOR3) != 0) {
+ GPIO_ResetBits(GPIOC, GPIO_Pin_9);
+ indMotors &= ~MOTOR3;
+ }
}
}
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/motor.h b/elec/boards/Controller_HolonomicDrive/Firmware/motor.h
index dd0b148..c08e974 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/motor.h
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/motor.h
@@ -13,7 +13,7 @@
#define MOTOR_STOP 1
#define MOTOR_BRAKE 2
-#define MAX_PWM 35000
+#define MAX_PWM 3600
#include "ch.h"
#include "nvic.h"
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
index 9ae4b91..b689068 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.c
@@ -8,7 +8,7 @@
#define MAX(x,y) ((x) > (y) ? (x) : (y))
#define MIN(x,y) ((x) > (y) ? (y) : (x))
-int32_t ref_speeds[3] = {0, 0, 0};
+volatile int32_t ref_speeds[3] = {0, 0, 0};
volatile int32_t cur_speeds[3] = {0, 0, 0};
/*
@@ -30,13 +30,14 @@ static msg_t ThreadSpeedController(void *arg) {
time = chTimeNow();
while (TRUE) {
- time += MS2ST(Te);
+ time += MS2ST(Tcomp);
prev_positions[0] = getEncoderPosition(ENCODER1);
prev_positions[1] = getEncoderPosition(ENCODER2);
prev_positions[2] = getEncoderPosition(ENCODER3);
- chThdSleepMilliseconds(Tcomp);
+ time += MS2ST(Te-Tcomp);
+ chThdSleepUntil(time);
positions[0] = getEncoderPosition(ENCODER1);
positions[1] = getEncoderPosition(ENCODER2);
@@ -51,9 +52,9 @@ static msg_t ThreadSpeedController(void *arg) {
errors[2] = ref_speeds[2] - cur_speeds[2];
//--> Command computation
- commands[0] = (K_Pn + K_In)*errors[0] - K_Pn*last_errors[0] + last_commands[0];
- commands[1] = (K_Pn + K_In)*errors[1] - K_Pn*last_errors[1] + last_commands[1];
- commands[2] = (K_Pn + K_In)*errors[2] - K_Pn*last_errors[2] + last_commands[2];
+ 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;
//--> End of command computation
if (commands[0] >= 0)
@@ -81,7 +82,7 @@ static msg_t ThreadSpeedController(void *arg) {
commands[2] = 0;
last_errors[0] = errors[0];
last_errors[1] = errors[1];
- last_errors[2] = errors[2];
+ last_errors[2] = errors[2];
motorSetSpeed(MOTOR1, commands[0]);
motorSetSpeed(MOTOR2, commands[1]);
diff --git a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.h b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.h
index b97ea45..9354b1b 100644
--- a/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.h
+++ b/elec/boards/Controller_HolonomicDrive/Firmware/speed_control.h
@@ -6,17 +6,17 @@
#ifndef HEADER__SPEEDCONTROL
#define HEADER__SPEEDCONTROL
-#define K_P 17
-#define K_I 1
-#define Tcomp 15 // speed will be computed on a Tcomp timesample
-#define Te 50
+#define K_P 80
+#define K_I 1
+#define Tcomp 100 // speed will be computed on a Tcomp timesample
+#define Te 110
#define K_v (1000/Tcomp)
#define K_Pn K_P
-#define K_In 10//(K_I)*Te
+#define K_In 10//(K_I)*Te
-#define DEAD_ZONE 100
-#define MAX_COMMAND 35500
+#define DEAD_ZONE 0
+#define MAX_COMMAND 3600
#include "ch.h"
#include "motor.h"
hooks/post-receive
--
krobot
|
