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

Start on v2

Browse source
This commit is contained in:
root 2022-05-21 13:10:28 +01:00
parent 4d5c26d10c
commit e9ae1a320a
43 changed files with 608 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

34
server_prototype/src/encoder.cpp Normal file
View file

@ -0,0 +1,34 @@
#include "include/encoder.hpp"
#include "include/mathUtils.hpp"
#include "include/spi.hpp"
std::vector<int32_t> Encoder::read_all() {
std::vector<int32_t> result;
uint8_t result_bytes[ENCODER_COUNT * 4] = {0};
Spi::getInstance().read_array(Spi::Register::MOTOR_1_POS_B3, ENCODER_COUNT * 4, result_bytes);
for (int i = 0; i < ENCODER_COUNT; i++) {
int32_t encoder_value = mathUtils::int_from_bytes(result_bytes + i * 4, 4);
result.push_back(encoder_value);
encoder_cache[i] = encoder_value;
}
last_read = std::chrono::system_clock::now();
return result;
}
std::vector<int32_t> Encoder::read_all_cached() {
auto last_read_delay_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::system_clock::now() - last_read
).count();
if (last_read_delay_ms > ENCODER_CACHE_DURATION_MS) {
return read_all();
}
return std::vector<int32_t>(std::begin(encoder_cache), std::end(encoder_cache));
}
Encoder::Encoder() {
}

167
server_prototype/src/main.cpp Normal file
View file

@ -0,0 +1,167 @@
#include <iostream>
#include <CompLib.pb.h>
#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>
#include <chrono>
#include <algorithm>
#include "include/spi.hpp"
#include "include/reset.hpp"
#include "include/encoder.hpp"
#include "include/motor.hpp"
#include "include/robot.hpp"
#define SOCKET_PATH "/tmp/compLib"
#define BUFFER_SIZE 64
google::protobuf::Message *processMessage(const std::string &serializedMessage) {
CompLib::GenericRequest message;
message.ParseFromString(serializedMessage);
auto messageTypeName = message.header().message_type();
if (messageTypeName == CompLib::ReadSensorsRequest::GetDescriptor()->full_name()) {
CompLib::ReadSensorsRequest readSensorsRequest;
readSensorsRequest.ParseFromString(serializedMessage);
} else if (messageTypeName == CompLib::ReadSensorsResponse::GetDescriptor()->full_name()) {
CompLib::ReadSensorsResponse readSensorsResponse;
readSensorsResponse.ParseFromString(serializedMessage);
std::cout << readSensorsResponse.ir_1() << std::endl;
std::cout << readSensorsResponse.ir_2() << std::endl;
std::cout << readSensorsResponse.ir_3() << std::endl;
std::cout << readSensorsResponse.ir_4() << std::endl;
std::cout << readSensorsResponse.ir_5() << std::endl;
} else {
std::cout << messageTypeName << " not found!" << std::endl;
}
google::protobuf::Message *returnMessage = MessageBuilder::genericResponse(false, ERROR_MESSAGE_UNKNOWN);
return returnMessage;
}
[[noreturn]] void socketServer() {
struct sockaddr_un socketAddress;
int socketFileDescriptor = socket(AF_UNIX, SOCK_STREAM, 0);
remove(SOCKET_PATH);
memset(&socketAddress, 0, sizeof(struct sockaddr_un));
socketAddress.sun_family = AF_UNIX;
strncpy(socketAddress.sun_path, SOCKET_PATH, sizeof(socketAddress.sun_path) - 1);
if (bind(socketFileDescriptor, (struct sockaddr *) &socketAddress, sizeof(struct sockaddr_un)) == -1) {
exit(-1);
}
if (listen(socketFileDescriptor, 1) == -1) {
exit(-2);
}
char readBuffer[BUFFER_SIZE];
char writeBuffer[BUFFER_SIZE];
for (;;) {
int clientFileDescriptor = accept(socketFileDescriptor, NULL, NULL);
auto numRead = read(clientFileDescriptor, readBuffer, 1);
std::cout << numRead << std::endl;
uint8_t messageSize = readBuffer[0];
std::cout << std::to_string(messageSize) << std::endl;
numRead = read(clientFileDescriptor, readBuffer, readBuffer[0]);
std::cout << numRead << std::endl;
std::string stringMessage;
for (int i{}; i < messageSize; i++) {
stringMessage += readBuffer[i];
}
auto response = processMessage(stringMessage);
uint8_t responseSize = response->ByteSizeLong();
writeBuffer[0] = responseSize;
std::cout << std::to_string(responseSize) << std::endl;
write(clientFileDescriptor, writeBuffer, 1);
response->SerializeToArray(writeBuffer, BUFFER_SIZE);
write(clientFileDescriptor, writeBuffer, responseSize);
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);
// double count = 20000;
// double start_time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
// for (int i{0}; i < count; i++){
// spi.read(1, 1);
// }
// 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 = 75;
// 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;
while (1 == 1) {
// 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;
// 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));
// spdlog::info("{} {:05.0f} {:05.0f}", delta_seconds_total, setpoint, speeds.at(0));
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);
// 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;
}

155
server_prototype/src/motor.cpp Normal file
View file

