diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index b300c7d..1f60cea 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h
index 92119f6..81b6472 100644
--- a/Core/Inc/modelec.h
+++ b/Core/Inc/modelec.h
@@ -32,7 +32,7 @@ extern TIM_HandleTypeDef htim2;
class DiffBot {
public:
- Point pose;
+ Point pos;
Point targets[MAX_WAYPOINTS] = {
Point(0, FINAL, 0, 0, 0),
@@ -64,7 +64,7 @@ public:
float readEncoderLeft();
- DiffBot(Point pose, float dt);
+ DiffBot(Point pos, float dt);
void stop(bool stop);
@@ -75,6 +75,8 @@ public:
void addTarget(int id, int type, float x, float y, float theta);
void resetPID();
+
+ void publishStatus();
};
#endif // MODELEC_H
diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp
index 8aeb9f7..a0f0b8a 100644
--- a/Core/Src/CommCallbacks.cpp
+++ b/Core/Src/CommCallbacks.cpp
@@ -16,15 +16,15 @@
extern DiffBot bot;
void Comm_GetPos(float& x, float& y, float& theta) {
- x = bot.pose.x;
- y = bot.pose.y;
- theta = bot.pose.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.pose.x = x / 1000;
- bot.pose.y = y / 1000;
- bot.pose.theta = theta;
+ bot.pos.x = x / 1000;
+ bot.pos.y = y / 1000;
+ bot.pos.theta = theta;
}
void Comm_GetSpeed(float& vx, float& vy, float& omega) {
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 4ee8ace..8b70302 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -32,7 +32,7 @@ float DiffBot::readEncoderRight() {
return (2.0f*M_PI*WHEEL_RADIUS*revs); // m
}
-DiffBot::DiffBot(Point pose, float dt) : pose(pose), dt(dt) {
+DiffBot::DiffBot(Point pos, float dt) : pos(pos), dt(dt) {
};
void DiffBot::stop(bool stop) {
@@ -41,10 +41,10 @@ void DiffBot::stop(bool stop) {
}
void DiffBot::setup() {
- pidLeft = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
- pidRight = PID(1, 0.0, 0.0, -PWM_MAX, PWM_MAX);
- pidPos = PID(1, 0.0, 0.0, -V_MAX, V_MAX);
- pidTheta = PID(1, 0.0, 0.0, -2.0f, 2);
+ pidLeft = PID(2, 0.0, 0.0, -PWM_MAX, PWM_MAX);
+ pidRight = PID(2, 0.0, 0.0, -PWM_MAX, PWM_MAX);
+ pidPos = PID(3, 0.0, 0.0, -V_MAX, V_MAX);
+ pidTheta = PID(4, 0.0, 0.0, -2.0f, 2);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -57,15 +57,23 @@ void DiffBot::update(float dt) {
float rightVel = readEncoderRight();
float leftVel = readEncoderLeft();
+ if (rightVel == 0 && leftVel == 0 && (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0)) {
+ // TODO add something when the robot is stuck on the wall so the motor value are >= 0 but the encoder value are = 0
+ }
+
// update pos
float v = (leftVel + rightVel) / 2.0f;
float w = (rightVel - leftVel) / WHEEL_BASE;
- pose.x += v * cosf(pose.theta - w/2);
- pose.y += v * sinf(pose.theta - w/2);
- pose.theta += w;
+ pos.x += v * cosf(pos.theta - w/2);
+ pos.y += v * sinf(pos.theta - w/2);
+ pos.theta += w;
- while (pose.theta > M_PI) pose.theta -= 2*M_PI;
- while (pose.theta < -M_PI) pose.theta += 2*M_PI;
+ while (pos.theta > M_PI) pos.theta -= 2*M_PI;
+ while (pos.theta < -M_PI) pos.theta += 2*M_PI;
+
+ if (odo_active) {
+ publishStatus();
+ }
if (!odo_active || !targets[index].active) {
motor.update();
@@ -73,13 +81,13 @@ void DiffBot::update(float dt) {
}
// pid setup
- float dx = targets[index].x - pose.x;
- float dy = targets[index].y - pose.y;
+ float dx = targets[index].x - pos.x;
+ float dy = targets[index].y - pos.y;
switch (targets[index].state) {
case FINAL:
- if (std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL && std::fabs(targets[index].theta - pose.theta) < PRECISE_ANGLE) {
+ if (std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL && std::fabs(targets[index].theta - pos.theta) < PRECISE_ANGLE) {
targets[index].active = false;
motor.stop(true);
@@ -111,8 +119,8 @@ void DiffBot::update(float dt) {
resetPID();
- dx = targets[index].x - pose.x;
- dy = targets[index].y - pose.y;
+ dx = targets[index].x - pos.x;
+ dy = targets[index].y - pos.y;
}
@@ -124,7 +132,7 @@ void DiffBot::update(float dt) {
float dist = sqrtf(dx*dx + dy*dy);
float angleTarget = atan2f(dy, dx);
- float angleError = angleTarget - pose.theta;
+ float angleError = angleTarget - pos.theta;
while (angleError > M_PI) angleError -= 2*M_PI;
while (angleError < -M_PI) angleError += 2*M_PI;
@@ -145,7 +153,7 @@ void DiffBot::update(float dt) {
float wRef;
if (targets[index].state == FINAL && std::fabs(dx) <= PRECISE_POS_FINAL && std::fabs(dy) <= PRECISE_POS_FINAL) {
- wRef = pidTheta.compute(targets[index].theta, pose.theta, dt);
+ wRef = pidTheta.compute(targets[index].theta, pos.theta, dt);
vRef = 0;
}
else {
@@ -197,3 +205,9 @@ void DiffBot::resetPID() {
pidPos.reset();
pidTheta.reset();
}
+
+void DiffBot::publishStatus() {
+ char response[64];
+ snprintf(response, sizeof(response), "SET;POS;%.4f;%.4f;%.4f\n", pos.x*1000, pos.y*1000, pos.theta);
+ CDC_Transmit_FS((uint8_t*)response, strlen(response));
+}