diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml
index 814742c..4dd9c21 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 b136962..87662d7 100644
--- a/Core/Inc/modelec.h
+++ b/Core/Inc/modelec.h
@@ -18,9 +18,9 @@
#define MAX_WAYPOINTS 16
#define PWM_MAX 626.0f
#define ENCODER_RES 2400.0f
-#define WHEEL_DIAMETER 0.09735f
+#define WHEEL_DIAMETER 0.0985f
#define WHEEL_RADIUS (WHEEL_DIAMETER/2.0f)
-#define WHEEL_BASE 0.2844f
+#define WHEEL_BASE 0.289f
#define WHEEL_BASE_2 (WHEEL_BASE/2.0f)
#define V_MAX 0.643f // m/s
diff --git a/Core/Src/CommCallbacks.cpp b/Core/Src/CommCallbacks.cpp
index 6268dcc..30d768e 100644
--- a/Core/Src/CommCallbacks.cpp
+++ b/Core/Src/CommCallbacks.cpp
@@ -23,6 +23,8 @@ void Comm_GetPos(float& x, float& y, float& theta) {
}
void Comm_SetPos(float x, float y, float theta) {
+ theta = std::atan2(std::sin(theta), std::cos(theta));
+
bot.pos.x = x / 1000;
bot.pos.y = y / 1000;
bot.pos.theta = theta;
@@ -76,6 +78,8 @@ void Comm_StartOdometry(bool on) {
}
void Comm_AddWaypoint(int id, int type, float x, float y, float theta) {
+ theta = std::atan2(std::sin(theta), std::cos(theta));
+
bot.addTarget(id, type, x / 1000.0f, y / 1000.0f, theta);
}
diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp
index 60700fd..f1d4504 100644
--- a/Core/Src/modelec.cpp
+++ b/Core/Src/modelec.cpp
@@ -41,10 +41,10 @@ void DiffBot::stop(bool stop) {
}
void DiffBot::setup() {
- 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, -M_PI, M_PI);
+ 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(2, 0.0, 0.0, -M_PI, M_PI);
prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2);
prevCountRight = __HAL_TIM_GET_COUNTER(&htim3);
@@ -59,13 +59,11 @@ void DiffBot::update(float dt) {
if (rightVel == 0 && leftVel == 0) {
if (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)) {
- // BUG : apres ca le robot ne s'arrete plus quand on le relance
motor.stop(true);
targets[index].active = false;
diff --git a/Core/Startup/startup_stm32g491keux.s b/Core/Startup/startup_stm32g491keux.s
index 1a79404..1195817 100644
--- a/Core/Startup/startup_stm32g491keux.s
+++ b/Core/Startup/startup_stm32g491keux.s
@@ -59,7 +59,7 @@ defined in linker script */
.weak Reset_Handler
.type Reset_Handler, %function
Reset_Handler:
- ldr r0, =_estack
+ ldr r0, =_estack
mov sp, r0 /* set stack pointer */
/* Call the clock system initialization function.*/