NTRT Simulator  Version: Master
 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 
23 #include "core/tgBasicActuator.h"
26 #include "core/tgCast.h"
27 #include "util/CPGEquations.h"
28 #include "util/CPGEquationsFB.h"
29 
30 // The C++ Standard Library
31 #include <iostream>
32 #include <stdexcept>
33 #include <vector>
34 
35 tgCPGCableControl::tgCPGCableControl(tgPIDController::Config pid_config, const double controlStep) :
36 m_config(pid_config),
37 tgCPGActuatorControl(controlStep),
38 m_PID(NULL),
39 usePID(true)
40 {
41  if (m_controlStep < 0.0)
42  {
43  throw std::invalid_argument("Negative control step");
44  }
45 }
46 
47 tgCPGCableControl::~tgCPGCableControl()
48 {
49  delete m_PID;
50 }
51 
53 {
54  m_PID = new tgPIDController(&subject, m_config);
55 
56  tgBasicActuator* basicAct = tgCast::cast<tgSpringCableActuator, tgBasicActuator>(&subject);
57 
58  if (basicAct != NULL)
59  {
60  usePID = false;
61  }
62 }
63 
65 {
66  assert(&subject == m_PID->getControllable());
67 
68  m_controlTime += dt;
69  m_totalTime += dt;
70 
71  if (m_controlTime >= m_controlStep)
72  {
73  if (usePID)
74  {
75  m_commandedTension = motorControl().control(*m_PID, dt, controlLength(), getCPGValue());
76  }
77  else
78  {
79  tgBasicActuator* basicAct = tgCast::cast<tgSpringCableActuator, tgBasicActuator>(&subject);
80  m_commandedTension = motorControl().control(*basicAct, dt, controlLength(), getCPGValue());
81  }
82  m_controlTime = 0;
83  }
84  else
85  {
86  const double currentTension = subject.getTension();
87  if(usePID)
88  {
89  m_PID->control(dt, m_commandedTension, currentTension);
90  }
91  else
92  {
93  tgBasicActuator* basicAct = tgCast::cast<tgSpringCableActuator, tgBasicActuator>(&subject);
94  basicAct->moveMotors(dt);
95  }
96  }
97 }
98 
99 void tgCPGCableControl::assignNodeNumberFB (CPGEquationsFB& CPGSys, array_2D nodeParams)
100 {
101  // Ensure that this hasn't already been assigned
102  assert(m_nodeNumber == -1);
103 
104  m_pCPGSystem = &CPGSys;
105 
106  std::vector<double> params (11);
107  params[0] = nodeParams[0][0]; // Frequency Offset
108  params[1] = nodeParams[0][0]; // Frequency Scale
109  params[2] = nodeParams[0][1]; // Radius Offset
110  params[3] = nodeParams[0][1]; // Radius Scale
111  params[4] = 1.0; // rConst (a constant)
112  params[5] = 0.0; // dMin for descending commands
113  params[6] = 5.0; // dMax for descending commands
114  params[6] = 5.0; // dMax for descending commands
115  params[7] = nodeParams[0][0]; // Omega (initialize variable)
116  params[8] = nodeParams[0][2]; // Frequency feedback
117  params[9] = nodeParams[0][3]; // Amplitude feedback
118  params[10] = nodeParams[0][4]; // Phase feedback
119 
120  m_nodeNumber = CPGSys.addNode(params);
121 }
Contains the definition of class ImpedanceControl. $Id$.
virtual void moveMotors(double dt)
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.
virtual void control(double dt)
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
const tgControllable *const getControllable() const
Contains the definition of class tgBasicActuator.
Definitions of class tgBulletSpringCableAnchor, formerly muscleAnchor.
Definition of class CPGEquations.
Definition of class CPGEquationsFB.
double control(tgBasicController &mLocalController, double deltaTimeSeconds, double newPosition, double offsetVel=0)
Control Functions.
Definition of the tgPIDController class.
void assignNodeNumberFB(CPGEquationsFB &CPGSys, array_2D nodeParams)