mirror of
https://github.com/modelec/odo_STM32.git
synced 2026-03-18 21:30:38 +01:00
new odo
This commit is contained in:
acki
@@ -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<float>(M_PI) / 2.0f) {
|
||||
direction = -1.0f;
|
||||
angleError = normalizeAngle(angleError - static_cast<float>(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<int16_t>(applyLimits(pwmLeft));
|
||||
motor.rightTarget_PWM = static_cast<int16_t>(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<int16_t>(pwmLeft);
|
||||
motor.rightTarget_PWM = static_cast<int16_t>(pwmRight);
|
||||
motor.update();*/
|
||||
}
|
||||
|
||||
void DiffBot::addTarget(int id, int type, float x, float y, float theta)
|
||||
|
||||
Reference in New Issue
Block a user