Newer
Older
Alexandre Manoury
committed
#include "angle.h"
#include <math.h>
Alexandre Manoury
committed
#define abs(x) fabs(x)
Alexandre Manoury
committed
#ifndef ROBOTHW
Alexandre Manoury
committed
#include "debugwindow.h"
Alexandre Manoury
committed
#endif
CommandAllerA::CommandAllerA(Position p, bool reculer, float vitesseLineaireMax, float vitesseFin, float /*precisionAngle*/, float distance): Command()
smoothFactor = 0.f;
requireSmoothMovement = false;
bonAngle = false;
angleSmoothEnd = false;
stopAtDistance = distance;
this->precisionAngle = -1.f;//precisionAngle;
Alexandre Manoury
committed
// maximum values
float accAngMax = ACCELERATION_ANGULAIRE_MAX;
float vitAngMax = VITESSE_ANGULAIRE_MAX;
float accLinMax = ACCELERATION_LINEAIRE_MAX;
float decLinMax = DECELERATION_LINEAIRE_MAX;
if(this->getLimit())
{
vitLinMax = vitesseLinMax/3;
allumerLED();
allumerLED2();
}
else
{
//eteindreLED();
}
Alexandre Manoury
committed
// settings
//float angleVitesseMax = M_PI/10.0f;
float angleVitesseMax = 0.5f*vitAngMax*vitAngMax/accAngMax;
//float distanceVitesseMax = 350.0f;
float angle = Odometrie::odometrie->getPos().getAngle();
Position pos = Odometrie::odometrie->getPos().getPosition();
Alexandre Manoury
committed
Position computedGoal = but;
Vec2d deltaFirst = but - pos;
Vec2d deltaNext;
Alexandre Manoury
committed
if (this->requireSmoothMovement)
deltaNext = nextGoal - but;
// distances
float distanceNext = (this->requireSmoothMovement ? deltaNext.getNorme() : 0);
// disable smooth if short moves
/*if (this->requireSmoothMovement && distanceNext < 50.)
this->requireSmoothMovement = false;*/
float distanceIntermediate = (this->requireSmoothMovement ? deltaFirst.getNorme() : 0);
bool isChangingToNext = false;
// smooth movement
Alexandre Manoury
committed
if (this->requireSmoothMovement)
{
if (distanceIntermediate > this->smoothFactor)
delta = deltaFirst;
else
{
isChangingToNext = true;
delta = deltaFirst + (deltaNext - deltaFirst) * (0.5f - distanceIntermediate / (this->smoothFactor * 2.));
}
}
else
delta = computedGoal - pos;
// distances
float distanceFinal = (this->requireSmoothMovement ? delta.getNorme() + distanceNext : delta.getNorme() - stopAtDistance);
Alexandre Manoury
committed
// angles
float angleVise = atan2(delta.getY(),delta.getX());
if (m_reculer)
angleVise += M_PI;
Alexandre Manoury
committed
float diffAng = AngleTools::diffAngle(angleVise,angle);
// reste sur place tant que le robot n'a pas le bon angle
float angleMaxPourAvancer;
if (precisionAngle<0.)
angleMaxPourAvancer = M_PI/25.0f;//25.0f;
else
angleMaxPourAvancer = DEGTORAD(3.);//25.0f;
Alexandre Manoury
committed
lastDistance = 1000000.0f;
Alexandre Manoury
committed
else if (!fromSmoothMovement)
Alexandre Manoury
committed
// sharps
StrategieV2::setTourneSurSoiMeme((!bonAngle)&&(abs(linSpeed)<0.2f));
if (linSpeed > 0.2f)
StrategieV2::enableSharpsGroup(true);
else if (linSpeed < -0.2f)
StrategieV2::enableSharpsGroup(false);
else
StrategieV2::emptySharpsToCheck();
Alexandre Manoury
committed
bool distanceOk = /*(distanceBut > derniereDistance) || */(distanceFinal < 30.0f);
Alexandre Manoury
committed
/*if (bonAngle && precisionAngle > 0.)
Alexandre Manoury
committed
else */
if (fabs(diffAng) > angleVitesseMax / 10.f)
angleSmoothEnd = false;
/*else if (fabs(diffAng) < angleVitesseMax / 20.f)
angleSmoothEnd = true;*/
if (angleSmoothEnd)
{
angSpeed = diffAng*vitAngMax/angleVitesseMax;
}
else
Alexandre Manoury
committed
bool hasToDecelerate = (fabs(diffAng) < (angSpeed * angSpeed / accAngMax - accAngMax*2.));
if (diffAng > 0)
{
if (!hasToDecelerate)
angSpeed += accAngMax;
else if (angSpeed > accAngMax*2)
angSpeed -= accAngMax*2;
Alexandre Manoury
committed
else
angleSmoothEnd = true;
if (angSpeed > vitAngMax)
angSpeed = vitAngMax;
}
else
{
if (!hasToDecelerate)
angSpeed -= accAngMax;
else if (angSpeed < -accAngMax*2)
angSpeed += accAngMax*2;
Alexandre Manoury
committed
else
angleSmoothEnd = true;
if (angSpeed < -vitAngMax)
angSpeed = -vitAngMax;
}
}
// vitesse angulaire
/*if (abs(diffAng) > angleVitesseMax)
{
if (diffAng > 0)
angSpeed += accAngMax;
else
angSpeed -= accAngMax;
if (angSpeed > vitAngMax)
angSpeed = vitAngMax;
else if (angSpeed < -vitAngMax)
angSpeed = -vitAngMax;
}
else
{
angSpeed = diffAng*vitAngMax/angleVitesseMax;
}*/
Alexandre Manoury
committed
if (bonAngle || fromSmoothMovement)
Alexandre Manoury
committed
if (fabs(diffAng) > angleMaxPourAvancer && !isChangingToNext)
Alexandre Manoury
committed
if (!fromSmoothMovement)
linSpeed *= 0.97f;
Alexandre Manoury
committed
else if (isChangingToNext)
Alexandre Manoury
committed
float linSpeedVisee = (m_reculer ? -vitLinIntermediate : vitLinIntermediate);
if (abs(linSpeed) < abs(linSpeedVisee))
{
linSpeed += (m_reculer ? -accLinMax : accLinMax);
if (abs(linSpeed) > abs(linSpeedVisee))
linSpeed = linSpeedVisee;
}
else if (abs(linSpeed) > abs(linSpeedVisee))
{
linSpeed += (m_reculer ? accLinMax : -accLinMax);
if (abs(linSpeed) < abs(linSpeedVisee))
linSpeed = linSpeedVisee;
}
}
else if (distanceFinal > distanceVitesseMax)
{
linSpeed += (m_reculer ? -accLinMax : accLinMax);
//DebugWindow::getInstance()->plot(4, "status", 2, 10);
if (linSpeed > vitLinMax)
linSpeed = vitLinMax;
else if (linSpeed < -vitLinMax)
linSpeed = -vitLinMax;
Alexandre Manoury
committed
float linSpeedVisee = sqrt(vFin2+2.0f*distanceFinal*decLinMax);
Alexandre Manoury
committed
linSpeedVisee = -linSpeedVisee;
//DebugWindow::getInstance()->plot(4, "status", 2, 20);
Alexandre Manoury
committed
linSpeed += (m_reculer ? -accLinMax : accLinMax);
Alexandre Manoury
committed
//linSpeed = distanceBut * vitesseLinMax / distanceVitesseMax;
Alexandre Manoury
committed
if ((isChangingToNext && distanceIntermediate > lastDistance) || distanceFinal < 10.0f)
{
Alexandre Manoury
committed
mFinished = true;
Alexandre Manoury
committed
}
lastDistance = distanceIntermediate;
Alexandre Manoury
committed
void CommandAllerA::smoothMovement(Position nextGoal, float smoothFactor)
{
this->smoothFactor = smoothFactor;
this->nextGoal = nextGoal;
this->requireSmoothMovement = true;
markAsSmooth();
}