source: trunk/FACT++/src/feedback.cc@ 12771

Last change on this file since 12771 was 12716, checked in by tbretz, 13 years ago
Make sure fSP always has a valid size.
File size: 30.4 KB
Line 
1#include <valarray>
2
3#include "Dim.h"
4#include "Event.h"
5#include "Shell.h"
6#include "StateMachineDim.h"
7#include "Connection.h"
8#include "Configuration.h"
9#include "Console.h"
10#include "Converter.h"
11#include "DimServiceInfoList.h"
12#include "PixelMap.h"
13
14#include "tools.h"
15
16#include "LocalControl.h"
17
18#include "HeadersFAD.h"
19#include "HeadersBIAS.h"
20
21namespace ba = boost::asio;
22namespace bs = boost::system;
23namespace dummy = ba::placeholders;
24
25using namespace std;
26
27// ------------------------------------------------------------------------
28
29#include "DimDescriptionService.h"
30
31// ------------------------------------------------------------------------
32
33class StateMachineFeedback : public StateMachineDim, public DimInfoHandler
34{
35 /*
36 int Wrap(boost::function<void()> f)
37 {
38 f();
39 return T::GetCurrentState();
40 }
41
42 boost::function<int(const EventImp &)> Wrapper(boost::function<void()> func)
43 {
44 return bind(&StateMachineMCP::Wrap, this, func);
45 }*/
46
47private:
48 enum states_t
49 {
50 kStateDimNetworkNA = 1,
51 kStateDisconnected,
52 kStateConnecting,
53 kStateConnected,
54 kStateTempCtrlIdle,
55 kStateFeedbackCtrlIdle,
56 kStateTempCtrlRunning,
57 kStateFeedbackCtrlRunning,
58 kStateCalibrating,
59 };
60
61 enum control_t
62 {
63 kIdle,
64 kTemp,
65 kFeedback
66 };
67
68 control_t fControlType;
69
70 PixelMap fMap;
71
72 DimServiceInfoList fNetwork;
73
74 pair<Time, int> fStatusDim;
75 pair<Time, int> fStatusFAD;
76 pair<Time, int> fStatusFSC;
77 pair<Time, int> fStatusBias;
78
79 DimStampedInfo fDim;
80 DimStampedInfo fFAD;
81 DimStampedInfo fFSC;
82 DimStampedInfo fBias;
83 DimStampedInfo fBiasA;
84
85 DimStampedInfo fBiasData;
86 DimStampedInfo fCameraTemp;
87
88 DimDescribedService fDimReference;
89 DimDescribedService fDimDeviation;
90 DimDescribedService fDimCalibration;
91
92 vector<int64_t> fCurrentsAvg;
93 vector<int64_t> fCurrentsRms;
94
95 vector<vector<float>> fData;
96
97 uint64_t fCursor;
98
99 Time fBiasLast;
100 Time fStartTime;
101
102 valarray<double> fPV[3]; // Process variable (intgerated/averaged amplitudes)
103 valarray<double> fSP; // Set point (target amplitudes)
104
105 double fKp; // Proportional constant
106 double fKi; // Integral constant
107 double fKd; // Derivative constant
108 double fT; // Time constant (cycle time)
109 double fGain; // Gain (conversion from a DRS voltage deviation into a BIAS voltage change at G-APD reference voltage)
110
111 double fT21;
112
113 double fBiasOffset;
114
115 bool fOutputEnabled;
116
117 pair<Time, int> GetNewState(DimStampedInfo &info) const
118 {
119 const bool disconnected = info.getSize()==0;
120
121 // Make sure getTimestamp is called _before_ getTimestampMillisecs
122 const int tsec = info.getTimestamp();
123 const int tms = info.getTimestampMillisecs();
124
125 return make_pair(Time(tsec, tms*1000),
126 disconnected ? -2 : info.getQuality());
127 }
128
129 void ResetData()
130 {
131 fData.clear();
132 fData.resize(500);
133 fCursor = 0;
134 fStartTime = Time();
135
136 fSP = valarray<double>(0., 416);
137
138 vector<float> vec(2*BIAS::kNumChannels);
139 fDimDeviation.Update(vec);
140
141 fPV[0].resize(0);
142 fPV[1].resize(0);
143 fPV[2].resize(0);
144
145 if (fKp==0 && fKi==0 && fKd==0)
146 Warn("Control loop parameters are all set to zero.");
147 }
148
149 void infoHandler()
150 {
151 DimInfo *curr = getInfo(); // get current DimInfo address
152 if (!curr)
153 return;
154
155 if (curr==&fBias)
156 {
157 fStatusBias = GetNewState(fBias);
158 return;
159 }
160
161 if (curr==&fFAD)
162 {
163 fStatusFAD = GetNewState(fFAD);
164 return;
165 }
166
167 if (curr==&fFSC)
168 {
169 fStatusFSC = GetNewState(fFSC);
170 return;
171 }
172
173 if (curr==&fDim)
174 {
175 fStatusDim = GetNewState(fDim);
176 fStatusDim.second = curr->getSize()==4 ? curr->getInt() : 0;
177 return;
178 }
179
180 if (curr==&fCameraTemp)
181 {
182 if (curr->getSize()!=60*sizeof(float))
183 return;
184
185 const float *ptr = static_cast<float*>(curr->getData());
186
187 double avg = 0;
188 int num = 0;
189 for (int i=1; i<32; i++)
190 if (ptr[i]!=0)
191 {
192 avg += ptr[i];
193 num++;
194 }
195
196 if (num==0)
197 return;
198
199 avg /= num;
200
201
202 const float diff = (avg-25)*4./70 + fBiasOffset;
203
204 vector<float> vec(2*BIAS::kNumChannels);
205 for (int i=0; i<BIAS::kNumChannels; i++)
206 vec[i+416] = diff;
207
208 if (fControlType!=kTemp)
209 return;
210
211 fDimDeviation.Update(vec);
212
213 if (fOutputEnabled && fStatusBias.second==BIAS::kVoltageOn)
214 {
215 Info("Sending correction to feedback.");
216
217 DimClient::sendCommandNB((char*)"BIAS_CONTROL/SET_GAPD_REFERENCE_OFFSET",
218 (void*)&diff, sizeof(float));
219 }
220 }
221
222 if (curr==&fBiasA && fControlType==kTemp && GetCurrentState()==kStateCalibrating)
223 {
224 if (curr->getSize()!=416*sizeof(int16_t))
225 return;
226
227 if (fStatusBias.second==BIAS::kRamping)
228 return;
229
230 const int16_t *ptr = static_cast<int16_t*>(curr->getData());
231
232 for (int i=0; i<416; i++)
233 if (ptr[i]>0)
234 {
235 fCurrentsAvg[i] += ptr[i];
236 fCurrentsRms[i] += ptr[i]*ptr[i];
237 }
238
239 if (++fCursor<100)
240 {
241 DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
242 return;
243 }
244
245 vector<float> val(2*416, 0);
246 for (int i=0; i<416; i++)
247 {
248 val[i] = double(fCurrentsAvg[i])/fCursor;
249 val[i+416] = sqrt(double(fCurrentsRms[i])/fCursor-val[i]*val[i]);
250 }
251
252 fDimCalibration.Update(val);
253
254 fOutputEnabled = false;
255 fControlType = kIdle;
256
257 DimClient::sendCommandNB("BIAS_CONTROL/REQUEST_STATUS", NULL, 0);
258 }
259
260 if (curr==&fBiasData && fControlType==kFeedback)
261 {
262 if (curr->getSize()!=1440*sizeof(float))
263 return;
264
265 // -------- Check age of last stored event --------
266
267 // Must be called in this order
268 const int tsec = curr->getTimestamp();
269 const int tms = curr->getTimestampMillisecs();
270
271 const Time tm(tsec, tms*1000);
272
273 if (Time()-fBiasLast>boost::posix_time::seconds(30))
274 {
275 Warn("Last received event data older than 30s... resetting average calculation.");
276 ResetData();
277 }
278 fBiasLast = tm;
279
280 // -------- Store new event --------
281
282 fData[fCursor%fData.size()].assign(reinterpret_cast<float*>(curr->getData()),
283 reinterpret_cast<float*>(curr->getData())+1440);
284
285
286 fCursor++;
287
288 if (fCursor<fData.size())
289 return;
290
291 // -------- Calculate statistics --------
292
293 valarray<double> med(1440);
294
295 for (int ch=0; ch<1440; ch++)
296 {
297 vector<float> arr(fData.size());
298 for (size_t i=0; i<fData.size(); i++)
299 arr[i] = fData[i][ch];
300
301 sort(arr.begin(), arr.end());
302
303 med[ch] = arr[arr.size()/2];
304 }
305
306 /*
307 vector<float> med(1440);
308 vector<float> rms(1440);
309 for (size_t i=0; i<fData.size(); i++)
310 {
311 if (fData[i].size()==0)
312 return;
313
314 for (int j=0; j<1440; j++)
315 {
316 med[j] += fData[i][j];
317 rms[j] += fData[i][j]*fData[i][j];
318 }
319 }
320 */
321
322 vector<double> avg(BIAS::kNumChannels);
323 vector<int> num(BIAS::kNumChannels);
324 for (int i=0; i<1440; i++)
325 {
326 const PixelMapEntry &ch = fMap.hw(i);
327
328 // FIXME: Add a consistency check if the median makes sense...
329 // FIXME: Add a consistency check to remove pixels with bright stars (median?)
330
331 avg[ch.hv()] += med[i];
332 num[ch.hv()]++;
333 }
334
335 for (int i=0; i<BIAS::kNumChannels; i++)
336 {
337 if (num[i])
338 avg[i] /= num[i];
339
340 }
341
342 // -------- Calculate correction --------
343
344 // http://bestune.50megs.com/typeABC.htm
345
346 // CO: Controller output
347 // PV: Process variable
348 // SP: Set point
349 // T: Sampling period (loop update period)
350 // e = SP - PV
351 //
352 // Kp : No units
353 // Ki : per seconds
354 // Kd : seconds
355
356 // CO(k)-CO(k-1) = - Kp[ PV(k) - PV(k-1) ] + Ki * T * (SP(k)-PV(k)) - Kd/T [ PV(k) - 2PV(k-1) + PV(k-2) ]
357
358 if (fCursor%fData.size()==0)
359 {
360 // FIXME: Take out broken / dead boards.
361
362 const Time tm0 = Time();
363
364 /*const*/ double T21 = fT>0 ? fT : (tm0-fStartTime).total_microseconds()/1000000.;
365 const double T10 = fT21;
366 fT21 = T21;
367
368 fStartTime = tm0;
369
370 ostringstream out;
371 out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
372 Info(out);
373
374 if (fPV[0].size()==0)
375 {
376 fPV[0].resize(avg.size());
377 fPV[0] = valarray<double>(avg.data(), avg.size());
378 }
379 else
380 if (fPV[1].size()==0)
381 {
382 fPV[1].resize(avg.size());
383 fPV[1] = valarray<double>(avg.data(), avg.size());
384 }
385 else
386 if (fPV[2].size()==0)
387 {
388 fPV[2].resize(avg.size());
389 fPV[2] = valarray<double>(avg.data(), avg.size());
390 }
391 else
392 {
393 fPV[0] = fPV[1];
394 fPV[1] = fPV[2];
395
396 fPV[2].resize(avg.size());
397 fPV[2] = valarray<double>(avg.data(), avg.size());
398
399 if (T10<=0 || T21<=0)
400 return;
401
402 cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl;
403
404 // fKi[j] = response[j]*gain;
405 // Kp = 0;
406 // Kd = 0;
407
408 // => Kp = 0.01 * gain = 0.00005
409 // => Ki = 0.8 * gain/20s = 0.00025
410 // => Kd = 0.1 * gain/20s = 0.00003
411
412 fKp = 0;
413 fKd = 0;
414 fKi = 0.00003*20;
415 T21 = 1;
416
417 //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
418 //valarray<double> correction =
419 // - Kp * (PV[2] - PV[1])
420 // + dT * Ki * (SP - PV[2])
421 // - Kd / dT * (PV[2] - 2*PV[1] + PV[0]);
422 //
423 // - (Kp+Kd/dT1) * (PV[2] - PV[1])
424 // + dT2 * Ki * (SP - PV[2])
425 // + Kd / dT1 * (PV[1] - PV[0]);
426 //
427 // - Kp * (PV[2] - PV[1])
428 // + Ki * (SP - PV[2])*dT
429 // - Kd * (PV[2] - PV[1])/dT
430 // + Kd * (PV[1] - PV[0])/dT;
431 //
432 //valarray<double> correction =
433 // - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
434 const valarray<double> correction = fGain/1000*
435 (
436 - (fKp+fKd/T21)*(fPV[2] - fPV[1])
437 + fKi*T21*(fSP-fPV[2])
438 + fKd/T10*(fPV[1]-fPV[0])
439 );
440
441 /*
442 integral = 0
443 start:
444 integral += (fSP - fPV[2])*dt
445
446 output = Kp*(fSP - fPV[2]) + Ki*integral - Kd*(fPV[2] - fPV[1])/dt
447
448 wait(dt)
449
450 goto start
451 */
452
453 vector<float> vec(2*BIAS::kNumChannels);
454 for (int i=0; i<BIAS::kNumChannels; i++)
455 vec[i] = fPV[2][i]-fSP[i];
456
457 for (int i=0; i<BIAS::kNumChannels; i++)
458 vec[i+416] = avg[i]<5*2.5 ? 0 : correction[i];
459
460 fDimDeviation.Update(vec);
461
462 if (fOutputEnabled && fStatusBias.second==BIAS::kVoltageOn)
463 {
464 Info("Sending correction to feedback.");
465
466 DimClient::sendCommandNB((char*)"BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
467 (void*)(vec.data()+416), 416*sizeof(float));
468
469 /*
470 if (!Dim::SendCommand("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
471 (const void*)(vec.data()+416), 416*sizeof(float)))
472 {
473 Error("Sending correction to bias control failed... switching off.");
474 fOutputEnabled=false;
475 }
476 else
477 Info("Success!");
478 */
479 }
480 }
481
482 }
483 }
484
485 }
486
487 bool CheckEventSize(size_t has, const char *name, size_t size)
488 {
489 if (has==size)
490 return true;
491
492 ostringstream msg;
493 msg << name << " - Received event has " << has << " bytes, but expected " << size << ".";
494 Fatal(msg);
495 return false;
496 }
497
498 void PrintState(const pair<Time,int> &state, const char *server)
499 {
500 const State rc = fNetwork.GetState(server, state.second);
501
502 Out() << state.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
503 Out() << kBold << server << ": ";
504 Out() << rc.name << "[" << rc.index << "]";
505 Out() << kReset << " - " << kBlue << rc.comment << endl;
506 }
507
508 int Print()
509 {
510 Out() << fStatusDim.first.GetAsStr("%H:%M:%S.%f").substr(0, 12) << " - ";
511 Out() << kBold << "DIM_DNS: ";
512 if (fStatusDim.second==0)
513 Out() << "Offline" << endl;
514 else
515 Out() << "V" << fStatusDim.second/100 << 'r' << fStatusDim.second%100 << endl;
516
517 PrintState(fStatusFAD, "FAD_CONTROL");
518 PrintState(fStatusFSC, "FSC_CONTROL");
519 PrintState(fStatusBias, "BIAS_CONTROL");
520
521 return GetCurrentState();
522 }
523
524 int SetConstant(const EventImp &evt, int constant)
525 {
526 if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
527 return kSM_FatalError;
528
529 switch (constant)
530 {
531 case 0: fKi = evt.GetDouble(); break;
532 case 1: fKp = evt.GetDouble(); break;
533 case 2: fKd = evt.GetDouble(); break;
534 case 3: fT = evt.GetDouble(); break;
535 case 4: fGain = evt.GetDouble(); break;
536 default:
537 Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
538 return kSM_FatalError;
539 }
540
541 return GetCurrentState();
542 }
543
544 int EnableOutput(const EventImp &evt)
545 {
546 if (!CheckEventSize(evt.GetSize(), "EnableOutput", 1))
547 return kSM_FatalError;
548
549 fOutputEnabled = evt.GetBool();
550
551 return GetCurrentState();
552 }
553
554 int StartFeedback()
555 {
556 ResetData();
557
558 fControlType = kFeedback;
559
560 return GetCurrentState();
561 }
562
563 int StartTempCtrl(const EventImp &evt)
564 {
565 if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
566 return kSM_FatalError;
567
568 ostringstream out;
569 out << "Starting temperature feedback with an offset of " << fBiasOffset << "V";
570 Message(out);
571
572 fBiasOffset = evt.GetFloat();
573 fControlType = kTemp;
574
575 return GetCurrentState();
576 }
577
578 int StopFeedback()
579 {
580 fControlType = kIdle;
581
582 return GetCurrentState();
583 }
584
585 int StoreReference()
586 {
587 if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
588 {
589 Warn("No values in memory. Take enough events first!");
590 return GetCurrentState();
591 }
592
593 // FIXME: Check age
594
595 if (!fPV[1].size() && !fPV[2].size())
596 fSP = fPV[0];
597
598 if (!fPV[2].size())
599 fSP = fPV[1];
600 else
601 fSP = fPV[2];
602
603 vector<float> vec(BIAS::kNumChannels);
604 for (int i=0; i<BIAS::kNumChannels; i++)
605 vec[i] = fSP[i];
606 fDimReference.Update(vec);
607
608 return GetCurrentState();
609 }
610
611 int SetReference(const EventImp &evt)
612 {
613 if (!CheckEventSize(evt.GetSize(), "SetReference", 4))
614 return kSM_FatalError;
615
616 const float val = evt.GetFloat();
617 /*
618 if (!fPV[0].size() && !fPV[1].size() && !fPV[2].size())
619 {
620 Warn("No values in memory. Take enough events first!");
621 return GetCurrentState();
622 }*/
623
624 vector<float> vec(BIAS::kNumChannels);
625 for (int i=0; i<BIAS::kNumChannels; i++)
626 vec[i] = fSP[i] = val;
627 fDimReference.Update(vec);
628
629 return GetCurrentState();
630 }
631
632 int CalibrateCurrents()
633 {
634// if (!CheckEventSize(evt.GetSize(), "StartTempCtrl", 4))
635// return kSM_FatalError;
636
637 ostringstream out;
638 out << "Starting temperature feedback with an offset of -2V";
639 Message(out);
640
641 fBiasOffset = -2;
642 fControlType = kTemp;
643 fCursor = 0;
644 fCurrentsAvg.assign(416, 0);
645 fCurrentsRms.assign(416, 0);
646 fStartTime = Time();
647 fOutputEnabled = true;
648
649 return kStateCalibrating;
650 }
651
652 int Execute()
653 {
654 // Dispatch (execute) at most one handler from the queue. In contrary
655 // to run_one(), it doesn't wait until a handler is available
656 // which can be dispatched, so poll_one() might return with 0
657 // handlers dispatched. The handlers are always dispatched/executed
658 // synchronously, i.e. within the call to poll_one()
659 //poll_one();
660
661 if (fStatusDim.second==0)
662 return kStateDimNetworkNA;
663
664 // All subsystems are not connected
665 if (fStatusFAD.second<FAD::kConnected &&
666 fStatusBias.second<BIAS::kConnecting &&
667 fStatusFSC.second<2)
668 return kStateDisconnected;
669
670 // At least one subsystem is not connected
671 if (fStatusFAD.second<FAD::kConnected ||
672 fStatusBias.second<BIAS::kConnected ||
673 fStatusFSC.second<2)
674 return kStateConnecting;
675
676 if (GetCurrentState()==kStateCalibrating && fCursor<100)
677 return GetCurrentState();
678
679/*
680 // All subsystems are connected
681 if (GetCurrentStatus()==kStateConfiguringStep1)
682 {
683 if (fCursor<1)
684 return kStateConfiguringStep1;
685
686 if (fCursor==1)
687 {
688 fStartTime = Time();
689 return kStateConfiguringStep2;
690 }
691 }
692 if (GetCurrentStatus()==kStateConfiguringStep2)
693 {
694 if (fCursor==1)
695 {
696 if ((Time()-fStartTime).total_microseconds()/1000000.<1.5)
697 return kStateConfiguringStep2;
698
699 Dim::SendCommand("BIAS_CONTROL/REQUEST_STATUS");
700 }
701 if (fCursor==2)
702 {
703
704 int n=0;
705 double avg = 0;
706 for (size_t i=0; i<fCurrents.size(); i++)
707 if (fCurrents[i]>=0)
708 {
709 avg += fCurrents[i];
710 n++;
711 }
712
713 cout << avg/n << endl;
714 }
715 return kStateConnected;
716 }
717*/
718 if (fControlType==kFeedback)
719 return fOutputEnabled ? kStateFeedbackCtrlRunning : kStateFeedbackCtrlIdle;
720
721 if (fControlType==kTemp)
722 return fOutputEnabled ? kStateTempCtrlRunning : kStateTempCtrlIdle;
723
724 return kStateConnected;
725 }
726
727public:
728 StateMachineFeedback(ostream &out=cout) : StateMachineDim(out, "FEEDBACK"),
729 fStatusDim(make_pair(Time(), -2)),
730 fStatusFAD(make_pair(Time(), -2)),
731 fStatusBias(make_pair(Time(), -2)),
732 fDim("DIS_DNS/VERSION_NUMBER", (void*)NULL, 0, this),
733 fFAD("FAD_CONTROL/STATE", (void*)NULL, 0, this),
734 fFSC("FSC_CONTROL/STATE", (void*)NULL, 0, this),
735 fBias("BIAS_CONTROL/STATE", (void*)NULL, 0, this),
736 fBiasA("BIAS_CONTROL/CURRENT", (void*)NULL, 0, this),
737 fBiasData("FAD_CONTROL/FEEDBACK_DATA", (void*)NULL, 0, this),
738 fCameraTemp("FSC_CONTROL/TEMPERATURE", (void*)NULL, 0, this),
739 fDimReference("FEEDBACK/REFERENCE", "F:416", ""),
740 fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416", ""),
741 fDimCalibration("FEEDBACK/CALIBRATION", "F:416;F:416", ""),
742 fSP(416),
743 fKp(0), fKi(0), fKd(0), fT(-1), fOutputEnabled(false)
744 {
745 // ba::io_service::work is a kind of keep_alive for the loop.
746 // It prevents the io_service to go to stopped state, which
747 // would prevent any consecutive calls to run()
748 // or poll() to do nothing. reset() could also revoke to the
749 // previous state but this might introduce some overhead of
750 // deletion and creation of threads and more.
751
752// fSP.resize(416);
753
754 // State names
755 AddStateName(kStateDimNetworkNA, "DimNetworkNotAvailable",
756 "The Dim DNS is not reachable.");
757
758 AddStateName(kStateDisconnected, "Disconnected",
759 "The Dim DNS is reachable, but the required subsystems are not available.");
760
761 AddStateName(kStateConnecting, "Connecting",
762 "Not all subsystems (FAD, FSC, Bias) are connected to their hardware.");
763
764 AddStateName(kStateConnected, "Connected",
765 "All needed subsystems are connected to their hardware, no action is performed.");
766
767 AddStateName(kStateFeedbackCtrlIdle, "FeedbackIdle",
768 "Feedback control activated, but voltage output disabled.");
769
770 AddStateName(kStateTempCtrlIdle, "FeedbackIdle",
771 "Temperature control activated, but voltage output disabled.");
772
773 AddStateName(kStateFeedbackCtrlRunning, "FeedbackControl",
774 "Feedback control activated and voltage output enabled.");
775
776 AddStateName(kStateTempCtrlRunning, "TempControl",
777 "Temperature control activated and voltage output enabled.");
778
779 AddStateName(kStateCalibrating, "Calibrating",
780 "Calibrating current offsets.");
781
782 AddEvent("START_FEEDBACK_CONTROL", kStateConnected)
783 (bind(&StateMachineFeedback::StartFeedback, this))
784 ("Start the feedback control loop");
785
786 AddEvent("START_TEMP_CONTROL", "F:1", kStateConnected)
787 (bind(&StateMachineFeedback::StartTempCtrl, this, placeholders::_1))
788 ("Start the temperature control loop"
789 "|offset[V]:Offset from the nominal temperature corrected value in Volts");
790
791 // kStateTempCtrlIdle, kStateFeedbackCtrlIdle, kStateTempCtrlRunning, kStateFeedbackCtrlRunning
792 AddEvent("STOP")
793 (bind(&StateMachineFeedback::StopFeedback, this))
794 ("Stop any control loop");
795
796 AddEvent("ENABLE_OUTPUT", "B:1")//, kStateIdle)
797 (bind(&StateMachineFeedback::EnableOutput, this, placeholders::_1))
798 ("Enable sending of correction values caluclated by the control loop to the biasctrl");
799
800 AddEvent("STORE_REFERENCE")//, kStateIdle)
801 (bind(&StateMachineFeedback::StoreReference, this))
802 ("Store the last (averaged) value as new reference (for debug purpose only)");
803
804 AddEvent("SET_REFERENCE", "F:1")//, kStateIdle)
805 (bind(&StateMachineFeedback::SetReference, this, placeholders::_1))
806 ("Set a new global reference value (for debug purpose only)");
807
808 AddEvent("SET_Ki", "D:1")//, kStateIdle)
809 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 0))
810 ("Set integral constant Ki");
811
812 AddEvent("SET_Kp", "D:1")//, kStateIdle)
813 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 1))
814 ("Set proportional constant Kp");
815
816 AddEvent("SET_Kd", "D:1")//, kStateIdle)
817 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 2))
818 ("Set derivative constant Kd");
819
820 AddEvent("SET_T", "D:1")//, kStateIdle)
821 (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 3))
822 ("Set time-constant. (-1 to use the cycle time, i.e. the time for the last average cycle, instead)");
823
824 AddEvent("CALIBRATE_CURRENTS", kStateConnected)//, kStateIdle)
825 (bind(&StateMachineFeedback::CalibrateCurrents, this))
826 ("");
827
828 // Verbosity commands
829// AddEvent("SET_VERBOSE", "B:1")
830// (bind(&StateMachineMCP::SetVerbosity, this, placeholders::_1))
831// ("set verbosity state"
832// "|verbosity[bool]:disable or enable verbosity for received data (yes/no), except dynamic data");
833
834 AddEvent("PRINT")
835 (bind(&StateMachineFeedback::Print, this))
836 ("");
837 }
838
839 int EvalOptions(Configuration &conf)
840 {
841 if (!fMap.Read(conf.Get<string>("pixel-map-file")))
842 {
843 Error("Reading mapping table from "+conf.Get<string>("pixel-map-file")+" failed.");
844 return 1;
845 }
846
847 // -110 / -110 (-23 DAC / -0.51V)
848 // Reference voltage: -238 / -203
849 // -360 / -343 ( 23 DAC / 0.51V)
850
851 // 0.005 A/V
852 // 220 Amplitude / 1V
853
854 // Gain = 1V / 200 = 0.005
855
856 fGain = 5; // (BIAS)V / (DRS)V ( 1V / 0.22V )
857
858 fKp = 0;
859 fKd = 0;
860 fKi = 0.12;
861 fT = 1;
862
863 ostringstream msg;
864 msg << "Control loop parameters: ";
865 msg << "Kp=" << fKp << ", Kd=" << fKd << ", Ki=" << fKi << ", ";
866 if (fT>0)
867 msg << fT;
868 else
869 msg << "<auto>";
870 msg << ", Gain(BIAS/DRS)=" << fGain << "V/V";
871
872 Message(msg);
873
874 return -1;
875 }
876};
877
878// ------------------------------------------------------------------------
879
880#include "Main.h"
881
882template<class T>
883int RunShell(Configuration &conf)
884{
885 return Main::execute<T, StateMachineFeedback>(conf);
886}
887
888void SetupConfiguration(Configuration &conf)
889{
890 po::options_description control("Feedback options");
891 control.add_options()
892 ("pixel-map-file", var<string>("FACTmapV5a.txt"), "Pixel mapping file. Used here to get the default reference voltage.")
893 ;
894
895 conf.AddOptions(control);
896}
897
898/*
899 Extract usage clause(s) [if any] for SYNOPSIS.
900 Translators: "Usage" and "or" here are patterns (regular expressions) which
901 are used to match the usage synopsis in program output. An example from cp
902 (GNU coreutils) which contains both strings:
903 Usage: cp [OPTION]... [-T] SOURCE DEST
904 or: cp [OPTION]... SOURCE... DIRECTORY
905 or: cp [OPTION]... -t DIRECTORY SOURCE...
906 */
907void PrintUsage()
908{
909 cout <<
910 "The feedback control the BIAS voltages based on the calibration signal.\n"
911 "\n"
912 "The default is that the program is started without user intercation. "
913 "All actions are supposed to arrive as DimCommands. Using the -c "
914 "option, a local shell can be initialized. With h or help a short "
915 "help message about the usuage can be brought to the screen.\n"
916 "\n"
917 "Usage: feedback [-c type] [OPTIONS]\n"
918 " or: feedback [OPTIONS]\n";
919 cout << endl;
920}
921
922void PrintHelp()
923{
924 Main::PrintHelp<StateMachineFeedback>();
925
926 /* Additional help text which is printed after the configuration
927 options goes here */
928
929 /*
930 cout << "bla bla bla" << endl << endl;
931 cout << endl;
932 cout << "Environment:" << endl;
933 cout << "environment" << endl;
934 cout << endl;
935 cout << "Examples:" << endl;
936 cout << "test exam" << endl;
937 cout << endl;
938 cout << "Files:" << endl;
939 cout << "files" << endl;
940 cout << endl;
941 */
942}
943
944int main(int argc, const char* argv[])
945{
946 Configuration conf(argv[0]);
947 conf.SetPrintUsage(PrintUsage);
948 Main::SetupConfiguration(conf);
949 SetupConfiguration(conf);
950
951 if (!conf.DoParse(argc, argv, PrintHelp))
952 return -1;
953
954 //try
955 {
956 // No console access at all
957 if (!conf.Has("console"))
958 {
959// if (conf.Get<bool>("no-dim"))
960// return RunShell<LocalStream, StateMachine, ConnectionFSC>(conf);
961// else
962 return RunShell<LocalStream>(conf);
963 }
964 // Cosole access w/ and w/o Dim
965/* if (conf.Get<bool>("no-dim"))
966 {
967 if (conf.Get<int>("console")==0)
968 return RunShell<LocalShell, StateMachine, ConnectionFSC>(conf);
969 else
970 return RunShell<LocalConsole, StateMachine, ConnectionFSC>(conf);
971 }
972 else
973*/ {
974 if (conf.Get<int>("console")==0)
975 return RunShell<LocalShell>(conf);
976 else
977 return RunShell<LocalConsole>(conf);
978 }
979 }
980 /*catch (std::exception& e)
981 {
982 cerr << "Exception: " << e.what() << endl;
983 return -1;
984 }*/
985
986 return 0;
987}
Note: See TracBrowser for help on using the repository browser.