mirror of
https://github.com/modelec/odo_STM32.git
synced 2026-01-19 00:37:33 +01:00
122 lines
2.6 KiB
C++
122 lines
2.6 KiB
C++
/*
|
|
* CommCallbacks.cpp
|
|
*
|
|
* Created on: May 25, 2025
|
|
* Author: maxch
|
|
*/
|
|
|
|
|
|
|
|
#include "CommCallbacks.h"
|
|
#include "main.h"
|
|
#include "modelec.h"
|
|
#include "motors.h"
|
|
#include "pid.h"
|
|
#include <cmath>
|
|
|
|
extern DiffBot bot;
|
|
|
|
void Comm_GetPos(float& x, float& y, float& theta) {
|
|
x = bot.pos.x * 1000;
|
|
y = bot.pos.y * 1000;
|
|
theta = bot.pos.theta;
|
|
}
|
|
|
|
void Comm_SetPos(float x, float y, float theta) {
|
|
bot.pos.x = x / 1000;
|
|
bot.pos.y = y / 1000;
|
|
bot.pos.theta = theta;
|
|
}
|
|
|
|
void Comm_GetSpeed(float& vx, float& vy, float& omega) {
|
|
vx = bot.vx;
|
|
vy = bot.vy;
|
|
omega = bot.vtheta;
|
|
}
|
|
|
|
bool Comm_GetPID(char *pid_name, float &p, float &i, float &d, float &v_min, float &v_max) {
|
|
PID* pid = nullptr;
|
|
|
|
if (strcmp(pid_name, "LEFT") == 0) pid = &bot.pidLeft;
|
|
else if (strcmp(pid_name, "RIGHT") == 0) pid = &bot.pidRight;
|
|
else if (strcmp(pid_name, "POS") == 0) pid = &bot.pidPos;
|
|
else if (strcmp(pid_name, "THETA") == 0) pid = &bot.pidTheta;
|
|
else return false;
|
|
|
|
p = pid->getKp();
|
|
i = pid->getKi();
|
|
d = pid->getKd();
|
|
v_min = pid->getOutMin();
|
|
v_max = pid->getOutMax();
|
|
|
|
return true;
|
|
}
|
|
|
|
bool Comm_SetPID(char *pid_name, float p, float i, float d, float out_min, float out_max) {
|
|
PID* pid = nullptr;
|
|
|
|
if (strcmp(pid_name, "LEFT") == 0) pid = &bot.pidLeft;
|
|
else if (strcmp(pid_name, "RIGHT") == 0) pid = &bot.pidRight;
|
|
else if (strcmp(pid_name, "POS") == 0) pid = &bot.pidPos;
|
|
else if (strcmp(pid_name, "THETA") == 0) pid = &bot.pidTheta;
|
|
else return false;
|
|
|
|
pid->setTunings(p, i, d);
|
|
|
|
// only set limits if user provided them
|
|
if (!std::isnan(out_min) && !std::isnan(out_max)) {
|
|
pid->setLimits(out_min, out_max);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void Comm_StartOdometry(bool on) {
|
|
bot.stop(!on);
|
|
}
|
|
|
|
void Comm_AddWaypoint(int id, int type, float x, float y, float theta) {
|
|
bot.addTarget(id, type, x / 1000.0f, y / 1000.0f, theta);
|
|
}
|
|
|
|
float Comm_GetDistance(int n) {
|
|
return 0.0f;
|
|
}
|
|
|
|
void Comm_SetPWM(float left, float right) {
|
|
bot.motor.leftTarget_PWM = left;
|
|
bot.motor.rightTarget_PWM = right;
|
|
}
|
|
|
|
void Comm_SetPublishFrequency(uint32_t frequencyPublish) {
|
|
bot.frequencyPublish = frequencyPublish;
|
|
}
|
|
|
|
void Comm_GetPublishFrequency(uint32_t &frequencyPublish) {
|
|
frequencyPublish = bot.frequencyPublish;
|
|
}
|
|
|
|
float Comm_GetPreciseAngle() {
|
|
return bot.preciseAngle;
|
|
}
|
|
|
|
float Comm_GetPrecisePos() {
|
|
return bot.precisePos;
|
|
}
|
|
|
|
float Comm_GetPrecisePosFinal() {
|
|
return bot.precisePosFinal;
|
|
}
|
|
|
|
void Comm_SetPreciseAngle(float d) {
|
|
bot.preciseAngle = d;
|
|
}
|
|
|
|
void Comm_SetPrecisePos(float d) {
|
|
bot.precisePos = d;
|
|
}
|
|
|
|
void Comm_SetPrecisePosFinal(float d) {
|
|
bot.precisePosFinal = d;
|
|
}
|