Quentin Mettraux / Mbed OS MAVErIC_TEST

Working Maveric

MaxonEPOS4.cpp@6:ef95300898b2, 2017-05-30 (annotated)

Committer:
mettrque
Date:
Tue May 30 21:11:54 2017 +0000
Revision:
6:ef95300898b2
Parent:
0:bdca5e4773dd
Child:
10:36a2131f636c
Working now

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mettrque 0:bdca5e4773dd 1 /*
mettrque 0:bdca5e4773dd 2 * MaxonEPOS4.cpp
mettrque 0:bdca5e4773dd 3 * Copyright (c) 2017, ZHAW
mettrque 0:bdca5e4773dd 4 * All rights reserved.
mettrque 0:bdca5e4773dd 5 *
mettrque 0:bdca5e4773dd 6 * Created on: 06.02.2017
mettrque 0:bdca5e4773dd 7 * Author: Marcel Honegger
mettrque 0:bdca5e4773dd 8 */
mettrque 0:bdca5e4773dd 9
mettrque 0:bdca5e4773dd 10 #include "MaxonEPOS4.h"
mettrque 0:bdca5e4773dd 11
mettrque 0:bdca5e4773dd 12 using namespace std;
mettrque 0:bdca5e4773dd 13
mettrque 0:bdca5e4773dd 14 /**
mettrque 0:bdca5e4773dd 15 * Creates and initializes this device.
mettrque 0:bdca5e4773dd 16 * @param canOpen a reference to a CANopen device driver.
mettrque 0:bdca5e4773dd 17 * @param nodeID the node ID of this device on the CANopen network.
mettrque 0:bdca5e4773dd 18 * @param period the period of the cyclic communication on the CAN bus, given in [s].
mettrque 0:bdca5e4773dd 19 */
mettrque 0:bdca5e4773dd 20 MaxonEPOS4::MaxonEPOS4(CANopen& canOpen, uint32_t nodeID, float period) : canOpen(canOpen), thread(osPriorityRealtime, STACK_SIZE) {
mettrque 0:bdca5e4773dd 21
mettrque 0:bdca5e4773dd 22 // initialize local values
mettrque 0:bdca5e4773dd 23
mettrque 0:bdca5e4773dd 24 this->nodeID = nodeID;
mettrque 0:bdca5e4773dd 25
mettrque 0:bdca5e4773dd 26 enable = false;
mettrque 0:bdca5e4773dd 27 targetTorque = 0;
mettrque 6:ef95300898b2 28 targetVelocity = 0;
mettrque 0:bdca5e4773dd 29 statusword = 0x0000;
mettrque 0:bdca5e4773dd 30 positionActualValue = 0;
mettrque 0:bdca5e4773dd 31
mettrque 0:bdca5e4773dd 32 // register this CANopen slave with the CANopen device driver
mettrque 0:bdca5e4773dd 33
mettrque 0:bdca5e4773dd 34 canOpen.registerCANopenSlave(nodeID, this);
mettrque 0:bdca5e4773dd 35
mettrque 0:bdca5e4773dd 36 // set slave into preoperational state
mettrque 0:bdca5e4773dd 37
mettrque 0:bdca5e4773dd 38 canOpen.transmitNMTObject(CANopen::ENTER_PREOPERATIONAL_STATE, nodeID);
mettrque 0:bdca5e4773dd 39
mettrque 6:ef95300898b2 40 // initialize slave CYCLIC_SYNCHRONOUS_TORQUE_MODE
mettrque 0:bdca5e4773dd 41
mettrque 0:bdca5e4773dd 42 canOpen.writeSDO(nodeID, 0x1600, 0x00, 0x00, 1); // disable RPDO1
mettrque 0:bdca5e4773dd 43 canOpen.writeSDO(nodeID, 0x1600, 0x01, 0x60400010, 4); // controlword, 2 bytes
mettrque 0:bdca5e4773dd 44 canOpen.writeSDO(nodeID, 0x1600, 0x02, 0x60600008, 4); // modes of operation, 1 byte
mettrque 0:bdca5e4773dd 45 canOpen.writeSDO(nodeID, 0x1600, 0x03, 0x60710010, 4); // target torque, 2 bytes
mettrque 0:bdca5e4773dd 46 canOpen.writeSDO(nodeID, 0x1600, 0x00, 0x03, 1); // enable RPDO1
mettrque 0:bdca5e4773dd 47
mettrque 6:ef95300898b2 48
mettrque 6:ef95300898b2 49 // initialize slave PROFILE_VELOCITY_MODE
mettrque 6:ef95300898b2 50 /*
mettrque 6:ef95300898b2 51 canOpen.writeSDO(nodeID, 0x1600, 0x00, 0x00, 1); // disable RPDO1
mettrque 6:ef95300898b2 52 canOpen.writeSDO(nodeID, 0x1600, 0x01, 0x60400010, 4); // controlword, 2 bytes
mettrque 6:ef95300898b2 53 canOpen.writeSDO(nodeID, 0x1600, 0x02, 0x60600008, 4); // modes of operation, 1 byte
mettrque 6:ef95300898b2 54 canOpen.writeSDO(nodeID, 0x1600, 0x03, 0x60FF0020, 4); // target velocity, 4 bytes
mettrque 6:ef95300898b2 55 canOpen.writeSDO(nodeID, 0x1600, 0x00, 0x03, 1); // enable RPDO1
mettrque 6:ef95300898b2 56 */
mettrque 0:bdca5e4773dd 57 canOpen.writeSDO(nodeID, 0x1400, 0x01, CANopen::RPDO1+nodeID, 4); // reconfigure communication with RPDO1
mettrque 0:bdca5e4773dd 58
mettrque 0:bdca5e4773dd 59 canOpen.writeSDO(nodeID, 0x1A00, 0x00, 0x00, 1); // disable TPDO1
mettrque 0:bdca5e4773dd 60 canOpen.writeSDO(nodeID, 0x1A00, 0x01, 0x60410010, 4); // statusword, 2 bytes
mettrque 0:bdca5e4773dd 61 canOpen.writeSDO(nodeID, 0x1A00, 0x02, 0x60640020, 4); // position actual value, 4 bytes
mettrque 0:bdca5e4773dd 62 canOpen.writeSDO(nodeID, 0x1A00, 0x00, 0x02, 1); // enable TPDO1
mettrque 0:bdca5e4773dd 63
mettrque 0:bdca5e4773dd 64 canOpen.writeSDO(nodeID, 0x1800, 0x01, 0x80000000, 4); // reconfigure communication with TPDO1
mettrque 0:bdca5e4773dd 65 canOpen.writeSDO(nodeID, 0x1800, 0x02, 253, 1);
mettrque 0:bdca5e4773dd 66 canOpen.writeSDO(nodeID, 0x1800, 0x03, static_cast<uint16_t>(period*10000/2), 2);
mettrque 0:bdca5e4773dd 67 canOpen.writeSDO(nodeID, 0x1800, 0x01, CANopen::TPDO1+nodeID, 4);
mettrque 0:bdca5e4773dd 68
mettrque 0:bdca5e4773dd 69 // set slave into operational state
mettrque 0:bdca5e4773dd 70
mettrque 0:bdca5e4773dd 71 canOpen.transmitNMTObject(CANopen::START_REMOTE_NODE, nodeID);
mettrque 0:bdca5e4773dd 72
mettrque 0:bdca5e4773dd 73 // start thread and timer interrupt
mettrque 0:bdca5e4773dd 74
mettrque 0:bdca5e4773dd 75 thread.start(callback(this, &MaxonEPOS4::run));
mettrque 0:bdca5e4773dd 76 ticker.attach(callback(this, &MaxonEPOS4::sendSignal), period);
mettrque 0:bdca5e4773dd 77 }
mettrque 0:bdca5e4773dd 78
mettrque 0:bdca5e4773dd 79 /**
mettrque 0:bdca5e4773dd 80 * Deletes this device driver object and releases all allocated resources.
mettrque 0:bdca5e4773dd 81 */
mettrque 0:bdca5e4773dd 82 MaxonEPOS4::~MaxonEPOS4() {
mettrque 0:bdca5e4773dd 83
mettrque 0:bdca5e4773dd 84 ticker.detach();
mettrque 0:bdca5e4773dd 85 }
mettrque 0:bdca5e4773dd 86
mettrque 0:bdca5e4773dd 87 /**
mettrque 0:bdca5e4773dd 88 * Enables or disables the power stage of this servo controller.
mettrque 0:bdca5e4773dd 89 * @param enable flag to indicate if the power stage should be enabled.
mettrque 0:bdca5e4773dd 90 */
mettrque 0:bdca5e4773dd 91 void MaxonEPOS4::setEnable(bool enable) {
mettrque 0:bdca5e4773dd 92
mettrque 0:bdca5e4773dd 93 this->enable = enable;
mettrque 0:bdca5e4773dd 94 }
mettrque 0:bdca5e4773dd 95
mettrque 0:bdca5e4773dd 96 /**
mettrque 0:bdca5e4773dd 97 * Checks if the power stage of this servo controller is enabled and operational.
mettrque 0:bdca5e4773dd 98 * @return <code>true</code> if the power stage is enabled, <code>false</code> otherwise.
mettrque 0:bdca5e4773dd 99 */
mettrque 0:bdca5e4773dd 100 bool MaxonEPOS4::isEnabled() {
mettrque 0:bdca5e4773dd 101
mettrque 0:bdca5e4773dd 102 return (statusword & STATUSWORD_MASK) == OPERATION_ENABLED;
mettrque 0:bdca5e4773dd 103 }
mettrque 0:bdca5e4773dd 104
mettrque 0:bdca5e4773dd 105 /**
mettrque 0:bdca5e4773dd 106 * Sets the desired torque of the servo controller.
mettrque 0:bdca5e4773dd 107 * @param targetTorque the desired torque, given in [&permil;] of the rated torque.
mettrque 0:bdca5e4773dd 108 */
mettrque 0:bdca5e4773dd 109 void MaxonEPOS4::writeTorque(int16_t targetTorque) {
mettrque 0:bdca5e4773dd 110
mettrque 0:bdca5e4773dd 111 this->targetTorque = targetTorque;
mettrque 0:bdca5e4773dd 112 }
mettrque 0:bdca5e4773dd 113
mettrque 0:bdca5e4773dd 114 /**
mettrque 6:ef95300898b2 115 * Sets the desired velocity of the servo controller.
mettrque 6:ef95300898b2 116 * @param targetVelocity the desired velocity, given in [rpm].
mettrque 6:ef95300898b2 117 */
mettrque 6:ef95300898b2 118 void MaxonEPOS4::writeVelocity(int32_t targetVelocity) {
mettrque 6:ef95300898b2 119
mettrque 6:ef95300898b2 120 this->targetVelocity = targetVelocity;
mettrque 6:ef95300898b2 121 }
mettrque 6:ef95300898b2 122
mettrque 6:ef95300898b2 123 /**
mettrque 0:bdca5e4773dd 124 * Gets the actual position value of the drive conntected to the servo controller.
mettrque 0:bdca5e4773dd 125 * @return the actual position value, given in [counts].
mettrque 0:bdca5e4773dd 126 */
mettrque 0:bdca5e4773dd 127 int32_t MaxonEPOS4::readPositionValue() {
mettrque 0:bdca5e4773dd 128
mettrque 0:bdca5e4773dd 129 return positionActualValue;
mettrque 0:bdca5e4773dd 130 }
mettrque 0:bdca5e4773dd 131
mettrque 0:bdca5e4773dd 132 /**
mettrque 0:bdca5e4773dd 133 * Implements the interface of the CANopen delegate class to receive
mettrque 0:bdca5e4773dd 134 * CANopen messages targeted to this device driver.
mettrque 0:bdca5e4773dd 135 */
mettrque 0:bdca5e4773dd 136 void MaxonEPOS4::receiveObject(uint32_t functionCode, uint8_t object[]) {
mettrque 0:bdca5e4773dd 137
mettrque 0:bdca5e4773dd 138 if (functionCode == CANopen::TPDO1) {
mettrque 0:bdca5e4773dd 139
mettrque 0:bdca5e4773dd 140 statusword = (static_cast<uint16_t>(object[0]) & 0xFF) | ((static_cast<uint16_t>(object[1]) & 0xFF) << 8);
mettrque 0:bdca5e4773dd 141 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);
mettrque 0:bdca5e4773dd 142 }
mettrque 0:bdca5e4773dd 143 }
mettrque 0:bdca5e4773dd 144
mettrque 0:bdca5e4773dd 145 /**
mettrque 0:bdca5e4773dd 146 * This method is called by the ticker timer interrupt service routine.
mettrque 0:bdca5e4773dd 147 * It sends a signal to the thread to make it run again.
mettrque 0:bdca5e4773dd 148 */
mettrque 0:bdca5e4773dd 149 void MaxonEPOS4::sendSignal() {
mettrque 0:bdca5e4773dd 150
mettrque 0:bdca5e4773dd 151 thread.signal_set(signal);
mettrque 0:bdca5e4773dd 152 }
mettrque 0:bdca5e4773dd 153
mettrque 0:bdca5e4773dd 154 /**
mettrque 0:bdca5e4773dd 155 * This <code>run()</code> method contains an infinite loop with the run logic.
mettrque 0:bdca5e4773dd 156 */
mettrque 0:bdca5e4773dd 157 void MaxonEPOS4::run() {
mettrque 0:bdca5e4773dd 158
mettrque 0:bdca5e4773dd 159 while (true) {
mettrque 0:bdca5e4773dd 160
mettrque 0:bdca5e4773dd 161 // wait for the periodic signal
mettrque 0:bdca5e4773dd 162
mettrque 0:bdca5e4773dd 163 thread.signal_wait(signal);
mettrque 0:bdca5e4773dd 164
mettrque 0:bdca5e4773dd 165 // set new controlword
mettrque 0:bdca5e4773dd 166
mettrque 0:bdca5e4773dd 167 uint16_t controlword = 0x0000;
mettrque 0:bdca5e4773dd 168
mettrque 0:bdca5e4773dd 169 if (enable) {
mettrque 0:bdca5e4773dd 170
mettrque 0:bdca5e4773dd 171 if ((statusword & STATUSWORD_MASK) == NOT_READY_TO_SWITCH_ON) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 172 else if ((statusword & STATUSWORD_MASK) == SWITCH_ON_DISABLED) controlword = SHUTDOWN;
mettrque 0:bdca5e4773dd 173 else if ((statusword & STATUSWORD_MASK) == READY_TO_SWITCH_ON) controlword = SWITCH_ON;
mettrque 0:bdca5e4773dd 174 else if ((statusword & STATUSWORD_MASK) == SWITCHED_ON) controlword = ENABLE_OPERATION;
mettrque 0:bdca5e4773dd 175 else if ((statusword & STATUSWORD_MASK) == OPERATION_ENABLED) controlword = ENABLE_OPERATION;
mettrque 0:bdca5e4773dd 176 else if ((statusword & STATUSWORD_MASK) == QUICK_STOP_ACTIVE) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 177 else if ((statusword & STATUSWORD_MASK) == FAULT_REACTION_ACTIVE) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 178 else if ((statusword & STATUSWORD_MASK) == FAULT) controlword = FAULT_RESET;
mettrque 0:bdca5e4773dd 179
mettrque 0:bdca5e4773dd 180 } else {
mettrque 0:bdca5e4773dd 181
mettrque 0:bdca5e4773dd 182 if ((statusword & STATUSWORD_MASK) == NOT_READY_TO_SWITCH_ON) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 183 else if ((statusword & STATUSWORD_MASK) == SWITCH_ON_DISABLED) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 184 else if ((statusword & STATUSWORD_MASK) == READY_TO_SWITCH_ON) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 185 else if ((statusword & STATUSWORD_MASK) == SWITCHED_ON) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 186 else if ((statusword & STATUSWORD_MASK) == OPERATION_ENABLED) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 187 else if ((statusword & STATUSWORD_MASK) == QUICK_STOP_ACTIVE) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 188 else if ((statusword & STATUSWORD_MASK) == FAULT_REACTION_ACTIVE) controlword = DISABLE_VOLTAGE;
mettrque 0:bdca5e4773dd 189 else if ((statusword & STATUSWORD_MASK) == FAULT) controlword = FAULT_RESET;
mettrque 0:bdca5e4773dd 190 }
mettrque 0:bdca5e4773dd 191
mettrque 6:ef95300898b2 192 // request TPDO1
mettrque 6:ef95300898b2 193
mettrque 6:ef95300898b2 194 uint8_t tpdo1[6];
mettrque 6:ef95300898b2 195
mettrque 6:ef95300898b2 196 canOpen.transmitObject(CANopen::TPDO1, nodeID, tpdo1, 6, CANRemote);
mettrque 6:ef95300898b2 197
mettrque 6:ef95300898b2 198 Thread::wait(1);
mettrque 6:ef95300898b2 199
mettrque 0:bdca5e4773dd 200 // transmit RPDO1
mettrque 0:bdca5e4773dd 201
mettrque 6:ef95300898b2 202 //CYCLIC_SYNCHRONOUS_TORQUE_MODE
mettrque 6:ef95300898b2 203
mettrque 0:bdca5e4773dd 204 uint8_t rpdo1[5];
mettrque 0:bdca5e4773dd 205
mettrque 0:bdca5e4773dd 206 rpdo1[0] = static_cast<uint8_t>(controlword & 0xFF);
mettrque 0:bdca5e4773dd 207 rpdo1[1] = static_cast<uint8_t>((controlword >> 8) & 0xFF);
mettrque 0:bdca5e4773dd 208 rpdo1[2] = static_cast<uint8_t>(CYCLIC_SYNCHRONOUS_TORQUE_MODE);
mettrque 0:bdca5e4773dd 209 rpdo1[3] = static_cast<uint8_t>(targetTorque & 0xFF);
mettrque 0:bdca5e4773dd 210 rpdo1[4] = static_cast<uint8_t>((targetTorque >> 8) & 0xFF);
mettrque 0:bdca5e4773dd 211
mettrque 0:bdca5e4773dd 212 canOpen.transmitObject(CANopen::RPDO1, nodeID, rpdo1, 5);
mettrque 0:bdca5e4773dd 213
mettrque 6:ef95300898b2 214
mettrque 6:ef95300898b2 215 //PROFILE_VELOCITY_MODE
mettrque 6:ef95300898b2 216 /*
mettrque 6:ef95300898b2 217 uint8_t rpdo1[7];
mettrque 0:bdca5e4773dd 218
mettrque 6:ef95300898b2 219 rpdo1[0] = static_cast<uint8_t>(controlword & 0xFF);
mettrque 6:ef95300898b2 220 rpdo1[1] = static_cast<uint8_t>((controlword >> 8) & 0xFF);
mettrque 6:ef95300898b2 221 rpdo1[2] = static_cast<uint8_t>(PROFILE_VELOCITY_MODE);
mettrque 6:ef95300898b2 222 rpdo1[3] = static_cast<uint8_t>(targetVelocity & 0xFF);
mettrque 6:ef95300898b2 223 rpdo1[4] = static_cast<uint8_t>((targetVelocity >> 8) & 0xFF);
mettrque 6:ef95300898b2 224 rpdo1[5] = static_cast<uint8_t>((targetVelocity >> 16) & 0xFF);
mettrque 6:ef95300898b2 225 rpdo1[6] = static_cast<uint8_t>((targetVelocity >> 24) & 0xFF);
mettrque 0:bdca5e4773dd 226
mettrque 6:ef95300898b2 227 canOpen.transmitObject(CANopen::RPDO1, nodeID, rpdo1, 7);
mettrque 6:ef95300898b2 228 */
mettrque 0:bdca5e4773dd 229 }
mettrque 0:bdca5e4773dd 230 }
mettrque 0:bdca5e4773dd 231