File: WORKFLOW\TControlFlankCmd.cpp
1 #include "workflow\TControlFlankCmd.h"
2
3 extern TSteering Steering;
4
5
6 TControlFlankCmd::TControlFlankCmd ( TCmdTag ct ) : TCmd(ct), dDistance(0), fIntensity(0)
7 {
8 nMotor= mlGetAxis();
9 bSmallAngleSide= (ct.P1 == SmallSide);
10 fControlRange= atof(ct.P3);
11 //neben dem aktuellen Werten sollen die letzten drei Werte gespeichert
12 //werden -> es wird aber nur der aktuelle und der zuletzt gemessene
13 //Werte betrachtet -> nLifo= 2 wuerde ausreichen
14 dDistance= new double [nLifo];
15 fIntensity= new float [nLifo];
16 eStep= CFirstStep;
17 };
18
19 TControlFlankCmd::~TControlFlankCmd ()
20 {
21 _FREELIST(dDistance);
22 _FREELIST(fIntensity);
23 };
24
25
26 //Ausgabe des aktuellen Zustands in der Kontrollzeile anhand des
27 //aktuellen Steuer-Codes fuer Commandos (eStep)
28 bool TControlFlankCmd::GetShowData ( LPSTR buffer )
29 {
30 buffer[0]= 0;
31 if ( eStep == CReady )
32 {
33 #ifdef GermanVersion
34 sprintf( buffer, "\"ControlFlank\"-Kommando erfolgreich." );
37 #endif
38 }
39 else if ( eStep == CControlStep )
40 {
41 sprintf(buffer, "D:%.3lf I:%.2f", dDistance[0], fIntensity[0]);
42 }
43 return true;
44 }
45
46 void TControlFlankCmd::GetName ( LPSTR aName )
47 {
48 strcpy( aName, "ControlFlank" );
49 }
50
51 ECmdCode TControlFlankCmd::FirstStep ( void )
52 {
53 // Feststellen, ob die gewählten Parameter konsistent sind
54
55 eStep= CControlStep;
56 fIntensity[0]= Steering.GetIntensity();
57 dDistance[0]= Steering.GetDistance();
58 fSearchIntensity= fIntensity[0];
59 Steering.dStartPoint= dDistance[0];
60
61 //##Berechnen der Regelgrenzen
62 fMinIntensity= fSearchIntensity * (1.0 - fControlRange / 2.0);
63 fMaxIntensity= fSearchIntensity * (1.0 + fControlRange / 2.0);
64 fIntensityRange= (fMaxIntensity - fMinIntensity) / 2.0;
65
66 return CMeasure;
67 };
68
69 //Prüfen ob Intensitaet im vorgegebenen Bereich und gegebenenfalls nachregeln
70 ECmdCode TControlFlankCmd::ControlStep ( void )
71 {
72 float steprel, idif0, idif1;
73 memmove((LPSTR)(fIntensity + 1), (LPSTR)fIntensity, (nLifo - 1) * sizeof(float));
74 memmove((LPSTR)(dDistance + 1), (LPSTR)dDistance, (nLifo - 1) * sizeof(double));
75 //aktuelle Werte
76 fIntensity[0]= Steering.GetIntensity();
77 dDistance[0]= Steering.GetDistance();
78 //Berechnet vom aktuellen Wert und vom zuletzt gemessenen Wert die Abweichung
79 //vom Startwert (positive Wert -> Intensitaet ist kleiner, negative Wert ->
80 //Intensitaet ist groesser)
81 idif0= fSearchIntensity - fIntensity[0];
82 idif1= fSearchIntensity - fIntensity[1];
83
84 // Fall 1: Intensitaet liegt im Bereich
85 if (fabs(idif0) < fIntensityRange)
86 {
87 //ueber Arbeitspunkt bewegt
88 if (((idif1*idif0) < 0) && (fabs(idif1) > fIntensityRange))
89 mSetValue(Width, 0.75 * mGetValue(Width)); // Schrittweite vermindern
90 return CRecall;
91
92 // Fall 2: Nur mit der letzten Messung wurde der Bereich verlassen
93 }
94 else if (fabs(idif1) < fIntensityRange)
95 {
96 // d.h. aktueller Wert liegt ausserhalb des Bereichs
97 // nichts zu tun
98
99 // Fall 3: Weiter in falsche Richtung bewegt
100 }
101 else if (((idif1 * idif0) > 0) && (fabs(idif0) > fabs(idif1)))
102 {
103 //aktueller Wert und Wert davor liegen ausserhalb des Bereichs
104 //aktuelle Abweichung ist groesser, als Abweichung im Schritt zuvor
105
106 //Hier wird zwar etwas berechnet, die Aktion selbst ist aber auskommentiert
107 //-> Was war hier eigentlich geplant?
108
109 // Ist die Flanke bestimmbar ?
110
111 //Hier wird die Bewegungsrichtung bestimmt, ist steprel negativ,
112 //so wurde nach links bewegt, positiv nach rechts und bei null lag
113 //keine Bewegung vor. Es gibt keine Aussage darueber, ob wir uns
114 //links oder rechts von der Flanke befinden.
115 steprel= (dDistance[0] - dDistance[1]);
116 steprel= fabs(steprel / mGetValue(Width));
117 if (steprel > 0.7)
118 {
119 // FlankenFlag ändern
120 // bSmallAngleSide= !bSmallAngleSide;
121 }
122
123 // Fall 4: Bewegung durch Arbeitspunkt und zu weit
124 }
125 else if ((idif1 * idif0) < 0)
126 {
127 mSetValue(Width, 0.75 * mGetValue(Width)); // Schrittweite vermindern
128
129 } // Kullmann+Reinecker: Gibt es hier keinen else-Fall mehr? Die Möglichkeiten für iDif0 und iDif1 sind nicht ausgeschöpft!!!
130
131 // float dummy= mGetValue(Width);
132 // eigentliches Nachregeln
133 // rechts vom Pik -> Intensitaet faellt bei Bewegung nach rechts ab
134 if (!bSmallAngleSide)
135 {
136 //Intensitaet ist kleiner -> rechts vom Arbeitspunkt
137 if (idif1 > 0.0)
138 StartMove(nMotor, dDistance[0] - mGetValue(Width)); //bewegt Probe nach links
139 //Intensitaet ist groesser -> links vom Arbeitspunkt
140 else
141 StartMove(nMotor, dDistance[0] + mGetValue(Width));
142
143 //links vom Pik -> Intensitaet faellt bei Bewegung nach links ab
144 }
145 else
146 {
147 //Intensitaet ist groesser -> rechts vom Arbeitspunkt
148 if (idif1 < 0.0)
149 StartMove(nMotor, dDistance[0] - mGetValue(Width)); //bewegt Probe nach links
150 //Intensitaet ist kleiner -> links vom Arbeitspunkt
151 else
152 StartMove(nMotor, dDistance[0] + mGetValue(Width));
153 }
154 return CRecall;
155 };
156 //-----------------------------------------------------------------------------
157
158 ECmdCode TControlFlankCmd::ReadyStep ( void )
159 {
160 eStep= CReady;
161 return CReady;
162 };
163