-
Notifications
You must be signed in to change notification settings - Fork 0
/
ctrl.py
59 lines (45 loc) · 1.41 KB
/
ctrl.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
import PID
euler_pidx = PID.PID()
euler_pidx.clear
euler_pidy = PID.PID()
euler_pidy.clear
euler_pidz = PID.PID()
euler_pidz.clear
class ctrl:
def __init__(self,rx0=0.0,ry0=0.0,rz0=0.0):
self.rollx = rx0
self.rolly = ry0
self.rollz = rz0
self.clear()
def clear(self):
self.dP=[1,1,1]
self.motorP=[60,60,60,60]
def motor(self,rx,ry,rz):
self.euler=[rx,ry,rz]
euler_pidx.update(rx)
d_rx=euler_pidx.output
euler_pidy.update(ry)
d_ry=euler_pidy.output
euler_pidy.update(rz)
d_rz=euler_pidz.output
self.deuler=[d_rx,d_ry,d_rz]
kx=1
ky=1
kz=0.1
"""
motor[n]は第n+1象限とする
motor0,motor2はroll_zと回転の方向が同じであるとする
x回転の調整 [+,+,-,-]
y回転の調整 [-,+,+,-]
z回転の調整 [+,-,+,-]
"""
self.motorP[0]=self.motorP[0]-kx*self.deuler[0]*self.dP[0]+ky*self.deuler[1]*self.dP[1]-kz*self.deuler[2]*self.dP[2]
self.motorP[1]=self.motorP[1]-kx*self.deuler[0]*self.dP[0]-ky*self.deuler[1]*self.dP[1]+kz*self.deuler[2]*self.dP[2]
self.motorP[2]=self.motorP[2]+kx*self.deuler[0]*self.dP[0]-ky*self.deuler[1]*self.dP[1]-kz*self.deuler[2]*self.dP[2]
self.motorP[3]=self.motorP[3]+kx*self.deuler[0]*self.dP[0]+ky*self.deuler[1]*self.dP[1]+kz*self.deuler[2]*self.dP[2]
for i in range(4):
if self.motorP[i] > 66:
self.motorP[i]= 66
elif self.motorP[i]<54:
self.motorP[i]=54
print(self.motorP)