odo_STM32/Core/Src/CommCallbacks.cpp

/*
 * 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_GetPosition(float* x, float* y, float* theta) {
	*x = bot.pose.x;
	*y = bot.pose.y;
	*theta = bot.pose.theta;
}

void Comm_SetPosition(float x, float y, float theta) {
	bot.pose.x = x;
	bot.pose.y = y;
	bot.pose.theta = theta;
}

void Comm_GetSpeed(float* vx, float* vy, float* omega){
}

void Comm_GetPID(float* p, float* i, float* d) {
}

void Comm_SetPID(float p, float i, float d) {
}

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;
}