NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgCPGCableControl.cpp
1 /*
2  * Copyright © 2012, United States Government, as represented by the
3  * Administrator of the National Aeronautics and Space Administration.
4  * All rights reserved.
5  *
6  * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
7  * under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0.
11  *
12  * Unless required by applicable law or agreed to in writing,
13  * software distributed under the License is distributed on an
14  * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
15  * either express or implied. See the License for the specific language
16  * governing permissions and limitations under the License.
17 */
18 
19 #include "tgCPGCableControl.h"
20 
25 #include "core/tgCast.h"
26 #include "util/CPGEquations.h"
28 
29 // The C++ Standard Library
30 #include <iostream>
31 #include <stdexcept>
32 #include <vector>
33 
34 tgCPGCableControl::tgCPGCableControl(tgPIDController::Config pid_config, const double controlStep) :
35 m_config(pid_config),
36 tgCPGActuatorControl(controlStep),
37 m_PID(NULL)
38 {
39  if (m_controlStep < 0.0)
40  {
41  throw std::invalid_argument("Negative control step");
42  }
43 }
44 
45 tgCPGCableControl::~tgCPGCableControl()
46 {
47  delete m_PID;
48 }
49 
51 {
52  m_PID = new tgPIDController(&subject, m_config);
53 }
54 
56 {
57  assert(&subject == m_PID->getControllable());
58 
59  m_controlTime += dt;
60  m_totalTime += dt;
61 
62  if (m_controlTime >= m_controlStep)
63  {
64 
65  m_commandedTension = motorControl().control(*m_PID, dt, controlLength(), getCPGValue());
66 
67  m_controlTime = 0;
68  }
69  else
70  {
71  const double currentTension = subject.getTension();
72  m_PID->control(dt, m_commandedTension, currentTension);
73  }
74 }
75 
76 void tgCPGCableControl::assignNodeNumberFB (CPGEquationsFB& CPGSys, array_2D nodeParams)
77 {
78  // Ensure that this hasn't already been assigned
79  assert(m_nodeNumber == -1);
80 
81  m_pCPGSystem = &CPGSys;
82 
83  std::vector<double> params (11);
84  params[0] = nodeParams[0][0]; // Frequency Offset
85  params[1] = nodeParams[0][0]; // Frequency Scale
86  params[2] = nodeParams[0][1]; // Radius Offset
87  params[3] = nodeParams[0][1]; // Radius Scale
88  params[4] = 1.0; // rConst (a constant)
89  params[5] = 0.0; // dMin for descending commands
90  params[6] = 5.0; // dMax for descending commands
91  params[6] = 5.0; // dMax for descending commands
92  params[7] = nodeParams[0][0]; // Omega (initialize variable)
93  params[8] = nodeParams[0][2]; // Frequency feedback
94  params[9] = nodeParams[0][3]; // Amplitude feedback
95  params[10] = nodeParams[0][4]; // Phase feedback
96 
97  m_nodeNumber = CPGSys.addNode(params);
98 }
Contains the definition of class ImpedanceControl. $Id$.
virtual const double getTension() const
virtual void onStep(tgSpringCableActuator &subject, double dt)
virtual void onSetup(tgSpringCableActuator &subject)
Utility class for class casting and filtering collections by type.
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
Definitions of class tgBulletSpringCableAnchor, formerly muscleAnchor.
Definition of class CPGEquations.
Definition of class CPGEquationsFB.
Definition of the tgPIDController class.
void assignNodeNumberFB(CPGEquationsFB &CPGSys, array_2D nodeParams)