-
Notifications
You must be signed in to change notification settings - Fork 0
/
BalanceCar.h
179 lines (162 loc) · 4.98 KB
/
BalanceCar.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
/*
* @Description: In User Settings Edit
* @Author: your name
* @Date: 2019-10-08 09:35:07
* @LastEditTime: 2019-10-11 16:25:04
* @LastEditors: Please set LastEditors
*/
#include "MsTimer2.h"
#include "KalmanFilter.h"
#include "I2Cdev.h"
//#include "MPU6050_6Axis_MotionApps20.h"
#include "MPU6050.h"
#include "Wire.h"
MPU6050 mpu;
KalmanFilter kalmanfilter;
//Setting PID parameters
double kp_balance = 55, kd_balance = 0.75;
double kp_speed = 10, ki_speed = 0.26;
double kp_turn = 2.5, kd_turn = 0.5;
//Setting MPU6050 calibration parameters
double angle_zero = 0; //x axle angle calibration
double angular_velocity_zero = 0; //x axle angular velocity calibration
volatile unsigned long encoder_count_right_a = 0;
volatile unsigned long encoder_count_left_a = 0;
int16_t ax, ay, az, gx, gy, gz;
float dt = 0.005, Q_angle = 0.001, Q_gyro = 0.005, R_angle = 0.5, C_0 = 1, K1 = 0.05;
int encoder_left_pulse_num_speed = 0;
int encoder_right_pulse_num_speed = 0;
double speed_control_output = 0;
double rotation_control_output = 0;
double speed_filter = 0;
int speed_control_period_count = 0;
double car_speed_integeral = 0;
double speed_filter_old = 0;
int setting_car_speed = 0;
int setting_turn_speed = 0;
double pwm_left = 0;
double pwm_right = 0;
float kalmanfilter_angle;
// char balance_angle_min = -27;
// char balance_angle_max = 27;
char balance_angle_min = -22;
char balance_angle_max = 22;
void carStop()
{
digitalWrite(AIN1, HIGH);
digitalWrite(BIN1, LOW);
digitalWrite(STBY_PIN, HIGH);
analogWrite(PWMA_LEFT, 0);
analogWrite(PWMB_RIGHT, 0);
}
void carForward(unsigned char speed)
{
digitalWrite(AIN1, 0);
digitalWrite(BIN1, 0);
analogWrite(PWMA_LEFT, speed);
analogWrite(PWMB_RIGHT, speed);
}
void carBack(unsigned char speed)
{
digitalWrite(AIN1, 1);
digitalWrite(BIN1, 1);
analogWrite(PWMA_LEFT, speed);
analogWrite(PWMB_RIGHT, speed);
}
void balanceCar()
{
sei();
encoder_left_pulse_num_speed += pwm_left < 0 ? -encoder_count_left_a : encoder_count_left_a;
encoder_right_pulse_num_speed += pwm_right < 0 ? -encoder_count_right_a : encoder_count_right_a;
encoder_count_left_a = 0;
encoder_count_right_a = 0;
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
kalmanfilter.Angle(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);
kalmanfilter_angle = kalmanfilter.angle;
double balance_control_output = kp_balance * (kalmanfilter_angle - angle_zero) + kd_balance * (kalmanfilter.Gyro_x - angular_velocity_zero);
speed_control_period_count++;
if (speed_control_period_count >= 8)
{
speed_control_period_count = 0;
double car_speed = (encoder_left_pulse_num_speed + encoder_right_pulse_num_speed) * 0.5;
encoder_left_pulse_num_speed = 0;
encoder_right_pulse_num_speed = 0;
speed_filter = speed_filter_old * 0.7 + car_speed * 0.3;
speed_filter_old = speed_filter;
car_speed_integeral += speed_filter;
car_speed_integeral += -setting_car_speed;
car_speed_integeral = constrain(car_speed_integeral, -3000, 3000);
speed_control_output = -kp_speed * speed_filter - ki_speed * car_speed_integeral;
rotation_control_output = setting_turn_speed + kd_turn * kalmanfilter.Gyro_z;
}
pwm_left = balance_control_output - speed_control_output - rotation_control_output;
pwm_right = balance_control_output - speed_control_output + rotation_control_output;
pwm_left = constrain(pwm_left, -255, 255);
pwm_right = constrain(pwm_right, -255, 255);
if (motion_mode != START && motion_mode != STOP && (kalmanfilter_angle < balance_angle_min || balance_angle_max < kalmanfilter_angle))
{
motion_mode = STOP;
carStop();
}
if (motion_mode == STOP && key_flag != '4')
{
car_speed_integeral = 0;
setting_car_speed = 0;
pwm_left = 0;
pwm_right = 0;
carStop();
}
else if (motion_mode == STOP)
{
car_speed_integeral = 0;
setting_car_speed = 0;
pwm_left = 0;
pwm_right = 0;
}
else
{
if (pwm_left < 0)
{
digitalWrite(AIN1, 1);
analogWrite(PWMA_LEFT, -pwm_left);
}
else
{
digitalWrite(AIN1, 0);
analogWrite(PWMA_LEFT, pwm_left);
}
if (pwm_right < 0)
{
digitalWrite(BIN1, 1);
analogWrite(PWMB_RIGHT, -pwm_right);
}
else
{
digitalWrite(BIN1, 0);
analogWrite(PWMB_RIGHT, pwm_right);
}
}
}
void encoderCountRightA()
{
encoder_count_right_a++;
}
void encoderCountLeftA()
{
encoder_count_left_a++;
}
void carInitialize()
{
pinMode(AIN1, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(PWMA_LEFT, OUTPUT);
pinMode(PWMB_RIGHT, OUTPUT);
pinMode(STBY_PIN, OUTPUT);
carStop();
Wire.begin();
mpu.initialize();
attachInterrupt(digitalPinToInterrupt(ENCODER_LEFT_A_PIN), encoderCountLeftA, CHANGE);
attachPinChangeInterrupt(ENCODER_RIGHT_A_PIN, encoderCountRightA, CHANGE);
MsTimer2::set(5, balanceCar);
MsTimer2::start();
}