-
Notifications
You must be signed in to change notification settings - Fork 4
/
esc_angular_stabilization.py
137 lines (90 loc) · 3.21 KB
/
esc_angular_stabilization.py
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
from math import atan, pi, sqrt
from time import sleep
from machine import I2C, PWM, Pin
from imu import MPU6050
i2c = I2C(1, sda=Pin(26), scl=Pin(27), freq=400000)
imu = MPU6050(i2c)
esc_pitch = PWM(Pin(5))
esc_pitch.freq(50)
esc_roll = PWM(Pin(13))
esc_roll.freq(50)
gx_cal = 0
gy_cal = 0
gz_cal = 0
kalman_angle_roll = 0
kalman_uncertainty_angle_roll = 4
kalman_angle_pitch = 0
kalman_uncertainty_angle_pitch = 4
kalman_1d_output = [0, 0]
duty = 0
def calculateAngles(ax, ay, az):
angle_roll = atan(ay/sqrt(ax**2 + az**2)) * (180 / pi)
angle_pitch = -atan(ax/sqrt(ay**2 + az**2)) * (180 / pi)
return angle_roll, angle_pitch
def calibrateGyro():
global gx_cal, gy_cal, gz_cal
gyro_cal_count = 1000
for i in range(gyro_cal_count):
print("Calibrating gyroscope...", i, end="\r")
gx_cal += imu.gyro.x
gy_cal += imu.gyro.y
gz_cal += imu.gyro.z
sleep(0.001) # sleep for 10ms
gx_cal /= gyro_cal_count
gy_cal /= gyro_cal_count
gz_cal /= gyro_cal_count
print("Gyroscope calibration complete! :", gx_cal, gy_cal, gz_cal)
input("Press any key then enter to start...")
def kalman_1d(kalman_state, kalman_uncertainty, kalman_input, kalman_measurement):
global kalman_1d_output
kalman_state += (0.004 * kalman_input)
kalman_uncertainty += ((0.004 ** 2) * (8**2))
kalman_gain = kalman_uncertainty * 1 / (1 * kalman_uncertainty + 3 * 3)
kalman_state += kalman_gain * (kalman_measurement - kalman_state)
kalman_uncertainty *= (1 - kalman_gain)
kalman_1d_output[0] = kalman_state
kalman_1d_output[1] = kalman_uncertainty
def rangify(value):
init_start = -45
init_end = 45
final_start = 3700 # 2293
final_end = 4200 # 6553
init_range = init_end - init_start
final_range = final_end - final_start
ratio = final_range / init_range
distance = value - init_start
new_value = distance * ratio + final_start
return int(new_value)
if __name__ == "__main__":
calibrateGyro()
while True:
ax = imu.accel.x - 0.06 # calibration
ay = imu.accel.y + 0.01 # calibration
az = imu.accel.z + 0.116 # calibration
gx = imu.gyro.x - gx_cal
gy = imu.gyro.y - gy_cal
gz = imu.gyro.z - gz_cal
tem = imu.temperature
roll, pitch = calculateAngles(ax, ay, az)
kalman_1d(kalman_angle_roll, kalman_uncertainty_angle_roll, gx, roll)
kalman_angle_roll = kalman_1d_output[0]
kalman_uncertainty_angle_roll = kalman_1d_output[1]
kalman_1d(kalman_angle_pitch,
kalman_uncertainty_angle_pitch, gy, pitch)
kalman_angle_pitch = kalman_1d_output[0]
kalman_uncertainty_angle_pitch = kalman_1d_output[1]
print("Roll:", kalman_angle_roll, "Pitch:", kalman_angle_pitch)
if kalman_angle_pitch < -15:
duty = 0
elif kalman_angle_pitch > 15:
duty = 65535
else:
duty = rangify(kalman_angle_pitch)
esc_pitch.duty_u16(duty)
if kalman_angle_roll < -15:
duty = 0
elif kalman_angle_roll > 15:
duty = 65535
else:
duty = rangify(kalman_angle_roll)
esc_roll.duty_u16(duty)