ipa_canopen_core: ipa_canopen_core: canopen.cpp Source File

00001 #include "canopen.h"
00002 
00003 namespace canopen {
00004 
00005   // ----------------- Variable definitions: --------------
00006 
00007   std::chrono::milliseconds syncInterval;
00008   HANDLE h;
00009   std::map<uint8_t, Device> devices;
00010   std::map<std::string, DeviceGroup> deviceGroups;
00011 
00012   std::map<SDOkey, std::function<void (uint8_t CANid, BYTE data[8])> > incomingDataHandlers
00013   { { statusword, statusword_incoming },
00014       { modes_of_operation_display, modes_of_operation_display_incoming }  };
00015 
00016   std::map<uint16_t, std::function<void (const TPCANRdMsg m)> > incomingPDOHandlers;
00017 
00018   TPCANMsg NMTmsg;
00019   TPCANMsg syncMsg;
00020   TPCANMsg nodeguardMsg;
00021   bool atFirstInit = true;
00022 
00023   // ----------------- Function definitions: --------------
00024 
00025   bool openConnection(std::string devName) {
00026     h = LINUX_CAN_Open(devName.c_str(), O_RDWR);
00027     if (!h) return false;
00028     errno = CAN_Init(h, CAN_BAUD_500K, CAN_INIT_TYPE_ST);
00029     return true;
00030   }
00031 
00032   void setMotorState(uint16_t CANid, std::string targetState) { // todo: not finished
00033     // if (devices[CANid].motorState_ == "fault")
00034     while (devices[CANid].motorState_ != targetState) {
00035       canopen::sendSDO(CANid, canopen::statusword);
00036       if (devices[CANid].motorState_ == "fault") {
00037         canopen::sendSDO(CANid, canopen::controlword, canopen::controlword_fault_reset_0);
00038         std::this_thread::sleep_for(std::chrono::milliseconds(50));
00039         canopen::sendSDO(CANid, canopen::controlword, canopen::controlword_fault_reset_1);
00040         std::this_thread::sleep_for(std::chrono::milliseconds(200));
00041       }
00042       if (devices[CANid].motorState_ == "switch_on_disabled") {
00043         canopen::sendSDO(CANid, canopen::controlword, canopen::controlword_shutdown);
00044       }
00045       if (devices[CANid].motorState_ == "ready_to_switch_on") {
00046         canopen::sendSDO(CANid, canopen::controlword, canopen::controlword_switch_on);
00047       }
00048       if (devices[CANid].motorState_ == "switched_on") {
00049         canopen::sendSDO(CANid, canopen::controlword, canopen::controlword_enable_operation);
00050       }
00051       std::this_thread::sleep_for(std::chrono::milliseconds(50));
00052     }
00053   }
00054 
00055   void init(std::string deviceFile, std::chrono::milliseconds syncInterval) {
00056     // canopen::devices must be set up before this function is called
00057     CAN_Close(h);
00058 
00059     syncMsg.ID = 0x80;
00060     syncMsg.MSGTYPE = 0x00;
00061     syncMsg.LEN = 0x00;
00062     NMTmsg.ID = 0;
00063     NMTmsg.MSGTYPE = 0x00;
00064     NMTmsg.LEN = 2;
00065     nodeguardMsg.MSGTYPE = 0x01; // remote frame
00066     nodeguardMsg.LEN = 0;
00067 
00068     if (!canopen::openConnection(deviceFile)) {
00069       std::cout << "Cannot open CAN device; aborting." << std::endl;
00070       exit(EXIT_FAILURE);
00071     }
00072 
00073     if (atFirstInit)
00074       canopen::initListenerThread(canopen::defaultListener);
00075       // atFirstInit = false;
00076     // }
00077 
00078     for (auto device : devices) {
00079       sendSDO(device.second.CANid_, ip_time_units, (uint8_t) syncInterval.count() );
00080       sendSDO(device.second.CANid_, ip_time_index, ip_time_index_milliseconds);
00081       sendSDO(device.second.CANid_, sync_timeout_factor, sync_timeout_factor_disable_timeout);
00082 
00083       // NMT & motor state machine:
00084       if (atFirstInit) {
00085         canopen::sendNMT(device.second.CANid_, canopen::NMT_stop);
00086         std::this_thread::sleep_for(std::chrono::milliseconds(100));
00087         canopen::sendNMT(device.second.CANid_, canopen::NMT_start);
00088         std::this_thread::sleep_for(std::chrono::milliseconds(100));
00089       }
00090 
00091       setMotorState(device.second.CANid_, "operation_enable");
00092       /* canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_shutdown);
00093       canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_switch_on);
00094       canopen::sendSDO(device.second.CANid_, canopen::controlword, canopen::controlword_enable_operation); */
00095     }
00096     
00097     if (atFirstInit)
00098       atFirstInit = false;
00099   }
00100 
00101   void defaultListener() {
00102     while (true) {
00103       TPCANRdMsg m;
00104       if ((errno = LINUX_CAN_Read(h, &m))) {
00105         perror("LINUX_CAN_Read() error");
00106       }
00107 
00108       if (m.Msg.ID >= 0x180 && m.Msg.ID <= 0x4ff) { // incoming PDO 
00109         if (incomingPDOHandlers.find(m.Msg.ID) != incomingPDOHandlers.end())
00110           incomingPDOHandlers[m.Msg.ID](m);
00111       }
00112       else if (m.Msg.ID >= 0x580 && m.Msg.ID <= 0x5ff) { // incoming SDO 
00113         SDOkey sdoKey(m);
00114         if (incomingDataHandlers.find(sdoKey) != incomingDataHandlers.end())
00115           incomingDataHandlers[sdoKey](m.Msg.ID - 0x580, m.Msg.DATA);
00116       } else if (m.Msg.ID >= 0x700 && m.Msg.ID <= 0x7FF) 
00117         incomingNodeguardHandler(m.Msg.ID - 0x700, m.Msg.DATA);
00118     }
00119   }
00120 
00121   void initListenerThread(std::function<void ()> const& listener) {
00122     std::thread listener_thread(listener);
00123     listener_thread.detach();
00124     std::this_thread::sleep_for(std::chrono::milliseconds(10));
00125   }
00126 
00127   void initDeviceManagerThread(std::function<void ()> const& deviceManager) {
00128     std::thread device_manager_thread(deviceManager);
00129     device_manager_thread.detach();
00130     std::this_thread::sleep_for(std::chrono::milliseconds(10));
00131   }
00132 
00133   void deviceManager() {
00134     // todo: init, recover... (e.g. when to start/stop sending SYNCs)
00135     while (true) {
00136       auto tic = std::chrono::high_resolution_clock::now();
00137       for (auto device : devices) {
00138         if (device.second.initialized) {
00139           devices[device.first].updateDesiredPos();
00140           sendPos(device.second.CANid_, device.second.desiredPos_);
00141         }
00142       }
00143       sendSync();
00144       std::this_thread::sleep_for(syncInterval - (std::chrono::high_resolution_clock::now() - tic ));
00145     }
00146   }
00147 
00148   // ----------------- Functions to handle incoming data: --------------
00149 
00150   void incomingNodeguardHandler(uint8_t CANid, BYTE data[8]) {
00151     // update variables of the corresponding device object with nodeguard info
00152   }
00153 
00154   void statusword_incoming(uint8_t CANid, BYTE data[8]) {
00155 
00156     /* for (int i=0; i<8; i++)
00157       printf("%02x ", data[i]);
00158       std::cout << std::endl; */
00159 
00160     uint16_t mydata = data[4] + (data[5] << 8);
00161     // update variables of the corresponding device object, e.g.
00162     // printf("mydata: %04x\n", mydata);
00163     if ((mydata & 8) == 8) 
00164       devices[CANid].motorState_ = "fault";
00165     else if ((mydata & 0x4f) == 0x40) 
00166       devices[CANid].motorState_ = "switch_on_disabled";
00167     else if ((mydata & 0x6f) == 0x21)
00168       devices[CANid].motorState_ = "ready_to_switch_on";
00169     else if ((mydata & 0x6f) == 0x23)
00170       devices[CANid].motorState_ = "switched_on";
00171     else if ((mydata & 0x6f) == 0x27)
00172       devices[CANid].motorState_ = "operation_enable";
00173 
00174     devices[CANid].voltage_enabled_ = (mydata >> 4) & 0x1;
00175     devices[CANid].driveReferenced_ = (mydata >> 15) & 0x1;
00176     
00177     std::cout << "STATUSWORD incoming; drive referenced? " << 
00178       devices[CANid].driveReferenced_ << std::endl;
00179   }
00180 
00181   void modes_of_operation_display_incoming(uint8_t CANid, BYTE data[8]) {
00182     // update variables of the corresponding device object
00183   }
00184 
00185   void schunkDefaultPDO_incoming(uint8_t CANid, const TPCANRdMsg m) {
00186     double newPos = mdeg2rad(m.Msg.DATA[4] + (m.Msg.DATA[5] << 8) + 
00187                              (m.Msg.DATA[6] << 16) + (m.Msg.DATA[7] << 24) );
00188 
00189     if (devices[CANid].timeStamp_msec_ != std::chrono::milliseconds(0) ||  // already 1 msg received
00190         devices[CANid].timeStamp_usec_ != std::chrono::microseconds(0)) {
00191       auto deltaTime_msec = std::chrono::milliseconds(m.dwTime) - devices[CANid].timeStamp_msec_;
00192       auto deltaTime_usec = std::chrono::microseconds(m.wUsec) - devices[CANid].timeStamp_usec_;
00193       double deltaTime_double = static_cast<double>
00194         (deltaTime_msec.count()*1000 + deltaTime_usec.count()) * 0.000001;
00195       devices[ CANid ].actualVel_ = (newPos - devices[CANid].actualPos_) / deltaTime_double;
00196 
00197       if (! devices[CANid].initialized) {
00198         devices[ CANid ].desiredPos_ = devices[ CANid ].actualPos_;
00199         devices[ CANid ].initialized = true;
00200       }
00201     }
00202      
00203     devices[ CANid ].actualPos_ = newPos;
00204     devices[ CANid ].timeStamp_msec_ = std::chrono::milliseconds(m.dwTime);
00205     devices[ CANid ].timeStamp_usec_ = std::chrono::microseconds(m.wUsec);
00206   }
00207   
00208   // ----------------- Functions for sending out data: --------------
00209 
00210   void sendSDO(uint8_t CANid, SDOkey sdo) {
00211     // for SDO read commands, e.g. statusword
00212     TPCANMsg msg;
00213     msg.ID = CANid + 0x600; // 0x600 = SDO identifier
00214     msg.MSGTYPE = 0x00; // standard message
00215     msg.LEN = 4;
00216     msg.DATA[0] = 0x40;
00217     msg.DATA[1] = (sdo.index & 0xFF);
00218     msg.DATA[2] = (sdo.index >> 8) & 0xFF; 
00219     msg.DATA[3] = sdo.subindex;
00220     CAN_Write(h, &msg);
00221   }
00222 
00223   void sendSDO(uint8_t CANid, SDOkey sdo, uint32_t value) {
00224     TPCANMsg msg;
00225     msg.ID = CANid + 0x600; // 0x600 = SDO identifier
00226     msg.LEN = 8;
00227     msg.DATA[0] = 0x23;
00228     msg.DATA[1] = (sdo.index & 0xFF);
00229     msg.DATA[2] = (sdo.index >> 8) & 0xFF; 
00230     msg.DATA[3] = sdo.subindex;
00231     msg.DATA[4] = value & 0xFF;
00232     msg.DATA[5] = (value >> 8) & 0xFF;
00233     msg.DATA[6] = (value >> 16) & 0xFF;
00234     msg.DATA[7] = (value >> 24) & 0xFF;
00235     CAN_Write(h, &msg);
00236   } 
00237 
00238   void sendSDO(uint8_t CANid, SDOkey sdo, int32_t value) {
00239     TPCANMsg msg;
00240     msg.ID = CANid + 0x600; // 0x600 = SDO identifier
00241     msg.LEN = 8;
00242     msg.DATA[0] = 0x23;
00243     msg.DATA[1] = (sdo.index & 0xFF);
00244     msg.DATA[2] = (sdo.index >> 8) & 0xFF; // todo: & 0xFF not needed, I think
00245     msg.DATA[3] = sdo.subindex;
00246     msg.DATA[4] = value & 0xFF;
00247     msg.DATA[5] = (value >> 8) & 0xFF;
00248     msg.DATA[6] = (value >> 16) & 0xFF;
00249     msg.DATA[7] = (value >> 24) & 0xFF;
00250     CAN_Write(h, &msg);
00251   } 
00252 
00253   void sendSDO(uint8_t CANid, SDOkey sdo, uint8_t value) {
00254     TPCANMsg msg;
00255     msg.ID = CANid + 0x600; // 0x600 = SDO identifier
00256     msg.LEN = 5;
00257     msg.DATA[0] = 0x2F;
00258     msg.DATA[1] = (sdo.index & 0xFF);
00259     msg.DATA[2] = (sdo.index >> 8) & 0xFF; // todo: & 0xFF not needed, I think
00260     msg.DATA[3] = sdo.subindex;
00261     msg.DATA[4] = value & 0xFF;
00262     CAN_Write(h, &msg);
00263   } 
00264 
00265   void sendSDO(uint8_t CANid, SDOkey sdo, uint16_t value) {
00266     TPCANMsg msg;
00267     msg.ID = CANid + 0x600; // 0x600 = SDO identifier
00268     msg.LEN = 6;
00269     msg.DATA[0] = 0x2B;
00270     msg.DATA[1] = (sdo.index & 0xFF);
00271     msg.DATA[2] = (sdo.index >> 8) & 0xFF; // todo: & 0xFF not needed, I think
00272     msg.DATA[3] = sdo.subindex;
00273     msg.DATA[4] = value & 0xFF;
00274     msg.DATA[5] = (value >> 8) & 0xFF;
00275     CAN_Write(h, &msg);
00276   } 
00277 
00278   std::function< void (uint16_t CANid, double positionValue) > sendPos;
00279 
00280   void schunkDefaultPDOOutgoing(uint16_t CANid, double positionValue) {
00281     static const uint16_t myControlword = 
00282       controlword_enable_operation | controlword_enable_ip_mode;
00283     TPCANMsg msg;
00284     msg.ID = 0x200 + CANid;
00285     msg.MSGTYPE = 0x00;
00286     msg.LEN = 8;
00287     msg.DATA[0] = myControlword & 0xFF;
00288     msg.DATA[1] = (myControlword >> 8) & 0xFF; // todo: & not needed (?)
00289     msg.DATA[2] = 0;
00290     msg.DATA[3] = 0;
00291     int32_t mdegPos = rad2mdeg(positionValue);
00292     msg.DATA[4] = mdegPos & 0xFF;
00293     msg.DATA[5] = (mdegPos >> 8) & 0xFF;
00294     msg.DATA[6] = (mdegPos >> 16) & 0xFF;
00295     msg.DATA[7] = (mdegPos >> 24) & 0xFF;
00296     CAN_Write(h, &msg);
00297   }
00298 
00299 }