DesiredStateCommand< T > Class Template Reference

#include <DesiredStateCommand.h>

Inheritance diagram for DesiredStateCommand< T >:

Collaboration diagram for DesiredStateCommand< T >:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	DesiredStateCommand (GamepadCommand *command, StateEstimate< T > *sEstimate)
void	convertToStateCommands ()
void	desiredStateTrajectory (int N, Vec10< T > dtVec)
void	printRawInfo ()
void	printStateCommandInfo ()
float	deadband (float command, T minVal, T maxVal)

Public Attributes
T	maxRoll = 0.4
T	minRoll = -0.4
T	maxPitch = 0.4
T	minPitch = -0.4
T	maxVelX = 1.0
T	minVelX = -1.0
T	maxVelY = 0.5
T	minVelY = -0.5
T	maxTurnRate = 2.0
T	minTurnRate = -2.0
DesiredStateData< T >	data

Private Attributes
GamepadCommand *	gamepadCommand
StateEstimate< T > *	stateEstimate
Mat12< T >	A
T	dt = 0.001
T	deadbandRegion = 0.075
int	printNum = 5
int	printIter = 0

Detailed Description

template<typename T>
class DesiredStateCommand< T >

Definition at line 35 of file DesiredStateCommand.h.

Constructor & Destructor Documentation

template<typename T>

EIGEN_MAKE_ALIGNED_OPERATOR_NEW DesiredStateCommand< T >::DesiredStateCommand ( GamepadCommand *  command, StateEstimate< T > *  sEstimate  )

inline

Definition at line 39 of file DesiredStateCommand.h.

                                                                            {
    gamepadCommand = command;
    stateEstimate = sEstimate;
  }

Member Function Documentation

template<typename T >

void DesiredStateCommand< T >::convertToStateCommands

(

)

Definition at line 26 of file DesiredStateCommand.cpp.

References deadband().

                                                    {
  data.zero();

  // Forward linear velocity
  data.stateDes(6) =
      deadband(gamepadCommand->leftStickAnalog[1], minVelX, maxVelX);

  // Lateral linear velocity
  data.stateDes(7) =
      deadband(gamepadCommand->leftStickAnalog[0], minVelY, maxVelY);

  // VErtical linear velocity
  data.stateDes(8) = 0.0;

  // X position
  data.stateDes(0) = stateEstimate->position(0) + dt * data.stateDes(6);

  // Y position
  data.stateDes(1) = stateEstimate->position(1) + dt * data.stateDes(7);

  // Z position height
  data.stateDes(2) = 0.45;

  // Roll rate
  data.stateDes(9) = 0.0;

  // Pitch rate
  data.stateDes(10) = 0.0;

  // Yaw turn rate
  data.stateDes(11) =
      deadband(gamepadCommand->rightStickAnalog[0], minTurnRate, maxTurnRate);

  // Roll
  data.stateDes(3) = 0.0;

  // Pitch
  data.stateDes(4) =
      deadband(gamepadCommand->rightStickAnalog[1], minPitch, maxPitch);

  // Yaw
  data.stateDes(5) = stateEstimate->rpy(2) + dt * data.stateDes(11);
}

Here is the call graph for this function:

template<typename T>

float DesiredStateCommand< T >::deadband	(	float	command,
		T	minVal,
		T	maxVal
	)

Definition at line 110 of file DesiredStateCommand.cpp.

                                                                        {
  if (command < deadbandRegion && command > -deadbandRegion) {
    return 0.0;
  } else {
    return (command / (2)) * (maxVal - minVal);
  }
}

template<typename T>

void DesiredStateCommand< T >::desiredStateTrajectory	(	int	N,
		Vec10< T >	dtVec
	)

Definition at line 74 of file DesiredStateCommand.cpp.

                                                                         {
  A = Mat12<T>::Zero();
  A(0, 0) = 1;
  A(1, 1) = 1;
  A(2, 2) = 1;
  A(3, 3) = 1;
  A(4, 4) = 1;
  A(5, 5) = 1;
  A(6, 6) = 1;
  A(7, 7) = 1;
  A(8, 8) = 1;
  A(9, 9) = 1;
  A(10, 10) = 1;
  A(11, 11) = 1;
  data.stateTrajDes.col(0) = data.stateDes;

  for (int k = 1; k < N; k++) {
    A(0, 6) = dtVec(k - 1);
    A(1, 7) = dtVec(k - 1);
    A(2, 8) = dtVec(k - 1);
    A(3, 9) = dtVec(k - 1);
    A(4, 10) = dtVec(k - 1);
    A(5, 11) = dtVec(k - 1);
    data.stateTrajDes.col(k) = A * data.stateTrajDes.col(k - 1);
    for (int i = 0; i < 12; i++) {
      // std::cout << data.stateTrajDes(i, k) << " ";
    }
    // std::cout << std::endl;
  }
  // std::cout << std::endl;
}

