plugin.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #include <class_loader/class_loader.h>
00019 #include <canopen_402/motor.h>
00020 
00021 namespace cob_elmo_homing {
00022 
00023 class ElmoMotor402 : public canopen::Motor402 {
00024 
00025     canopen::ObjectStorage::Entry<uint64_t> command_entry_;
00026     canopen::ObjectStorage::Entry<uint64_t> response_entry_;
00027 
00028     static const uint64_t byte_3_bit_6 = (1  << (3*8+6));;
00029     static const uint64_t compare_mask = 0xFFFFFFFF ^ byte_3_bit_6;
00030 
00031     int32_t offset_;
00032     int32_t event_;
00033     int32_t speed_;
00034     uint32_t timeout_;
00035 
00036     uint64_t readResponse(uint64_t command)  {
00037         boost::this_thread::sleep_for(boost::chrono::milliseconds(10));
00038         uint64_t response = response_entry_.get();
00039         return (response & compare_mask) == (command & compare_mask) ? response : 0;
00040     }
00041 
00042     bool set(char c1, char c2, uint16_t index, uint32_t val){
00043         uint64_t response = 0;
00044         uint64_t command = c1 | (c2 << (1*8)) | ((index & 0x3FFF) << (2*8)) |  static_cast<uint64_t>(val) << (4*8);
00045         command_entry_.set(command);
00046 
00047         canopen::time_point timeout = canopen::get_abs_time(boost::chrono::seconds(1));
00048         do{
00049             response = readResponse(command);
00050         }while(response != command && (response & byte_3_bit_6) == 0 && canopen::get_abs_time() < timeout);
00051 
00052         bool ok = response == command && (response & byte_3_bit_6) == 0;
00053         return ok;
00054     }
00055     template<typename T> bool test(char c1, char c2, uint16_t index, uint32_t val, T dur){
00056         uint64_t response = 0;
00057         uint64_t command =  c1 | (c2 << (1*8)) | ((index & 0x3FFF) << (2*8)) | byte_3_bit_6;
00058 
00059         canopen::time_point timeout = canopen::get_abs_time(dur);
00060         do{
00061             command_entry_.set(command);
00062             response = readResponse(command);
00063         }while( (response >> (4*8)) != val  && canopen::get_abs_time() < timeout);
00064 
00065         return (response & byte_3_bit_6) == 0  && (response >> (4*8)) == val;
00066     }
00067 
00068 public:
00069     ElmoMotor402(const std::string &name, boost::shared_ptr<canopen::ObjectStorage> storage, const canopen::Settings &settings)
00070     : Motor402(name, storage, settings)
00071     {
00072         storage->entry(command_entry_, 0x2012);
00073         storage->entry(response_entry_, 0x2013);
00074         event_ = settings.get_optional<int32_t>("homing_event", -1);
00075         speed_ = settings.get_optional<int32_t>("homing_speed", 50000);
00076         offset_ = settings.get_optional<int32_t>("homing_offset", 0);
00077         timeout_= settings.get_optional<uint32_t>("homing_timeout", 60);
00078     }
00079 
00080     void handleInit(canopen::LayerStatus &status){
00081         Motor402::handleInit(status);
00082         if(status.bounded<canopen::LayerStatus::Ok>() && event_ >= 0){
00083             if(!command_entry_.valid()){
00084                 status.error("Command entry is not valid");
00085             }else if(!response_entry_.valid()){
00086                 status.error("Response entry is not valid");
00087             }else if(!enterModeAndWait(Profiled_Velocity)){
00088                 status.error("Could not switch mode");
00089             }else{
00090                 if(!set('H','M', 1, 0) || // reset homing
00091                 !set('H','M', 2, offset_) ||
00092                 !set('H','M', 3, event_) ||
00093                 !set('H','M', 4, 2) || // do not stop after homing
00094                 !set('H','M', 5, 0) || // set PX to offset
00095                 !set('H','M', 1, 1) || // start homing
00096                 !setTarget(speed_)){
00097                     status.error("could not initialize homing");
00098                     return;
00099                 }
00100 
00101                 if(!test('H','M', 1, 0, boost::chrono::seconds(timeout_))) status.error("homing timeout");
00102 
00103                 if(!setTarget(0)) status.error("could not stop motor");
00104                 if(!set('H','M', 1, 0)) status.error("could not stop homing");
00105             }
00106         }
00107     }
00108 
00109     class Allocator : public canopen::MotorBase::Allocator{
00110     public:
00111         virtual boost::shared_ptr<canopen::MotorBase> allocate(const std::string &name, boost::shared_ptr<canopen::ObjectStorage> storage, const canopen::Settings &settings) {
00112             return boost::make_shared<ElmoMotor402>(name, storage, settings);
00113         }
00114     };
00115 };
00116 
00117 }
00118 
00119 CLASS_LOADER_REGISTER_CLASS(cob_elmo_homing::ElmoMotor402::Allocator, canopen::MotorBase::Allocator);


cob_elmo_homing
Author(s): Mathias L├╝dtke
autogenerated on Sat Jun 8 2019 21:02:06