diff --git a/Core/Inc/modelec.h b/Core/Inc/modelec.h index cdacbf4..f787018 100644 --- a/Core/Inc/modelec.h +++ b/Core/Inc/modelec.h @@ -17,6 +17,7 @@ #define PWM_MAX 640.0f #define V_MAX 0.643f // m/s +#define A_MAX 10.0f // m/s^2 huge value cause idk what to put there #define MAX_WAYPOINTS 16 #define ENCODER_RES 2400.0f @@ -69,7 +70,7 @@ public: float precisePos = 0.15f; float currentV = 0; - const float maxAccel = 0.5f; + const float maxAccel = A_MAX; static bool isDelayPassedFrom(uint32_t delay, uint32_t& lastTick); diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index 82d5700..8306531 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -60,10 +60,10 @@ void DiffBot::stop(bool stop) void DiffBot::setup() { - pidLeft = PID(4.5f, 2.0f, 2.0f, -PWM_MAX, PWM_MAX); - pidRight = PID(4.5f, 2.0f, 2.0f, -PWM_MAX, PWM_MAX); - pidPos = PID(3.0f, 0.0f, 0.0f, -V_MAX, V_MAX); - pidTheta = PID(13.0f, 0.0f, 0.0f, -M_PI, M_PI); + pidLeft = PID(5.0f, 0.0f, 0.0f, -PWM_MAX, PWM_MAX); + pidRight = PID(5.0f, 0.0f, 0.0f, -PWM_MAX, PWM_MAX); + pidPos = PID(3.5f, 0.0f, 0.0f, -V_MAX, V_MAX); + pidTheta = PID(11.0f, 0.0f, 0.0f, -M_PI, M_PI); prevCountLeft = __HAL_TIM_GET_COUNTER(&htim2); prevCountRight = __HAL_TIM_GET_COUNTER(&htim3); @@ -182,7 +182,6 @@ void DiffBot::update(float dt_actual) } wRef = pidTheta.compute(0, -angleError, dt); - float alignScale = std::max(0.0f, cosf(angleError)); float targetV = pidPos.compute(0, -dist * direction, dt) * alignScale; diff --git a/Core/Src/motors.cpp b/Core/Src/motors.cpp index bf764ca..538e110 100644 --- a/Core/Src/motors.cpp +++ b/Core/Src/motors.cpp @@ -14,7 +14,7 @@ float approach(float current, float target, float step) { void Motor::update() { - int16_t max_step = 10; + int16_t max_step = 20; leftTarget_PWM = std::max(-PWM_MAX, std::min((float)leftTarget_PWM, PWM_MAX)); rightTarget_PWM = std::max(-PWM_MAX, std::min((float)rightTarget_PWM, PWM_MAX));