diff --git a/lib_planes/control.lua b/lib_planes/control.lua index 3c85e22..f8f797c 100755 --- a/lib_planes/control.lua +++ b/lib_planes/control.lua @@ -235,7 +235,7 @@ function airutils.autopilot(self, dtime, hull_direction, longit_speed, accel, cu local retval_accel = accel - local max_autopilot_power = 85 + --[[local max_autopilot_power = 85 --climb local velocity = self.object:get_velocity() @@ -248,7 +248,7 @@ function airutils.autopilot(self, dtime, hull_direction, longit_speed, accel, cu self._acceleration = 0 if self._engine_running then --engine acceleration calc - local engineacc = (self._power_lever * airutils.max_engine_acc) / 100; + local engineacc = (self._power_lever * self._max_engine_acc) / 100; --self.engine:set_animation_frame_speed(60 + self._power_lever) local factor = math.abs(climb_rate * 0.1) --getAdjustFactor(curr_pos.y, self._auto_pilot_altitude) @@ -261,7 +261,7 @@ function airutils.autopilot(self, dtime, hull_direction, longit_speed, accel, cu airutils.powerAdjust(self, dtime, factor, 1, max_autopilot_power) end --do not exceed - local max_speed = airutils.max_speed + local max_speed = self._max_speed if longit_speed > max_speed then engineacc = engineacc - (longit_speed-max_speed) if engineacc < 0 then engineacc = 0 end @@ -287,7 +287,7 @@ function airutils.autopilot(self, dtime, hull_direction, longit_speed, accel, cu if airutils.elevator_auto_correction then self._elevator_angle = airutils.elevator_auto_correction(self, longit_speed, self.dtime, airutils.max_speed, self._elevator_angle, airutils.elevator_limit, airutils.ideal_step, self._elevator_auto_estabilize) end - end + end]]-- return retval_accel end diff --git a/lib_planes/entities.lua b/lib_planes/entities.lua index d8f4fe6..1bc6500 100644 --- a/lib_planes/entities.lua +++ b/lib_planes/entities.lua @@ -108,16 +108,28 @@ end local function ground_pitch(self, longit_speed, curr_pitch) newpitch = curr_pitch + if self._last_longit_speed == nil then self._last_longit_speed = 0 end + + -- Estado atual do sistema + if self._current_value == nil then self._current_value = 0 end -- Valor atual do sistema + if self._last_error == nil then self._last_error = 0 end -- Último erro registrado + -- adjust pitch at ground if math.abs(longit_speed) < self._tail_lift_max_speed then - --minetest.chat_send_all(math.abs(longit_speed)) local speed_range = self._tail_lift_max_speed - self._tail_lift_min_speed local percentage = 1-((math.abs(longit_speed) - self._tail_lift_min_speed)/speed_range) if percentage > 1 then percentage = 1 end if percentage < 0 then percentage = 0 end local angle = self._tail_angle * percentage - local calculated_newpitch = math.rad(angle) - if newpitch < calculated_newpitch then newpitch = calculated_newpitch end --ja aproveita o pitch atual se ja estiver cerrto + local rad_angle = math.rad(angle) + + if newpitch < rad_angle then newpitch = rad_angle end --ja aproveita o pitch atual se ja estiver cerrto + --[[self._current_value = curr_pitch + local kp = (longit_speed - self._tail_lift_min_speed)/10 + local output, last_error = airutils.pid_controller(self._current_value, rad_angle, self._last_error, self.dtime, kp) + self._last_error = last_error + newpitch = output]]-- + if newpitch > math.rad(self._tail_angle) then newpitch = math.rad(self._tail_angle) end --não queremos arrastar o cauda no chão end @@ -489,6 +501,7 @@ function airutils.logic(self) --saves last velocity for collision detection (abrupt stop) self._last_vel = self.object:get_velocity() + self._last_longit_speed = longit_speed end local function damage_vehicle(self, toolcaps, ttime, damage) diff --git a/lib_planes/utilities.lua b/lib_planes/utilities.lua index a9034fd..a314564 100644 --- a/lib_planes/utilities.lua +++ b/lib_planes/utilities.lua @@ -551,6 +551,25 @@ function airutils.paint_with_mask(self, colstr, target_texture, mask_texture) end end +function airutils.pid_controller(current_value, setpoint, last_error, d_time, kp, ki, kd) + kp = kp or 0 + ki = ki or 0.00000000000001 + kd = kd or 0.005 + + local ti = kp/ki + local td = kd/kp + local delta_t = d_time + + 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 + function airutils.add_destruction_effects(pos, radius) local node = airutils.nodeatpos(pos) local is_liquid = false