Wheeledbase.h

Aller à la documentation de ce fichier.

//
// Created by boris on 28/10/2024.
//
#ifndef WHEELEDBASE_H
#define WHEELEDBASE_H
 
#include <functional>
 
#include "WheelMotor.h"
#include "Components/Codewheel.h"
#include "Components/Odometry.h"
#include "Controller/PID.h"
#include "Controller/VelocityController.h"
#include "Controller/PositionController.h"
#include "MoveStrategy/PurePursuit.h"
#include "MoveStrategy/TurnOnTheSpot.h"
#include <memory>
 
#include "DRV8844.h"
#include "Logger.h"
 
// Parameters identifiers
#define SLOWDOWN_FACTOR                 0.5
#define SLOWDOWN_DISTANCE               0
#define ALIGN_DISTANCE                  130

namespace  Wheeledbase {

    struct WBConstants
    {
        const float LEFTWHEEL_RADIUS;
        const float LEFTWHEEL_CONSTANT;
        const float LEFTWHEEL_MAXPWM;
        const float RIGHTWHEEL_RADIUS;
        const float RIGHTWHEEL_CONSTANT;
        const float RIGHTWHEEL_MAXPWM;
        const long LEFTCODEWHEEL_COUNTSPERREV;
        const long RIGHTCODEWHEEL_COUNTSPERREV;
        const float LEFTCODEWHEEL_RADIUS;
        const float RIGHTCODEWHEEL_RADIUS;
 
        const float ODOMETRY_AXLETRACK;
        const float ODOMETRY_SLIPPAGE;
 
        const float VELOCITYCONTROL_AXLETRACK;
        const float VELOCITYCONTROL_MAXLINACC;
        const float VELOCITYCONTROL_MAXLINDEC;
        const float VELOCITYCONTROL_MAXANGACC;
        const float VELOCITYCONTROL_MAXANGDEC;
        const bool VELOCITYCONTROL_SPINSHUTDOWN;
 
        /*---- LINEAR VELOCITIES PID  ----*/
        const float LINVELPID_KP;
        const float LINVELPID_KI;
        const float LINVELPID_KD;
 
        const float LINVELPID_MINOUTPUT;
        const float LINVELPID_MAXOUTPUT;
 
        /*---- ANGULAR VELOCITIES PID ----*/
        const float ANGVELPID_KP;
        const float ANGVELPID_KI;
        const float ANGVELPID_KD;
 
        const float ANGVELPID_MINOUTPUT;
        const float ANGVELPID_MAXOUTPUT;
 
        /*---- POSITION PID ----*/
        const float POSITIONCONTROL_LINVELKP;
        const float POSITIONCONTROL_ANGVELKP;
 
        const float POSITIONCONTROL_LINVELMAX;
        const float POSITIONCONTROL_ANGVELMAX;
        const float POSITIONCONTROL_LINPOSTHRESHOLD;
        const float POSITIONCONTROL_ANGPOSTHRESHOLD;
        const float PUREPURSUIT_LOOKAHEAD;
        const float PUREPURSUIT_LOOKAHEADBIS;
 
        // Timesteps
        const float ODOMETRY_TIMESTEP;
        const float PID_CONTROLLERS_TIMESTEP;
        const float POSITIONCONTROL_TIMESTEP;
 
        // DC motors driver
        const int LEFT_MOTOR_EN;
        const int LEFT_MOTOR_PWM;
        const int LEFT_MOTOR_DIR;
        const int RIGHT_MOTOR_EN;
        const int RIGHT_MOTOR_PWM;
        const int RIGHT_MOTOR_DIR;
        const int DRIVER_RESET;
        const int DRIVER_FAULT;
    };

    struct WheeledBase {
        std::unique_ptr<WheelMotor> leftWheel;
        std::unique_ptr<WheelMotor> rightWheel;
 
        std::unique_ptr<Codewheel> leftCodewheel;
        std::unique_ptr<Codewheel> rightCodewheel;
 
        std::unique_ptr<Odometry> odometry;
 
        std::unique_ptr<VelocityController> velocityControl;
        std::unique_ptr<PositionController> positionControl;

        WheeledBase(
            std::unique_ptr<DRV8844> driver,
            std::unique_ptr<WheelMotor> leftWheel,
            std::unique_ptr<WheelMotor> rightWheel,
            std::unique_ptr<Codewheel> leftCodewheel,
            std::unique_ptr<Codewheel> rightCodewheel,
            std::unique_ptr<Odometry> odometry,
            std::unique_ptr<VelocityController> velocityControl,
            std::unique_ptr<PositionController> positionControl,
            std::unique_ptr<PID> linVelPID,
            std::unique_ptr<PID> angVelPID,
            WBConstants& param,
            const std::function<void()>& halSetup
            );
    };
 
    [[noreturn]] void run(WheeledBase* wheeledBase, const std::function<void()>& delayFunc);

