Quentin Mettraux
Working Maveric
Diff: MaxonEPOS4.cpp
- Revision:
- 0:bdca5e4773dd
- Child:
- 6:ef95300898b2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MaxonEPOS4.cpp Tue Apr 04 15:18:23 2017 +0000
@@ -0,0 +1,192 @@
+/*
+ * MaxonEPOS4.cpp
+ * Copyright (c) 2017, ZHAW
+ * All rights reserved.
+ *
+ * Created on: 06.02.2017
+ * Author: Marcel Honegger
+ */
+
+#include "MaxonEPOS4.h"
+
+using namespace std;
+
+/**
+ * Creates and initializes this device.
+ * @param canOpen a reference to a CANopen device driver.
+ * @param nodeID the node ID of this device on the CANopen network.
+ * @param period the period of the cyclic communication on the CAN bus, given in [s].
+ */
+MaxonEPOS4::MaxonEPOS4(CANopen& canOpen, uint32_t nodeID, float period) : canOpen(canOpen), thread(osPriorityRealtime, STACK_SIZE) {
+
+ // initialize local values
+
+ this->nodeID = nodeID;
+
+ enable = false;
+ targetTorque = 0;
+ statusword = 0x0000;
+ positionActualValue = 0;
+
+ // register this CANopen slave with the CANopen device driver
+
+ canOpen.registerCANopenSlave(nodeID, this);
+
+ // set slave into preoperational state
+
+ canOpen.transmitNMTObject(CANopen::ENTER_PREOPERATIONAL_STATE, nodeID);
+
+ // initialize slave
+
+ canOpen.writeSDO(nodeID, 0x1600, 0x00, 0x00, 1); // disable RPDO1
+ canOpen.writeSDO(nodeID, 0x1600, 0x01, 0x60400010, 4); // controlword, 2 bytes
+ canOpen.writeSDO(nodeID, 0x1600, 0x02, 0x60600008, 4); // modes of operation, 1 byte
+ canOpen.writeSDO(nodeID, 0x1600, 0x03, 0x60710010, 4); // target torque, 2 bytes
+ canOpen.writeSDO(nodeID, 0x1600, 0x00, 0x03, 1); // enable RPDO1
+
+ canOpen.writeSDO(nodeID, 0x1400, 0x01, CANopen::RPDO1+nodeID, 4); // reconfigure communication with RPDO1
+
+ canOpen.writeSDO(nodeID, 0x1A00, 0x00, 0x00, 1); // disable TPDO1
+ canOpen.writeSDO(nodeID, 0x1A00, 0x01, 0x60410010, 4); // statusword, 2 bytes
+ canOpen.writeSDO(nodeID, 0x1A00, 0x02, 0x60640020, 4); // position actual value, 4 bytes
+ canOpen.writeSDO(nodeID, 0x1A00, 0x00, 0x02, 1); // enable TPDO1
+
+ canOpen.writeSDO(nodeID, 0x1800, 0x01, 0x80000000, 4); // reconfigure communication with TPDO1
+ canOpen.writeSDO(nodeID, 0x1800, 0x02, 253, 1);
+ canOpen.writeSDO(nodeID, 0x1800, 0x03, static_cast<uint16_t>(period*10000/2), 2);
+ canOpen.writeSDO(nodeID, 0x1800, 0x01, CANopen::TPDO1+nodeID, 4);
+
+ // set slave into operational state
+
+ canOpen.transmitNMTObject(CANopen::START_REMOTE_NODE, nodeID);
+
+ // start thread and timer interrupt
+
+ thread.start(callback(this, &MaxonEPOS4::run));
+ ticker.attach(callback(this, &MaxonEPOS4::sendSignal), period);
+}
+
+/**
+ * Deletes this device driver object and releases all allocated resources.
+ */
+MaxonEPOS4::~MaxonEPOS4() {
+
+ ticker.detach();
+}
+
+/**
+ * Enables or disables the power stage of this servo controller.
+ * @param enable flag to indicate if the power stage should be enabled.
+ */
+void MaxonEPOS4::setEnable(bool enable) {
+
+ this->enable = enable;
+}
+
+/**
+ * Checks if the power stage of this servo controller is enabled and operational.
+ * @return <code>true</code> if the power stage is enabled, <code>false</code> otherwise.
+ */
+bool MaxonEPOS4::isEnabled() {
+
+ return (statusword & STATUSWORD_MASK) == OPERATION_ENABLED;
+}
+
+/**
+ * Sets the desired torque of the servo controller.
+ * @param targetTorque the desired torque, given in [‰] of the rated torque.
+ */
+void MaxonEPOS4::writeTorque(int16_t targetTorque) {
+
+ this->targetTorque = targetTorque;
+}
+
+/**
+ * Gets the actual position value of the drive conntected to the servo controller.
+ * @return the actual position value, given in [counts].
+ */
+int32_t MaxonEPOS4::readPositionValue() {
+
+ return positionActualValue;
+}
+
+/**
+ * Implements the interface of the CANopen delegate class to receive
+ * CANopen messages targeted to this device driver.
+ */
+void MaxonEPOS4::receiveObject(uint32_t functionCode, uint8_t object[]) {
+
+ if (functionCode == CANopen::TPDO1) {
+
+ statusword = (static_cast<uint16_t>(object[0]) & 0xFF) | ((static_cast<uint16_t>(object[1]) & 0xFF) << 8);
+ positionActualValue = (static_cast<int32_t>(object[2]) & 0xFF) | ((static_cast<int32_t>(object[3]) & 0xFF) << 8) | ((static_cast<int32_t>(object[4]) & 0xFF) << 16) | ((static_cast<int32_t>(object[5]) & 0xFF) << 24);
+ }
+}
+
+/**
+ * This method is called by the ticker timer interrupt service routine.
+ * It sends a signal to the thread to make it run again.
+ */
+void MaxonEPOS4::sendSignal() {
+
+ thread.signal_set(signal);
+}
+
+/**
+ * This <code>run()</code> method contains an infinite loop with the run logic.
+ */
+void MaxonEPOS4::run() {
+
+ while (true) {
+
+ // wait for the periodic signal
+
+ thread.signal_wait(signal);
+
+ // set new controlword
+
+ uint16_t controlword = 0x0000;
+
+ if (enable) {
+
+ if ((statusword & STATUSWORD_MASK) == NOT_READY_TO_SWITCH_ON) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == SWITCH_ON_DISABLED) controlword = SHUTDOWN;
+ else if ((statusword & STATUSWORD_MASK) == READY_TO_SWITCH_ON) controlword = SWITCH_ON;
+ else if ((statusword & STATUSWORD_MASK) == SWITCHED_ON) controlword = ENABLE_OPERATION;
+ else if ((statusword & STATUSWORD_MASK) == OPERATION_ENABLED) controlword = ENABLE_OPERATION;
+ else if ((statusword & STATUSWORD_MASK) == QUICK_STOP_ACTIVE) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == FAULT_REACTION_ACTIVE) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == FAULT) controlword = FAULT_RESET;
+
+ } else {
+
+ if ((statusword & STATUSWORD_MASK) == NOT_READY_TO_SWITCH_ON) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == SWITCH_ON_DISABLED) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == READY_TO_SWITCH_ON) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == SWITCHED_ON) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == OPERATION_ENABLED) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == QUICK_STOP_ACTIVE) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == FAULT_REACTION_ACTIVE) controlword = DISABLE_VOLTAGE;
+ else if ((statusword & STATUSWORD_MASK) == FAULT) controlword = FAULT_RESET;
+ }
+
+ // transmit RPDO1
+
+ uint8_t rpdo1[5];
+
+ rpdo1[0] = static_cast<uint8_t>(controlword & 0xFF);
+ rpdo1[1] = static_cast<uint8_t>((controlword >> 8) & 0xFF);
+ rpdo1[2] = static_cast<uint8_t>(CYCLIC_SYNCHRONOUS_TORQUE_MODE);
+ rpdo1[3] = static_cast<uint8_t>(targetTorque & 0xFF);
+ rpdo1[4] = static_cast<uint8_t>((targetTorque >> 8) & 0xFF);
+
+ canOpen.transmitObject(CANopen::RPDO1, nodeID, rpdo1, 5);
+
+ // request TPDO1
+
+ uint8_t tpdo1[6];
+
+ canOpen.transmitObject(CANopen::TPDO1, nodeID, tpdo1, 6, CANRemote);
+ }
+}
+