diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 1f60cea..814742c 100644
--- a/.settings/language.settings.xml
+++ b/.settings/language.settings.xml
@@ -5,7 +5,7 @@
-
+
@@ -16,7 +16,7 @@
-
+
diff --git a/Core/Inc/CommCallbacks.h b/Core/Inc/CommCallbacks.h
index d3b019d..5ccb783 100644
--- a/Core/Inc/CommCallbacks.h
+++ b/Core/Inc/CommCallbacks.h
@@ -25,6 +25,8 @@ float Comm_GetDistance(int sensorId);
void Comm_SetPWM(float left, float right);
+void Comm_GetPWM(float &left, float &right);
+
void Comm_SetPublishFrequency(uint32_t frequencyPublish = 20);
void Comm_GetPublishFrequency(uint32_t &frequencyPublish);
@@ -41,6 +43,10 @@ void Comm_SetPrecisePos(float d);
void Comm_SetPrecisePosFinal(float d);
+float Comm_GetNotMoveTime();
+
+void Comm_SetNotMoveTime(float time);
+
#ifdef __cplusplus
}
#endif
diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h
index 8efc19b..20fa8d1 100644
--- a/Core/Inc/modelec.h
+++ b/Core/Inc/modelec.h
@@ -51,9 +51,13 @@ public:
bool odo_active = false;
bool arrive = false;
+ uint32_t publishNotMoved = 0;
+ uint32_t notMovedMaxTime = 500;
+ bool no_move = false;
+
uint32_t lastTick = 0;
uint32_t publishLastTick = 0;
- uint32_t frequencyPublish = 200;
+ uint32_t frequencyPublish = 100;
float preciseAngle = 0.017f;
float precisePosFinal = 0.01f;
diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp
index b818c13..b086cfa 100644
--- a/Core/Src/CommCallbacks.cpp
+++ b/Core/Src/CommCallbacks.cpp
@@ -88,6 +88,12 @@ void Comm_SetPWM(float left, float right) {
bot.motor.rightTarget_PWM = right;
}
+
+void Comm_GetPWM(float &left, float &right) {
+ left = bot.motor.leftCurrent_PWM;
+ right = bot.motor.rightCurrent_PWM;
+}
+
void Comm_SetPublishFrequency(uint32_t frequencyPublish) {
bot.frequencyPublish = frequencyPublish;
}
@@ -119,3 +125,11 @@ void Comm_SetPrecisePos(float d) {
void Comm_SetPrecisePosFinal(float d) {
bot.precisePosFinal = d;
}
+
+float Comm_GetNotMoveTime() {
+ return bot.notMovedMaxTime;
+}
+
+void Comm_SetNotMoveTime(float time) {
+ bot.notMovedMaxTime = time;
+}
diff --git a/Core/Src/commSTM.cpp b/Core/Src/commSTM.cpp
index 276ecdc..109ebe4 100644
--- a/Core/Src/commSTM.cpp
+++ b/Core/Src/commSTM.cpp
@@ -115,6 +115,13 @@ void USB_Comm_Process(void) {
snprintf(response, sizeof(response), "SET;FREQUENCY;%ld\n", freq);
USB_Comm_Send(response);
}
+ else if (strcmp(token, "MOTOR") == 0) {
+ float l, r;
+ Comm_GetPWM(l, r);
+ char response[64];
+ snprintf(response, sizeof(response), "SET;MOTOR;%.2f,%.2f\n", l, r);
+ USB_Comm_Send(response);
+ }
else if (strcmp(token, "PRECISE") == 0) {
token = strtok(NULL, ";");
@@ -148,6 +155,21 @@ void USB_Comm_Process(void) {
}
}
}
+ else if (strcmp(token, "DATA") == 0) {
+ token = strtok(NULL, ";");
+
+ if (token) {
+ if (strcmp(token, "NOT") == 0) {
+ token = strtok(NULL, ";");
+ if (strcmp(token, "MOVE") == 0) {
+ uint32_t time = Comm_GetNotMoveTime();
+ char response[64];
+ snprintf(response, sizeof(response), "SET;NOT;MOVE;%ld\n", time);
+ USB_Comm_Send(response);
+ }
+ }
+ }
+ }
else {
USB_Comm_Send("KO;UNKNOWN\n");
}
@@ -269,8 +291,6 @@ void USB_Comm_Process(void) {
USB_Comm_Send("KO;UNKNOWN;NEED_DATA");
}
else {
- float d;
-
if (strcmp(token, "POS") == 0) {
token = strtok(NULL, ";");
@@ -301,6 +321,24 @@ void USB_Comm_Process(void) {
}
}
}
+ else if (strcmp(token, "DATA") == 0) {
+ token = strtok(NULL, ";");
+
+ if (token) {
+ if (strcmp(token, "NOT") == 0) {
+ token = strtok(NULL, ";");
+ if (strcmp(token, "MOVE") == 0) {
+ token = strtok(NULL, ";");
+
+ uint32_t time = strtoul(token, NULL, 10);
+ Comm_SetNotMoveTime(time);
+ char response[64];
+ snprintf(response, sizeof(response), "SET;NOT;MOVE;%ld\n", time);
+ USB_Comm_Send(response);
+ }
+ }
+ }
+ }
else {
char response[268];
snprintf(response, sizeof(response), "KO;UNKNOWN;%s\n", parse_buffer);
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index bc4ebb1..a6dbc66 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -59,6 +59,18 @@ void DiffBot::update(float dt) {
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
+
+ if (!no_move) {
+ no_move = true;
+ publishNotMoved = HAL_GetTick();
+ }
+ else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) {
+ motor.stop(true);
+ }
+
+
+ } else if (no_move) {
+ no_move = false;
}
// update pos