pid_controller.cpp

Go to the documentation of this file.

/*------------------------------------------------------------------------------
* Copyright (C) 2024 LEIDOS.
*
* Licensed under the Apache License, Version 2.0 (the "License"); you may not
* use this file except in compliance with the License. You may obtain a copy of
* the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
* License for the specific language governing permissions and limitations under
* the License.
 
------------------------------------------------------------------------------*/
 
#include "platooning_control/pid_controller.hpp"
 
namespace platooning_control
{
    PIDController::PIDController(){}
 
    double PIDController::calculate( double setpoint, double pv ){
        // Calculate error
        double error = setpoint - pv;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"),"PID error: " << error);
 
        // Proportional term
        double Pout = config_->kp * error;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Proportional term: " << Pout);
 
        // Integral term
        _integral += error * config_->dt;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"),"Integral term: " << _integral);
 
        if (_integral > config_->integrator_max){
             _integral = config_->integrator_max;
        }
        else if (_integral < config_->integrator_min){
            _integral = config_->integrator_min;
        }
        double Iout = config_->ki * _integral;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Iout: " << Iout);
 
        // Derivative term
        double derivative = (error - _pre_error) / config_->dt;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "derivative term: " << derivative);
        double Dout = config_->kd * derivative;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "Dout: " << Dout);
 
        // Calculate total output
        double output = Pout + Iout + Dout;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("platooning_control"), "total controller output: " << output);
 
        // Restrict to max/min
        if( output > config_->max_delta_speed_per_timestep )
            output = config_->max_delta_speed_per_timestep;
        else if( output < config_->min_delta_speed_per_timestep )
            output = config_->min_delta_speed_per_timestep;
        // Save error to previous error
        _pre_error = error;
 
        return output;
 
    }
 
    void PIDController::reset() {
        _integral = 0.0;
        _pre_error = 0.0;
    }
 
 
}