Changeset 18724

Timestamp:

01/10/17 13:45:56 (8 years ago)

Author:

(none)

Message:

working version; needs cleanup

Location:

branches/FACT++_lidctrl_usb/src

Files:

• HeadersLid.h (modified) (1 diff)
• lidctrl.cc (modified) (3 diffs)

branches/FACT++_lidctrl_usb/src/HeadersLid.h

@@ -6 +6 @@
     namespace State
     {
-        enum states_t
-        {
-            kOpen = 1,
-            kClosed,
-        };
+       enum states_t
+       {
+           kClosed = 1,
+           kUpperOpening,
+           kHalfOpen,
+           kLowerOpening,
+           kOpen,
+           kLowerClosing,
+           kUpperClosing,
+       };
     }
 }

branches/FACT++_lidctrl_usb/src/lidctrl.cc

@@ -28 +28 @@
 #include "DimDescriptionService.h"
 
+
 class ConnectionLid : public ConnectionUSB
 {
+public:
     vector<char> fRecievedBytes;
-
+    vector<char> fCollectedBytes;
     State::states_t state;
-private:
-
-
-    void HandleReceivedData(const boost::system::error_code&, size_t bytes_received, int type, int counter)
-    {
-        string search_string("system_state=");
-        string s(fRecievedBytes.begin(), fRecievedBytes.end());
-        Out() << s << endl << endl;
-        int pos=s.find(search_string);
-        string part = s.substr(pos + search_string.size(), 1);
-        int state_number = atoi(part.c_str());        
-        Out() << "system state = " << state_number << endl;
-        if (state_number == 0)
+    State::states_t last_state;
+    struct mean_std_t {
+      uint32_t mean;
+      uint32_t var;
+      uint16_t samples;
+    }__attribute__((packed));
+
+   struct motor_status_t{
+      float current_mean;
+      float position_mean;
+      int32_t speed;
+   } __attribute__((__packed__));
+
+   struct message_t {
+      mean_std_t inner_motor_current;
+      mean_std_t outer_motor_current;
+
+      mean_std_t inner_motor_position;
+      mean_std_t outer_motor_position;
+
+      int16_t inner_motor_speed;
+      int16_t outer_motor_speed;
+
+      char current_cmd;
+      uint8_t system_state;
+  }__attribute__((packed));
+
+  DimDescribedService fDimInnerMotor;
+  DimDescribedService fDimOuterMotor;
+    message_t fLastMessage;
+    void PrintMessage(const message_t msg)
+    {
+        Out() << "LastMessage:" << endl;
+        Out() << "    OuterMotor:" << endl;
+        Out() << "        Speed:    " << msg.outer_motor_speed << endl;
+        Out() << "        Current:  " << msg.outer_motor_current.mean << endl;
+        Out() << "        Position: " << msg.outer_motor_position.mean << endl;
+        Out() << "    InnerMotor:" << endl;
+        Out() << "        Speed:    " << msg.inner_motor_speed << endl;
+        Out() << "        Current:  " << msg.inner_motor_current.mean << endl;
+        Out() << "        Position: " << msg.inner_motor_position.mean << endl;
+        Out() << "    LastCommand: " << msg.current_cmd << endl;
+        Out() << "    State: " << (int)msg.system_state << endl;
+    }
+
+    void HandleReceivedData(const boost::system::error_code&, size_t bytes_received, int bar, int foo)
+    {
+        int complete_msg_size = 6 /*start:*/ + sizeof(message_t) + sizeof(uint16_t)/*chksum*/;  // 54 bytes
+ 
+        std::copy(fRecievedBytes.begin(), fRecievedBytes.begin()+bytes_received, std::back_inserter(fCollectedBytes));
+        if (fCollectedBytes.size() > 2*complete_msg_size)
         {
-            state = State::kClosed;
+            string s(fCollectedBytes.begin(), fCollectedBytes.end());
+            string search_string("start:");
+            int pos=s.find(search_string);
+            std::vector<decltype(fCollectedBytes)::value_type>(fCollectedBytes.begin()+pos, fCollectedBytes.end()).swap(fCollectedBytes);
+            string complete_message(fCollectedBytes.begin(), fCollectedBytes.begin()+complete_msg_size);
+            std::vector<decltype(fCollectedBytes)::value_type>(fCollectedBytes.begin()+complete_msg_size, fCollectedBytes.end()).swap(fCollectedBytes);
+            string part = s.substr(6, sizeof(message_t));
+            message_t msg;
+            memcpy(&msg, part.data(), sizeof(message_t));
+            
+            const uint16_t chk0 = Tools::Fletcher16(part.c_str(), sizeof(message_t));
+            uint16_t chk1;
+            part = s.substr(6+sizeof(message_t), sizeof(uint16_t));
+            memcpy(&chk1, part.data(), sizeof(uint16_t));
+            if (chk0==chk1)
+            {
+                fLastMessage = msg;
+                state = static_cast<State::states_t>(msg.system_state + 1);
+                part = s.substr(6, sizeof(message_t)-1); // -1: omit the state
+
+                motor_status_t inner_motor_status;
+                inner_motor_status.current_mean = (float)(msg.inner_motor_current.mean) * 3.4;
+                inner_motor_status.position_mean = (float)(msg.inner_motor_position.mean);
+                inner_motor_status.speed = (int32_t)msg.inner_motor_speed;
+                
+                motor_status_t outer_motor_status;
+                outer_motor_status.current_mean = (float)(msg.outer_motor_current.mean) * 3.4;
+                outer_motor_status.position_mean = (float)(msg.outer_motor_position.mean);
+                outer_motor_status.speed = (int32_t)msg.outer_motor_speed;
+                if ( (last_state != state) || !((state == State::kOpen) || (state == State::kClosed)) )
+                {
+                    fDimInnerMotor.Update(inner_motor_status);
+                    fDimOuterMotor.Update(outer_motor_status);
+                }
+
+                last_state = state;            
+            }
         }
-        if (state_number == 4)
-        {
-            state = State::kOpen;
-        }
-        AsyncRead(ba::buffer(fRecievedBytes, 1024), 0, 0);
-    }
-
-
-    // This is called when a connection was established
+
+        AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0);
+    }
+
     void ConnectionEstablished()
     {
-        AsyncRead(ba::buffer(fRecievedBytes, 1024), 0, 0);
-    }
-
-
-public:
+        AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0);
+    }
+
     ConnectionLid(ba::io_service& ioservice, MessageImp &imp) :
         ConnectionUSB(ioservice, imp()),
