Changeset 18724 for branches/FACT++_lidctrl_usb
- Timestamp:
- 01/10/17 13:45:56 (8 years ago)
- Location:
- branches/FACT++_lidctrl_usb/src
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
branches/FACT++_lidctrl_usb/src/HeadersLid.h
r18717 r18724 6 6 namespace State 7 7 { 8 enum states_t 9 { 10 kOpen = 1, 11 kClosed, 12 }; 8 enum states_t 9 { 10 kClosed = 1, 11 kUpperOpening, 12 kHalfOpen, 13 kLowerOpening, 14 kOpen, 15 kLowerClosing, 16 kUpperClosing, 17 }; 13 18 } 14 19 } -
branches/FACT++_lidctrl_usb/src/lidctrl.cc
r18718 r18724 28 28 #include "DimDescriptionService.h" 29 29 30 30 31 class ConnectionLid : public ConnectionUSB 31 32 { 33 public: 32 34 vector<char> fRecievedBytes; 33 35 vector<char> fCollectedBytes; 34 36 State::states_t state; 35 private: 36 37 38 void HandleReceivedData(const boost::system::error_code&, size_t bytes_received, int type, int counter) 39 { 40 string search_string("system_state="); 41 string s(fRecievedBytes.begin(), fRecievedBytes.end()); 42 Out() << s << endl << endl; 43 int pos=s.find(search_string); 44 string part = s.substr(pos + search_string.size(), 1); 45 int state_number = atoi(part.c_str()); 46 Out() << "system state = " << state_number << endl; 47 if (state_number == 0) 37 State::states_t last_state; 38 struct mean_std_t { 39 uint32_t mean; 40 uint32_t var; 41 uint16_t samples; 42 }__attribute__((packed)); 43 44 struct motor_status_t{ 45 float current_mean; 46 float position_mean; 47 int32_t speed; 48 } __attribute__((__packed__)); 49 50 struct message_t { 51 mean_std_t inner_motor_current; 52 mean_std_t outer_motor_current; 53 54 mean_std_t inner_motor_position; 55 mean_std_t outer_motor_position; 56 57 int16_t inner_motor_speed; 58 int16_t outer_motor_speed; 59 60 char current_cmd; 61 uint8_t system_state; 62 }__attribute__((packed)); 63 64 DimDescribedService fDimInnerMotor; 65 DimDescribedService fDimOuterMotor; 66 message_t fLastMessage; 67 void PrintMessage(const message_t msg) 68 { 69 Out() << "LastMessage:" << endl; 70 Out() << " OuterMotor:" << endl; 71 Out() << " Speed: " << msg.outer_motor_speed << endl; 72 Out() << " Current: " << msg.outer_motor_current.mean << endl; 73 Out() << " Position: " << msg.outer_motor_position.mean << endl; 74 Out() << " InnerMotor:" << endl; 75 Out() << " Speed: " << msg.inner_motor_speed << endl; 76 Out() << " Current: " << msg.inner_motor_current.mean << endl; 77 Out() << " Position: " << msg.inner_motor_position.mean << endl; 78 Out() << " LastCommand: " << msg.current_cmd << endl; 79 Out() << " State: " << (int)msg.system_state << endl; 80 } 81 82 void HandleReceivedData(const boost::system::error_code&, size_t bytes_received, int bar, int foo) 83 { 84 int complete_msg_size = 6 /*start:*/ + sizeof(message_t) + sizeof(uint16_t)/*chksum*/; // 54 bytes 85 86 std::copy(fRecievedBytes.begin(), fRecievedBytes.begin()+bytes_received, std::back_inserter(fCollectedBytes)); 87 if (fCollectedBytes.size() > 2*complete_msg_size) 48 88 { 49 state = State::kClosed; 89 string s(fCollectedBytes.begin(), fCollectedBytes.end()); 90 string search_string("start:"); 91 int pos=s.find(search_string); 92 std::vector<decltype(fCollectedBytes)::value_type>(fCollectedBytes.begin()+pos, fCollectedBytes.end()).swap(fCollectedBytes); 93 string complete_message(fCollectedBytes.begin(), fCollectedBytes.begin()+complete_msg_size); 94 std::vector<decltype(fCollectedBytes)::value_type>(fCollectedBytes.begin()+complete_msg_size, fCollectedBytes.end()).swap(fCollectedBytes); 95 string part = s.substr(6, sizeof(message_t)); 96 message_t msg; 97 memcpy(&msg, part.data(), sizeof(message_t)); 98 99 const uint16_t chk0 = Tools::Fletcher16(part.c_str(), sizeof(message_t)); 100 uint16_t chk1; 101 part = s.substr(6+sizeof(message_t), sizeof(uint16_t)); 102 memcpy(&chk1, part.data(), sizeof(uint16_t)); 103 if (chk0==chk1) 104 { 105 fLastMessage = msg; 106 state = static_cast<State::states_t>(msg.system_state + 1); 107 part = s.substr(6, sizeof(message_t)-1); // -1: omit the state 108 109 motor_status_t inner_motor_status; 110 inner_motor_status.current_mean = (float)(msg.inner_motor_current.mean) * 3.4; 111 inner_motor_status.position_mean = (float)(msg.inner_motor_position.mean); 112 inner_motor_status.speed = (int32_t)msg.inner_motor_speed; 113 114 motor_status_t outer_motor_status; 115 outer_motor_status.current_mean = (float)(msg.outer_motor_current.mean) * 3.4; 116 outer_motor_status.position_mean = (float)(msg.outer_motor_position.mean); 117 outer_motor_status.speed = (int32_t)msg.outer_motor_speed; 118 if ( (last_state != state) || !((state == State::kOpen) || (state == State::kClosed)) ) 119 { 120 fDimInnerMotor.Update(inner_motor_status); 121 fDimOuterMotor.Update(outer_motor_status); 122 } 123 124 last_state = state; 125 } 50 126 } 51 if (state_number == 4) 52 { 53 state = State::kOpen; 54 } 55 AsyncRead(ba::buffer(fRecievedBytes, 1024), 0, 0); 56 } 57 58 59 // This is called when a connection was established 127 128 AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0); 129 } 130 60 131 void ConnectionEstablished() 61 132 { 62 AsyncRead(ba::buffer(fRecievedBytes, 1024), 0, 0); 63 } 64 65 66 public: 133 AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0); 134 } 135 67 136 ConnectionLid(ba::io_service& ioservice, MessageImp &imp) : 68 137 ConnectionUSB(ioservice, imp()), 69 fRecievedBytes(1024), 70 state(State::kOpen) 138 fRecievedBytes(128), 139 fCollectedBytes(0), 140 state(State::kOpen), 141 fDimInnerMotor("LID_CONTROL/INNER_MOTOR", "F:1;F:1;I:1", 142 "|current_mean[mA]: " 143 "|position_mean[LSB]: " 144 "|speed[LSB]: "), 145 fDimOuterMotor("LID_CONTROL/OUTER_MOTOR", "F:1;F:1;I:1", 146 "|current_mean[mA]: " 147 "|position_mean[LSB]: " 148 "|speed[LSB]: "), 149 fLastMessage() 150 71 151 { 72 152 SetLogStream(&imp); … … 77 157 return state; 78 158 } 159 160 161 79 162 }; 80 163 … … 129 212 } 130 213 214 int Print() 215 { 216 fLid.PrintMessage(fLid.fLastMessage); 217 return T::GetCurrentState(); 218 } 219 131 220 public: 132 221 StateMachineLid(ostream &out=cout) : 133 StateMachineAsio<T>(out, "LID_C TRL_USB"), fLid(*this, *this)222 StateMachineAsio<T>(out, "LID_CONTROL"), fLid(*this, *this) 134 223 { 135 224 T::AddStateName(State::kOpen, "Open", "Lid is open"); 136 225 T::AddStateName(State::kClosed, "Closed", "Lid is closed"); 226 T::AddStateName(State::kUpperOpening, "UpperOpening", "upper(right) lid is opening"); 227 T::AddStateName(State::kHalfOpen, "HalfOpen", "upper(right) lid open, lower(left) lid closed"); 228 T::AddStateName(State::kLowerOpening, "LowerOpening", "lower(left) lid is opening"); 229 T::AddStateName(State::kLowerClosing, "LowerClosing", "lower(left) lid is closing"); 230 T::AddStateName(State::kUpperClosing, "UpperClosing", "upper(right) lid is closing"); 231 137 232 T::AddEvent("OPEN")(bind(&StateMachineLid::Open, this))("Open Lid"); 138 233 T::AddEvent("CLOSE")(bind(&StateMachineLid::Close, this))("Close Lid"); 234 T::AddEvent("PRINT")(bind(&StateMachineLid::Print, this))("Print the last message"); 139 235 } 140 236
Note:
See TracChangeset
for help on using the changeset viewer.