CscSequencePositionStrategy.h

Go to the documentation of this file.

#ifndef CscSequencePositionStrategy_H_
#define CscSequencePositionStrategy_H_
 
#include "./CoreObjects/CscSequencePositions.h"
#include "./CoreObjects/CscWorldEntity.h"
#include "CscAbstractSequenceProcessingStrategy.h"
 
namespace conscience_core::sequences_processor {
using std::fstream;
 
class CscSequencePositionStrategy : public CscAbstractSequenceProcessingStrategy {
private:
    CscSequencePositions *getSequencePositions();
 
protected:
    void computeWheelPower();
    void updateCurrentEntityInformations();
 
    void computeSpeedPowerFromTwoPositionAndDelta(CscPoint3d *currentPosition, CscPoint3d *firstPosition, CscPoint3d *secondPosition, double delta);
    double computeDistanceBetweenTwoPoint(CscPoint3d *firstPoint, CscPoint3d *secondPoint);
    void setReferenceStartForComputeSpeed(CscPoint3d *position);
 
    bool entityArrivedToTarget(CscPoint3d *currentPosition, CscPoint3d *projectedPosition, CscPoint3d *lastTarget, CscPoint3d *target, double distance);
    void entityForwardOrBackward(double speed, double delta);
    void entityTurn(double delta, double val, bool backward = false);
 
    CscState *getNewStateWithWheelPower(double leftPower, double rightPower);
 
    double positionErrorOnLine(CscPoint3d *currentPos, CscPoint3d *linePoint1, CscPoint3d *linePoint2);
    void enslavementWheelPower(double speed, double error);
    double computeEnslavementError(CscPoint3d *position, double heading, CscPoint3d *linePoint1, CscPoint3d *linePoint2);
    CscPoint3d getProjectedPositionOnLine(CscPoint3d *position, CscPoint3d *linePoint1, CscPoint3d *linePoint2);
 
    double getYawInDegreesFromSimulatorPositionAndRotation(CscPoint3dOriented *positionAndRotation);
 
    void updateDeltaAndPositionError();
 
    int index = 0;
    CscPoint3d *currentPosition = new CscPoint3d(0, 0, 0);
    CscPoint3d *positionForComputeSpeed = nullptr;
    CscPoint3d *targetPosition = nullptr;
    CscPoint3d *lastTargetPosition = nullptr;
    CscPoint3d *lastLastTargetPosition = nullptr;
    CscPoint3d *referenceStart = new CscPoint3d(0, 0, 0);
    CscPoint3d *referenceStartForComputeSpeed = new CscPoint3d(0.0, 0.0, 0.0);
    double lastCorrection = 0;
 
    double maxCorrectionStep = 1;
 
    double yaw = 0.0;
    double accuracy = 0.0;
    double powerLeft = 0.0;
    double powerRight = 0.0;
 
    double currentTurnSpeed = 0.0f;
    double currentForwardSpeed = 0.0f;
 
    double delta = 0;
    double deltaPosition = 0;
 
    bool backwardRequest = false;
 
    double previousError = 0.0;
    double integral = 0.0;
 
    const double DISTANCE_FOR_ERROR_PREDICTION_IN_CM = 10.0;
 
public:
    CscSequencePositionStrategy(CscSequence *globalSequence, const string &worldEntityId, CscEnvironmentSimulator *simulator);
    ~CscSequencePositionStrategy() override;
    void process() override;
    bool isProcessed() override;
    void lastProcess() override;
    int getSequenceIndex() override;
    void breakSequence() override;
    void terminateSequence() override;
};
}
 
#endif