.. _program_listing_file_src_LouLib_Controllers_RamseteController.cpp:

Program Listing for File RamseteController.cpp
==============================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_LouLib_Controllers_RamseteController.cpp>` (``src/LouLib/Controllers/RamseteController.cpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   
   #include "RamseteController.hpp"
   
   namespace LouLib {
       namespace Controllers {
   
           RamseteController::RamseteController(double b, double zeta) : b(b), zeta(zeta) {}
   
           void RamseteController::computeControl(Paths::Trajectory& t, int idx, Math::Pose2D robotPose) {
               double targX = t.getX(idx).to(Units::METER);
               double targY = t.getY(idx).to(Units::METER);
               double targTheta = t.getTheta(idx).to(Units::RADIAN);
               double targV = t.getVelocity(idx).to(Units::METER_PER_SECOND);
               double targOmega = t.getAngularVelocity(idx).to(Units::RADIAN_PER_SECOND);
   
               double robotX = robotPose.getX().to(Units::METER);
               double robotY = robotPose.getY().to(Units::METER);
               double robotTheta = robotPose.getTheta().to(Units::RADIAN);
   
               double targTheta_deg = t.getTheta(idx).to(Units::DEGREE);
               double robotTheta_deg = robotPose.getTheta().to(Units::DEGREE);
   
               double deltaX = targX - robotX;
               double deltaY = targY - robotY;
               double deltaTheta = (Math::angleDifference(targTheta_deg, robotTheta_deg)*Units::DEGREE).to(Units::RADIAN);
   
               if(std::abs(deltaTheta) < Math::EPS) deltaTheta += 0.0001;
   
               double ex = std::cos(robotTheta)*deltaX + std::sin(robotTheta)*deltaY;
               double ey = -std::sin(robotTheta)*deltaX + std::cos(robotTheta)*deltaY;
               double et = deltaTheta;
   
               double k = 2*zeta*std::sqrt(targOmega*targOmega + b*targV*targV);
   
               v = (targV*std::cos(et) + k*ex) * Units::METER_PER_SECOND;
               omega = (targOmega + k*et + b*targV*std::sin(et)*ey/et) * Units::RADIAN_PER_SECOND;
           }
   
           const Units::Velocity &RamseteController::getV() const {
               return v;
           }
   
           const Units::AngularVelocity &RamseteController::getOmega() const {
               return omega;
           }
   
   
       } // LouLib
   } // Controllers