template<typename T >

void DesiredStateCommand< T >::printRawInfo

(

)

Definition at line 122 of file DesiredStateCommand.cpp.

                                          {
  // Increment printing iteration
  printIter++;

  // Print at requested frequency
  if (printIter == printNum) {
    std::cout << "[DESIRED STATE COMMAND] Printing Raw Gamepad Info...\n";
    std::cout << "---------------------------------------------------------\n";
    std::cout << "Button Start: " << gamepadCommand->start
              << " | Back: " << gamepadCommand->back << "\n";
    std::cout << "Button A: " << gamepadCommand->a
              << " | B: " << gamepadCommand->b << " | X: " << gamepadCommand->x
              << " | Y: " << gamepadCommand->y << "\n";
    std::cout << "Left Stick Button: " << gamepadCommand->leftStickButton
              << " | X: " << gamepadCommand->leftStickAnalog[0]
              << " | Y: " << gamepadCommand->leftStickAnalog[1] << "\n";
    std::cout << "Right Analog Button: " << gamepadCommand->rightStickButton
              << " | X: " << gamepadCommand->rightStickAnalog[0]
              << " | Y: " << gamepadCommand->rightStickAnalog[1] << "\n";
    std::cout << "Left Bumper: " << gamepadCommand->leftBumper
              << " | Trigger Switch: " << gamepadCommand->leftTriggerButton
              << " | Trigger Value: " << gamepadCommand->leftTriggerAnalog
              << "\n";
    std::cout << "Right Bumper: " << gamepadCommand->rightBumper
              << " | Trigger Switch: " << gamepadCommand->rightTriggerButton
              << " | Trigger Value: " << gamepadCommand->rightTriggerAnalog
              << "\n\n";
    std::cout << std::endl;

    // Reset iteration counter
    printIter = 0;
  }
}

template<typename T >

void DesiredStateCommand< T >::printStateCommandInfo

(

)

Definition at line 160 of file DesiredStateCommand.cpp.

                                                   {
  // Increment printing iteration
  printIter++;

  // Print at requested frequency
  if (printIter == printNum) {
    std::cout << "[DESIRED STATE COMMAND] Printing State Command Info...\n";
    std::cout << "---------------------------------------------------------\n";
    std::cout << "Position X: " << data.stateDes(0)
              << " | Y: " << data.stateDes(1) << " | Z: " << data.stateDes(2)
              << "\n";
    std::cout << "Orientation Roll: " << data.stateDes(3)
              << " | Pitch: " << data.stateDes(4)
              << " | Yaw: " << data.stateDes(5) << "\n";
    std::cout << "Velocity X: " << data.stateDes(6)
              << " | Y: " << data.stateDes(7) << " | Z: " << data.stateDes(8)
              << "\n";
    std::cout << "Angular Velocity X: " << data.stateDes(9)
              << " | Y: " << data.stateDes(10) << " | Z: " << data.stateDes(11)
              << "\n";
    std::cout << std::endl;
    std::cout << std::endl;

    // Reset iteration counter
    printIter = 0;
  }
}

Member Data Documentation

template<typename T>

Mat12<T> DesiredStateCommand< T >::A

private

Definition at line 70 of file DesiredStateCommand.h.

template<typename T>

DesiredStateData<T> DesiredStateCommand< T >::data

Definition at line 63 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::deadbandRegion = 0.075

private

Definition at line 76 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::dt = 0.001

private

Definition at line 73 of file DesiredStateCommand.h.

template<typename T>

GamepadCommand* DesiredStateCommand< T >::gamepadCommand

private

Definition at line 66 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::maxPitch = 0.4

Definition at line 53 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::maxRoll = 0.4

Definition at line 51 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::maxTurnRate = 2.0

Definition at line 59 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::maxVelX = 1.0

Definition at line 55 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::maxVelY = 0.5

Definition at line 57 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::minPitch = -0.4

Definition at line 54 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::minRoll = -0.4

Definition at line 52 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::minTurnRate = -2.0

Definition at line 60 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::minVelX = -1.0

Definition at line 56 of file DesiredStateCommand.h.

template<typename T>

T DesiredStateCommand< T >::minVelY = -0.5

Definition at line 58 of file DesiredStateCommand.h.

template<typename T>

int DesiredStateCommand< T >::printIter = 0

private

Definition at line 82 of file DesiredStateCommand.h.

template<typename T>

int DesiredStateCommand< T >::printNum = 5

private

Definition at line 79 of file DesiredStateCommand.h.

template<typename T>

StateEstimate<T>* DesiredStateCommand< T >::stateEstimate

private

Definition at line 67 of file DesiredStateCommand.h.

---

The documentation for this class was generated from the following files:

• DesiredStateCommand.h
• DesiredStateCommand.cpp