Index: /trunk/FACT++/src/feedback.cc
===================================================================
--- /trunk/FACT++/src/feedback.cc	(revision 12046)
+++ /trunk/FACT++/src/feedback.cc	(revision 12047)
@@ -53,4 +53,5 @@
         kStateConnected,
         kStateRunning,
+        kStateControlling,
     };
 
@@ -73,5 +74,5 @@
     vector<vector<float>> fData;
 
-    int fCursor;
+    uint64_t fCursor;
 
     Time fBiasLast;
@@ -86,4 +87,6 @@
     double fKd;               // Derivative   constant
 
+    bool fOutputEnabled;
+
     pair<Time, int> GetNewState(DimStampedInfo &info) const
     {
@@ -108,7 +111,13 @@
         fSP = valarray<double>(0., 416);
 
+        vector<float> vec(2*BIAS::kNumChannels);
+        fDimDeviation.Update(vec);
+
         fPV[0].resize(0);
         fPV[1].resize(0);
         fPV[2].resize(0);
+
+        if (fKp==0 && fKi==0 && fKd==0)
+            Warn("Control loop parameters are all set to zero.");
     }
 
@@ -165,8 +174,4 @@
                                                reinterpret_cast<float*>(curr->getData())+1440);
 
-
-            ostringstream out;
-            out << "New event received: " << fCursor;
-            Info(out);
 
             fCursor++;
@@ -248,5 +253,5 @@
                 const Time tm0 = Time();
 
-                const double T21 = (tm0-fStartTime).total_microseconds()/1000000.;
+                /*const*/ double T21 = (tm0-fStartTime).total_microseconds()/1000000.;
                 const double T10 = fT;
                 fT = T21;
@@ -254,5 +259,7 @@
                 fStartTime = tm0;
 
-                cout << "Step..." << endl;
+                ostringstream out;
+                out << "New " << fData.size() << " event received: " << fCursor << " / " << setprecision(3) << T21 << "s";
+                Info(out);
 
                 if (fPV[0].size()==0)
@@ -284,5 +291,5 @@
                                 return;
 
-                            cout << "Calculating (" << fCursor << ")... " << endl;
+                            cout << "Calculating (" << fCursor << ":" << T21 << ")... " << endl;
 
                             // fKi[j] = response[j]*gain;
@@ -290,14 +297,33 @@
                             // Kd = 0;
 
-                            //                                                                                  (PV[2]-PV[1] + PV[0]-PV[1])
+                            //                    -110 / -110 (-23 DAC / -0.51V)
+                            // Reference voltage: -238 / -203
+                            //                    -360 / -343 ( 23 DAC /  0.51V)
+
+                            // 0.005 A/V
+                            // 220 Amplitude / 1V
+
+                            // Gain = 1V / 200 = 0.005
+
+                            // => Kp = 0.01 * gain     = 0.00005
+                            // => Ki = 0.8  * gain/20s = 0.00025
+                            // => Kd = 0.1  * gain/20s = 0.00003
+
+                            fKp = 0;
+                            fKd = 0;
+                            fKi = 0.00003*20;
+                            T21 = 1;
+
                             //valarray<double> correction = - Kp*(PV[2] - PV[1]) + Ki * dT * (SP-PV[2]) - Kd/dT * (PV[2] - 2*PV[1] + PV[0]);
                             //valarray<double> correction =
                             //    - Kp*(PV[2] - PV[1]) + Ki * T21 * (SP-PV[2]) - Kd*(PV[2]-PV[1])/T21 - Kd*(PV[0]-PV[1])/T01;
                             const valarray<double> correction =
-                                - (fKp+fKd/T21)*(fPV[2] - fPV[1]) + fKi*T21*(fSP-fPV[2]) + fKd/T10*(fPV[1]-fPV[0]);
+                                - (fKp+fKd/T21)*(fPV[2] - fPV[1])
+                                +  fKi*T21*(fSP-fPV[2])
+                                +  fKd/T10*(fPV[1]-fPV[0]);
 
                             vector<float> vec(2*BIAS::kNumChannels);
                             for (int i=0; i<BIAS::kNumChannels; i++)
