Skip to content

Commit 9f6e693

Browse files
fdilafdila
authored
Merge pull request #22 from iralabdisco/pid_antiwindup
Pid antiwindup
2 parents 911d0c7 + 2db8a30 commit 9f6e693

10 files changed

Lines changed: 238 additions & 242 deletions

File tree

‎otto_controller/Core/Inc/motor_controller.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,11 +22,12 @@ class MotorController {
2222
this->dir_pin_ = dir_pin;
2323
this->pwm_timer_ = pwm_timer;
2424
this->pwm_channel_ = pwm_channel;
25-
this->max_dutycycle_ = pwm_timer->Instance->ARR;
25+
this->max_dutycycle_ = 0;
2626
}
2727

2828
void setup() {
2929
HAL_TIM_PWM_Start(pwm_timer_, pwm_channel_);
30+
this->max_dutycycle_ = pwm_timer_->Instance->ARR;
3031
}
3132

3233
void set_speed(int duty_cycle) {

‎otto_controller/Core/Inc/pid.h‎

Lines changed: 17 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ class Pid {
2020
int min_;
2121
int max_;
2222

23-
Pid(float kp, float ki, float kd) {
23+
Pid(float kp, float ki, float kd, int min, int max) {
2424
this->kp_ = kp;
2525
this->ki_ = ki;
2626
this->kd_ = kd;
@@ -31,9 +31,12 @@ class Pid {
3131
this->previous_error_ = 0;
3232
this->error_sum_ = 0;
3333

34+
this->min_ = min;
35+
this->max_ = max;
36+
3437
}
3538

36-
void config(float kp, float ki, float kd) {
39+
void config(float kp, float ki, float kd, int min, int max) {
3740
this->kp_ = kp;
3841
this->ki_ = ki;
3942
this->kd_ = kd;
@@ -44,6 +47,9 @@ class Pid {
4447
this->previous_error_ = 0;
4548
this->error_sum_ = 0;
4649

50+
this->min_ = min;
51+
this->max_ = max;
52+
4753
}
4854

4955
void set(float setpoint) {
@@ -65,12 +71,16 @@ class Pid {
6571
output += (this->error_ - this->previous_error_) * kd_;
6672
this->previous_error_ = this->error_;
6773

68-
int integer_output = static_cast<int> (output);
74+
int integer_output = static_cast<int>(output);
6975

70-
// if(integer_output > this->max_)
71-
// integer_output = this->max_;
72-
// else if (integer_output < this->min_)
73-
// integer_output = this->min_;
76+
//anti windup
77+
if (integer_output > this->max_) {
78+
integer_output = this->max_;
79+
this->error_sum_ -= this->error_;
80+
} else if (integer_output < this->min_){
81+
integer_output = this->min_;
82+
this->error_sum_ -= this->error_;
83+
}
7484

7585
return integer_output;
7686

‎otto_controller/Core/Src/main.cpp‎

Lines changed: 19 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -69,10 +69,12 @@ float left_velocity;
6969
float right_velocity;
7070

7171
//PID
72+
int pid_min = 0;
73+
int pid_max = 0;
7274

73-
Pid left_pid(180, 200, 0);
74-
Pid right_pid(185, 195, 0);
75-
Pid cross_pid(50, 20, 0);
75+
Pid left_pid(180, 200, 0, pid_min, pid_max);
76+
Pid right_pid(185, 195, 0, pid_min, pid_max);
77+
Pid cross_pid(50, 20, 0, pid_min, pid_max);
7678

7779
int left_dutycycle;
7880
int right_dutycycle;
@@ -82,14 +84,12 @@ MotorController right_motor(sleep1_GPIO_Port,
8284
sleep1_Pin,
8385
dir1_GPIO_Port,
8486
dir1_Pin,
85-
&htim4,
86-
TIM_CHANNEL_4);
87+
&htim4, TIM_CHANNEL_4);
8788
MotorController left_motor(sleep2_GPIO_Port,
8889
sleep2_Pin,
8990
dir2_GPIO_Port,
9091
dir2_Pin,
91-
&htim4,
92-
TIM_CHANNEL_3);
92+
&htim4, TIM_CHANNEL_3);
9393

9494
//Communication
9595
uint8_t *tx_buffer;
@@ -157,6 +157,14 @@ int main(void) {
157157
left_motor.setup();
158158
right_motor.setup();
159159

160+
//right and left motors have the same parameters
161+
pid_min = -left_motor.max_dutycycle_;
162+
pid_max = left_motor.max_dutycycle_;
163+
164+
left_pid.config(180, 200, 0, pid_min, pid_max);
165+
right_pid.config(185, 195, 0, pid_min, pid_max);
166+
cross_pid.config(50, 20, 0, pid_min, pid_max);
167+
160168
left_motor.coast();
161169
right_motor.coast();
162170

@@ -190,8 +198,7 @@ void SystemClock_Config(void) {
190198
/** Configure the main internal regulator output voltage
191199
*/
192200
__HAL_RCC_PWR_CLK_ENABLE();
193-
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);
194-
/** Initializes the CPU, AHB and APB busses clocks
201+
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE3);/** Initializes the CPU, AHB and APB busses clocks
195202
*/
196203
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
197204
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
@@ -226,13 +233,13 @@ void SystemClock_Config(void) {
226233
static void MX_NVIC_Init(void) {
227234
/* TIM3_IRQn interrupt configuration */
228235
HAL_NVIC_SetPriority(TIM3_IRQn, 2, 1);
229-
HAL_NVIC_EnableIRQ(TIM3_IRQn);
236+
HAL_NVIC_EnableIRQ (TIM3_IRQn);
230237
/* TIM6_DAC_IRQn interrupt configuration */
231238
HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 2, 2);
232-
HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
239+
HAL_NVIC_EnableIRQ (TIM6_DAC_IRQn);
233240
/* USART6_IRQn interrupt configuration */
234241
HAL_NVIC_SetPriority(USART6_IRQn, 1, 0);
235-
HAL_NVIC_EnableIRQ(USART6_IRQn);
242+
HAL_NVIC_EnableIRQ (USART6_IRQn);
236243
}
237244

238245
/* USER CODE BEGIN 4 */

‎utils/pid_tuning/otto_pid_tuning/Core/Inc/constants.h‎

Lines changed: 0 additions & 10 deletions
This file was deleted.

‎utils/pid_tuning/otto_pid_tuning/Core/Inc/encoder.h‎

Lines changed: 36 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,6 @@
22
#define ENCODER_H
33

44
#include "main.h"
5-
#include "constants.h"
65

76
class Encoder {
87
public:
@@ -11,15 +10,21 @@ class Encoder {
1110
uint32_t current_millis_;
1211
int32_t ticks_; //if negative the wheel is going backwards
1312
float wheel_circumference_;
13+
int ticks_per_revolution_;
1414

1515
Encoder() {
1616
timer_ = NULL;
1717
wheel_circumference_ = 0;
18+
ticks_per_revolution_ = 0;
1819
}
1920

20-
Encoder(TIM_HandleTypeDef *timer, float wheel_circ);
21+
Encoder(TIM_HandleTypeDef *timer, float wheel_circ,
22+
int ticks_per_revolution) {
23+
timer_ = timer;
24+
wheel_circumference_ = wheel_circ;
25+
ticks_per_revolution_ = ticks_per_revolution;
2126

22-
void Setup();
27+
}
2328

2429
int GetCount() {
2530
int count = ((int) __HAL_TIM_GET_COUNTER(this->timer_)
@@ -29,14 +34,38 @@ class Encoder {
2934

3035
void ResetCount() {
3136
//set counter to half its maximum value
32-
__HAL_TIM_SET_COUNTER(timer_, (timer_->Init.Period) / 2);
37+
__HAL_TIM_SET_COUNTER(timer_, (timer_->Init.Period / 2));
38+
}
39+
40+
void Setup() {
41+
HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL);
42+
this->ResetCount();
43+
this->previous_millis_ = 0;
44+
this->current_millis_ = HAL_GetTick();
3345
}
3446

35-
void UpdateValues();
47+
void UpdateValues() {
48+
this->previous_millis_ = this->current_millis_;
49+
this->current_millis_ = HAL_GetTick();
50+
this->ticks_ = this->GetCount();
51+
this->ResetCount();
52+
}
3653

37-
float GetMeters();
54+
float GetMeters() {
55+
float meters = ((float) this->ticks_ * this->wheel_circumference_)
56+
/ ticks_per_revolution_;
57+
return meters;
58+
}
3859

39-
float GetLinearVelocity();
60+
float GetLinearVelocity() {
61+
this->UpdateValues();
62+
float meters = this->GetMeters();
63+
float deltaTime = this->current_millis_ - this->previous_millis_;
64+
if (deltaTime == 0)
65+
return 0;
66+
float linear_velocity = (meters / (deltaTime / 1000));
67+
return linear_velocity;
68+
}
4069

4170
};
4271
#endif

‎utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,6 @@
22
#define MOTOR_CONTROLLER_H
33

44
#include "main.h"
5-
#include "constants.h"
65

76
class MotorController {
87
public:
@@ -12,6 +11,7 @@ class MotorController {
1211
uint16_t dir_pin_;
1312
TIM_HandleTypeDef *pwm_timer_;
1413
uint32_t pwm_channel_;
14+
int32_t max_dutycycle_;
1515

1616
MotorController(GPIO_TypeDef *sleep_gpio_port, uint16_t sleep_pin,
1717
GPIO_TypeDef *dir_gpio_port, uint16_t dir_pin,
@@ -22,10 +22,12 @@ class MotorController {
2222
this->dir_pin_ = dir_pin;
2323
this->pwm_timer_ = pwm_timer;
2424
this->pwm_channel_ = pwm_channel;
25+
this->max_dutycycle_ = 0;
2526
}
2627

2728
void setup() {
2829
HAL_TIM_PWM_Start(pwm_timer_, pwm_channel_);
30+
this->max_dutycycle_ = pwm_timer_->Instance->ARR;
2931
}
3032

3133
void set_speed(int duty_cycle) {
@@ -34,8 +36,8 @@ class MotorController {
3436
HAL_GPIO_WritePin(dir_gpio_port_, dir_pin_, GPIO_PIN_SET);
3537

3638
//check if duty_cycle exceeds maximum
37-
if (duty_cycle > MAX_DUTY_CYCLE)
38-
__HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, MAX_DUTY_CYCLE);
39+
if (duty_cycle > max_dutycycle_)
40+
__HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, max_dutycycle_);
3941
else
4042
__HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, duty_cycle);
4143

@@ -44,8 +46,8 @@ class MotorController {
4446
HAL_GPIO_WritePin(dir_gpio_port_, dir_pin_, GPIO_PIN_RESET);
4547

4648
//check if duty_cycle is lower than minimum
47-
if (duty_cycle < -MAX_DUTY_CYCLE)
48-
__HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, MAX_DUTY_CYCLE);
49+
if (duty_cycle < -max_dutycycle_)
50+
__HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, max_dutycycle_);
4951
else
5052
//invert sign to make duty_cycle positive
5153
__HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, -duty_cycle);

‎utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h‎

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1,30 +1,35 @@
11
#ifndef ODOMETRY_H
22
#define ODOMETRY_H
33

4-
#include "constants.h"
5-
64
class Odometry {
75
private:
86

97
float left_velocity_;
108
float right_velocity_;
11-
9+
float baseline_;
1210

1311
public:
1412
Odometry() {
1513
left_velocity_ = 0;
1614
right_velocity_ = 0;
15+
baseline_ = 0;
16+
}
17+
18+
Odometry(float baseline) {
19+
left_velocity_ = 0;
20+
right_velocity_ = 0;
21+
baseline_ = baseline;
1722
}
1823

1924
void UpdateValues(float linear_vel, float angular_vel) {
20-
left_velocity_ = linear_vel - (BASELINE * angular_vel)/2;
21-
right_velocity_ = 2 * linear_vel - left_velocity_;
25+
left_velocity_ = linear_vel - (baseline_ * angular_vel) / 2;
26+
right_velocity_ = linear_vel + (baseline_ * angular_vel) / 2;
2227
}
2328

24-
float GetLeftVelocity(){
29+
float GetLeftVelocity() {
2530
return left_velocity_;
2631
}
27-
float GetRightVelocity(){
32+
float GetRightVelocity() {
2833
return right_velocity_;
2934
}
3035

‎utils/pid_tuning/otto_pid_tuning/Core/Inc/pid.h‎

Lines changed: 16 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@ class Pid {
2222
int min_;
2323
int max_;
2424

25-
Pid(float kp, float ki, float kd) {
25+
Pid(float kp, float ki, float kd, int min, int max) {
2626
this->kp_ = kp;
2727
this->ki_ = ki;
2828
this->kd_ = kd;
@@ -33,12 +33,12 @@ class Pid {
3333
this->previous_error_ = 0;
3434
this->error_sum_ = 0;
3535

36-
this->min_ = -MAX_DUTY_CYCLE;
37-
this->max_ = MAX_DUTY_CYCLE;
36+
this->min_ = min;
37+
this->max_ = max;
3838

3939
}
4040

41-
void config(float kp, float ki, float kd) {
41+
void config(float kp, float ki, float kd, int min, int max) {
4242
this->kp_ = kp;
4343
this->ki_ = ki;
4444
this->kd_ = kd;
@@ -49,6 +49,9 @@ class Pid {
4949
this->previous_error_ = 0;
5050
this->error_sum_ = 0;
5151

52+
this->min_ = min;
53+
this->max_ = max;
54+
5255
}
5356

5457
void set(float setpoint) {
@@ -70,12 +73,16 @@ class Pid {
7073
output += (this->error_ - this->previous_error_) * kd_;
7174
this->previous_error_ = this->error_;
7275

73-
int integer_output = static_cast<int> (output);
76+
int integer_output = static_cast<int>(output);
7477

75-
// if(integer_output > this->max_)
76-
// integer_output = this->max_;
77-
// else if (integer_output < this->min_)
78-
// integer_output = this->min_;
78+
//anti windup
79+
if (integer_output > this->max_) {
80+
integer_output = this->max_;
81+
this->error_sum_ -= this->error_;
82+
} else if (integer_output < this->min_){
83+
integer_output = this->min_;
84+
this->error_sum_ -= this->error_;
85+
}
7986

8087
return integer_output;
8188

0 commit comments

Comments
 (0)