motor.cpp
Go to the documentation of this file.
00001 #include <canopen_402/motor.h>
00002 #include <boost/thread/reverse_lock.hpp>
00003 
00004 namespace canopen
00005 {
00006 
00007 State402::InternalState State402::getState(){
00008     boost::mutex::scoped_lock lock(mutex_);
00009     return state_;
00010 }
00011 
00012 State402::InternalState State402::read(uint16_t sw) {
00013     static const uint16_t r = (1 << SW_Ready_To_Switch_On);
00014     static const uint16_t s = (1 << SW_Switched_On);
00015     static const uint16_t o = (1 << SW_Operation_enabled);
00016     static const uint16_t f = (1 << SW_Fault);
00017     static const uint16_t q = (1 << SW_Quick_stop);
00018     static const uint16_t d = (1 << SW_Switch_on_disabled);
00019 
00020     InternalState new_state = Unknown;
00021 
00022     uint16_t state = sw & ( d | q | f | o | s | r );
00023     switch ( state )
00024     {
00025     //   ( d | q | f | o | s | r ):
00026     case ( 0 | 0 | 0 | 0 | 0 | 0 ):
00027     case ( 0 | q | 0 | 0 | 0 | 0 ):
00028         new_state = Not_Ready_To_Switch_On;
00029         break;
00030 
00031     case ( d | 0 | 0 | 0 | 0 | 0 ):
00032     case ( d | q | 0 | 0 | 0 | 0 ):
00033         new_state =  Switch_On_Disabled;
00034         break;
00035 
00036     case ( 0 | q | 0 | 0 | 0 | r ):
00037         new_state =  Ready_To_Switch_On;
00038         break;
00039 
00040     case ( 0 | q | 0 | 0 | s | r ):
00041         new_state =  Switched_On;
00042         break;
00043 
00044     case ( 0 | q | 0 | o | s | r ):
00045         new_state =  Operation_Enable;
00046         break;
00047 
00048     case ( 0 | 0 | 0 | o | s | r ):
00049         new_state =  Quick_Stop_Active;
00050         break;
00051 
00052     case ( 0 | 0 | f | o | s | r ):
00053     case ( 0 | q | f | o | s | r ):
00054         new_state =  Fault_Reaction_Active;
00055         break;
00056 
00057     case ( 0 | 0 | f | 0 | 0 | 0 ):
00058     case ( 0 | q | f | 0 | 0 | 0 ):
00059         new_state =  Fault;
00060         break;
00061 
00062     default:
00063         LOG("Motor is currently in an unknown state: " << std::hex <<  state << std::dec);
00064     }
00065     boost::mutex::scoped_lock lock(mutex_);
00066     if(new_state != state_){
00067         state_ = new_state;
00068         cond_.notify_all();
00069     }
00070     return state_;
00071 }
00072 bool State402::waitForNewState(const time_point &abstime, State402::InternalState &state){
00073     boost::mutex::scoped_lock lock(mutex_);
00074     while(state_ == state && cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {}
00075     bool res = state != state_;
00076     state = state_;
00077     return res;
00078 }
00079 
00080 const Command402::TransitionTable Command402::transitions_;
00081 
00082 Command402::TransitionTable::TransitionTable(){
00083     typedef State402 s;
00084 
00085     transitions_.reserve(32);
00086 
00087     Op disable_voltage(0,(1<<CW_Fault_Reset) | (1<<CW_Enable_Voltage));
00088     /* 7*/ add(s::Ready_To_Switch_On, s::Switch_On_Disabled, disable_voltage);
00089     /* 9*/ add(s::Operation_Enable, s::Switch_On_Disabled, disable_voltage);
00090     /*10*/ add(s::Switched_On, s::Switch_On_Disabled, disable_voltage);
00091     /*12*/ add(s::Quick_Stop_Active, s::Switch_On_Disabled, disable_voltage);
00092 
00093     Op automatic(0,0);
00094     /* 0*/ add(s::Start, s::Not_Ready_To_Switch_On, automatic);
00095     /* 1*/ add(s::Not_Ready_To_Switch_On, s::Switch_On_Disabled, automatic);
00096     /*14*/ add(s::Fault_Reaction_Active, s::Fault, automatic);
00097 
00098     Op shutdown((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage), (1<<CW_Fault_Reset) | (1<<CW_Switch_On));
00099     /* 2*/ add(s::Switch_On_Disabled, s::Ready_To_Switch_On, shutdown);
00100     /* 6*/ add(s::Switched_On, s::Ready_To_Switch_On, shutdown);
00101     /* 8*/ add(s::Operation_Enable, s::Ready_To_Switch_On, shutdown);
00102 
00103     Op switch_on((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage) | (1<<CW_Switch_On), (1<<CW_Fault_Reset) | (1<<CW_Enable_Operation));
00104     /* 3*/ add(s::Ready_To_Switch_On, s::Switched_On, switch_on);
00105     /* 5*/ add(s::Operation_Enable, s::Switched_On, switch_on);
00106 
00107     Op enable_operation((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage) | (1<<CW_Switch_On) | (1<<CW_Enable_Operation), (1<<CW_Fault_Reset));
00108     /* 4*/ add(s::Switched_On, s::Operation_Enable, enable_operation);
00109     /*16*/ add(s::Quick_Stop_Active, s::Operation_Enable, enable_operation);
00110 
00111     Op quickstop((1<<CW_Enable_Voltage), (1<<CW_Fault_Reset) | (1<<CW_Quick_Stop));
00112     /* 7*/ add(s::Ready_To_Switch_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled
00113     /*10*/ add(s::Switched_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled
00114     /*11*/ add(s::Operation_Enable, s::Quick_Stop_Active, quickstop);
00115 
00116     // fault reset
00117     /*15*/ add(s::Fault, s::Switch_On_Disabled, Op((1<<CW_Fault_Reset), 0));
00118 }
00119 State402::InternalState Command402::nextStateForEnabling(State402::InternalState state){
00120     switch(state){
00121     case State402::Start:
00122         return State402::Not_Ready_To_Switch_On;
00123 
00124     case State402::Fault:
00125     case State402::Not_Ready_To_Switch_On:
00126         return State402::Switch_On_Disabled;
00127 
00128     case State402::Switch_On_Disabled:
00129         return State402::Ready_To_Switch_On;
00130 
00131     case State402::Ready_To_Switch_On:
00132         return State402::Switched_On;
00133 
00134     case State402::Switched_On:
00135     case State402::Quick_Stop_Active:
00136     case State402::Operation_Enable:
00137         return State402::Operation_Enable;
00138 
00139     case State402::Fault_Reaction_Active:
00140         return State402::Fault;
00141     }
00142 }
00143 
00144 bool Command402::setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next){
00145     try{
00146         if(from != to){
00147             State402::InternalState hop = to;
00148             if(next){
00149                 if(to == State402::Operation_Enable) hop = nextStateForEnabling(from);
00150                 *next = hop;
00151             }
00152             transitions_.get(from, hop)(cw);
00153         }
00154         return true;
00155     }
00156     catch(...){
00157         LOG("illegal tranistion " << from << " -> " << to);
00158     }
00159     return false;
00160 }
00161 
00162 template<uint16_t mask, uint16_t not_equal> struct masked_status_not_equal {
00163     uint16_t &status_;
00164     masked_status_not_equal(uint16_t &status) : status_(status) {}
00165     bool operator()() const { return (status_ & mask) != not_equal; }
00166 };
00167 bool DefaultHomingMode::start() {
00168     execute_ = false;
00169     return read(0);
00170 }
00171 bool DefaultHomingMode::read(const uint16_t &sw) {
00172     boost::mutex::scoped_lock lock(mutex_);
00173     uint16_t old = status_;
00174     status_ = sw & (MASK_Reached | MASK_Attained | MASK_Error);
00175     if(old != status_){
00176         cond_.notify_all();
00177     }
00178     return true;
00179 }
00180 bool DefaultHomingMode::write(Mode::OpModeAccesser& cw) {
00181     cw = 0;
00182     if(execute_){
00183         cw.set(CW_StartHoming);
00184         return true;
00185     }
00186     return false;
00187 }
00188 
00189 bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) {
00190     if(!homing_method_.valid()){
00191         return error(status, "homing method entry is not valid");
00192     }
00193 
00194     if(homing_method_.get_cached() == 0){
00195         return true;
00196     }
00197 
00198     time_point prepare_time = get_abs_time(boost::chrono::seconds(1));
00199     // ensure homing is not running
00200     boost::mutex::scoped_lock lock(mutex_);
00201     if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
00202         return error(status, "could not prepare homing");
00203     }
00204     if(status_ & MASK_Error){
00205         return error(status, "homing error before start");
00206     }
00207 
00208     execute_ = true;
00209 
00210     // ensure start
00211     if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Attained | MASK_Reached, MASK_Reached> (status_))){
00212         return error(status, "homing did not start");
00213     }
00214     if(status_ & MASK_Error){
00215         return error(status, "homing error at start");
00216     }
00217 
00218     time_point finish_time = get_abs_time(boost::chrono::seconds(10)); //
00219 
00220     // wait for attained
00221     if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Attained, 0> (status_))){
00222         return error(status, "homing not attained");
00223     }
00224     if(status_ & MASK_Error){
00225         return error(status, "homing error during process");
00226     }
00227 
00228     // wait for motion stop
00229     if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
00230         return error(status, "homing did not stop");
00231     }
00232     if(status_ & MASK_Error){
00233         return error(status, "homing error during stop");
00234     }
00235 
00236     if((status_ & MASK_Reached) && (status_ & MASK_Attained)){
00237         execute_ = false;
00238         return true;
00239     }
00240 
00241     return error(status, "something went wrong while homing");
00242 }
00243 
00244 bool Motor402::setTarget(double val){
00245     if(state_handler_.getState() == State402::Operation_Enable){
00246         boost::mutex::scoped_lock lock(mode_mutex_);
00247         return selected_mode_ && selected_mode_->setTarget(val);
00248     }
00249     return false;
00250 }
00251 bool Motor402::isModeSupported(uint16_t mode) { return mode != MotorBase::Homing && allocMode(mode); }
00252 
00253 bool Motor402::enterModeAndWait(uint16_t mode) {
00254     LayerStatus s;
00255     bool okay = mode != MotorBase::Homing && switchMode(s, mode);
00256     if(!s.bounded<LayerStatus::Ok>()){
00257         LOG("Could not switch to mode " << mode << ", reason: " << s.reason());
00258     }
00259     return okay;
00260 }
00261 
00262 uint16_t Motor402::getMode() {
00263     boost::mutex::scoped_lock lock(mode_mutex_);
00264     return selected_mode_ ? selected_mode_->mode_id_ :  MotorBase::No_Mode;
00265 }
00266 
00267 bool Motor402::isModeSupportedByDevice(uint16_t mode){
00268     return mode > 0 && supported_drive_modes_.valid() && (supported_drive_modes_.get_cached() & (1<<(mode-1)));
00269 }
00270 void Motor402::registerMode(uint16_t id, const boost::shared_ptr<Mode> &m){
00271     boost::mutex::scoped_lock map_lock(map_mutex_);
00272     if(m && m->mode_id_ == id) modes_.insert(std::make_pair(id, m));
00273 }
00274 
00275 boost::shared_ptr<Mode> Motor402::allocMode(uint16_t mode){
00276     boost::shared_ptr<Mode> res;
00277     if(isModeSupportedByDevice(mode)){
00278         boost::mutex::scoped_lock map_lock(map_mutex_);
00279         boost::unordered_map<uint16_t, boost::shared_ptr<Mode> >::iterator it = modes_.find(mode);
00280         if(it != modes_.end()){
00281             res = it->second;
00282         }
00283     }
00284     return res;
00285 }
00286 
00287 bool Motor402::switchMode(LayerStatus &status, uint16_t mode) {
00288 
00289     if(mode == MotorBase::No_Mode){
00290         boost::mutex::scoped_lock lock(mode_mutex_);
00291         selected_mode_.reset();
00292         try{ // try to set mode
00293             op_mode_.set(mode);
00294         }catch(...){}
00295         return true;
00296     }
00297 
00298     boost::shared_ptr<Mode> next_mode = allocMode(mode);
00299     if(!next_mode){
00300         status.error("Mode is not supported.");
00301         return false;
00302     }
00303 
00304     if(!next_mode->start()){
00305         status.error("Could not start mode.");
00306         return false;
00307     }
00308 
00309     { // disable mode handler
00310         boost::mutex::scoped_lock lock(mode_mutex_);
00311 
00312         if(mode_id_ == mode && selected_mode_ && selected_mode_->mode_id_ == mode){
00313             // nothing to do
00314             return true;
00315         }
00316 
00317         selected_mode_.reset();
00318     }
00319 
00320     if(!switchState(status, switching_state_)) return false;
00321 
00322     op_mode_.set(mode);
00323 
00324     bool okay = false;
00325 
00326     {  // wait for switch
00327         boost::mutex::scoped_lock lock(mode_mutex_);
00328 
00329         time_point abstime = get_abs_time(boost::chrono::seconds(5));
00330         if(monitor_mode_){
00331             while(mode_id_ != mode && mode_cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {}
00332         }else{
00333             while(mode_id_ != mode && get_abs_time() < abstime){
00334                 boost::reverse_lock<boost::mutex::scoped_lock> reverse(lock); // unlock inside loop
00335                 op_mode_display_.get(); // poll
00336                 boost::this_thread::sleep_for(boost::chrono::milliseconds(20)); // wait some time
00337             }
00338         }
00339 
00340         if(mode_id_ == mode){
00341             selected_mode_ = next_mode;
00342             okay = true;
00343         }else{
00344             status.error("Mode switch timed out.");
00345             op_mode_.set(mode_id_);
00346         }
00347     }
00348 
00349     if(!switchState(status, State402::Operation_Enable)) return false;
00350 
00351     return okay;
00352 
00353 }
00354 
00355 bool Motor402::switchState(LayerStatus &status, const State402::InternalState &target){
00356     time_point abstime = get_abs_time(boost::chrono::seconds(5));
00357     State402::InternalState state = state_handler_.getState();
00358     target_state_ = target;
00359     while(state != target_state_){
00360         boost::mutex::scoped_lock lock(cw_mutex_);
00361         State402::InternalState next = State402::Unknown;
00362         if(!Command402::setTransition(control_word_ ,state, target_state_ , &next)){
00363             status.error("Could not set transition");
00364             return false;
00365         }
00366         lock.unlock();
00367         if(state != next && !state_handler_.waitForNewState(abstime, state)){
00368             status.error("Transition timeout");
00369             return false;
00370         }
00371     }
00372     return state == target;
00373 }
00374 
00375 bool Motor402::readState(LayerStatus &status, const LayerState &current_state){
00376     uint16_t old_sw, sw = status_word_entry_.get(); // TODO: added error handling
00377     old_sw = status_word_.exchange(sw);
00378 
00379     state_handler_.read(sw);
00380 
00381     boost::mutex::scoped_lock lock(mode_mutex_);
00382     uint16_t new_mode = monitor_mode_ ? op_mode_display_.get() : op_mode_display_.get_cached();
00383     if(selected_mode_ && selected_mode_->mode_id_ == new_mode){
00384         if(!selected_mode_->read(sw)){
00385             status.error("Mode handler has error");
00386         }
00387     }
00388     if(new_mode != mode_id_){
00389         mode_id_ = new_mode;
00390         mode_cond_.notify_all();
00391     }
00392     if(selected_mode_ && selected_mode_->mode_id_ != new_mode){
00393         status.warn("mode does not match");
00394     }
00395     if(sw & (1<<State402::SW_Internal_limit)){
00396         if(old_sw & (1<<State402::SW_Internal_limit) || current_state != Ready){
00397             status.warn("Internal limit active");
00398         }else{
00399             status.error("Internal limit active");
00400         }
00401     }
00402 
00403     return true;
00404 }
00405 void Motor402::handleRead(LayerStatus &status, const LayerState &current_state){
00406     if(current_state > Off){
00407         readState(status, current_state);
00408     }
00409 }
00410 void Motor402::handleWrite(LayerStatus &status, const LayerState &current_state){
00411     if(current_state > Off){
00412         boost::mutex::scoped_lock lock(cw_mutex_);
00413         control_word_ |= (1<<Command402::CW_Halt);
00414         if(state_handler_.getState() == State402::Operation_Enable){
00415             boost::mutex::scoped_lock lock(mode_mutex_);
00416             Mode::OpModeAccesser cwa(control_word_);
00417             bool okay = false;
00418             if(selected_mode_ && selected_mode_->mode_id_ == mode_id_){
00419                 okay = selected_mode_->write(cwa);
00420             } else {
00421                 cwa = 0;
00422             }
00423             if(okay) {
00424                 control_word_ &= ~(1<<Command402::CW_Halt);
00425             }
00426         }
00427         if(start_fault_reset_.exchange(false)){
00428             control_word_entry_.set_cached(control_word_ & ~(1<<Command402::CW_Fault_Reset));
00429         }else{
00430             control_word_entry_.set_cached(control_word_);
00431         }
00432     }
00433 }
00434 void Motor402::handleDiag(LayerReport &report){
00435     uint16_t sw = status_word_;
00436     State402::InternalState state = state_handler_.getState();
00437 
00438     switch(state){
00439     case State402::Not_Ready_To_Switch_On:
00440     case State402::Switch_On_Disabled:
00441     case State402::Ready_To_Switch_On:
00442     case State402::Switched_On:
00443         report.warn("Motor operation is not enabled");
00444     case State402::Operation_Enable:
00445         break;
00446 
00447     case State402::Quick_Stop_Active:
00448         report.error("Quick stop is active");
00449         break;
00450     case State402::Fault:
00451     case State402::Fault_Reaction_Active:
00452         report.error("Motor has fault");
00453         break;
00454     case State402::Unknown:
00455         report.error("State is unknown");
00456         report.add("status_word", sw);
00457         break;
00458     }
00459 
00460     if(sw & (1<<State402::SW_Warning)){
00461         report.warn("Warning bit is set");
00462     }
00463     if(sw & (1<<State402::SW_Internal_limit)){
00464         report.error("Internal limit active");
00465     }
00466 }
00467 void Motor402::handleInit(LayerStatus &status){
00468     for(boost::unordered_map<uint16_t, boost::function<void()> >::iterator it = mode_allocators_.begin(); it != mode_allocators_.end(); ++it){
00469         (it->second)();
00470     }
00471 
00472     if(!readState(status, Init)){
00473         status.error("Could not read motor state");
00474         return;
00475     }
00476     {
00477         boost::mutex::scoped_lock lock(cw_mutex_);
00478         control_word_ = 0;
00479         start_fault_reset_ = true;
00480     }
00481     if(!switchState(status, State402::Operation_Enable)){
00482         status.error("Could not enable motor");
00483         return;
00484     }
00485 
00486     boost::shared_ptr<Mode> m = allocMode(MotorBase::Homing);
00487     if(!m){
00488         return; // homing not supported
00489     }
00490 
00491     HomingMode *homing = dynamic_cast<HomingMode*>(m.get());
00492 
00493     if(!homing){
00494         status.error("Homing mode has incorrect handler");
00495         return;
00496     }
00497 
00498     if(!switchMode(status, MotorBase::Homing)){
00499         status.error("Could not enter homing mode");
00500         return;
00501     }
00502 
00503     if(!homing->executeHoming(status)){
00504         status.error("Homing failed");
00505         return;
00506     }
00507 
00508     switchMode(status, No_Mode);
00509 }
00510 void Motor402::handleShutdown(LayerStatus &status){
00511     switchMode(status, MotorBase::No_Mode);
00512     switchState(status, State402::Switch_On_Disabled);
00513 }
00514 void Motor402::handleHalt(LayerStatus &status){
00515     State402::InternalState state = state_handler_.getState();
00516     boost::mutex::scoped_lock lock(cw_mutex_);
00517 
00518     // do not demand quickstop in case of fault
00519     if(state == State402::Fault_Reaction_Active || state == State402::Fault) return;
00520 
00521     if(state != State402::Operation_Enable){
00522         target_state_ = state;
00523     }else{
00524         target_state_ = State402::Quick_Stop_Active;
00525         if(!Command402::setTransition(control_word_ ,state, State402::Quick_Stop_Active, 0)){
00526             status.warn("Could not quick stop");
00527         }
00528     }
00529 }
00530 void Motor402::handleRecover(LayerStatus &status){
00531     start_fault_reset_ = true;
00532     {
00533         boost::mutex::scoped_lock lock(mode_mutex_);
00534         if(selected_mode_ && !selected_mode_->start()){
00535             status.error("Could not restart mode.");
00536             return;
00537         }
00538     }
00539     if(!switchState(status, State402::Operation_Enable)){
00540         status.error("Could not enable motor");
00541         return;
00542     }
00543 }
00544 
00545 } // namespace


canopen_402
Author(s): Thiago de Freitas , Mathias Lüdtke
autogenerated on Sun Sep 3 2017 03:10:49