mirror of
https://github.com/modelec/odo_STM32.git
synced 2026-01-18 16:27:25 +01:00
98 lines
1.9 KiB
C++
98 lines
1.9 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"
|
|
|
|
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, float &p, float &i, float &d) {
|
|
if (strcmp(pid, "LEFT") == 0) {
|
|
p = bot.pidLeft.getKp();
|
|
i = bot.pidLeft.getKi();
|
|
d = bot.pidLeft.getKd();
|
|
}
|
|
else if (strcmp(pid, "RIGHT") == 0) {
|
|
p = bot.pidRight.getKp();
|
|
i = bot.pidRight.getKi();
|
|
d = bot.pidRight.getKd();
|
|
}
|
|
else if (strcmp(pid, "POS") == 0) {
|
|
p = bot.pidPos.getKp();
|
|
i = bot.pidPos.getKi();
|
|
d = bot.pidPos.getKd();
|
|
}
|
|
else if (strcmp(pid, "THETA") == 0) {
|
|
p = bot.pidTheta.getKp();
|
|
i = bot.pidTheta.getKi();
|
|
d = bot.pidTheta.getKd();
|
|
}
|
|
else {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool Comm_SetPID(char *pid, float p, float i, float d) {
|
|
if (strcmp(pid, "LEFT") == 0) {
|
|
bot.pidLeft.setTunings(p, i, d);
|
|
}
|
|
else if (strcmp(pid, "RIGHT") == 0) {
|
|
bot.pidRight.setTunings(p, i, d);
|
|
}
|
|
else if (strcmp(pid, "POS") == 0) {
|
|
bot.pidPos.setTunings(p, i, d);
|
|
}
|
|
else if (strcmp(pid, "THETA") == 0) {
|
|
bot.pidTheta.setTunings(p, i, d);
|
|
}
|
|
else {
|
|
return false;
|
|
}
|
|
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;
|
|
}
|