/
PID-ctl.lua
187 lines (162 loc) · 4.69 KB
/
PID-ctl.lua
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
180
181
182
183
184
185
186
-------------------------------------------------------------------------------
--
-- Anubisway control
-- A self balancing two wheel robotic vehicle
--
-- March 2010, César Raúl Mamani Choquehuanca, PUC-Rio
-- Riobotz (www.riobotz.com.br)
-- Led Lab (www.giga.puc-rio.br)
--
-------------------------------------------------------------------------------
--PID Control
--------------------------------------------------------------------------------
if pd.board()~="EK-LM3S8962" then
print "Placa nao suportada"
return
end
term.clrscr()
KP,KD,KS=5,4,0.3 --constantes de controle PD e direcao
KI=0.70 --constante integral
I_acc=0 --Acumulador de integral
bufferint=0
local adcvals={} --matriz com os dados do ADC
ang,vel=5,0 --variaveis de inicio do filtro
U=0 --variavel de controle
FLAG=1 --0 REVERSE 1 FORWARD
direc=0
f=io.open("/mmc/datasensor.txt","w") --criando arquivo para salvar os dados dos sensores aquisitados
--variaveis offset para controle
offsetzacc=571 --offset do acelerometro y
offsetgyr=872 --offset do gyroscopio (346.4) min=86.6 max=1646.35
offsetdir=498 --offset do potenciomentro
ang_ref=5 --angulo de referenza em grau
--configuracao do adc
ch={1,2,3}
adcsmoothing={16,16,16}
numiter=50
for i,v in ipairs(ch) do
adc.setblocking(v,1)
adc.setclock(v,0) --linha 10
adc.setsmoothing(v,adcsmoothing[i])
end
numiter=50
--configuracao do pwm
duty1,duty2=0,0
pwmid1=4
pwmid2=5
pwm.setclock(pwmid1,5000)
pwm.setclock(pwmid2,5000)
clock1=5000
clock2=5000
pwm.setup(pwmid1,clock1,duty1)
pwm.setup(pwmid2,clock2,duty2)
pwm.start(pwmid1)
pwm.start(pwmid2)
--configuracao dos pinos de habilitação e reversa
pio.pin.setdir( pio.OUTPUT, pio.PF_3 ) -- SSICLK
pio.pin.setdir( pio.OUTPUT, pio.PC_7 ) -- SFSS
pio.pin.setdir( pio.OUTPUT, pio.PF_2 ) -- SSIRX
pio.pin.setdir( pio.OUTPUT, pio.PC_5 ) -- SSITX
--Habilitação dos motores
pio.pin.setlow(pio.PF_2) --SSITX low para abilitar
pio.pin.setlow(pio.PC_5) --SSIRX low pra habilitazao
while true do --inicio do loop
--Aquisitando os dados do adc
for j=1,numiter do
adc.sample(ch,1)
for i,v in ipairs(ch) do
adc.insertsamples(v,adcvals,i,1)
end
end
d_adc=adcvals[3] --direcao ADC3
a_adc=adcvals[1] --acelerometro ADC1
g_adc=adcvals[2] --giroscopio ADC2
--Tratamento dos dados
a_seg=math.floor((a_adc-offsetzacc)*0.75) --em graus seg
g_seg=math.floor((g_adc-offsetgyr)*-0.35) --em graus seg/s
d_seg=math.floor((d_adc-offsetdir)*0.1) --em graus seg
delta=tmr.read(0)
ang=0.9*(ang+g_seg*delta*0.000002) --filtragem do angulo
delta=tmr.start(0)
ang=ang+0.1*a_seg --filtragem do angulo
vel=0.85*vel+0.15*g_seg --filtragem da velocidade
direc=0.6*direc+0.4*d_seg --filtragem da sinal de direcao
direc=KS*direc
------------------------------------------------------------------------------------
-- Controle PID - End - U eh a saida de controle
------------------------------------------------------------------------------------
I_acc=I_acc+(ang_ref-ang)
if I_acc>15 then
I_acc=15
elseif I_acc<-15 then
I_acc=-15
end
if bufferint*(ang_ref-ang)<0 then
I_acc=0
end
bufferint=ang_ref-ang
U=KP*(ang_ref-ang)+KD*vel+KI*I_acc
-----------------------------------------------------------------------------------
-- Salvando as leturas das variaveis de controle
f:write(tostring(ang) .. " " .. tostring(vel) .. " " .. tostring(direc) .. " " .. tostring(bufferint) .. "\n")
--enviando pra os motores
duty1=U
duty2=U
if direc<8 and direc>-8 then
direc=0
end
if direc>12 then
direc=12
elseif direc<-12 then
direc=-12
end
duty1=duty1-direc
duty2=duty2+direc
duty1=math.floor(duty1)
duty2=math.floor(duty2)
if duty1<8 and duty1>-8 and FLAG==1 then
duty1=8
elseif duty1<8 and duty1>-8 and FLAG==0 then
duty1=-8
end
if duty2<8 and duty2>-8 and FLAG==1 then
duty2=8
elseif duty2<8 and duty2>-8 and FLAG==0 then
duty2=-8
end
if duty1>=8 then
FLAG=1
pio.pin.sethigh(pio.PC_7) -- motor ezquerdo se e positivo avanza low
elseif duty1<=-8 then
FLAG=0
pio.pin.setlow(pio.PC_7) -- motor ezquerdo se e negativo retrocede high
end
if duty2>=8 then
FLAG=1
pio.pin.setlow(pio.PF_3) -- motor direito se e positivo avanza low
elseif duty2<=-8 then
FLAG=0
pio.pin.sethigh(pio.PF_3) -- motor direito se e negativo retrocede high
end
duty1=math.abs(duty1)
duty2=math.abs(duty2)
if duty1>60 then
duty1=60
end
if duty2>60 then
duty2=60
end
pwm.setup(pwmid1,clock1,duty1)
pwm.setup(pwmid2,clock2,duty2)
e_count,c_count=0,0
e_int,c_int=0,0
if ang>18 then
break
elseif ang<-18 then
break
end
end
--Fechando os motores
pio.pin.sethigh(pio.PF_2) --SSITX low para abilitar
pio.pin.sethigh(pio.PC_5) --SSIRX low pra habilitazao
f:close()