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

Add get speed function to motor class

Browse source
This commit is contained in:
root 2022-03-21 18:51:16 +00:00
parent 8537e8504b
commit 1a488065ac
2 changed files with 31 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

60
server/src/main.cpp
View file

@ -7,6 +7,7 @@
#include "include/errorMessages.hpp"
#include <spdlog/spdlog.h>
#include <chrono>
#include <algorithm>
#include "include/spi.hpp"
#include "include/reset.hpp"
@ -88,12 +89,12 @@ google::protobuf::Message *processMessage(const std::string &serializedMessage)
close(clientFileDescriptor);
}
}
int main() {
// socketServer();
Reset::reset_robot();
spdlog::set_pattern("%H:%M:%S.%e %^%-8l%$: %v");
// Spi &spi = Spi::getInstance();
// int version = spi.read(1, 1);
@ -104,43 +105,44 @@ int main() {
// }
// double end_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
double power = 0;
double kp = 0.5;
double ki = 5.0;
double kd = 0.025;
double setpoint = 0;
double errror_sum = 0;
double last_error = 0;
// int port = 0;
// long double accumulator = 0;
// long double alpha = 0.2;
while (power < 100) {
power += 0.1;
Motor::power(0, power);
Motor::power(3, power);
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;
last_time = current_time;
auto speeds = Motor::get_instance().get_speed();
// spdlog::info("{:05.0f} {:05.0f}", speeds.at(0), speeds.at(3));
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));
spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0));
// auto start_values = Encoder::read_all();
// spdlog::info("{}", start_values.at(3));
// auto start_time = std::chrono::high_resolution_clock::now();
usleep(1000.0 * (1000.0 / 150.0));
// auto end_values = Encoder::read_all();
// auto elapsed_time = std::chrono::high_resolution_clock::now() - start_time;
// long double elapsed_ticks = end_values.at(port) - start_values.at(port);
// long double elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(
// elapsed_time).count();
// long double elapsed_s = std::chrono::duration_cast<std::chrono::microseconds>(
// elapsed_time).count() / 1000000.0;
// long double vel_ticks_s = elapsed_ticks / elapsed_s;
// long double vel_rpm = vel_ticks_s * 60.0L / TICKS_PER_TURN;
// accumulator = (alpha * vel_rpm) + (1.0 - alpha) * accumulator;
// spdlog::info("{:05.0f} {:05.0f} {:05.0f} {:05.0f} {:05.0f}", elapsed_us, elapsed_ticks, vel_ticks_s, vel_rpm, accumulator);
i += 1;
// if (i % 1000 == 0) {
// setpoint *= -1;
// }
}
// spdlog::info(count / (end_time - start_time) * 1000.0);
// spi.write(Spi::Register::DISPLAY_LINE_1_C0, 1, 66);
return 0;
}

3
server/src/motor.cpp
View file

@ -76,7 +76,6 @@ std::vector<double> Motor::get_speed() {
velocities_rpm.push_back(filtered_speeds[i]);
last_encoder_values[i] = current_ticks.at(i);
spdlog::info("{} {}", i, current_ticks.at(i));
}
return velocities_rpm;
@ -85,8 +84,6 @@ std::vector<double> Motor::get_speed() {
void Motor::reset_speed() {
last_time_read = std::chrono::high_resolution_clock::now();
spdlog::info("Reeeeeset");
auto current_ticks = Encoder::read_all();
for (int i = 0; i < MOTOR_COUNT; i++) {
filtered_speeds[i] = 0;