diff --git a/Core/Src/modelec.cpp b/Core/Src/modelec.cpp index bda03d1..a7f582c 100644 --- a/Core/Src/modelec.cpp +++ b/Core/Src/modelec.cpp @@ -75,7 +75,6 @@ void DiffBot::update(float dt_actual) if (!isDelayPassed(dt * 1000)) return; - // read encoder float rightVel = readEncoderRight(); float leftVel = readEncoderLeft(); @@ -89,76 +88,6 @@ void DiffBot::update(float dt_actual) pos.y += dDistance * sinf(avgTheta); pos.theta = normalizeAngle(pos.theta + dTheta); - /*if (rightVel == 0 && leftVel == 0) { - if (motor.rightTarget_PWM != 0 || motor.leftTarget_PWM != 0) { - if (!no_move) { - no_move = true; - publishNotMoved = HAL_GetTick(); - } - else if (isDelayPassedFrom(notMovedMaxTime, publishNotMoved)) { - motor.stop(true); - - if (targets[index].active) - { - char log[64]; - sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); - CDC_Transmit_FS((uint8_t*)log, strlen(log)); - - targets[index].active = false; - } - - no_move = false; - - if (action != 0) { - switch (action) { - case 1: // LEFT - if (cos(pos.theta) > 0) { - pos.x = 0.1f; // dist fron back to center - } - else { - pos.x = 0.1f; // dist fron center to front - } - break; - case 2: // TOP - if (sin(pos.theta) > 0) { - pos.y = 2.0f - 0.1f; // dist fron center to front - } - else { - pos.y = 2.0f - 0.1f; // dist fron back to center - } - break; - case 3: // RIGHT - if (cos(pos.theta) > 0) { - pos.x = 3.0f - 0.1f; // dist fron center to front - } - else { - pos.x = 3.0f - 0.1f; // dist fron back to center - } - break; - case 4: // BOTTOM - if (sin(pos.theta) > 0) { - pos.y = 0.1f; // dist fron back to center - } - else { - pos.y = 0.1f; // dist fron center to front - } - break; - default: - break; - } - - action = 0; - - addTarget(0, 1, pos.x, pos.y, pos.theta); - } - } - } - } else { - if (no_move) { - no_move = false; - } - }*/ - if (std::abs(rightVel) < 0.001f && std::abs(leftVel) < 0.001f && (std::abs(motor.rightTarget_PWM) > 50 || std::abs(motor.leftTarget_PWM) > 50)) { @@ -187,6 +116,10 @@ void DiffBot::update(float dt_actual) no_move = false; } + if (odo_active && isDelayPassedFrom(frequencyPublish, publishLastTick)) { + publishStatus(); + } + if (!odo_active || !targets[index].active) { motor.update(); return; @@ -195,46 +128,47 @@ void DiffBot::update(float dt_actual) float dx = targets[index].x - pos.x; float dy = targets[index].y - pos.y; float dist = sqrtf(dx*dx + dy*dy); - float angleTarget = atan2f(dy, dx); - float angleError = normalizeAngle(angleTarget - pos.theta); - - float direction = 1.0f; - if (std::fabs(angleError) > (float)M_PI / 2.0f) { - direction = -1.0f; - angleError = normalizeAngle(angleError - (float)M_PI); - } float vRef = 0.0f; float wRef = 0.0f; - bool reachedPos = (dist < (targets[index].state == FINAL ? precisePosFinal : precisePos)); - - if (reachedPos) { - bool finalized = true; + bool isAtPoint = (dist < (targets[index].state == FINAL ? precisePosFinal : precisePos)); + if (isAtPoint) { if (targets[index].state == FINAL) { float finalAngleErr = normalizeAngle(targets[index].theta - pos.theta); - if (std::fabs(finalAngleErr) > preciseAngleFinal) { - wRef = pidTheta.compute(0, -finalAngleErr, dt); - vRef = 0; - finalized = false; + + if (std::fabs(finalAngleErr) <= preciseAngleFinal) { + char log[64]; + int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index); + CDC_Transmit_FS((uint8_t*)log, len); + + targets[index].active = false; + index = (index + 1) % MAX_WAYPOINTS; + motor.stop(true); + resetPID(); + return; } - } - if (finalized) { - char log[64]; - int len = snprintf(log, sizeof(log), "SET;WAYPOINT;REACH;%d\n", index); - CDC_Transmit_FS((uint8_t*)log, len); + vRef = 0; + wRef = pidTheta.compute(0, -finalAngleErr, dt); + } else { targets[index].active = false; - index = (index + 1) % MAX_WAYPOINTS; - - motor.stop(true); resetPID(); return; } } else { + float angleTarget = atan2f(dy, dx); + float angleError = normalizeAngle(angleTarget - pos.theta); + + float direction = 1.0f; + if (std::fabs(angleError) > static_cast(M_PI) / 2.0f) { + direction = -1.0f; + angleError = normalizeAngle(angleError - static_cast(M_PI)); + } + wRef = pidTheta.compute(0, -angleError, dt); float alignmentScale = cosf(angleError); @@ -258,115 +192,6 @@ void DiffBot::update(float dt_actual) motor.leftTarget_PWM = static_cast(applyLimits(pwmLeft)); motor.rightTarget_PWM = static_cast(applyLimits(pwmRight)); motor.update(); - - if (isDelayPassedFrom(frequencyPublish, publishLastTick)) { - publishStatus(); - } - - /* - // pid setup - float dx = targets[index].x - pos.x; - float dy = targets[index].y - pos.y; - - switch (targets[index].state) - { - case FINAL: - - if (std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal && std::fabs( - targets[index].theta - pos.theta) < preciseAngleFinal) - { - targets[index].active = false; - motor.stop(true); - - char log[64]; - sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); - CDC_Transmit_FS((uint8_t*)log, strlen(log)); - - resetPID(); - - return; - } - - break; - case INTERMEDIAIRE: - - if (std::fabs(dx) < precisePos && std::fabs(dy) < precisePos) - { - char log[64]; - sprintf(log, "SET;WAYPOINT;REACH;%d\n", index); - CDC_Transmit_FS((uint8_t*)log, strlen(log)); - - targets[index].active = false; - index++; - - if (index >= 9) - { - index = 0; - return; - } - - resetPID(); - - dx = targets[index].x - pos.x; - dy = targets[index].y - pos.y; - } - - break; - default: - break; - } - - float dist = sqrtf(dx * dx + dy * dy); - - float angleTarget = atan2f(dy, dx); - float angleError = angleTarget - pos.theta; - - while (angleError > M_PI) angleError -= 2 * M_PI; - while (angleError < -M_PI) angleError += 2 * M_PI; - - float direction = 1.0f; - if (std::fabs(angleError) > M_PI / 2) - { - direction = -1.0f; - angleError > 0 ? angleError -= M_PI : angleError += M_PI; - } - - float distError = dist * cosf(angleError); - - distError *= direction; - - float vRef = pidPos.compute(0.0, -distError, dt); - float wRef = 0.0f; - - if (targets[index].state == FINAL && std::fabs(dx) <= precisePosFinal && std::fabs(dy) <= precisePosFinal) - { - wRef = pidTheta.compute(targets[index].theta, pos.theta, dt); - vRef = 0; - } - else if (precisePos2 < std::abs(distError)) - { - wRef = pidTheta.compute(0.0, -angleError, dt); - } - - float vLeft = vRef - (WHEEL_BASE_2) * wRef; - float vRight = vRef + (WHEEL_BASE_2) * wRef; - - float pwm_ff_left = (vLeft / V_MAX) * PWM_MAX; - float pwm_ff_right = (vRight / V_MAX) * PWM_MAX; - - float pwm_corr_left = pidLeft.compute(vLeft, leftVel, dt); - float pwm_corr_right = pidRight.compute(vRight, rightVel, dt); - - float pwmLeft = pwm_ff_left + pwm_corr_left; - float pwmRight = pwm_ff_right + pwm_corr_right; - - // saturation - pwmLeft = std::max(-PWM_MAX, std::min(pwmLeft, PWM_MAX)); - pwmRight = std::max(-PWM_MAX, std::min(pwmRight, PWM_MAX)); - - motor.leftTarget_PWM = static_cast(pwmLeft); - motor.rightTarget_PWM = static_cast(pwmRight); - motor.update();*/ } void DiffBot::addTarget(int id, int type, float x, float y, float theta)