@ -0,0 +1,155 @@
#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 "include/mathUtils.hpp"
#include "include/pid.hpp"
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) {
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");
}
Mode mode = Motor::Mode::COAST;
if (percent < 0) {
mode = Motor::Mode::BACKWARD;
} else if (percent > 0) {
mode = Motor::Mode::FORWARD;
}
uint16_t pwm = abs(percent) * (MAX_MOTOR_SPEED / 100.0);
Motor::set_pwm(port, pwm, mode);
}
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");
}
if (port == 0) {
Spi::getInstance().write(Spi::Register::MOTOR_1_PWM_H, 2, pwm);
Spi::getInstance().write(Spi::Register::PWM_1_CTRL, 1, mode);
} else if (port == 1) {
Spi::getInstance().write(Spi::Register::MOTOR_2_PWM_H, 2, pwm);
Spi::getInstance().write(Spi::Register::PWM_2_CTRL, 1, mode);
} else if (port == 2) {
Spi::getInstance().write(Spi::Register::MOTOR_3_PWM_H, 2, pwm);
Spi::getInstance().write(Spi::Register::PWM_3_CTRL, 1, mode);
} else if (port == 3) {
Spi::getInstance().write(Spi::Register::MOTOR_4_PWM_H, 2, pwm);
Spi::getInstance().write(Spi::Register::PWM_4_CTRL, 1, 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;
// 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();
// }
// 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<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++) {
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;
}
void Motor::reset_speed() {
last_time_encoders_read = std::chrono::high_resolution_clock::now();
auto current_ticks = Encoder::get_instance().read_all_cached();
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() {
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}
);
}
while (1 == 1) {
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++) {
if (motor_control_modes[i] == Motor::Control::SPEED) {
should_control_speed = true;
}
}
if (!should_control_speed) {
// 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) {
float power = pids.at(i)(speed_targets[i], speeds.at(i));
_set_power(i, power);
}
}
}
}

64
server_prototype/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();
}

113
server_prototype/src/spi.cpp Normal file
View file

@ -0,0 +1,113 @@
#include <fcntl.h> //Needed for SPI port
#include <sys/ioctl.h> //Needed for SPI port
#include <linux/spi/spidev.h> //Needed for SPI port
#include <unistd.h> //Needed for SPI port
#include <iostream>
#include <string.h>
#include <spdlog/spdlog.h>
#include "include/spi.hpp"
#include "include/mathUtils.hpp"
void check_for_error(int error_code, std::string error_message) {
if (error_code < 0) {
spdlog::error(error_message);
exit(1);
}
}
Spi::Spi() {
int spi_mode = SPI_MODE_0;
int spi_bits_per_workd = SPI_BITS_PER_WORD;
int spi_speed = SPI_SPEED;
check_for_error(spi_file_descriptor = open("/dev/spidev1.2", O_RDWR), "Could not open SPI device");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MODE, &spi_mode), "Could not set SPI_IOC_WR_MODE");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MODE, &spi_mode), "Could not set SPI_IOC_RD_MODE");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_WR_BITS_PER_WORD");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_BITS_PER_WORD, &spi_bits_per_workd), "Could not set SPI_IOC_RD_BITS_PER_WORD");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed), "Could not set SPI_IOC_WR_MAX_SPEED_HZ");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_RD_MAX_SPEED_HZ, &spi_mode), "Could not set SPI_IOC_RD_MAX_SPEED_HZ");
}
void Spi::clear_buffers() {
memset(tx_buffer, 0, SPI_BUFFER_SIZE);
memset(rx_buffer, 0, SPI_BUFFER_SIZE);
}
uint8_t Spi::calculate_checksum(uint8_t* data, uint8_t length) {
int sum = 0;
for (int i = 0; i < length; i++) {
sum += data[i];
}
return sum % 256;
}
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);
return mathUtils::int_from_bytes(read_buffer, 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;
tx_buffer[2] = length;
transfer();
uint8_t checksum = calculate_checksum(rx_buffer, length + 3);
if (checksum != rx_buffer[length + 3]) {
spdlog::error("Received invalid checksum {}. Should be {}", rx_buffer[length +3], checksum);
}
memcpy(data, rx_buffer + 2, length);
}
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;
tx_buffer[2] = length;
memcpy(tx_buffer + 3, data, length);
transfer();
}
void Spi::transfer() {
std::lock_guard<std::recursive_mutex> lock(spi_mutex);
struct spi_ioc_transfer spi;
memset(&spi, 0, sizeof(spi));
spi.tx_buf = (unsigned long) tx_buffer;
spi.rx_buf = (unsigned long) rx_buffer;
spi.len = SPI_BUFFER_SIZE;
spi.delay_usecs = 0;
spi.speed_hz = SPI_SPEED;
spi.bits_per_word = SPI_BITS_PER_WORD;
spi.cs_change = 0;
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
check_for_error(ioctl(spi_file_descriptor, SPI_IOC_MESSAGE(1), &spi), "Problem transmitting spi data");
if (tx_buffer[1] != rx_buffer[1]) {
spdlog::error("SPI error during read/write of register {}. Got reg {} instead!", tx_buffer[1], rx_buffer[1]);
}
}