NTRT Simulator  v1.1
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgSineStringControl.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 "tgSineStringControl.h"
20 
21 #include "core/tgBasicActuator.h"
23 
24 #include <iostream>
25 #include <stdexcept>
26 #include <cmath>
27 
29  tgImpedanceController* p_ipc,
30  const double amplitude,
31  const double frequency,
32  const double phase,
33  const double offset,
34  const double length) :
35 m_controlTime(0.0),
36 m_totalTime(0.0),
37 m_controlStep(controlStep),
38 m_commandedTension(0.0),
39 cpgAmplitude(amplitude),
40 cpgFrequency(frequency),
41 phaseOffset(phase),
42 offsetSpeed(offset),
43 cycle(0.0),
44 target (0.0),
45 m_controlLength(length),
46 m_pMotorControl(p_ipc)
47 {
48  if (m_controlStep < 0.0)
49  {
50  throw std::invalid_argument("Negative control step");
51  }
52 
54 
55  assert(p_ipc);
56 }
57 
58 tgSineStringControl::~tgSineStringControl()
59 {
60 
61 }
62 
64 {
65  m_controlTime += dt;
66  m_totalTime += dt;
69  // Workaround until we implement PID
70  tgBasicActuator& m_sca = *(tgCast::cast<tgSpringCableActuator, tgBasicActuator>(subject));
71 
72 
73  if (m_controlTime >= m_controlStep)
74  {
75  // Yep, its a misnomer. Had to change it for In Won
76  cycle = cos(m_totalTime * 2.0 * M_PI * cpgFrequency + phaseOffset);
77  target = cycle*cpgAmplitude + offsetSpeed;
78  #if (0)
79  if (phaseOffset == 0.0 && m_totalTime < 4.0)
80  {
81  target = 0.0;
82  }
83  #endif
84  m_commandedTension = m_pMotorControl->control(m_sca, m_controlTime, m_controlLength, target);
85  //std::cout << m_commandedTension << std::endl;
86  m_controlTime = 0;
87  }
88 /*
89  else
90  {
91  subject.moveMotors(dt);
92  }
93  */
94 }
Contains the definition of class ImpedanceControl. $Id$.
virtual void onStep(tgSpringCableActuator &subject, double dt)
Contains the definition of class tgBasicActuator.
tgSineStringControl(const double controlStep, tgImpedanceController *p_ipc, const double amplitude, const double frequency, const double phase, const double offset, const double length)
double control(tgBasicController &mLocalController, double deltaTimeSeconds, double newPosition, double offsetVel=0)
Control Functions.