your-flight
/
automobiles_pck
mirror of https://github.com/APercy/automobiles_pck
4
0
Fork 0
Code Issues Projects Releases Wiki Activity

experiments with pid

This commit is contained in:
Alexsandro Percy 2023-05-01 09:42:12 -03:00
parent ba0f5ace51
commit bd86860267
1 changed files with 35 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
automobiles_lib/ground_detection.lua
View File

@ -1,14 +1,29 @@
function automobiles_lib.pd_controller(current_value, setpoint, last_error, kp, kd)
-- parameters
kp = kp or 0.5 -- Ganho proporcional
kd = kd or 0.2 -- Ganho derivativo
--calculo
local error = setpoint - current_value
local derivative = error - last_error
local output = kp * error + kd * derivative
last_error = error
function automobiles_lib.putAngleOnRange(angle)
local n_angle = angle/360
n_angle = (n_angle - math.floor(n_angle))*360
if n_angle < -180 then n_angle = n_angle + 360 end
if n_angle > 180 then n_angle = n_angle - 360 end
return n_angle
end
function automobiles_lib.pid_controller(current_value, setpoint, last_error, delta_t, kp, ki, kd)
kp = kp or 0
ki = ki or 0.00000000000001
kd = kd or 0.05
delta_t = delta_t or 0.100;
ti = kp/ki
td = kd/kp
local _error = setpoint - current_value
local derivative = _error - last_error
--local output = kpv*erro + (kpv/Tiv)*I + kpv*Tdv*((erro - erro_passado)/delta_t);
if integrative == nil then integrative = 0 end
integrative = integrative + (((_error + last_error)/delta_t)/2);
local output = kp*_error + (kp/ti)*integrative + kp * td*((_error - last_error)/delta_t)
last_error = _error
return output, last_error
end
--lets assume that the rear axis is at object center, so we will use the distance only for front wheels
@ -57,17 +72,6 @@ function automobiles_lib.ground_get_distances(self, radius, axis_distance)
local new_pitch = math.atan(m) --math.atan2(deltaY, deltaX);
--local deg_angle = math.deg(new_pitch)
--[[if self._last_error == nil then self._last_error = 0 end -- Último erro registrado
if math.abs(self._pitch) < math.abs(new_pitch) and self._longit_speed > 2 then
-- Estado atual do sistema
local output, last_error = automobiles_lib.pd_controller(self._pitch, new_pitch, self._last_error, 1.5, 0.0003)
self._last_error = last_error
new_pitch = output
minetest.chat_send_all(new_pitch)
else
self._last_error = 0
end]]--
--first test front wheels before
if (front_obstacle_level.y-self._last_front_detection) <= self.initial_properties.stepheight then
pitch = new_pitch --math.atan2(deltaY, deltaX);
@ -76,6 +80,16 @@ function automobiles_lib.ground_get_distances(self, radius, axis_distance)
else
--until 20 deg, climb
if math.deg(new_pitch - self._last_pitch) <= 20 then
--[[if self._last_error == nil then self._last_error = 0 end -- Último erro registrado
-- Estado atual do sistema
local output, last_error = automobiles_lib.pid_controller(self._last_pitch, new_pitch, self._last_error, self.dtime/2, 0.5)
self._last_error = last_error
new_pitch = output
local conversion = automobiles_lib.putAngleOnRange(math.deg(new_pitch))
new_pitch = math.rad(conversion)
minetest.chat_send_all("last: "..self._last_pitch.." - new: "..new_pitch)]]--
pitch = new_pitch
self._last_pitch = new_pitch
self._last_front_detection = front_obstacle_level.y