From ccb6a870a26f194d3a0c57ce7f9873d92ed650cf Mon Sep 17 00:00:00 2001 From: dd060606 Date: Tue, 22 Apr 2025 17:28:46 +0200 Subject: [PATCH] ajout PID sur branche master --- Core/Inc/motors.h | 2 + Core/Inc/pid.h | 57 +++++++++++++++ Core/Inc/pidPosition.h | 56 +++++++++++++++ Core/Inc/pidVitesse.h | 40 +++++++++++ Core/Inc/point.h | 50 +++++++++++++ Core/Src/modelec.cpp | 134 +++++++++++++++++++++------------- Core/Src/motors.cpp | 12 ++++ Core/Src/pid.cpp | 60 ++++++++++++++++ Core/Src/pidPosition.cpp | 152 +++++++++++++++++++++++++++++++++++++++ Core/Src/pidVitesse.cpp | 100 ++++++++++++++++++++++++++ Core/Src/point.cpp | 55 ++++++++++++++ 11 files changed, 667 insertions(+), 51 deletions(-) create mode 100644 Core/Inc/pid.h create mode 100644 Core/Inc/pidPosition.h create mode 100644 Core/Inc/pidVitesse.h create mode 100644 Core/Inc/point.h create mode 100644 Core/Src/pid.cpp create mode 100644 Core/Src/pidPosition.cpp create mode 100644 Core/Src/pidVitesse.cpp create mode 100644 Core/Src/point.cpp diff --git a/Core/Inc/motors.h b/Core/Inc/motors.h index c07ec5d..421aa53 100644 --- a/Core/Inc/motors.h +++ b/Core/Inc/motors.h @@ -15,6 +15,8 @@ private: public: Motor(TIM_TypeDef *timer); + void setTargetSpeed(int pwm); + float getCurrentSpeed(); void accelerer(int speed); void reculer(int speed); void stop(); diff --git a/Core/Inc/pid.h b/Core/Inc/pid.h new file mode 100644 index 0000000..ca1610f --- /dev/null +++ b/Core/Inc/pid.h @@ -0,0 +1,57 @@ +/* + * pid.h + * + * Created on: Mar 25, 2025 + * Author: CHAUVEAU Maxime + */ + +#ifndef INC_PID_H_ +#define INC_PID_H_ +#include "point.h" +#include +#include + +/* + Pour notre robot, on va faire 3 PID. On va avoir un premier PID qui prend une position et qui + doit donner en sortie deux ordres pour les moteurs (un moteur gauche et un moteur droit). + Ainsi on aura aussi un PID par moteur qui eux recevront la consigne du pid position, donc 3 en tout. + + Le premier PID (position) prend en entrée un Point de la carte (objectif) et renvoie en sortie un tableau de 2 val en m/s. + les pid vitesse de chaque moteurs prennent en entrée une vitesse à atteindre et en sortie une consigne à envoyer au moteur (qu'il faudra traduire en pwm). + */ + +class Pid { +protected: + + float Vmax; //vitesse linéaire max du robot (mps) + float Wmax; //vitesse angulaire max du robot (mps) + float L; //Largeur entre les deux roues (en mètre) + + float kp; + float ki; + float kd; + + + + +public: + // Constructeur + Pid(float kp, float ki, float kd); + + // Setters + void setKp(float k); + void setKi(float k); + void setKd(float k); + + // Getters + float getKp(); + float getKi(); + float getKd(); + + //Methodes + int getPWMCommand(float vitesse); //convertir des m/s en notre signal pwm +}; + + + +#endif /* INC_PID_H_ */ diff --git a/Core/Inc/pidPosition.h b/Core/Inc/pidPosition.h new file mode 100644 index 0000000..673363c --- /dev/null +++ b/Core/Inc/pidPosition.h @@ -0,0 +1,56 @@ +#ifndef INC_PID_POSITION_H_ +#define INC_PID_POSITION_H_ + +#include "pid.h" + +class PidPosition : public Pid { +private: + Point erreurPosition, erreurPosition_old; + Point consignePositionFinale; + + float d[2]; //valeurs calculé dans les méthodes update (position) + float i[2]; + + float Kp_theta, Ki_theta, Kd_theta; + +public: + // Constructeur + PidPosition(float kp, float ki, float kd, + float Kp_theta, float Ki_theta, float Kd_theta, + Point consignePosition); + + // Setters + void setConsignePositionFinale(Point consigne); + void setErreurPosition(Point erreur); + void setErreurPosition_old(Point erreur_old); + + void setD(float d[2]); + void setI(float d[2]); + + void setKpTheta(float kp); + void setKiTheta(float ki); + void setKdTheta(float kd); + + // Getters + Point getConsignePositionFinale(); + Point getErreurPosition(); + Point getErreurPosition_old(); + + float getKpTheta(); + float getKiTheta(); + float getKdTheta(); + + std::array getD(); + std::array getI(); + + // Méthodes spécifiques à la position + Point calculErreurPosition(Point p); + void updateErreurPosition(Point pointActuel); + std::array updateNouvelOrdreVitesse(Point pointActuel); + + // Surcharge des méthodes virtuelles + void updateErreur(); + void updateNouvelleConsigne(); +}; + +#endif /* INC_PID_POSITION_H_ */ diff --git a/Core/Inc/pidVitesse.h b/Core/Inc/pidVitesse.h new file mode 100644 index 0000000..25070a5 --- /dev/null +++ b/Core/Inc/pidVitesse.h @@ -0,0 +1,40 @@ +#ifndef INC_PIDVITESSE_H_ +#define INC_PIDVITESSE_H_ + +#include "pid.h" + +class PidVitesse : public Pid { +private: + float erreurVitesse, erreurVitesse_old; + float consigneVitesseFinale; + float derivee, integral; + float nouvelleConsigneVitesse; + +public: + // Constructeur + PidVitesse(float kp, float ki, float kd, float consigneVitesseFinale); + + // Setters + void setErreurVitesse(float e); + void setErreurVitesse_old(float e_old); + void setConsigneVitesseFinale(float vf); + void setNouvelleConsigneVitesse(float v); + void setDerivee(float d); + void setIntegral(float i); + + // Getters + float getErreurVitesse(); + float getErreurVitesse_old(); + float getConsigneVitesseFinale(); + float getNouvelleConsigneVitesse(); + float getDerivee(); + float getIntegral(); + + // Méthodes spécifiques au PID vitesse + float calculErreurVitesse(float vitesseActuelle); + void updateErreurVitesse(float vitesseActuelle); + void updateNouvelleVitesse(float vitesseActuelle); + +}; + +#endif /* INC_PIDVITESSE_H_ */ diff --git a/Core/Inc/point.h b/Core/Inc/point.h new file mode 100644 index 0000000..eccb2cc --- /dev/null +++ b/Core/Inc/point.h @@ -0,0 +1,50 @@ +/* + * point.h + * + * Created on: Mar 24, 2025 + * Author: CHAUVEAU Maxime + */ + +#ifndef INC_POINT_H_ +#define INC_POINT_H_ + +#include +#include + +typedef enum StatePoint { + INTERMEDIAIRE, + FINAL, + NONDETERMINE +}StatePoint; + +class Point { +private: + float x; + float y; + float theta; + uint32_t id; + StatePoint state; + +public: + // Constructeur + Point(float x = 0.0, float y = 0.0, float theta = 0.0, StatePoint state = StatePoint::INTERMEDIAIRE); + + // Setters + void setX(float x); + void setY(float y); + void setTheta(float theta); + void setState(StatePoint s); + void setID(uint32_t id); + + // Getters + float getX(); + float getY(); + float getTheta(); + std::array getPoint(); + StatePoint getState(); + uint32_t getID(); +}; + + + +#endif /* INC_POINT_H_ */ diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 3493a8d..8997f93 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -4,6 +4,10 @@ #include #include #include +#include "pidVitesse.h" +#include "pid.h" +#include "point.h" +#include "pidPosition.h" extern "C" { @@ -29,78 +33,106 @@ float x, y, theta; uint32_t lastTick = 0; - bool isDelayPassedFrom(uint32_t delay, uint32_t *lastTick) { - if (HAL_GetTick() - *lastTick >= delay) { - *lastTick = HAL_GetTick(); - return true; - } - return false; + if (HAL_GetTick() - *lastTick >= delay) { + *lastTick = HAL_GetTick(); + return true; + } + return false; } bool isDelayPassed(uint32_t delay) { - return isDelayPassedFrom(delay, &lastTick); + return isDelayPassedFrom(delay, &lastTick); } +//PID +void determinationCoefPosition(Point objectifPoint, Point pointActuel){ + //à AJUSTER CAR NORMALEMENT IL Y A UN SEUL OBJET MOTEUR, il faut contrôler le signal avec CCR1, CCR2,CCR3,CCR4 + Motor motorG(TIM3); + Motor motorD(TIM3); + + PidPosition pid(0,0,0,0,0,0,objectifPoint); + + std::array vitesse = pid.updateNouvelOrdreVitesse(pointActuel); + PidVitesse pidG(0, 0, 0, vitesse[0]); + PidVitesse pidD(0, 0, 0, vitesse[1]); + + pidG.updateNouvelleVitesse(motorG.getCurrentSpeed()); + pidD.updateNouvelleVitesse(motorD.getCurrentSpeed()); + + float nouvelOrdreG = pidG.getNouvelleConsigneVitesse(); + float nouvelOrdreD = pidD.getNouvelleConsigneVitesse(); + + int ordrePWMG = pidG.getPWMCommand(nouvelOrdreG); + int ordrePWMD = pidD.getPWMCommand(nouvelOrdreD); + + + motorG.setTargetSpeed(ordrePWMG); + motorD.setTargetSpeed(ordrePWMD); + + +} +//Odométrie + void ModelecOdometrySetup() { - HAL_UART_Transmit(&huart2, (uint8_t*) "SETUP COMPLETE\n", 15, HAL_MAX_DELAY); - lastPosRight = __HAL_TIM_GET_COUNTER(&htim2); - lastPosLeft = __HAL_TIM_GET_COUNTER(&htim21); - x=0.0f; - y=0.0f; - theta=0.0f; - motor.accelerer(300); + HAL_UART_Transmit(&huart2, (uint8_t*) "SETUP COMPLETE\n", 15, + HAL_MAX_DELAY); + lastPosRight = __HAL_TIM_GET_COUNTER(&htim2); + lastPosLeft = __HAL_TIM_GET_COUNTER(&htim21); + x = 0.0f; + y = 0.0f; + theta = 0.0f; + motor.accelerer(300); } void ModelecOdometryUpdate() { //On récupère la valeur des compteurs - uint16_t posRight = __HAL_TIM_GET_COUNTER(&htim2); - uint16_t posLeft = __HAL_TIM_GET_COUNTER(&htim21); + uint16_t posRight = __HAL_TIM_GET_COUNTER(&htim2); + uint16_t posLeft = __HAL_TIM_GET_COUNTER(&htim21); - //On calcule les deltas - int16_t deltaLeft = (int16_t)(posLeft - lastPosLeft); - int16_t deltaRight = (int16_t)(posRight - lastPosRight); + //On calcule les deltas + int16_t deltaLeft = (int16_t) (posLeft - lastPosLeft); + int16_t deltaRight = (int16_t) (posRight - lastPosRight); - //On met à jour la dernière position - lastPosLeft = posLeft; - lastPosRight = posRight; + //On met à jour la dernière position + lastPosLeft = posLeft; + lastPosRight = posRight; - //On convertit en distance (mètres) - float distLeft = (deltaLeft / COUNTS_PER_REV) * WHEEL_CIRCUMFERENCE; - float distRight = (deltaRight / COUNTS_PER_REV) * WHEEL_CIRCUMFERENCE; + //On convertit en distance (mètres) + float distLeft = (deltaLeft / COUNTS_PER_REV) * WHEEL_CIRCUMFERENCE; + float distRight = (deltaRight / COUNTS_PER_REV) * WHEEL_CIRCUMFERENCE; - //On calcule les déplacements - float linear = (distLeft + distRight) / 2.0f; - float deltaTheta = (distRight - distLeft) / WHEEL_BASE; + //On calcule les déplacements + float linear = (distLeft + distRight) / 2.0f; + float deltaTheta = (distRight - distLeft) / WHEEL_BASE; - //On met à jour la position - float avgTheta = theta + deltaTheta / 2.0f; - x += linear * cosf(avgTheta); - y += linear * sinf(avgTheta); - theta += deltaTheta; + //On met à jour la position + float avgTheta = theta + deltaTheta / 2.0f; + x += linear * cosf(avgTheta); + y += linear * sinf(avgTheta); + theta += deltaTheta; - //On normalise theta - theta = fmodf(theta, 2.0f * M_PI); - if (theta < 0) theta += 2.0f * M_PI; + //On normalise theta + theta = fmodf(theta, 2.0f * M_PI); + if (theta < 0) + theta += 2.0f * M_PI; - char msg[128]; - sprintf(msg, "X: %.3f m, Y: %.3f m, Theta: %.3f rad\r\n", x, y, theta); - HAL_UART_Transmit(&huart2, (uint8_t *)msg, strlen(msg), HAL_MAX_DELAY); + char msg[128]; + sprintf(msg, "X: %.3f m, Y: %.3f m, Theta: %.3f rad\r\n", x, y, theta); + HAL_UART_Transmit(&huart2, (uint8_t*) msg, strlen(msg), HAL_MAX_DELAY); } - - void ModelecOdometryLoop() { - GPIOC->ODR ^= (1 << 10); + GPIOC->ODR ^= (1 << 10); - //On met à jour toutes les 10ms - if (isDelayPassed(10)) { - ModelecOdometryUpdate(); - motor.update(); - // à 20 cm on s'arrête - if (x >= 0.20) { - motor.stop(); - } - } + //On met à jour toutes les 10ms + if (isDelayPassed(10)) { + ModelecOdometryUpdate(); + motor.update(); + // à 20 cm on s'arrête + if (x >= 0.20) { + motor.stop(); + } + } } diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp index 28df08d..01323b2 100644 --- a/Core/Src/motors.cpp +++ b/Core/Src/motors.cpp @@ -19,6 +19,18 @@ void Motor::reculer(int speed) { this->stopTurning(); } +void Motor::setTargetSpeed(int pwm){ + if(pwm < 626){ + this->targetSpeed = pwm; + }else{ + this->targetSpeed = 626; + } + +} +float Motor::getCurrentSpeed(){ + return this->currentSpeed; +} + void Motor::stop() { targetSpeed = 0; this->stopTurning(); diff --git a/Core/Src/pid.cpp b/Core/Src/pid.cpp new file mode 100644 index 0000000..d6290ae --- /dev/null +++ b/Core/Src/pid.cpp @@ -0,0 +1,60 @@ + + +#include "pid.h" + + +// Constructeur +Pid::Pid(float kp, float ki, float kd){ + this->kp = kp; + this->ki = ki; + this->kd = kd; + + this->Vmax = 1; //à remplacer par la vraie valeur, calculable à partir de la rpm + this->Wmax = 1; //pareil, à remplacer par la vraie valeur + this->L = 0.3; //à remplacer, à mesurer direct entre les deux roues motrices du robot (attention, valeur à mettre en mètre) +} + +// Setters +void Pid::setKp(float k){ + this->kp = k; +} + +void Pid::setKi(float k){ + this->ki = k; +} + +void Pid::setKd(float k){ + this->kp = k; +} + +// Getters +float Pid::getKp(){ + return this->kp; +} + +float Pid::getKi(){ + return this->ki; +} + +float Pid::getKd(){ + return this->kd; +} + + +//Methodes +int Pid::getPWMCommand(float vitesse){ + /* + * Passer du rpm à m/s : 75/60 = 1.25 (nb tour en 1s). circonférence = pi*diamètre roues. Vitesse en m/s = circonférence * nb tour/s + * + */ + if(vitesse >= 0.235){ //si on dépasse la vmax qu'on peut atteindre, on bride + return 626; + } else{ + float pwm = (626*vitesse) / 0.235; //produit en croix + return std::floor(pwm); + } +} + + + + diff --git a/Core/Src/pidPosition.cpp b/Core/Src/pidPosition.cpp new file mode 100644 index 0000000..e125d72 --- /dev/null +++ b/Core/Src/pidPosition.cpp @@ -0,0 +1,152 @@ + +#include "pidPosition.h" + + +PidPosition::PidPosition(float kp, float ki, float kd,float Kp_theta, float Ki_theta, float Kd_theta, Point consignePosition): Pid(kp, ki, kd){ + + this->Kp_theta = Kp_theta; + this->Ki_theta = Ki_theta; + this->Kd_theta = Kd_theta; + + this->erreurPosition = consignePosition; // à l'init, la position est à 0,0,0, donc on a consigne - position actuelle qui vaut juste consigne +} + + + + + +// Setters +void PidPosition::setConsignePositionFinale(Point consigne) { + this->consignePositionFinale = consigne; +} + +void PidPosition::setErreurPosition(Point erreur) { + this->erreurPosition = erreur; +} + +void PidPosition::setErreurPosition_old(Point erreur_old) { + this->erreurPosition_old = erreur_old; +} + +void PidPosition::setD(float d[2]) { + this->d[0] = d[0]; + this->d[1] = d[1]; +} + +void PidPosition::setI(float i[2]) { + this->i[0] = i[0]; + this->i[1] = i[1]; +} + +void PidPosition::setKpTheta(float kp) { + this->Kp_theta = kp; +} + +void PidPosition::setKiTheta(float ki) { + this->Ki_theta = ki; +} + +void PidPosition::setKdTheta(float kd) { + this->Kd_theta = kd; +} + + + + + +// Getters +Point PidPosition::getConsignePositionFinale() { + return this->consignePositionFinale; +} + +Point PidPosition::getErreurPosition() { + return this->erreurPosition; +} + +Point PidPosition::getErreurPosition_old() { + return this->erreurPosition_old; +} + +float PidPosition::getKpTheta() { + return this->Kp_theta; +} + +float PidPosition::getKiTheta() { + return this->Ki_theta; +} + +float PidPosition::getKdTheta() { + return this->Kd_theta; +} + +std::array PidPosition::getD() { + return {this->d[0], this->d[1]}; +} + +std::array PidPosition::getI() { + return {this->i[0], this->i[1]}; +} + + +//Méthodes + +// Mise à jour de l'erreur de position +void PidPosition::updateErreurPosition(Point PointActuel) { + this->erreurPosition_old = erreurPosition; + this->erreurPosition = calculErreurPosition(PointActuel); +} + +// Calcul de l'erreur de position +Point PidPosition::calculErreurPosition(Point p) { + return Point(this->consignePositionFinale.getX() - p.getX(), + this->consignePositionFinale.getY() - p.getY(), + this->consignePositionFinale.getTheta() - p.getTheta(), + StatePoint::NONDETERMINE); +} + +// Mise à jour et calcul du nouvel ordre de vitesse pour les moteurs +std::array PidPosition::updateNouvelOrdreVitesse(Point pointActuel) { + /* + La sortie est calculée en combinant les 3 termes : + - proportionnel : on multiplie kp par l'erreur + - integral : on multiplie ki par la somme des erreurs passées + - derivee : on multiplie kd par la variation de l'erreur + */ + this->updateErreurPosition(pointActuel); + + // Mise à jour du terme intégral + this->i[0] += this->erreurPosition.getX(); + this->i[1] += this->erreurPosition.getY(); + this->i[2] += this->erreurPosition.getTheta(); + + // Calcul du terme dérivé + this->d[0] = this->erreurPosition.getX() - this->erreurPosition_old.getX(); + this->d[1] = this->erreurPosition.getY() - this->erreurPosition_old.getY(); + this->d[2] = this->erreurPosition.getTheta() - this->erreurPosition_old.getTheta(); + + // Calcul du PID pour chaque axe + double commandeX = kp * this->erreurPosition.getX() + ki * this->i[0] + kd * this->d[0]; + double commandeY = kp * this->erreurPosition.getY() + ki * this->i[1] + kd * this->d[1]; + double commandeTheta = Kp_theta * this->erreurPosition.getTheta() + Ki_theta * this->i[2] + Kd_theta * this->d[2]; + + // Conversion des commandes X/Y en une vitesse linéaire et une rotation + double vitesseLineaire = sqrt(commandeX * commandeX + commandeY * commandeY); + double vitesseAngulaire = commandeTheta; + + // Écrêtage des valeurs + if (vitesseLineaire > this->Vmax){ + vitesseLineaire = this->Vmax; + } + if (vitesseAngulaire > this->Wmax){ + vitesseAngulaire = this->Wmax; + } + if (vitesseAngulaire < -this->Wmax){ + vitesseAngulaire = -this->Wmax; + } + + // Conversion en vitesse des roues + double vitesseGauche = vitesseLineaire - (this->L / 2) * vitesseAngulaire; + double vitesseDroite = vitesseLineaire + (this->L / 2) * vitesseAngulaire; + + return {vitesseGauche, vitesseDroite}; +} diff --git a/Core/Src/pidVitesse.cpp b/Core/Src/pidVitesse.cpp new file mode 100644 index 0000000..34a9e0f --- /dev/null +++ b/Core/Src/pidVitesse.cpp @@ -0,0 +1,100 @@ +#include "pidVitesse.h" + +// Constructeur +PidVitesse::PidVitesse(float kp, float ki, float kd, float consigneVitesseFinale) : Pid(kp, ki, kd) { + + this->erreurVitesse = 0; + this->erreurVitesse_old = 0; + + this->consigneVitesseFinale = consigneVitesseFinale; + + this->derivee = 0; + this->integral = 0; + + this->nouvelleConsigneVitesse = 0; +} + +// Setters +void PidVitesse::setErreurVitesse(float e) { + this->erreurVitesse = e; +} + +void PidVitesse::setErreurVitesse_old(float e_old) { + this->erreurVitesse_old = e_old; +} + +void PidVitesse::setConsigneVitesseFinale(float vf) { + this->consigneVitesseFinale = vf; +} + +void PidVitesse::setNouvelleConsigneVitesse(float v) { + this->nouvelleConsigneVitesse = v; +} + +void PidVitesse::setDerivee(float d){ + this->derivee = d; +} + +void PidVitesse::setIntegral(float i){ + this->integral = i; +} + +// Getters +float PidVitesse::getErreurVitesse() { + return this->erreurVitesse; +} + +float PidVitesse::getErreurVitesse_old() { + return this->erreurVitesse_old; +} + +float PidVitesse::getConsigneVitesseFinale() { + return this->consigneVitesseFinale; +} + +float PidVitesse::getNouvelleConsigneVitesse() { + return this->nouvelleConsigneVitesse; +} + +float PidVitesse::getDerivee(){ + return this->derivee; +} + +float PidVitesse::getIntegral(){ + return this->integral; +} + +// Méthodes spécifiques + +float PidVitesse::calculErreurVitesse(float vitesseActuelle) { + return this->consigneVitesseFinale - vitesseActuelle; +} + +void PidVitesse::updateErreurVitesse(float vitesseActuelle) { + this->erreurVitesse_old = this->erreurVitesse; + this->erreurVitesse = this->calculErreurVitesse(vitesseActuelle); +} + +void PidVitesse::updateNouvelleVitesse(float vitesseActuelle) { + /* + La sortie est calculée en combinant les 3 termes : + - proportionnel : on multiplie kp par l'erreur + - integral : on multiplie ki par la somme des erreurs passées + - derivee : on multiplie kd par la variation de l'erreur + */ + + + // Mise à jour de l'erreur de vitesse + this->updateErreurVitesse(vitesseActuelle); + + // Calcul du terme dérivé + this->derivee = this->erreurVitesse - this->erreurVitesse_old; + + // Mise à jour du terme intégral + this->integral += this->erreurVitesse; + + // Calcul de la nouvelle consigne de vitesse avec PID + this->nouvelleConsigneVitesse = this->getKp() * this->erreurVitesse + + this->getKi() * this->integral + + this->getKd() * this->derivee; +} diff --git a/Core/Src/point.cpp b/Core/Src/point.cpp new file mode 100644 index 0000000..77870b2 --- /dev/null +++ b/Core/Src/point.cpp @@ -0,0 +1,55 @@ + + +#include "point.h" + +Point::Point(float x, float y, float theta, StatePoint state){ + this->x = x; + this->y = y; + this->theta = theta; + this->state = state; +} + +void Point::setX(float x){ + this->x = x; +} + +void Point::setY(float y){ + this->y = y; +} + +void Point::setTheta(float theta){ + this->theta = theta; +} + +void Point::setState(StatePoint s){ + this->state = s; +} + +void Point::setID(uint32_t id){ + this->id = id; +} + + +float Point::getX(){ + return x; +} + +float Point::getY(){ + return y; +} + +float Point::getTheta(){ + return theta; +} + +std::array Point::getPoint(){ + return {this->x, this->y, this->theta}; +} + +StatePoint Point::getState(){ + return state; +} + +uint32_t Point::getID(){ + return id; +}