compAIR/compLIB
compAIR/compLIB
Archived
4
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Rework PID

Browse source
This commit is contained in:
root 2022-04-02 22:49:04 +01:00
parent 5d8df998ae
commit 1fc400c695
9 changed files with 186 additions and 56 deletions
Download patch file Download diff file Expand all files Collapse all files

2
server/CMakeLists.txt
View file

@ -27,6 +27,7 @@ set(SRC_FILES
src/spi.cpp
src/encoder.cpp
src/motor.cpp
src/pid.cpp
)
set(HDR_FILES
@ -38,6 +39,7 @@ set(HDR_FILES
include/encoder.hpp
include/robot.hpp
include/motor.hpp
include/pid.hpp
)
include_directories(third_party/asio)

2
server/include/motor.hpp
View file

@ -54,6 +54,8 @@ private:
void reset_speed();
void speed_control_loop();
void _set_power(uint8_t port, double percent);
};
#endif // COMPLIB_SERVER_MOTOR_HPP

30
server/include/pid.hpp Normal file
View file

@ -0,0 +1,30 @@
#ifndef COMPLIB_SERVER_PID_HPP
#define COMPLIB_SERVER_PID_HPP
#include <chrono>
#define PID_RESET_DELAY_S 0.1L
class PID
{
public:
PID(float P, float I, float D, float ramp, float limit);
~PID() = default;
float operator() (float setpoint, float process_variable);
void reset();
float P = 1;
float I = 0;
float D = 0;
float setpoint_ramp = 0;
float limit = 0;
protected:
float error_prev = 0;
float setpoint_prev = 0;
float integral_prev = 0;
std::chrono::system_clock::time_point timestamp_prev = std::chrono::high_resolution_clock::now();
};
#endif // COMPLIB_SERVER_PID_HPP

16
server/include/spi.hpp
View file

@ -1,6 +1,8 @@
#ifndef COMPLIB_SERVER_SPI_HPP
#define COMPLIB_SERVER_SPI_HPP
#include <mutex>
//SPI_MODE_0 (0,0) CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge, output data (change) on falling edge
//SPI_MODE_1 (0,1) CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge, output data (change) on rising edge
//SPI_MODE_2 (1,0) CPOL = 1, CPHA = 0, Clock idle high, data is clocked in on falling edge, output data (change) on rising edge
@ -106,7 +108,17 @@ public:
DISPLAY_LINE_1_C0 = 69,
DISPLAY_LINE_2_C0 = 85,
DISPLAY_LINE_3_C0 = 101,
DISPLAY_LINE_4_C0 = 117
DISPLAY_LINE_4_C0 = 117,
// Motor encoder velocities
MOTOR_1_VEL_H = 118,
MOTOR_1_VEL_L = 119,
MOTOR_2_VEL_H = 120,
MOTOR_2_VEL_L = 121,
MOTOR_3_VEL_H = 122,
MOTOR_3_VEL_L = 123,
MOTOR_4_VEL_H = 124,
MOTOR_4_VEL_L = 125
};
private:
@ -116,6 +128,8 @@ private:
uint8_t tx_buffer[SPI_BUFFER_SIZE] = {0};
uint8_t rx_buffer[SPI_BUFFER_SIZE] = {0};
std::recursive_mutex spi_mutex;
void transfer();
void clear_buffers();

2
server/src/encoder.cpp
View file

@ -9,7 +9,7 @@ std::vector<int32_t> Encoder::read_all() {
Spi::getInstance().read_array(Spi::Register::MOTOR_1_POS_B3, ENCODER_COUNT * 4, result_bytes);
for (int i = 0; i < ENCODER_COUNT; i++) {
result.push_back(mathUtils::int_from_bytes(result_bytes + i * ENCODER_COUNT, 4));
result.push_back(mathUtils::int_from_bytes(result_bytes + i * 4, 4));
}
return result;
}

18
server/src/main.cpp
View file

@ -3,6 +3,7 @@
#include <sys/un.h>
#include <sys/socket.h>
#include <zconf.h>
#include "include/mathUtils.hpp"
#include "include/messageBuilder.hpp"
#include "include/errorMessages.hpp"
#include <spdlog/spdlog.h>
@ -108,7 +109,7 @@ int main() {
// double kp = 0.5;
// double ki = 5.0;
// double kd = 0.025;
double setpoint = 50;
double setpoint = -50;
// double errror_sum = 0;
// double last_error = 0;
// int port = 0;
@ -145,6 +146,21 @@ int main() {
// }
auto speeds = Motor::get_instance().get_speed();
spdlog::info("Speed: {} {} Target: {}", speeds.at(0), speeds.at(3), setpoint);
// auto current_time = std::chrono::high_resolution_clock::now();
// long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
// last_time = current_time;
// uint8_t result_bytes[2] = {0};
// Spi::getInstance().read_array(Spi::Register::MOTOR_1_VEL_H, 2, result_bytes);
// int16_t tps = mathUtils::int_from_bytes(result_bytes, 2);
// std::vector<int32_t> current_ticks = Encoder::read_all();
// long double delta_ticks = current_ticks.at(0) - last_ticks;
// spdlog::info("{} {}", tps, delta_ticks / delta_seconds);
// last_ticks = current_ticks.at(0);
}
return 0;
}

