← Back to branch summary

~robot3d-team/robot3d/trunk

« back to all changes in this revision

Viewing changes to src/srActuator/screwDrive.cpp

  • Committer: Anne van Rossum
  • Date: 2010-08-10 15:58:55 UTC
  • Revision ID: anne@gamix-20100810155855-kve7x2vwouagdij9
Initial import

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/srActuator/screwDrive.cpp
 
1
/*
 
2
 * ScrewDrive.cpp
 
3
 *
 
4
 *  Created on: Nov 18, 2008
 
5
 *      Author: winkler
 
6
 */
 
7
 
 
8
#include <srActuator/screwDrive.h>
 
9
#include <srCore/actuator/hingeActuator.h>
 
10
#include <stdexcept>
 
11
#include <cmath>
 
12
 
 
13
#include <dtUtil/log.h>
 
14
 
 
15
#define MAX(X,Y) (X>Y?X:Y)
 
16
#define MIN(X,Y) (X<Y?X:Y)
 
17
 
 
18
namespace srCore {
 
19
 
 
20
 
 
21
ScrewDrive::ScrewDrive(const std::string name):
 
22
        ActuatorBase(name, "ScrewDrive")
 
23
{
 
24
        
 
25
        d_m = 0.0155;
 
26
        my_Screw = 0.1;
 
27
        l = 0.0099;
 
28
 
 
29
        //speed is here the torque *gulp*
 
30
        F_div_M = d_m/2.0 * ((l + M_PI*my_Screw*d_m) / (M_PI*d_m - my_Screw*l));
 
31
 
 
32
}
 
33
 
 
34
ScrewDrive::~ScrewDrive() {
 
35
        // TODO Auto-generated destructor stub
 
36
 
 
37
 
 
38
        frontActuator = NULL;
 
39
        backActuator = NULL;
 
40
}
 
41
 
 
42
void ScrewDrive::registerActuator() {
 
43
 
 
44
        ActuatorBase::jointActuatorMap.insert(std::pair<dJointID, ActuatorBase*>(frontActuator->getHinge(), frontActuator));
 
45
        ActuatorBase::jointActuatorMap.insert(std::pair<dJointID, ActuatorBase*>(backActuator->getHinge(), backActuator));
 
46
 
 
47
 
 
48
}
 
49
 
 
50
 
 
51
 
 
52
void ScrewDrive::setVel(float v, float deltaTime)
 
53
{
 
54
        velocity = v;
 
55
 
 
56
        float speedLeft, speedRight;
 
57
 
 
58
        if (secondScrewDrive != NULL) {
 
59
                if (this->isOnRightHandSide) {
 
60
                        speedRight = v;
 
61
                        speedLeft = secondScrewDrive->getVel(deltaTime);
 
62
                }
 
63
                else {
 
64
                        speedLeft = v;
 
65
                        speedRight = secondScrewDrive->getVel(deltaTime);
 
66
                }
 
67
 
 
68
                std::ostringstream msg; msg.clear(); msg.str("");
 
69
                msg << "Speed [left,right] = [" << speedLeft << "," << speedRight << "]";
 
70
                LOG_DEBUG(msg.str());
 
71
 
 
72
                float speedTrans = std::min(fabs(speedLeft), fabs(speedRight));
 
73
                float speedRot = fabs(speedLeft - speedRight);
 
74
                float sum = speedTrans + speedRot; // removed spurious calls to abs() below: both trans & rot speeds guaranteed positive now
 
75
                if (sum != 0) {
 
76
                        speedTrans = speedTrans / sum;
 
77
                        speedRot = speedRot / sum;
 
78
                }
 
79
 
 
80
                //std::cout << "v right =" << speedRight << ", v left = " << speedLeft <<std::endl;
 
81
                // actual super position of the movement
 
82
                if (this->isOnRightHandSide) {
 
83
 
 
84
                        speedRight *= 0.01;
 
85
                        speedLeft *= 0.01;
 
86
 
 
87
                        dJointID f0 = getHinge(0);
 
88
                        dJointID f1 = getHinge(1);
 
89
                        dJointID b0 = secondScrewDrive->getHinge(0);
 
90
                        dJointID b1 = secondScrewDrive->getHinge(1);
 
91
 
 
92
                        dBodyID frontBody = dJointGetBody( f0, 1);
 
93
//notused                       dBodyID backBody = dJointGetBody( b0, 1);
 
94
 
 
95
                        dVector3 pos0;
 
96
                        dJointGetHingeAnchor (f0, pos0);
 
97
                        dVector3 pos1;
 
98
                        dJointGetHingeAnchor (f1, pos1);
 
99
 
 
100
                        dVector3 pos2;
 
101
                        dJointGetHingeAnchor (b0, pos2);
 
102
                        dVector3 pos3;
 
103
                        dJointGetHingeAnchor (b1, pos3);
 
104
 
 
105
                        
 
106
                        float F_Screw_left = speedLeft * F_div_M;
 
107
                        float F_Screw_right = -speedRight * F_div_M;
 
108
 
 
109
                        float F_Roll_left = 2 * speedLeft / d_m;
 
110
                        float F_Roll_right = 2 * speedRight / d_m;
 
111
 
 
112
                        float F_Screw = 0;
 
113
                        if (F_Screw_left * F_Screw_right > 0) F_Screw = 2 * MIN(F_Screw_left, F_Screw_right);
 
114
 
 
115
                        float F_Roll = 0;
 
116
                        if (F_Roll_left * F_Roll_right > 0) F_Roll = 2 * MIN(F_Roll_left, F_Roll_right);
 
117
 
 
118
                        std::ostringstream msg; msg.clear(); msg.str("");
 
119
                        msg << "F_Roll = " << F_Roll << ", F_Screw = " << F_Screw;
 
120
                        msg << "Position = " << osg::Vec3((pos0[0] + pos1[0]) / 2, (pos0[1] + pos1[1]) / 2, (pos0[2] + pos1[2]) / 2);
 
121
                        LOG_DEBUG(msg.str());
 
122
 
 
123
                        //dBodyAddRelForceAtPos    (frontBody, 0, F_Roll, F_Screw*100, (pos0[0] + pos1[0]) / 2, (pos0[1] + pos1[1]) / 2, (pos0[2] + pos1[2]) / 2);
 
124
                        dBodyAddRelForceAtRelPos    (frontBody, 0, 0, F_Roll,0,0,0);//, F_Screw*100);
 
125
                        //dBodyAddRelForceAtPos    (backBody, 0, F_Roll, -F_Screw*100, (pos2[0] + pos3[0]) / 2, (pos2[1] + pos3[1]) / 2, (pos2[2] + pos3[2]) / 2);
 
126
                        //dBodyAddRelForce    (backBody, 0, 0, -F_Roll);//, F_Screw*100);
 
127
 
 
128
 
 
129
//                      frontActuator->setODEParameter(dParamVel, speedRight);
 
130
//                      backActuator->setODEParameter(dParamVel, -speedRight * speedTrans + speedRot * speedLeft);
 
131
//                      secondScrewDrive->frontActuator->setODEParameter(dParamVel,  speedLeft);
 
132
//                      secondScrewDrive->backActuator->setODEParameter(dParamVel, -speedLeft * speedTrans + speedRot * speedRight);
 
133
 
 
134
                } else {
 
135
  
 
136
                        secondScrewDrive->setVel(secondScrewDrive->getVel(deltaTime), deltaTime);
 
137
 
 
138
//                      frontActuator->setODEParameter(dParamVel,  speedLeft);
 
139
//                      backActuator->setODEParameter(dParamVel, -speedLeft * speedTrans + speedRot * speedRight);
 
140
//                      secondScrewDrive->frontActuator->setODEParameter(dParamVel,  speedRight);
 
141
//                      secondScrewDrive->backActuator->setODEParameter(dParamVel, -speedRight * speedTrans + speedRot * speedLeft);
 
142
                }
 
143
        }
 
144
 
 
145
#ifdef  AUDIO_ON
 
146
        if(screwSound != NULL)
 
147
        {
 
148
                float speed = fabs(v);
 
149
                if(speed > 0.0)
 
150
                {
 
151
                        screwSound->SetGain(0.1f+speed/10);
 
152
                        screwSound->SetPitch(0.5f+speed/100);
 
153
                        if (!screwSound->IsPlaying())
 
154
                                screwSound->Play();
 
155
                }
 
156
                else if(screwSound->IsPlaying())
 
157
                        screwSound->Stop();
 
158
        }
 
159
#endif
 
160
}
 
161
 
 
162
}