File: WORKFLOW\TGotoPeakCmd.cpp
1 #include "workflow\TGotoPeakCmd.h"
2
3 extern TSteering Steering;
4
5 TGotoPeakCmd::TGotoPeakCmd ( TCmdTag ct ) : TCmd(ct), dDistance(0), fIntensity(0)
6 {
7 bFlankDetected= FALSE;
8 bSmallAngleSide= (ToSmallerAngle == ct.P1);
9 nReadyAction= (ECmdParam)ct.P2;
10 nMotor= mlGetAxis();
11 mlGetDistance(nMotor, Steering.dStartPoint);
12 dDistance= new double [nLifo];
13 fIntensity= new float [nLifo];
14 eStep= CFirstStep;
15 };
16
17 TGotoPeakCmd::~TGotoPeakCmd ()
18 {
19 Steering.Monitor= 0;
20
21 _FREELIST(dDistance);
22 _FREELIST(fIntensity);
23 };
24
25 bool TGotoPeakCmd::GetShowData ( LPSTR buffer )
26 {
27 buffer[0]= 0;
28 if ( eStep == CReady )
29 {
30 #ifdef GermanVersion
31 sprintf( buffer, "\"GotoPeak\"-Kommando erfolgreich." );
34 #endif
35 }
36 else if ( eStep == CControlStep )
37 {
38 sprintf(buffer, "D:%.3lf I:%.2f", dDistance[0], fIntensity[0]);
39 }
40 return true;
41 }
42
43 void TGotoPeakCmd::GetName ( LPSTR aName )
44 {
45 strcpy( aName, "GotoPeak" );
46 }
47
48 //ermittelt die aktuelle Position und Intensitaet, bewegt Motor um Width
49 //nach rechts (bzw. links) wenn wir links (bzw. rechts) vom Peak sind
50 ECmdCode TGotoPeakCmd::FirstStep ( void )
51 {
52 eStep= CControlStep;
53 //ermittelt die aktuelle Intensitaet
54 fIntensity[0]= Steering.GetIntensity();
55 //ermitteln der aktuellen Motorposition (Distance)
56 //Steering.GetDistance() ruft auch nur mGetValue(Distance)
57 dDistance[0]= Steering.GetDistance();
58 Steering.dStartPoint= dDistance[0];
59 //wenn wir uns links vom Peak befinden, nach rechts bewegen,
60 //sonst nach links
61 if (bSmallAngleSide)
62 StartMove(nMotor, mGetValue(Distance) + mGetValue(Width));
63 else
64 StartMove(nMotor, mGetValue(Distance) - mGetValue(Width));
65 return CRecall;
66 };
67
68
69 ECmdCode TGotoPeakCmd::ControlStep ( void )
70 {
71 float idif0, idif1;
72 //verschiebt alle Werte im Intensitaets- und Distanzfeld um eine Position
73 memmove((LPSTR)(fIntensity + 1), (LPSTR)fIntensity, (nLifo - 1) * sizeof(float));
74 memmove((LPSTR)(dDistance + 1), (LPSTR)dDistance, (nLifo - 1) * sizeof(double));
75 //ermittelt den aktuellen Intensitaets- und Distanzwert
76 fIntensity[0]= Steering.GetIntensity();
77 dDistance[0]= Steering.GetDistance();
78 // Keine Reaktion unterhalb des Rausch-Level
79 if (fIntensity[0] < Steering.GetNoiseLevel())
80 {
81 //Motor ausserhalb des erlaubten Bereichs
82 if (mIsRangeHit())
83 {
84 //wechselt die Seite vom Peak, Bewegung zurueck zum Ausgangspunkt
85 bSmallAngleSide= !bSmallAngleSide;
86 StartMove(nMotor, Steering.dStartPoint);
87 return CRecall;
88 }
89 goto Exit;
90 }
91 //berechnet den Intensitaetsunterschied
92 idif0= fIntensity[0] - fIntensity[1];
93 // Abbruch-Bedingung
94 //falls die Flanke entdeckt wurde, und der letzte Intensitaetsunterschied
95 //negativ (aktuelle Intensitaet ist kleiner als die zu letzt gemessene)
96 if (bFlankDetected && (idif0 < 0.0))
97 {
98 eStep= CReadyStep;
99 return CRecall;
100 }
101 //hinreichend grosse Intensitaetsaenderung
102 if (fabs(idif0) > (3.0 * Steering.GetSigma() * fIntensity[1]))
103 {
104 bFlankDetected= TRUE;
105 //ist die aktuelle Intensitaet kleiner als die zuletzt gemessene tausche
106 //Seite bzgl. Peak
107 if (idif0 < 0.0)
108 bSmallAngleSide= !bSmallAngleSide;
109 goto Exit;
110 }
111 //Intensitaetsunterschied der Messung zuvor
112 //Beim zweiten Messchritt (erste Aufruf des ControlStep)
113 //ist fIntensity[2] unbestimmt, da erst [0] und [1] definierte
114 //Werte erhalten haben
115 idif1= fIntensity[1] - fIntensity[2];
116 //hat die Intensitaet bei den beiden letzten Messungen abgenommen,
117 //dann, Flanke entdeckt, Wechsel der Seite bzgl. Flanke
118 if ((idif0 < 0.0) && (idif1 < 0.0))
119 {
120 bFlankDetected= TRUE;
121 bSmallAngleSide= !bSmallAngleSide;
122 goto Exit;
123 }
124
125 Exit:
126 //Bewegung auf den Peak zu
127 if ( bSmallAngleSide )
128 StartMove(nMotor, dDistance[0] + mGetValue(Width));
129 else
130 StartMove(nMotor, dDistance[0] - mGetValue(Width));
131 return CRecall;
132 };
133
134
135 ECmdCode TGotoPeakCmd::ReadyStep ( void )
136 {
137 if (bBackMoveActive)
138 {
139 Steering.dPeakPoint= Steering.dGoalDistance= dDistance[0];
140 Steering.fPeakIntensity= Steering.fGoalIntensity= fIntensity[0];
141 eStep= CReady;
142 return CReady;
143 }
144 switch ( nReadyAction )
145 {
146 //Was ist die Grundlage dieser Berechnung ?
147 case Interpolation:
148 Steering.dGoalDistance= (fSearchIntensity - fIntensity[1]) * dDistance[0];
149 Steering.dGoalDistance += (fIntensity[0] - fSearchIntensity) * dDistance[1];
150 Steering.dGoalDistance /= (fIntensity[0] - fIntensity[1]);
151 Steering.dPeakPoint= Steering.dGoalDistance;
152 Steering.fPeakIntensity= Steering.fGoalIntensity= fIntensity[1];
153 eStep= CReady;
154 return CReady;
155
156 //geht zurueck zur letzten Position
157 default: // auch BackMove
158 StartMove(nMotor, dDistance[1]);
159 bBackMoveActive= TRUE;
160 return CRecall;
161 }
162 };
163