-        fRecievedBytes(1024),
-        state(State::kOpen)
+        fRecievedBytes(128),
+        fCollectedBytes(0),
+        state(State::kOpen),
+        fDimInnerMotor("LID_CONTROL/INNER_MOTOR", "F:1;F:1;I:1",
+            "|current_mean[mA]: "
+            "|position_mean[LSB]: "
+            "|speed[LSB]: "),
+        fDimOuterMotor("LID_CONTROL/OUTER_MOTOR", "F:1;F:1;I:1",
+            "|current_mean[mA]: "
+            "|position_mean[LSB]: "
+            "|speed[LSB]: "),
+       fLastMessage()
+
     {
         SetLogStream(&imp);
@@ -77 +157 @@
         return state;
     }
+
+
+
 };
 
@@ -129 +212 @@
     }
 
+    int Print()
+    {
+        fLid.PrintMessage(fLid.fLastMessage);
+        return T::GetCurrentState();
+    }
+
 public:
     StateMachineLid(ostream &out=cout) :
-        StateMachineAsio<T>(out, "LID_CTRL_USB"), fLid(*this, *this)
+        StateMachineAsio<T>(out, "LID_CONTROL"), fLid(*this, *this)
     {
         T::AddStateName(State::kOpen, "Open", "Lid is open");
         T::AddStateName(State::kClosed, "Closed", "Lid is closed");
+        T::AddStateName(State::kUpperOpening, "UpperOpening", "upper(right) lid is opening");
+        T::AddStateName(State::kHalfOpen, "HalfOpen", "upper(right) lid open, lower(left) lid closed");
+        T::AddStateName(State::kLowerOpening, "LowerOpening", "lower(left) lid is opening");
+        T::AddStateName(State::kLowerClosing, "LowerClosing", "lower(left) lid is closing");
+        T::AddStateName(State::kUpperClosing, "UpperClosing", "upper(right) lid is closing");
+
         T::AddEvent("OPEN")(bind(&StateMachineLid::Open, this))("Open Lid");
         T::AddEvent("CLOSE")(bind(&StateMachineLid::Close, this))("Close Lid");
+        T::AddEvent("PRINT")(bind(&StateMachineLid::Print, this))("Print the last message");
     }