Ignore:
Timestamp:
01/10/17 13:45:56 (8 years ago)
Author:
(none)
Message:
working version; needs cleanup
Location:
branches/FACT++_lidctrl_usb/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • branches/FACT++_lidctrl_usb/src/HeadersLid.h

    r18717 r18724  
    66    namespace State
    77    {
    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       };
    1318    }
    1419}
  • branches/FACT++_lidctrl_usb/src/lidctrl.cc

    r18718 r18724  
    2828#include "DimDescriptionService.h"
    2929
     30
    3031class ConnectionLid : public ConnectionUSB
    3132{
     33public:
    3234    vector<char> fRecievedBytes;
    33 
     35    vector<char> fCollectedBytes;
    3436    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)
    4888        {
    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            }
    50126        }
    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
    60131    void ConnectionEstablished()
    61132    {
    62         AsyncRead(ba::buffer(fRecievedBytes, 1024), 0, 0);
    63     }
    64 
    65 
    66 public:
     133        AsyncRead(ba::buffer(fRecievedBytes, 128), 0, 0);
     134    }
     135
    67136    ConnectionLid(ba::io_service& ioservice, MessageImp &imp) :
    68137        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
    71151    {
    72152        SetLogStream(&imp);
     
    77157        return state;
    78158    }
     159
     160
     161
    79162};
    80163
     
    129212    }
    130213
     214    int Print()
     215    {
     216        fLid.PrintMessage(fLid.fLastMessage);
     217        return T::GetCurrentState();
     218    }
     219
    131220public:
    132221    StateMachineLid(ostream &out=cout) :
    133         StateMachineAsio<T>(out, "LID_CTRL_USB"), fLid(*this, *this)
     222        StateMachineAsio<T>(out, "LID_CONTROL"), fLid(*this, *this)
    134223    {
    135224        T::AddStateName(State::kOpen, "Open", "Lid is open");
    136225        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
    137232        T::AddEvent("OPEN")(bind(&StateMachineLid::Open, this))("Open Lid");
    138233        T::AddEvent("CLOSE")(bind(&StateMachineLid::Close, this))("Close Lid");
     234        T::AddEvent("PRINT")(bind(&StateMachineLid::Print, this))("Print the last message");
    139235    }
    140236
Note: See TracChangeset for help on using the changeset viewer.