-                                vec[i] = fSP[i]-fPV[2][i];
+                                vec[i] = fPV[2][i]-fSP[i];
 
                             for (int i=0; i<BIAS::kNumChannels; i++)
@@ -305,4 +331,23 @@
 
                             fDimDeviation.Update(vec);
+
+                            if (fOutputEnabled)
+                            {
+                                Info("Sending correction to feedback.");
+
+                                dic_cmnd_service("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
+                                                 (void*)(vec.data()+416), 416*sizeof(float));
+
+                                /*
+                                if (!Dim::SendCommand("BIAS_CONTROL/ADD_REFERENCE_VOLTAGES",
+                                                      (const void*)(vec.data()+416), 416*sizeof(float)))
+                                {
+                                    Error("Sending correction to bias control failed... switching off.");
+                                    fOutputEnabled=false;
+                                }
+                                else
+                                   Info("Success!");
+                                */
+                            }
                         }
 
@@ -383,14 +428,25 @@
     {
         if (!CheckEventSize(evt.GetSize(), "SetConstant", 8))
-            return T::kSM_FatalError;
+            return kSM_FatalError;
 
         switch (constant)
         {
-        case 0: fKi = evt.GetDouble(); break,
+        case 0: fKi = evt.GetDouble(); break;
         case 1: fKp = evt.GetDouble(); break;
         case 2: fKd = evt.GetDouble(); break;
         default:
             Fatal("SetConstant got an unexpected constant id -- this is a program bug!");
-        }
+            return kSM_FatalError;
+        }
+
+        return GetCurrentState();
+    }
+
+    int EnableOutput(const EventImp &evt)
+    {
+        if (!CheckEventSize(evt.GetSize(), "EnableOutput", 1))
+            return kSM_FatalError;
+
+        fOutputEnabled = evt.GetBool();
 
         return GetCurrentState();
@@ -472,5 +528,5 @@
         fDimReference("FEEDBACK/REFERENCE", "F:416",        ""),
         fDimDeviation("FEEDBACK/DEVIATION", "F:416;F:416",  ""),
-        fBiasData(NULL)
+        fBiasData(NULL), fKp(0), fKi(0), fKd(0), fOutputEnabled(false)
     {
         // ba::io_service::work is a kind of keep_alive for the loop.
@@ -505,4 +561,8 @@
             ("");
 
+        AddEvent("ENABLE_OUTPUT", "B:1")//, kStateIdle)
+            (bind(&StateMachineFeedback::EnableOutput, this, placeholders::_1))
+            ("");
+
         AddEvent("STORE_REFERENCE")//, kStateIdle)
             (bind(&StateMachineFeedback::StoreReference, this))
@@ -510,13 +570,13 @@
 
         AddEvent("SET_Ki", "D:1")//, kStateIdle)
-            (bind(&StateMachineFeedback::SetConstant, this, 0))
+            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 0))
             ("Set integral constant Ki");
 
         AddEvent("SET_Kp", "D:1")//, kStateIdle)
-            (bind(&StateMachineFeedback::SetConstant, this, 1))
+            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 1))
             ("Set proportional constant Kp");
 
         AddEvent("SET_Kd", "D:1")//, kStateIdle)
-            (bind(&StateMachineFeedback::SetConstant, this, 2))
+            (bind(&StateMachineFeedback::SetConstant, this, placeholders::_1, 2))
             ("Set derivative constant Kd");
 
@@ -538,7 +598,7 @@
         //fFSC.SetVerbose(!conf.Get<bool>("quiet"));
 
-        if (!fMap.Read("FACTmapV5.txt"))
-        {
-            Error("Reading mapping table from FACTmapV5.txt failed.");
+        if (!fMap.Read("FACTmapV5a.txt"))
+        {
+            Error("Reading mapping table from FACTmapV5a.txt failed.");
             return 1;
         }
@@ -652,2 +712,4 @@
     return 0;
 }
+
+const PixelMapEntry PixelMap::empty = { 0, 0, 0, 0, 0, 0 };