From 5d8df998aeb24155a7d1b77722b9fe8a98489cfd Mon Sep 17 00:00:00 2001 From: root Date: Mon, 21 Mar 2022 22:37:13 +0000 Subject: [PATCH] Add motor speed control --- server/include/motor.hpp | 23 ++++++++-- server/include/robot.hpp | 5 ++ server/include/spi.hpp | 2 +- server/src/main.cpp | 53 +++++++++++---------- server/src/motor.cpp | 99 ++++++++++++++++++++++++++++++++++------ 5 files changed, 138 insertions(+), 44 deletions(-) diff --git a/server/include/motor.hpp b/server/include/motor.hpp index 7d16352..c728fb1 100644 --- a/server/include/motor.hpp +++ b/server/include/motor.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include "include/robot.hpp" @@ -18,6 +19,11 @@ public: NONE = 5 }; + enum Control : uint8_t { + POWER = 0, + SPEED = 1 + }; + static Motor& get_instance() { static Motor instance; @@ -27,18 +33,27 @@ public: Motor(Motor const&) = delete; void operator=(Motor const&) = delete; - static void power(uint8_t port, double percent); - static void pwm(uint8_t port, uint16_t pwm, Mode mode); + void set_speed(uint8_t port, double rpm); + void set_power(uint8_t port, double percent); + static void set_pwm(uint8_t port, uint16_t pwm, Mode mode); std::vector get_speed(); + private: Motor(); + // Speed calculation and speed filter int32_t last_encoder_values[MOTOR_COUNT] = {0}; double filtered_speeds[MOTOR_COUNT] = {0}; - std::chrono::system_clock::time_point last_time_read; - + std::chrono::system_clock::time_point last_time_encoders_read; + + // Speed control + double speed_targets[MOTOR_COUNT] = {0}; + double motor_control_modes[MOTOR_COUNT] = {0}; + std::thread speed_control_thread; + void reset_speed(); + void speed_control_loop(); }; #endif // COMPLIB_SERVER_MOTOR_HPP \ No newline at end of file diff --git a/server/include/robot.hpp b/server/include/robot.hpp index ae8c5b9..9fea841 100644 --- a/server/include/robot.hpp +++ b/server/include/robot.hpp @@ -15,6 +15,11 @@ #define MOTOR_MAX_DELTA_TIME_S 0.1L #define MOTOR_FILTER_ALPHA 0.05L #define MOTOR_FILTER_RESET_DELAY_US (1000 * 10) +#define MOTOR_SPEED_CONTROL_RESET_DELAY_S 0.01L +#define MOTOR_SPEED_CONTROL_LOOP_HZ 150 +#define MOTOR_SPEED_CONTROL_KP 0.5L +#define MOTOR_SPEED_CONTROL_KI 5.0L +#define MOTOR_SPEED_CONTROL_KD 0.025L #define LEFT_PORT 3 #define RIGHT_PORT 0 diff --git a/server/include/spi.hpp b/server/include/spi.hpp index d7526d6..b5883b7 100644 --- a/server/include/spi.hpp +++ b/server/include/spi.hpp @@ -7,7 +7,7 @@ //SPI_MODE_3 (1,1) CPOL = 1, CPHA = 1, Clock idle high, data is clocked in on rising, edge output data (change) on falling edge #define SPI_BUFFER_SIZE 20 -#define SPI_SPEED 4000000 // 4 MHz +#define SPI_SPEED 2000000 // 2 MHz #define SPI_BITS_PER_WORD 8 class Spi { diff --git a/server/src/main.cpp b/server/src/main.cpp index 6930681..f624249 100644 --- a/server/src/main.cpp +++ b/server/src/main.cpp @@ -105,43 +105,46 @@ int main() { // } // double end_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - double kp = 0.5; - double ki = 5.0; - double kd = 0.025; - double setpoint = 0; - double errror_sum = 0; - double last_error = 0; + // double kp = 0.5; + // double ki = 5.0; + // double kd = 0.025; + double setpoint = 50; + // double errror_sum = 0; + // double last_error = 0; // int port = 0; // long double accumulator = 0; // long double alpha = 0.2; - auto last_time = std::chrono::high_resolution_clock::now(); - auto start_time = last_time; - int i = 0; + // auto last_time = std::chrono::high_resolution_clock::now(); + // auto start_time = last_time; + // int i = 0; while (1 == 1) { - setpoint += 0.05; - auto current_time = std::chrono::high_resolution_clock::now(); - long double delta_seconds = std::chrono::duration_cast(current_time - last_time).count() / 1000000.0; - long double delta_seconds_total = std::chrono::duration_cast(current_time - start_time).count() / 1000000.0; + setpoint += 0.5; + // auto current_time = std::chrono::high_resolution_clock::now(); + // long double delta_seconds = std::chrono::duration_cast(current_time - last_time).count() / 1000000.0; + // long double delta_seconds_total = std::chrono::duration_cast(current_time - start_time).count() / 1000000.0; - last_time = current_time; + // last_time = current_time; - auto speeds = Motor::get_instance().get_speed(); - double e = setpoint - speeds.at(0); - errror_sum += e * delta_seconds; - double error_diff = (e - last_error) / delta_seconds; - last_error = e; - double u = kp * e + errror_sum * ki + error_diff * kd; + // auto speeds = Motor::get_instance().get_speed(); + // double e = setpoint - speeds.at(0); + // errror_sum += e * delta_seconds; + // double error_diff = (e - last_error) / delta_seconds; + // last_error = e; + // double u = kp * e + errror_sum * ki + error_diff * kd; - Motor::power(0, std::min(std::max(-100.0, u), 100.0)); + // Motor::power(0, std::min(std::max(-100.0, u), 100.0)); - spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0)); + // spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0)); - usleep(1000.0 * (1000.0 / 150.0)); - - i += 1; + usleep(1000.0 * (1000.0 / 10.0)); + Motor::get_instance().set_speed(0, setpoint); + Motor::get_instance().set_speed(3, -setpoint); + // i += 1; // if (i % 1000 == 0) { // setpoint *= -1; // } + auto speeds = Motor::get_instance().get_speed(); + spdlog::info("Speed: {} {} Target: {}", speeds.at(0), speeds.at(3), setpoint); } return 0; } diff --git a/server/src/motor.cpp b/server/src/motor.cpp index 5ea9ec6..3e9319f 100644 --- a/server/src/motor.cpp +++ b/server/src/motor.cpp @@ -1,5 +1,6 @@ #include #include +#include #include "include/motor.hpp" #include "include/spi.hpp" @@ -7,18 +8,34 @@ #include "include/encoder.hpp" #include -Motor::Motor() { - reset_speed(); -} - -void Motor::power(uint8_t port, double percent) { +void check_port(uint8_t port) { if (port >= MOTOR_COUNT) { throw std::invalid_argument("Invalid motor port specified"); } +} + +Motor::Motor() { + reset_speed(); + + speed_control_thread = std::thread(&Motor::speed_control_loop, this); + speed_control_thread.detach(); +} + +void Motor::set_speed(uint8_t port, double rpm) { + check_port(port); + + speed_targets[port] = rpm; + motor_control_modes[port] = Motor::Control::SPEED; +} + +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; @@ -27,14 +44,11 @@ void Motor::power(uint8_t port, double percent) { } uint16_t pwm = abs(percent) * (MAX_MOTOR_SPEED / 100.0); - Motor::pwm(port, pwm, mode); + Motor::set_pwm(port, pwm, mode); } -void Motor::pwm(uint8_t port, uint16_t pwm, Mode mode) { - if (port >= MOTOR_COUNT) { - throw std::invalid_argument("Invalid motor port specified"); - } - +void Motor::set_pwm(uint8_t port, uint16_t pwm, Mode mode) { + check_port(port); if (mode > Motor::Mode::BREAK) { throw std::invalid_argument("Invalid motor mode specified"); } @@ -56,9 +70,9 @@ void Motor::pwm(uint8_t port, uint16_t pwm, Mode mode) { std::vector Motor::get_speed() { auto current_time = std::chrono::high_resolution_clock::now(); - long double delta_seconds = std::chrono::duration_cast(current_time - last_time_read).count() / 1000000.0; + long double delta_seconds = std::chrono::duration_cast(current_time - last_time_encoders_read).count() / 1000000.0; - last_time_read = current_time; + last_time_encoders_read = current_time; if (delta_seconds > MOTOR_MAX_DELTA_TIME_S) { reset_speed(); @@ -82,11 +96,68 @@ std::vector Motor::get_speed() { } void Motor::reset_speed() { - last_time_read = std::chrono::high_resolution_clock::now(); + last_time_encoders_read = std::chrono::high_resolution_clock::now(); auto current_ticks = Encoder::read_all(); for (int i = 0; i < MOTOR_COUNT; i++) { filtered_speeds[i] = 0; last_encoder_values[i] = current_ticks.at(i); } +} + +void Motor::speed_control_loop() { + double errror_sums[MOTOR_COUNT] = {0}; + double last_errors[MOTOR_COUNT] = {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)); + + bool should_control_speed = false; + for (int i = 0; i < MOTOR_COUNT; i++) { + if (motor_control_modes[i] == Motor::Control::SPEED) { + should_control_speed = true; + } + } + + if (!should_control_speed) { + continue; + } + + auto current_time = std::chrono::high_resolution_clock::now(); + long double delta_seconds = std::chrono::duration_cast(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); + 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; + } + } + + } } \ No newline at end of file