amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

more PID

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-09 21:16:25 -07:00
parent 3f8ab289a9
commit 25cc0d0bc3
1 changed files with 23 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
main/ugv_main.cc
View File

@ -29,12 +29,13 @@ static const float RAD_PER_DEG = PI / 180.f;
// Radius of earth in meters // Radius of earth in meters
static const float EARTH_RAD = 6372795.f; static const float EARTH_RAD = 6372795.f;
static const float DRIVE_POWER = 0.5; static const float DRIVE_POWER = 0.5;
static const float ANGLE_P = 0.05; static const float ANGLE_P = 0.02;
static const float ANGLE_I = 0.00; static const float ANGLE_I = 0.000005;
static const float ANGLE_D = -0.05; static const float ANGLE_D = -0.01;
static const float MAX_ANGLE_POWER = 0.5; static const float MAX_ANGLE_POWER = 0.3;
static const float MIN_DIST = 10.0; static const float MAX_ANGLE_I_ERROR = 15.0;
static const float MIN_DIST = 10.0;
extern "C" void OnTimeout(void *arg); extern "C" void OnTimeout(void *arg);
@ -97,6 +98,9 @@ class PIDController {
void MaxOutput(float max_output) { max_output_ = max_output; } void MaxOutput(float max_output) { max_output_ = max_output; }
float MaxOutput() const { return max_output_; } float MaxOutput() const { return max_output_; }
void MaxIError(float max_i_error) { max_i_error_ = max_i_error; }
float MaxIError() const { return max_i_error_; }
void Setpoint(float setpoint) { setpoint_ = setpoint; } void Setpoint(float setpoint) { setpoint_ = setpoint; }
float Setpoint() const { return setpoint_; } float Setpoint() const { return setpoint_; }
@ -129,6 +133,7 @@ class PIDController {
float ki_; float ki_;
float kd_; float kd_;
float max_output_; float max_output_;
float max_i_error_;
bool enabled_; bool enabled_;
float setpoint_; float setpoint_;
@ -154,6 +159,7 @@ PIDController::PIDController(float dt, float kp, float ki, float kd)
ki_(ki), ki_(ki),
kd_(kd), kd_(kd),
max_output_(INFINITY), max_output_(INFINITY),
max_i_error_(INFINITY),
enabled_(false), enabled_(false),
setpoint_(0), setpoint_(0),
input_(0), input_(0),
@ -177,7 +183,11 @@ float PIDController::Update() {
} }
float error = Error(); float error = Error();
integral_ += error * dt_; if (fabsf(error) > max_i_error_) {
integral_ = 0.;
} else {
integral_ += error * dt_;
}
output_ += kp_ * error; output_ += kp_ * error;
output_ += ki_ * integral_; output_ += ki_ * integral_;
@ -216,8 +226,10 @@ struct State {
State() State()
: target{34.069022, -118.443067}, : target{34.069022, -118.443067},
angle_controller_(LOOP_PERIOD_S, ANGLE_P) { angle_controller_(LOOP_PERIOD_S, ANGLE_P, ANGLE_I, ANGLE_D) {
angle_controller_.MaxOutput(MAX_ANGLE_POWER); angle_controller_.MaxOutput(MAX_ANGLE_POWER);
angle_controller_.MaxIError(MAX_ANGLE_I_ERROR);
comms = new CommsClass(); comms = new CommsClass();
io = new IOClass(); io = new IOClass();
display = new DisplayClass(comms); display = new DisplayClass(comms);
@ -262,6 +274,7 @@ struct State {
inputs.mpu.mag.z); inputs.mpu.mag.z);
ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(), ESP_LOGD(TAG, "ahrs: yaw=%f, pitch=%f, roll=%f", ahrs_.getYaw(),
ahrs_.getPitch(), ahrs_.getRoll()); ahrs_.getPitch(), ahrs_.getRoll());
ESP_LOGD(TAG, "PID: error: %f", angle_controller_.Error());
last_print = time_us; last_print = time_us;
} }
@ -352,8 +365,8 @@ struct State {
if (angle_controller_.Enabled()) { if (angle_controller_.Enabled()) {
float angle_pwr = angle_controller_.Update(); float angle_pwr = angle_controller_.Update();
outputs.left_motor = drive_power + angle_pwr; outputs.left_motor = drive_power - angle_pwr;
outputs.right_motor = drive_power - angle_pwr; outputs.right_motor = drive_power + angle_pwr;
} }
io->WriteOutputs(outputs); io->WriteOutputs(outputs);