File content as of revision 6:ef95300898b2:

/*
 * 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;
    targetVelocity = 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 CYCLIC_SYNCHRONOUS_TORQUE_MODE
    
    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
    
    
    // initialize slave PROFILE_VELOCITY_MODE
    /*
    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, 0x60FF0020, 4);  // target velocity, 4 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 [&permil;] of the rated torque.
 */
void MaxonEPOS4::writeTorque(int16_t targetTorque) {
    
    this->targetTorque = targetTorque;
}

/**
 * Sets the desired velocity of the servo controller.
 * @param targetVelocity the desired velocity, given in [rpm].
 */
void MaxonEPOS4::writeVelocity(int32_t targetVelocity) {
    
    this->targetVelocity = targetVelocity;
}

/**
 * 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;
        }
        
        // request TPDO1
        
        uint8_t tpdo1[6];
        
        canOpen.transmitObject(CANopen::TPDO1, nodeID, tpdo1, 6, CANRemote);
        
        Thread::wait(1);
        
        // transmit RPDO1
        
        //CYCLIC_SYNCHRONOUS_TORQUE_MODE
        
        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);
        

        //PROFILE_VELOCITY_MODE
        /*
        uint8_t rpdo1[7];
        
        rpdo1[0] = static_cast<uint8_t>(controlword & 0xFF);
        rpdo1[1] = static_cast<uint8_t>((controlword >> 8) & 0xFF);
        rpdo1[2] = static_cast<uint8_t>(PROFILE_VELOCITY_MODE);
        rpdo1[3] = static_cast<uint8_t>(targetVelocity & 0xFF);
        rpdo1[4] = static_cast<uint8_t>((targetVelocity >> 8) & 0xFF);
        rpdo1[5] = static_cast<uint8_t>((targetVelocity >> 16) & 0xFF);
        rpdo1[6] = static_cast<uint8_t>((targetVelocity >> 24) & 0xFF);
        
        canOpen.transmitObject(CANopen::RPDO1, nodeID, rpdo1, 7);
        */
    }
}