dock.cpp

#include "dock.h"
#include <ros/ros.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>

double RobotDocking::refVal = .81; // add a ref. value 

double RobotDocking::FORWARD_SPEED_MPS = .05;

double RobotDocking::prev_error = 0;
double RobotDocking::curr_error = 0;
double RobotDocking::int_error = 0;
double RobotDocking::diff = 0;

double RobotDocking::p_term = 0;
double RobotDocking::i_term = 0;
double RobotDocking::d_term = 0;

double RobotDocking::prop_gain = 0.1;
double RobotDocking::integ_gain = 0.1;
double RobotDocking::deriv_gain = 0;

double RobotDocking::control_signal = 0;

//constructor declaration
RobotDocking::RobotDocking() : ac("Robot_move", true)
{
        keepMoving = true;
        
        commandPub = node.advertise<geometry_msgs::Twist>("/base_controller/command",1);
        
        PosSub = node.subscribe("/visualize_pose_ekf",1,&RobotDocking::RobCurrPose,this);
        

}


void RobotDocking::ProgStart()
{
        ros::Rate rate(100);
        
        while (ros::ok() && keepMoving)
        {
                //dock(FORWARD_SPEED_MPS);
                ros::spinOnce();
                rate.sleep();
        }

}

void RobotDocking::Controller(double Reference, double RobPose, double dt)
{
        // e(t) = setpoint - actual value;
        curr_error = Reference - RobPose;
        
        // Integrated error
        int_error +=  curr_error * dt;
        
        /*
        // -- windup gaurd -- 
        if (int_error < )
        {}
        else if ()
        {}
        */