    enum WBParameterID : uint8_t {
        WBPID_LEFTWHEEL_RADIUS_ID,
        WBPID_LEFTWHEEL_CONSTANT_ID,
        WBPID_LEFTWHEEL_MAXPWM_ID,
        WBPID_RIGHTWHEEL_RADIUS_ID,
        WBPID_RIGHTWHEEL_CONSTANT_ID,
        WBPID_RIGHTWHEEL_MAXPWM_ID,
        WBPID_LEFTCODEWHEEL_RADIUS_ID,
        WBPID_LEFTCODEWHEEL_COUNTSPERREV_ID,
        WBPID_RIGHTCODEWHEEL_RADIUS_ID,
        WBPID_RIGHTCODEWHEEL_COUNTSPERREV_ID,
        WBPID_ODOMETRY_AXLETRACK_ID,
        WBPID_ODOMETRY_SLIPPAGE_ID,
        WBPID_VELOCITYCONTROL_AXLETRACK_ID,
        WBPID_VELOCITYCONTROL_MAXLINACC_ID,
        WBPID_VELOCITYCONTROL_MAXLINDEC_ID,
        WBPID_VELOCITYCONTROL_MAXANGACC_ID,
        WBPID_VELOCITYCONTROL_MAXANGDEC_ID,
        WBPID_VELOCITYCONTROL_SPINSHUTDOWN_ID,
        WBPID_LINVELPID_KP_ID,
        WBPID_LINVELPID_KI_ID,
        WBPID_LINVELPID_KD_ID,
        WBPID_LINVELPID_MINOUTPUT_ID,
        WBPID_LINVELPID_MAXOUTPUT_ID,
        WBPID_ANGVELPID_KP_ID,
        WBPID_ANGVELPID_KI_ID,
        WBPID_ANGVELPID_KD_ID,
        WBPID_ANGVELPID_MINOUTPUT_ID,
        WBPID_ANGVELPID_MAXOUTPUT_ID,
        WBPID_POSITIONCONTROL_LINVELKP_ID,
        WBPID_POSITIONCONTROL_ANGVELKP_ID,
        WBPID_POSITIONCONTROL_LINVELMAX_ID,
        WBPID_POSITIONCONTROL_ANGVELMAX_ID,
        WBPID_POSITIONCONTROL_LINPOSTHRESHOLD_ID,
        WBPID_POSITIONCONTROL_ANGPOSTHRESHOLD_ID,
        WBPID_PUREPURSUIT_LOOKAHED_ID,
        WBPID_PUREPURSUIT_LOOKAHEADBIS_ID
    };

    inline auto wb_logger = Logger("WHEELEDBASE");
 
    // Instructions prototypes
    void disable(const WheeledBase* wheeledBase);

    void gotoDelta(WheeledBase* wheeledBase, PurePursuit* purePursuit, float dx, float dy, bool bloquant=true); //bloquant: do we pass at the next step after ?
 
    //void TURNTO_DELTA(WheeledBase* wheeledBase, float dtheta, bool bloquant=true);
 
    //void TENTATIVE_POUR_PLUSTARD();

    void setOpenloopVelocities(WheeledBase* wheeledBase, float leftWheelVel, float rightWheelVel);

    void getCodewheelsCounters(WheeledBase* wheeledBase, float *leftCodewheelCounter, float *rightCodewheelCounter);

    void setVelocities(WheeledBase* wheeledBase, float linVelSetpoint, float angVelSetpoint);

    void resetPurepursuit(WheeledBase* wheeledBase, PurePursuit* purePursuit);

    void addPurepursuitWaypoint(PurePursuit* purePursuit, float x, float y);

    void startPurepursuit(WheeledBase* wheeledBase, PurePursuit* purePursuit, PurePursuit::Direction direction, float finalAngle);

    void startTurnonthespot(WheeledBase* wheeledBase, TurnOnTheSpot* turnOnTheSpot, bool dir, float theta);

    void startTurnonthespotDir(const WheeledBase* wheeledBase, TurnOnTheSpot* turnOnTheSpot, TurnOnTheSpot::Direction dir, float theta);

    void purepursuit(WheeledBase* wheeledBase, PurePursuit* purePursuit, const Position** waypoints, uint16_t nb_waypoints, PurePursuit::Direction dir, float finalAngle);

    void wbGoto(WheeledBase* wheeledBase, PurePursuit* purePursuit, Position* pos, bool alignFirst=true, PurePursuit::Direction dir=PurePursuit::NONE,bool bloquant=true);

    void gotoWaypoints(WheeledBase* wheeledBase, PurePursuit* purePursuit, bool alignFirst=true, PurePursuit::Direction dir=PurePursuit::NONE, int nb_waypoints=1, ...);

    void gotoWaypointsArray(WheeledBase* wheeledBase, PurePursuit* purePursuit, Position* positions[], bool alignFirst=true, PurePursuit::Direction dir=PurePursuit::NONE, int nb_waypoints=1);

    void gotoFunct(WheeledBase* wheeledBase, PurePursuit* purePursuit, TurnOnTheSpot* turnOnTheSpot, Position* pos, void* duringMovingFunct, void* approachFunct, bool alignFirst=true, PurePursuit::Direction dir=PurePursuit::NONE, float finalAngle=MAXFLOAT);

    uint8_t positionReached(const WheeledBase* wheeledBase);

    void getVelocitiesWanted(const WheeledBase* wheeledBase, float *linOutput, float *angOutput, bool spin=true);

    void setPosition(WheeledBase* wheeledBase, const Position* pos);

    const Position* getPosition(const WheeledBase* wheeledBase);

    void getVelocities(const WheeledBase* wheeledBase, float *linVel, float *angVel);

    void setParameterValue(WheeledBase* wheeledBase, PurePursuit* purePursuit, WBParameterID paramID, float value);

    float getParameterValue(const WheeledBase* wheeledBase, const PurePursuit* purePursuit, WBParameterID paramID);

    void printParams(const WheeledBase* wheeledBase, const PurePursuit* purePursuit);
};
 
 
 
#endif //WHEELEDBASE_H