Index: branches/FACT++_lidctrl_usb/src/HeadersLid.h =================================================================== --- branches/FACT++_lidctrl_usb/src/HeadersLid.h (revision 18718) +++ branches/FACT++_lidctrl_usb/src/HeadersLid.h (revision 18724) @@ -6,9 +6,14 @@ namespace State { - enum states_t - { - kOpen = 1, - kClosed, - }; + enum states_t + { + kClosed = 1, + kUpperOpening, + kHalfOpen, + kLowerOpening, + kOpen, + kLowerClosing, + kUpperClosing, + }; } } Index: branches/FACT++_lidctrl_usb/src/lidctrl.cc =================================================================== --- branches/FACT++_lidctrl_usb/src/lidctrl.cc (revision 18718) +++ branches/FACT++_lidctrl_usb/src/lidctrl.cc (revision 18724) @@ -28,45 +28,125 @@ #include "DimDescriptionService.h" + class ConnectionLid : public ConnectionUSB { +public: vector fRecievedBytes; - + vector 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(fCollectedBytes.begin()+pos, fCollectedBytes.end()).swap(fCollectedBytes); + string complete_message(fCollectedBytes.begin(), fCollectedBytes.begin()+complete_msg_size); + std::vector(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(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,4 +157,7 @@ return state; } + + + }; @@ -129,12 +212,25 @@ } + int Print() + { + fLid.PrintMessage(fLid.fLastMessage); + return T::GetCurrentState(); + } + public: StateMachineLid(ostream &out=cout) : - StateMachineAsio(out, "LID_CTRL_USB"), fLid(*this, *this) + StateMachineAsio(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"); }