98
server/src/motor.cpp
View file

@ -1,12 +1,14 @@
#include <stdexcept>
#include <unistd.h>
#include <chrono>
#include <spdlog/spdlog.h>
#include "include/motor.hpp"
#include "include/spi.hpp"
#include "include/robot.hpp"
#include "include/encoder.hpp"
#include <spdlog/spdlog.h>
#include "include/mathUtils.hpp"
#include "include/pid.hpp"
void check_port(uint8_t port) {
if (port >= MOTOR_COUNT) {
@ -29,13 +31,16 @@ void Motor::set_speed(uint8_t port, double rpm) {
}
void Motor::set_power(uint8_t port, double percent) {
motor_control_modes[port] = Motor::Control::POWER;
_set_power(port, percent);
}
void Motor::_set_power(uint8_t port, double percent) {
check_port(port);
if (abs(percent) > 100) {
throw std::invalid_argument("Invalid motor percent specified. Should be -100 <= percent <= 100");
}
motor_control_modes[port] = Motor::Control::POWER;
Mode mode = Motor::Mode::COAST;
if (percent < 0) {
mode = Motor::Mode::BACKWARD;
@ -69,27 +74,36 @@ void Motor::set_pwm(uint8_t port, uint16_t pwm, Mode mode) {
}
std::vector<double> Motor::get_speed() {
auto current_time = std::chrono::high_resolution_clock::now();
long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time_encoders_read).count() / 1000000.0;
// auto current_time = std::chrono::high_resolution_clock::now();
// long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_time_encoders_read).count() / 1000000.0;
last_time_encoders_read = current_time;
// last_time_encoders_read = current_time;
if (delta_seconds > MOTOR_MAX_DELTA_TIME_S) {
reset_speed();
usleep(MOTOR_FILTER_RESET_DELAY_US);
return Motor::get_speed();
}
// if (delta_seconds > MOTOR_MAX_DELTA_TIME_S) {
// reset_speed();
// usleep(MOTOR_FILTER_RESET_DELAY_US);
// return Motor::get_speed();
// }
// std::vector<int32_t> current_ticks = Encoder::read_all();
// std::vector<double> velocities_rpm;
// for (int i = 0; i < MOTOR_COUNT; i++) {
// long double delta_ticks = current_ticks.at(i) - last_encoder_values[i];
// long double velocity_rpm = (delta_ticks / delta_seconds) * 60.0L / TICKS_PER_TURN;
// filtered_speeds[i] = (long double) ((MOTOR_FILTER_ALPHA * velocity_rpm) + (1.0 - MOTOR_FILTER_ALPHA) * filtered_speeds[i]);
// velocities_rpm.push_back(filtered_speeds[i]);
// last_encoder_values[i] = current_ticks.at(i);
// }
std::vector<int32_t> current_ticks = Encoder::read_all();
std::vector<double> velocities_rpm;
uint8_t result_bytes[MOTOR_COUNT * 2] = {0};
Spi::getInstance().read_array(Spi::Register::MOTOR_1_VEL_H, MOTOR_COUNT * 2, result_bytes);
for (int i = 0; i < MOTOR_COUNT; i++) {
long double delta_ticks = current_ticks.at(i) - last_encoder_values[i];
long double velocity_rpm = (delta_ticks / delta_seconds) * 60.0L / TICKS_PER_TURN;
filtered_speeds[i] = (long double) ((MOTOR_FILTER_ALPHA * velocity_rpm) + (1.0 - MOTOR_FILTER_ALPHA) * filtered_speeds[i]);
velocities_rpm.push_back(filtered_speeds[i]);
last_encoder_values[i] = current_ticks.at(i);
int16_t ticks_per_second = mathUtils::int_from_bytes(result_bytes + i * 2, 2);
velocities_rpm.push_back(ticks_per_second * 60.0L / TICKS_PER_TURN);
}
return velocities_rpm;
@ -106,12 +120,17 @@ void Motor::reset_speed() {
}
void Motor::speed_control_loop() {
double errror_sums[MOTOR_COUNT] = {0};
double last_errors[MOTOR_COUNT] = {0};
std::vector<PID> pids;
for (int i = 0; i < MOTOR_COUNT; i++) {
pids.push_back(
PID{MOTOR_SPEED_CONTROL_KP, MOTOR_SPEED_CONTROL_KI, MOTOR_SPEED_CONTROL_KD, 100.0, 100.0}
);
}
auto last_run = std::chrono::high_resolution_clock::now();
while (1 == 1) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000 / MOTOR_SPEED_CONTROL_LOOP_HZ));
float sleep_time = std::round(1000.0 / MOTOR_SPEED_CONTROL_LOOP_HZ);
std::this_thread::sleep_for(std::chrono::microseconds((int) sleep_time * 1000));
bool should_control_speed = false;
for (int i = 0; i < MOTOR_COUNT; i++) {
@ -121,43 +140,16 @@ void Motor::speed_control_loop() {
}
if (!should_control_speed) {
continue;
}
auto current_time = std::chrono::high_resolution_clock::now();
long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(current_time - last_run).count() / 1000000.0;
last_run = current_time;
if (delta_seconds > MOTOR_SPEED_CONTROL_RESET_DELAY_S) {
std::fill_n(errror_sums, MOTOR_COUNT, 0);
std::fill_n(last_errors, MOTOR_COUNT, 0);
spdlog::info("continue");
continue;
}
auto speeds = Motor::get_instance().get_speed();
for (int i = 0; i < MOTOR_COUNT; i++) {
if (motor_control_modes[i] == Motor::Control::SPEED) {
// P
double error = speed_targets[i] - speeds.at(i);
// I
errror_sums[i] += error * delta_seconds;
// D
if (last_errors[i] == 0) { // Needed to prevent spike on first loop iteration
last_errors[i] = error;
}
double error_diff = (error - last_errors[i]) / delta_seconds;
double power = MOTOR_SPEED_CONTROL_KP * error +
errror_sums[i] * MOTOR_SPEED_CONTROL_KI +
error_diff * MOTOR_SPEED_CONTROL_KD;
set_power(i, std::min(std::max(-100.0, power), 100.0));
// spdlog::info("{:03.2f} {:03.2f} {:03.2f} {:03.2f} {:03.2f} {:03.2f} {:03.2f}",
// speed_targets[i], speeds.at(i), error, delta_seconds, last_errors[i], error_diff, power);
last_errors[i] = error;
float power = pids.at(i)(speed_targets[i], speeds.at(i));
_set_power(i, power);
}
}
}
}

64
server/src/pid.cpp Normal file
View file

@ -0,0 +1,64 @@
#include "include/pid.hpp"
#include <chrono>
#include <algorithm>
#include <spdlog/spdlog.h>
PID::PID(float P, float I, float D, float ramp, float limit)
: P(P)
, I(I)
, D(D)
, setpoint_ramp(ramp)
, limit(limit)
, error_prev(0.0f)
, setpoint_prev(0.0f)
, integral_prev(0.0f)
{
timestamp_prev = std::chrono::high_resolution_clock::now();
}
float PID::operator() (float setpoint, float process_variable){
// calculate the time from the last call
auto timestamp_now = std::chrono::high_resolution_clock::now();
long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(timestamp_now - timestamp_prev).count() / 1000000.0;
if (delta_seconds > PID_RESET_DELAY_S) {
reset();
delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(timestamp_now - timestamp_prev).count() / 1000000.0;
}
if(setpoint_ramp > 0){
float setpoint_rate = (setpoint - setpoint_prev) / delta_seconds;
if (setpoint_rate > setpoint_ramp) {
setpoint = setpoint_prev + setpoint_ramp * delta_seconds;
} else if (setpoint_rate < -setpoint_ramp) {
setpoint = setpoint_prev - setpoint_ramp * delta_seconds;
}
}
float error = setpoint - process_variable;
float proportional = P * error;
// Tustin transform of the integral part
float integral = integral_prev + I * delta_seconds * 0.5f * (error + error_prev);
float derivative = D * ((error - error_prev) / delta_seconds);
float output = proportional + integral + derivative;
// antiwindup - limit the output variable
output = std::min(std::max(-limit, output), limit);
// spdlog::info("E{} P{} I{} D{} O{} EP{} DS{}", error, proportional, integral, derivative, output, error_prev, delta_seconds);
integral_prev = integral;
setpoint_prev = setpoint;
error_prev = error;
timestamp_prev = timestamp_now;
return output;
}
void PID::reset() {
spdlog::info("RESET");
integral_prev = 0;
setpoint_prev = 0;
error_prev = 0;
timestamp_prev = std::chrono::high_resolution_clock::now();
}

10
server/src/spi.cpp
View file

@ -44,6 +44,8 @@ uint8_t Spi::calculate_checksum(uint8_t* data, uint8_t length) {
}
int Spi::read(uint8_t reg, uint8_t length) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
uint8_t read_buffer[SPI_BUFFER_SIZE] = {0};
read_array(reg, length, read_buffer);
@ -51,6 +53,8 @@ int Spi::read(uint8_t reg, uint8_t length) {
}
void Spi::read_array(uint8_t reg, uint8_t length, uint8_t* data) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
clear_buffers();
tx_buffer[0] = 0;
tx_buffer[1] = reg;
@ -67,12 +71,16 @@ void Spi::read_array(uint8_t reg, uint8_t length, uint8_t* data) {
}
void Spi::write(uint8_t reg, uint8_t length, int value) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
uint8_t write_buffer[SPI_BUFFER_SIZE] = {0};
mathUtils::bytes_from_int(value, length, write_buffer);
write_array(reg, length, write_buffer);
}
void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t* data) {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
clear_buffers();
tx_buffer[0] = 1;
tx_buffer[1] = reg;
@ -83,6 +91,8 @@ void Spi::write_array(uint8_t reg, uint8_t length, const uint8_t* data) {
}
void Spi::transfer() {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
struct spi_ioc_transfer spi;
memset(&spi, 0, sizeof(spi));