File: WORKFLOW\TGotoIntensityCmd.cpp
1 #include "workflow\TGotoIntensityCmd.h"
2
3 //berechnet Zielintensitaet und bewegt Motor um zwei Schritte nach links bzw. rechts
4 TGotoIntensityCmd::TGotoIntensityCmd ( TCmdTag ct ) : TCmd(ct), dDistance(0), fIntensity(0)
5 {
6 nMotor= mlGetAxis();
7 bSmallAngleSide= (ct.P1 == SmallSide);
8 nReadyAction= (ECmdParam)ct.P2;
9 // Bezug auf Vorgeschichte
10 //##berechnet anzusteuernde Intensitaet
11 fSearchIntensity= Steering.fPeakIntensity * atof(ct.P3);
12 //##falls diese Null ist, liegt ein Fehler vor
13 if (!fSearchIntensity)
14 {
15 nFailureId= 11;
16 eStep= CReady;
17 return;
18 }
19 dDistance= new double [nLifo];
20 fIntensity= new float [nLifo];
21 //##bewege Motor um zweifache Schrittweite nach links
22 if (bSmallAngleSide)
23 StartMove(nMotor, mGetValue(Distance) - 2.0 * mGetValue(Width));
24 //##bewege Motor um zweifache Schrittweite nach rechts
25 else
26 StartMove(nMotor, mGetValue(Distance) + 2.0 * mGetValue(Width));
27 eStep= CFirstStep;
28 };
29
30 TGotoIntensityCmd::~TGotoIntensityCmd()
31 {
32 _FREELIST(dDistance);
33 _FREELIST(fIntensity);
34 };
35
36 //*****************************************************************************
37 // Fortschritt- Kommandoinformtionen
38 //*****************************************************************************
39
40 bool TGotoIntensityCmd::GetShowData ( LPSTR buffer )
41 {
42 buffer[0]= 0;
43 if ( eStep == CReady )
44 {
45 #ifdef GermanVersion
46 sprintf( buffer, "\"GotoIntensity\"-Kommando erfolgreich." );
49 #endif
50 }
51 else if ( eStep == CFirstStep )
52 {
53 #ifdef GermanVersion
54 sprintf(buffer, "Ziel-Intensität %.2f", fSearchIntensity);
57 #endif
58 }
59 else if ( eStep == CControlStep )
60 {
61 sprintf(buffer, "D:%.3lf I:%.2f", dDistance[0], fIntensity[0]);
62 }
63 return true;
64 }
65
66 //*****************************************************************************
67 // Schritte der Kommandoverarbeitung
68 //*****************************************************************************
69
70 ECmdCode TGotoIntensityCmd::FirstStep ( void )
71 {
72 eStep= CControlStep;
73 //##Intensitaet und Position erfassen
74 fIntensity[0]= Steering.GetIntensity();
75 dDistance[0]= Steering.GetDistance();
76 //##um einen Schritt bewegen
77 if (bSmallAngleSide)
78 StartMove(nMotor, dDistance[0] - mGetValue(Width));
79 else
80 StartMove(nMotor, dDistance[0] + mGetValue(Width));
81 return CRecall;
82 };
83 //-----------------------------------------------------------------------------
84
85 ECmdCode TGotoIntensityCmd::ControlStep ( void )
86 {
87 double dDist;
88 float idif0, idif1;
89
90 //##alle Intensitaets und Positionswerte um eins verschieben
91 memmove((LPSTR)(fIntensity + 1), (LPSTR)fIntensity, (nLifo - 1) * sizeof(float));
92 memmove((LPSTR)(dDistance + 1), (LPSTR)dDistance, (nLifo - 1) * sizeof(double));
93 //##aktuellen Intensitaets- und Positionswerte bestimmen
94 fIntensity[0]= Steering.GetIntensity();
95 dDistance[0]= Steering.GetDistance();
96 //##Abstand der aktuellen Intensitaet zur Zielintensitaet
97 idif0= fSearchIntensity - fIntensity[0];
98 //##Intensitaetsabstand eine Runde vorher
99 idif1= fSearchIntensity - fIntensity[1];
100 //*** Feststellen, ob Level durchschritten wurde
101 if ((idif0 * idif1) < 0.0)
102 {
103 eStep= CReadyStep;
104 //##berechnen der endgueltigen Position und anfahren
105 switch (nReadyAction)
106 {
107 case Interpolation:
108 dDist= (fSearchIntensity - fIntensity[1]) * dDistance[0];
109 dDist += (fIntensity[0] - fSearchIntensity) * dDistance[1];
110 dDist /= (fIntensity[0] - fIntensity[1]);
111 StartMove(nMotor, dDist);
112 break;
113
114 case BackMove:
115 StartMove(nMotor, dDistance[1]);
116 break;
117 }
118 return CRecall;
119 }
120
121 //##!! Hier wird nach links/rechts von der Flanke unterschieden und einmal
122 //##der aktuelle Intensitaetsabstand (idif0) betrachtet (links von der Flanke)
123 //##und das andere Mal der Intensitaetsabstand eine Runde zuvor (idif1) (rechts
124 //##von der Flanke)-> Hier liegt ein Fehler vor!
125 if (bSmallAngleSide)
126 {
127 if (idif0 > 0.0)
128 StartMove(nMotor, dDistance[0] + mGetValue(Width));
129 else
130 StartMove(nMotor, dDistance[0] - mGetValue(Width));
131 }
132 else
133 {
134 if (idif1 > 0)
135 StartMove(nMotor, dDistance[0] - mGetValue(Width));
136 else
137 StartMove(nMotor, dDistance[0] + mGetValue(Width));
138 }
139 return CRecall;
140 };
141 //-----------------------------------------------------------------------------
142
143 ECmdCode TGotoIntensityCmd::ReadyStep ( void )
144 {
145 double mm;
146 switch (nReadyAction)
147 {
148 case Interpolation:
149 mm= (fIntensity[1] - fIntensity[0]);
150 if (!mm)
151 {
152 Steering.dGoalDistance= dDistance[0];
153 Steering.fGoalIntensity= fIntensity[0];
154 eStep= CReady;
155 return CReady;
156 }
157 //##Was wird hier berechnet?
158 mm= (dDistance[1] - dDistance[0]) * (fSearchIntensity - fIntensity[0]) / mm;
159 Steering.dGoalDistance= dDistance[0] + mm;
160 Steering.fGoalIntensity= fSearchIntensity;
161 eStep= CReady;
162 return CReady;
163
164 default:
165 Steering.dGoalDistance= dDistance[0];
166 Steering.fGoalIntensity= fIntensity[0];
167 eStep= CReady;
168 return CReady;
169 }
170 };
171