mirror of
https://github.com/bjoernellens1/cps_microros_imu.git
synced 2024-11-27 18:25:46 +00:00
55 lines
1.4 KiB
C++
55 lines
1.4 KiB
C++
// Copyright (c) 2021 Juan Miguel Jimeno
|
|
//
|
|
// 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 "Arduino.h"
|
|
#include "pid.h"
|
|
|
|
PID::PID(float min_val, float max_val, float kp, float ki, float kd):
|
|
min_val_(min_val),
|
|
max_val_(max_val),
|
|
kp_(kp),
|
|
ki_(ki),
|
|
kd_(kd)
|
|
{
|
|
}
|
|
|
|
double PID::compute(float setpoint, float measured_value)
|
|
{
|
|
double error;
|
|
double pid;
|
|
|
|
//setpoint is constrained between min and max to prevent pid from having too much error
|
|
error = setpoint - measured_value;
|
|
integral_ += error;
|
|
derivative_ = error - prev_error_;
|
|
|
|
if(setpoint == 0 && error == 0)
|
|
{
|
|
integral_ = 0;
|
|
derivative_ = 0;
|
|
}
|
|
|
|
pid = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative_);
|
|
prev_error_ = error;
|
|
|
|
return constrain(pid, min_val_, max_val_);
|
|
}
|
|
|
|
void PID::updateConstants(float kp, float ki, float kd)
|
|
{
|
|
kp_ = kp;
|
|
ki_ = ki;
|
|
kd_ = kd;
|
|
}
|