SoftTurnController.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */

#ifndef SKID4_CONTROL_SOFTTURNCONTROLLER_TASK_HPP
#define SKID4_CONTROL_SOFTTURNCONTROLLER_TASK_HPP

#include "skid4_control/SoftTurnControllerBase.hpp"
#include <motor_controller/PID.hpp>
#include <base/samples/Joints.hpp>

namespace skid4_control {
    class SoftTurnController : public SoftTurnControllerBase
    {
        friend class SoftTurnControllerBase;
    protected:
        enum State {
            FORWARD,
            BACKWARD, 
        };
        
        double turnSpeed;
        double translationalSpeed;
        State state;
        double turnVariance;

        double Kp;
        double Ki;
        double Kd; 
        double N;
        double B; 
        double Tt;
        double YMin; 
        double YMax;

        
        motor_controller::PID leftPid;
        motor_controller::PID rightPid;
        base::samples::Joints lastStatus;

        base::samples::Joints startStatus;

        double leftController(double wantedSpeed, const base::samples::Joints &status);
        double rightController(double wantedSpeed, const base::samples::Joints &status);

        double getSpeed(const base::samples::Joints& status, const std::vector< std::string >& joints);
        
    public:
        SoftTurnController(std::string const& name = "skid4_control::SoftTurnController");

        ~SoftTurnController();

        bool configureHook();

        // bool startHook();

        void updateHook();

        // void errorHook();

        // void stopHook();

        // void cleanupHook();
    };
}

#endif