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

Add motor speed control

Browse source
This commit is contained in:
root 2022-03-21 22:37:13 +00:00
parent 1a488065ac
commit 5d8df998ae
5 changed files with 138 additions and 44 deletions
Download patch file Download diff file Expand all files Collapse all files

21
server/include/motor.hpp
View file

@ -4,6 +4,7 @@
#include <cstdint>
#include <vector>
#include <chrono>
#include <thread>
#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<double> 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

5
server/include/robot.hpp
View file

@ -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

2
server/include/spi.hpp
View file

@ -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 {

53
server/src/main.cpp
View file

@ -105,43 +105,46 @@ int main() {
// }
// double end_time = std::chrono::duration_cast<std::chrono::milliseconds>(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<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
long double delta_seconds_total = std::chrono::duration_cast<std::chrono::microseconds>(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<std::chrono::microseconds>(current_time - last_time).count() / 1000000.0;
// long double delta_seconds_total = std::chrono::duration_cast<std::chrono::microseconds>(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;
}

99
server/src/motor.cpp
View file

@ -1,5 +1,6 @@
#include <stdexcept>
#include <unistd.h>
#include <chrono>
#include "include/motor.hpp"
#include "include/spi.hpp"
@ -7,18 +8,34 @@
#include "include/encoder.hpp"
#include <spdlog/spdlog.h>
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<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_read).count() / 1000000.0;
long double delta_seconds = std::chrono::duration_cast<std::chrono::microseconds>(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,7 +96,7 @@ std::vector<double> 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++) {
@ -90,3 +104,60 @@ void Motor::reset_speed() {
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<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);
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;
}
}
}
}