-
Notifications
You must be signed in to change notification settings - Fork 7
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'cia-state-machine' into apocanlypse
- Loading branch information
Showing
15 changed files
with
556 additions
and
199 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,164 @@ | ||
| // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- | ||
|
|
||
| #include "DriveStatusMachine.hpp" | ||
|
|
||
| #include <bitset> | ||
| #include <unordered_map> | ||
|
|
||
| using namespace roboticslab; | ||
|
|
||
| namespace | ||
| { | ||
| DriveState parseDriveState(const std::bitset<16> & bits) | ||
| { | ||
| if (!bits.test(0) && !bits.test(1) && !bits.test(2) && !bits.test(3) && !bits.test(6)) | ||
| { | ||
| return DriveState::NOT_READY_TO_SWITCH_ON; // xxxx.xxxx.x0xx.0000 | ||
| } | ||
| else if (!bits.test(0) && !bits.test(1) && !bits.test(2) && !bits.test(3) && bits.test(6)) | ||
| { | ||
| return DriveState::SWITCH_ON_DISABLED; // xxxx.xxxx.x1xx.0000 | ||
| } | ||
| else if (bits.test(0) && !bits.test(1) && !bits.test(2) && !bits.test(3) && bits.test(5) && !bits.test(6)) | ||
| { | ||
| return DriveState::READY_TO_SWITCH_ON; // xxxx.xxxx.x01x.0001 | ||
| } | ||
| else if (bits.test(0) && bits.test(1) && !bits.test(2) && !bits.test(3) && bits.test(5) && !bits.test(6)) | ||
| { | ||
| return DriveState::SWITCHED_ON; // xxxx.xxxx.x01x.0011 | ||
| } | ||
| else if (bits.test(0) && bits.test(1) && bits.test(2) && !bits.test(3) && bits.test(5) && !bits.test(6)) | ||
| { | ||
| return DriveState::OPERATION_ENABLED; // xxxx.xxxx.x01x.0111 | ||
| } | ||
| else if (bits.test(0) && bits.test(1) && bits.test(2) && !bits.test(3) && !bits.test(5) && !bits.test(6)) | ||
| { | ||
| return DriveState::QUICK_STOP_ACTIVE; // xxxx.xxxx.x00x.0111 | ||
| } | ||
| else if (bits.test(0) && bits.test(1) && bits.test(2) && bits.test(3) && !bits.test(6)) | ||
| { | ||
| return DriveState::FAULT_REACTION_ACTIVE; // xxxx.xxxx.x0xx.1111 | ||
| } | ||
| else if (!bits.test(0) && !bits.test(1) && !bits.test(2) && bits.test(3) && !bits.test(6)) | ||
| { | ||
| return DriveState::FAULT; // xxxx.xxxx.x0xx.1000 | ||
| } | ||
| else | ||
| { | ||
| return DriveState::NOT_READY_TO_SWITCH_ON; // assume drive is dead | ||
| } | ||
| } | ||
|
|
||
| void prepareDriveTransition(DriveTransition transition, std::bitset<16> & bits) | ||
| { | ||
| switch (transition) | ||
| { | ||
| case DriveTransition::SHUTDOWN: // xxxx.xxxx.0xxx.x110 | ||
| bits.reset(0); | ||
| bits.set(1); | ||
| bits.set(2); | ||
| bits.reset(7); | ||
| break; | ||
| case DriveTransition::SWITCH_ON: // xxxx.xxxx.0xxx.0111 | ||
| case DriveTransition::DISABLE_OPERATION: | ||
| bits.set(0); | ||
| bits.set(1); | ||
| bits.set(2); | ||
| bits.reset(3); | ||
| bits.reset(7); | ||
| break; | ||
| case DriveTransition::DISABLE_VOLTAGE: // xxxx.xxxx.0xxx.xx0x | ||
| bits.reset(1); | ||
| bits.reset(7); | ||
| break; | ||
| case DriveTransition::QUICK_STOP: // xxxx.xxxx.0xxx.x01x | ||
| bits.set(1); | ||
| bits.reset(2); | ||
| bits.reset(7); | ||
| break; | ||
| case DriveTransition::ENABLE_OPERATION: // xxxx.xxxx.0xxx.1111 | ||
| bits.set(0); | ||
| bits.set(1); | ||
| bits.set(2); | ||
| bits.set(3); | ||
| bits.reset(7); | ||
| break; | ||
| case DriveTransition::FAULT_RESET: // xxxx.xxxx.^xxx.xxxx | ||
| bits.set(7); // FIXME: check this | ||
| break; | ||
| } | ||
| } | ||
|
|
||
| std::unordered_map<std::pair<DriveState, DriveTransition>, DriveState> transitionToState = { | ||
| {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveTransition::SHUTDOWN), DriveState::READY_TO_SWITCH_ON}, // 2 | ||
| {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveTransition::SWITCH_ON), DriveState::SWITCHED_ON}, // 3 | ||
| {std::make_pair(DriveState::SWITCHED_ON, DriveTransition::ENABLE_OPERATION), DriveState::OPERATION_ENABLED}, // 4 | ||
| {std::make_pair(DriveState::OPERATION_ENABLED, DriveTransition::DISABLE_OPERATION), DriveState::SWITCHED_ON}, // 5 | ||
| {std::make_pair(DriveState::OPERATION_ENABLED, DriveTransition::SWITCH_ON), DriveState::SWITCHED_ON}, // 5 (alias) | ||
| {std::make_pair(DriveState::SWITCHED_ON, DriveTransition::SHUTDOWN), DriveState::READY_TO_SWITCH_ON}, // 6 | ||
| {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveTransition::DISABLE_VOLTAGE), DriveState::SWITCH_ON_DISABLED}, // 7 | ||
| {std::make_pair(DriveState::OPERATION_ENABLED, DriveTransition::SHUTDOWN), DriveState::READY_TO_SWITCH_ON}, // 8 | ||
| {std::make_pair(DriveState::OPERATION_ENABLED, DriveTransition::DISABLE_VOLTAGE), DriveState::SWITCH_ON_DISABLED}, // 9 | ||
| {std::make_pair(DriveState::SWITCHED_ON, DriveTransition::DISABLE_VOLTAGE), DriveState::SWITCH_ON_DISABLED}, // 10 | ||
| {std::make_pair(DriveState::OPERATION_ENABLED, DriveTransition::QUICK_STOP), DriveState::QUICK_STOP_ACTIVE}, // 11 | ||
| {std::make_pair(DriveState::QUICK_STOP_ACTIVE, DriveTransition::DISABLE_VOLTAGE), DriveState::SWITCH_ON_DISABLED}, // 12 | ||
| {std::make_pair(DriveState::QUICK_STOP_ACTIVE, DriveTransition::ENABLE_OPERATION), DriveState::OPERATION_ENABLED}, // 16 | ||
| }; | ||
| } | ||
|
|
||
| bool DriveStatusMachine::update(std::uint16_t statusword) | ||
| { | ||
| { | ||
| std::lock_guard<std::mutex> lock(stateMutex); | ||
| _statusword = statusword; | ||
| } | ||
|
|
||
| return stateObserver.notify(); | ||
| } | ||
|
|
||
| std::bitset<16> & DriveStatusMachine::controlword() | ||
| { | ||
| return _controlword; | ||
| } | ||
|
|
||
| const std::bitset<16> & DriveStatusMachine::statusword() const | ||
| { | ||
| std::lock_guard<std::mutex> lock(stateMutex); | ||
| return _statusword; | ||
| } | ||
|
|
||
| DriveState DriveStatusMachine::getCurrentState() const | ||
| { | ||
| return parseDriveState(statusword()); | ||
| } | ||
|
|
||
| bool DriveStatusMachine::requestTransition(DriveTransition transition, bool wait) | ||
| { | ||
| prepareDriveTransition(transition, _controlword); | ||
| std::uint16_t data = _controlword.to_ulong(); | ||
|
|
||
| DriveState initialState = getCurrentState(); | ||
|
|
||
| if (!rpdo->write(data)) | ||
| { | ||
| return false; | ||
| } | ||
|
|
||
| if (wait) | ||
| { | ||
| auto op = std::make_pair(initialState, transition); | ||
| std::uint16_t resp; | ||
|
|
||
| if (!stateObserver.await()) | ||
| { | ||
| return false; | ||
| } | ||
|
|
||
| if (transitionToState[op] != resp) | ||
| { | ||
| return false; | ||
| } | ||
| } | ||
|
|
||
| return true; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,68 @@ | ||
| // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*- | ||
|
|
||
| #ifndef __DRIVE_STATUS_MACHINE_HPP__ | ||
| #define __DRIVE_STATUS_MACHINE_HPP__ | ||
|
|
||
| #include <cstdint> | ||
|
|
||
| #include <bitset> | ||
| #include <mutex> | ||
|
|
||
| #include "PdoProtocol.hpp" | ||
| #include "StateObserver.hpp" | ||
|
|
||
| namespace roboticslab | ||
| { | ||
|
|
||
| // associated to statusword | ||
| enum class DriveState | ||
| { | ||
| NOT_READY_TO_SWITCH_ON, | ||
| SWITCH_ON_DISABLED, | ||
| READY_TO_SWITCH_ON, | ||
| SWITCHED_ON, | ||
| OPERATION_ENABLED, | ||
| QUICK_STOP_ACTIVE, | ||
| FAULT_REACTION_ACTIVE, | ||
| FAULT | ||
| }; | ||
|
|
||
| // associated to controlword | ||
| enum class DriveTransition | ||
| { | ||
| SHUTDOWN, | ||
| SWITCH_ON, | ||
| DISABLE_VOLTAGE, | ||
| QUICK_STOP, | ||
| ENABLE_OPERATION, | ||
| DISABLE_OPERATION, // same as switch on | ||
| FAULT_RESET | ||
| }; | ||
|
|
||
| class DriveStatusMachine | ||
| { | ||
| public: | ||
| DriveStatusMachine(ReceivePdo * rpdo, double timeout) | ||
| : rpdo(rpdo), stateObserver(timeout) | ||
| { } | ||
|
|
||
| void configureRpdo(ReceivePdo * rpdo) | ||
| { this->rpdo = rpdo; } | ||
|
|
||
| bool update(std::uint16_t statusword); | ||
| std::bitset<16> & controlword(); | ||
| const std::bitset<16> & statusword() const; | ||
| DriveState getCurrentState() const; | ||
| bool requestTransition(DriveTransition transition, bool wait = true); | ||
|
|
||
| private: | ||
| std::bitset<16> _controlword; | ||
| std::bitset<16> _statusword; | ||
| ReceivePdo * rpdo; | ||
| StateObserver stateObserver; | ||
| mutable std::mutex stateMutex; | ||
| }; | ||
|
|
||
| } // namespace roboticslab | ||
|
|
||
| #endif // __DRIVE_STATUS_MACHINE_HPP__ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.