Skip to content

Commit a61c28a

Browse files
vcrochervcrocher
committed
Fixed PDO mapping in Drive (DIOs) and better managment of sub indexes. NOT TESTED on hardware.
1 parent ce0eeaf commit a61c28a

3 files changed

Lines changed: 56 additions & 63 deletions

File tree

‎src/core/robot/Drive.cpp‎

Lines changed: 17 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -78,13 +78,13 @@ bool Drive::setTorque(int torque) {
7878
* \todo add setTorque to object dictionary for all drives
7979
*
8080
*/
81-
spdlog::trace("Drive {} Writing {} to 0x{0:x}", NodeID, (short int)torque, OD_Addresses[TARGET_TOR]);
81+
spdlog::trace("Drive {} Writing {} to 0x{0:x}", NodeID, (short int)torque, OD_Addresses[TARGET_TOR][0]);
8282
targetTor = torque;
8383
return true;
8484
}
8585

8686
bool Drive::setDigitalOut(int digital_out) {
87-
spdlog::trace("Drive {} Writing {} to 0x{0:x}", NodeID, (short int)digitalOut, OD_Addresses[DIGITAL_OUT]);
87+
spdlog::trace("Drive {} Writing {} to 0x{0:x}", NodeID, (short int)digitalOut, OD_Addresses[DIGITAL_OUT][0]);
8888
digitalOut = digital_out;
8989
return true;
9090
}
@@ -170,10 +170,10 @@ bool Drive::posControlSetContinuousProfile(bool continuous) {
170170

171171
bool Drive::configureMasterPDOs(){
172172
// Set up the PDOs in the OD here
173-
for (int TPDO_Num = 1; TPDO_Num <= 4; TPDO_Num++){
173+
for (unsigned int TPDO_Num = 1; TPDO_Num <= TPDO_MappedObjects.size(); TPDO_Num++) {
174174
generateEquivalentMasterRPDO(TPDO_MappedObjects[TPDO_Num], TPDO_COBID[TPDO_Num] + NodeID, 0xff);
175-
}
176-
for (int RPDO_Num = 1; RPDO_Num <= 4; RPDO_Num++){
175+
}
176+
for (unsigned int RPDO_Num = 1; RPDO_Num <= RPDO_MappedObjects.size(); RPDO_Num++) {
177177
generateEquivalentMasterTPDO(RPDO_MappedObjects[RPDO_Num], RPDO_COBID[RPDO_Num] + NodeID, 0xff);
178178
}
179179

@@ -190,22 +190,21 @@ bool Drive::initPDOs() {
190190
if (sendSDOMessages(generateTPDOConfigSDO(TPDO_MappedObjects[TPDO_Num], TPDO_Num, TPDO_COBID[TPDO_Num] + NodeID, 0xFF)) < 0) {
191191
spdlog::error("Set up STATUS_WORD TPDO FAILED on node {}", NodeID);
192192
return false;
193-
}
193+
}
194194

195195
spdlog::debug("Set up ACTUAL_POS and ACTUAL_VEL TPDO on Node {}", NodeID);
196196
TPDO_Num = 2;
197197
if (sendSDOMessages(generateTPDOConfigSDO(TPDO_MappedObjects[TPDO_Num], TPDO_Num, TPDO_COBID[TPDO_Num] + NodeID, 0x01)) < 0) {
198198
spdlog::error("Set up ACTUAL_POS and ACTUAL_VEL TPDO FAILED on node {}", NodeID);
199199
return false;
200-
}
201-
200+
}
202201

203202
spdlog::debug("Set up ACTUAL_TOR TPDO on Node {}", NodeID);
204203
TPDO_Num = 3;
205204
if (sendSDOMessages(generateTPDOConfigSDO(TPDO_MappedObjects[TPDO_Num], TPDO_Num, TPDO_COBID[TPDO_Num] + NodeID, 0x01)) < 0) {
206205
spdlog::error("Set up ACTUAL_TOR TPDO FAILED on node {}", NodeID);
207206
return false;
208-
}
207+
}
209208

210209
// Calculate COB_ID. If RPDO:
211210
//int COB_ID = 0x100 * (PDO_Num+1) + NodeID;
@@ -214,25 +213,25 @@ bool Drive::initPDOs() {
214213
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
215214
spdlog::error("Set up CONTROL_WORD RPDO FAILED on node {}", NodeID);
216215
return false;
217-
}
216+
}
218217
spdlog::debug("Set up TARGET_POS RPDO on Node {}", NodeID);
219218
RPDO_Num = 2;
220219
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
221220
spdlog::error("Set up TARGET_POS RPDO FAILED on node {}", NodeID);
222221
return false;
223-
}
222+
}
224223
spdlog::debug("Set up TARGET_VEL RPDO on Node {}", NodeID);
225224
RPDO_Num = 3;
226225
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
227-
spdlog::error("Set up ARGET_VEL RPDO FAILED on node {}", NodeID);
226+
spdlog::error("Set up TARGET_VEL RPDO FAILED on node {}", NodeID);
228227
return false;
229-
}
228+
}
230229
spdlog::debug("Set up TARGET_TOR RPDO on Node {}", NodeID);
231230
RPDO_Num = 4;
232231
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
233232
spdlog::error("Set up TARGET_TOR RPDO FAILED on node {}", NodeID);
234233
return false;
235-
}
234+
}
236235
return true;
237236
}
238237

@@ -267,7 +266,7 @@ bool Drive::setMotorProfile(motorProfile profile) {
267266
return true;
268267
}
269268

270-
std::vector<std::string> Drive::generateTPDOConfigSDO(std::vector<OD_Entry_t> items, int PDO_Num, int COB_ID, int SyncRate, int sub_idx) {
269+
std::vector<std::string> Drive::generateTPDOConfigSDO(std::vector<OD_Entry_t> items, int PDO_Num, int COB_ID, int SyncRate) {
271270
// TODO: Do a check to make sure that the OD_Entry_t items can be transmitted.
272271

273272
// Calculate COB_ID. If TPDO:
@@ -296,7 +295,7 @@ std::vector<std::string> Drive::generateTPDOConfigSDO(std::vector<OD_Entry_t> it
296295

297296
for (unsigned int i = 1; i <= items.size(); i++) {
298297
// Set transmit parameters
299-
sstream << "[1] " << NodeID << " write 0x" << std::hex << 0x1A00 + PDO_Num - 1 << " " << i << " u32 0x" << std::hex << OD_Addresses[items[i - 1]] * 0x10000 + sub_idx * 0x100 + OD_DataSize[items[i - 1]]*8;
298+
sstream << "[1] " << NodeID << " write 0x" << std::hex << 0x1A00 + PDO_Num - 1 << " " << i << " u32 0x" << std::hex << OD_Addresses[items[i - 1]][0] * 0x10000 + OD_Addresses[items[i - 1]][1] * 0x100 + OD_DataSize[items[i - 1]]*8;
300299
CANCommands.push_back(sstream.str());
301300
sstream.str(std::string());
302301
}
@@ -327,7 +326,7 @@ void Drive::generateEquivalentMasterRPDO(std::vector<OD_Entry_t> items, int COB_
327326
//spdlog::debug("Master RPDO (COB-ID 0x{0:x}) Setup for Node {}", COB_ID, NodeID);
328327
}
329328

330-
std::vector<std::string> Drive::generateRPDOConfigSDO(std::vector<OD_Entry_t> items, int PDO_Num, int COB_ID, int UpdateTiming, int sub_idx) {
329+
std::vector<std::string> Drive::generateRPDOConfigSDO(std::vector<OD_Entry_t> items, int PDO_Num, int COB_ID, int UpdateTiming) {
331330
/**
332331
* \todo Do a check to make sure that the OD_Entry_t items can be Received
333332
*
@@ -357,7 +356,7 @@ std::vector<std::string> Drive::generateRPDOConfigSDO(std::vector<OD_Entry_t> it
357356

358357
for (unsigned int i = 1; i <= items.size(); i++) {
359358
// Set transmit parameters
360-
sstream << "[1] " << NodeID << " write 0x" << std::hex << 0x1600 + PDO_Num - 1 << " " << i << " u32 0x" << std::hex << OD_Addresses[items[i - 1]] * 0x10000 + sub_idx * 0x100 + OD_DataSize[items[i - 1]]*8;
359+
sstream << "[1] " << NodeID << " write 0x" << std::hex << 0x1600 + PDO_Num - 1 << " " << i << " u32 0x" << std::hex << OD_Addresses[items[i - 1]][0] * 0x10000 + OD_Addresses[items[i - 1]][1] * 0x100 + OD_DataSize[items[i - 1]]*8;
361360
CANCommands.push_back(sstream.str());
362361
sstream.str(std::string());
363362
}

‎src/core/robot/Drive.h‎

Lines changed: 28 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -123,28 +123,28 @@ class Drive {
123123

124124
/**
125125
* \brief Lists of PDOs
126-
*
126+
*
127127
*/
128128
std::vector<RPDO *> rpdos;
129129
std::vector<TPDO *> tpdos;
130-
130+
131131
/**
132132
* \brief Generates the list of commands required to configure TPDOs on the drives
133133
*
134134
* \param items A list of OD_Entry_t items which are to be configured with this TPDO
135135
* \param PDO_Num The number/index of this PDO
136-
* \param COB_ID the COB-ID of the PDO
136+
* \param COB_ID the COB-ID of the PDO
137137
* \param SyncRate The rate at which this PDO transmits (e.g. number of Sync Messages. 0xFF represents internal trigger event)
138138
* \param sub_idx The register sub index
139139
* \return std::string
140140
*/
141-
std::vector<std::string> generateTPDOConfigSDO(std::vector<OD_Entry_t> items, int PDO_Num, int COB_ID, int SyncRate, int sub_idx = 0);
141+
std::vector<std::string> generateTPDOConfigSDO(std::vector<OD_Entry_t> items, int PDO_Num, int COB_ID, int SyncRate);
142142

143143
/**
144144
* \brief Creates a RPDO in the Local Object Dictionary for the equivalent TPDO on the drive
145145
*
146146
* \param items A list of OD_Entry_t items which are to be configured with this RPDO
147-
* \param COB_ID the COB-ID of the PDO
147+
* \param COB_ID the COB-ID of the PDO
148148
* \param RPDOSyncRate The rate at which the RPDO processes NOTE: This is not the same as for the equivalent TPDO
149149
*/
150150
void generateEquivalentMasterRPDO(std::vector<OD_Entry_t> items, int COB_ID, int RPDOSyncRate);
@@ -155,18 +155,18 @@ class Drive {
155155
*
156156
* \param items A list of OD_Entry_t items which are to be configured with this RPDO
157157
* \param PDO_Num The number/index of this PDO
158-
* \param COB_ID the COB-ID of the PDO
158+
* \param COB_ID the COB-ID of the PDO
159159
* \param UpdateTiming 0-240 represents hold until next sync message, 0xFF represents immediate update
160160
* \param sub_idx The register sub index
161161
* \return std::string
162162
*/
163-
std::vector<std::string> generateRPDOConfigSDO(std::vector<OD_Entry_t> items, int PDO_Num, int COB_ID,int UpdateTiming, int sub_idx = 0);
163+
std::vector<std::string> generateRPDOConfigSDO(std::vector<OD_Entry_t> items, int PDO_Num, int COB_ID,int UpdateTiming);
164164

165165
/**
166166
* \brief Creates a TPDO in the Local Object Dictionary for the equivalent RPDO on the drive
167167
*
168168
* \param items A list of OD_Entry_t items which are to be configured with this TPDO
169-
* \param COB_ID the COB-ID of the PDO
169+
* \param COB_ID the COB-ID of the PDO
170170
* \param RPDOSyncRate The rate at which the TPDO sends messages NOTE: This is not the same as for the equivalent RPDO
171171
*/
172172
void generateEquivalentMasterTPDO(std::vector<OD_Entry_t> items, int COB_ID, int TPDOSyncRate);
@@ -239,7 +239,7 @@ class Drive {
239239

240240

241241
/**
242-
* \brief Map between the TPDO Number and their Mapped Objects
242+
* \brief Map between the TPDO Number and their Mapped Objects
243243
*
244244
*/
245245
std::map<UNSIGNED8, std::vector<OD_Entry_t>> TPDO_MappedObjects = {
@@ -248,18 +248,18 @@ class Drive {
248248
{3, {ACTUAL_TOR}},
249249
{4, {DIGITAL_IN}}};
250250

251-
/**
252-
* \brief Map between the RPDO Number and their Mapped Objects
251+
/**
252+
* \brief Map between the RPDO Number and their Mapped Objects
253253
*
254254
*/
255255
std::map<UNSIGNED8, std::vector<OD_Entry_t>> RPDO_MappedObjects = {
256-
{1, {DIGITAL_OUT}},
256+
{1, {CONTROL_WORD, DIGITAL_OUT}},
257257
{2, {TARGET_POS}},
258258
{3, {TARGET_VEL}},
259259
{4, {TARGET_TOR}}};
260260

261261
/**
262-
* \brief Map between the RPDO Number and their base COB-ID (actualy is base COB-ID + Node_ID)
262+
* \brief Map between the RPDO Number and their base COB-ID (actualy is base COB-ID + Node_ID)
263263
*
264264
* NOTE: This is an arbitrary default mapping unique to CORC. This can be changed in derived classes
265265
*/
@@ -270,7 +270,7 @@ class Drive {
270270
{4, 0x480}};
271271

272272
/**
273-
* \brief Map between the RPDO Number and their base COB-ID (actualy is base COB-ID + Node_ID)
273+
* \brief Map between the RPDO Number and their base COB-ID (actualy is base COB-ID + Node_ID)
274274
*
275275
* NOTE: This is an arbitrary default mapping unique to CORC. This can be changed in derived classes
276276
*/
@@ -299,22 +299,22 @@ class Drive {
299299
{DIGITAL_OUT, 2}};
300300

301301
/**
302-
* \brief Map between the Commonly-used OD entries and their addresses - used to generate PDO Configurations
302+
* \brief Map between the Commonly-used OD entries and their addresses and sub-index - used to generate PDO Configurations
303303
* NOTE: These are written in hexadecimal. They can be altered in derived classes if required.
304304
*
305305
*/
306-
std::map<OD_Entry_t, int> OD_Addresses = {
307-
{STATUS_WORD, 0x6041},
308-
{ERROR_WORD, 0x2601},
309-
{ACTUAL_POS, 0x6064},
310-
{ACTUAL_VEL, 0x606C},
311-
{ACTUAL_TOR, 0x6077},
312-
{CONTROL_WORD, 0x6040},
313-
{TARGET_POS, 0x607A},
314-
{TARGET_VEL, 0x60FF},
315-
{TARGET_TOR, 0x6071},
316-
{DIGITAL_IN, 0x2010},
317-
{DIGITAL_OUT, 0x2010}};
306+
std::map<OD_Entry_t, std::array<int, 2>> OD_Addresses = {
307+
{STATUS_WORD, {0x6041, 0x00}},
308+
{ERROR_WORD, {0x2601, 0x00}},
309+
{ACTUAL_POS, {0x6064, 0x00}},
310+
{ACTUAL_VEL, {0x606C, 0x00}},
311+
{ACTUAL_TOR, {0x6077, 0x00}},
312+
{CONTROL_WORD, {0x6040, 0x00}},
313+
{TARGET_POS, {0x607A, 0x00}},
314+
{TARGET_VEL, {0x60FF, 0x00}},
315+
{TARGET_TOR, {0x6071, 0x00}},
316+
{DIGITAL_IN, {0x2501, 0x00}},
317+
{DIGITAL_OUT, {0x2601, 0x00}}};
318318

319319
std::map<OD_Entry_t, void *> OD_MappedObjectAddresses = {
320320
{STATUS_WORD, (void *)&statusWord},
@@ -594,7 +594,7 @@ class Drive {
594594
* \brief Sets the continous/not continous profile bit
595595
*
596596
* \param continuous if this is true, continous movement is enabled, otherwise it is disabled
597-
* \return true If change is successful
597+
* \return true If change is successful
598598
* \return false If drive was not in position control mode
599599
*/
600600
virtual bool posControlSetContinuousProfile(bool continuous);

‎src/hardware/drives/KincoDrive.cpp‎

Lines changed: 11 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -4,8 +4,11 @@
44

55
KincoDrive::KincoDrive(int NodeID) : Drive::Drive(NodeID) {
66
//Remap torque reading and writting registers
7-
OD_Addresses[ACTUAL_TOR] = 0x6078;
8-
OD_Addresses[TARGET_TOR] = 0x60F6;
7+
OD_Addresses[ACTUAL_TOR] = {0x6078, 0x00};
8+
OD_Addresses[TARGET_TOR] = {0x60F6, 0x08};
9+
//Weird Kinco DIOs addresses
10+
OD_Addresses[DIGITAL_IN] = {0x2010, 0x0B};
11+
OD_Addresses[DIGITAL_OUT] = {0x2010, 0x0E};
912
}
1013
KincoDrive::~KincoDrive() {
1114
spdlog::debug("KincoDrive Deleted");
@@ -119,27 +122,18 @@ bool KincoDrive::initPDOs() {
119122

120123
spdlog::debug("Set up DIGITAL_IN RPDO on Node {}", NodeID);
121124
TPDO_Num = 4;
122-
if (sendSDOMessages(generateTPDOConfigSDO(TPDO_MappedObjects[TPDO_Num], TPDO_Num, TPDO_COBID[TPDO_Num] + NodeID, 0x01, 11)) < 0) {
125+
if (sendSDOMessages(generateTPDOConfigSDO(TPDO_MappedObjects[TPDO_Num], TPDO_Num, TPDO_COBID[TPDO_Num] + NodeID, 0x01)) < 0) {
123126
spdlog::error("Set up DIGITAL_IN TPDO FAILED on node {}", NodeID);
124127
return false;
125128
}
126129

127-
spdlog::debug("Set up DIGITAL_OUT RPDO on Node {}", NodeID);
130+
spdlog::debug("Set up CONTROL_WORD and DIGITAL_OUT RPDO on Node {}", NodeID);
128131
int RPDO_Num = 1;
129-
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff, 14)) < 0) {
130-
spdlog::error("Set up DIGITAL_OUT RPDO FAILED on node {}", NodeID);
132+
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
133+
spdlog::error("Set up CONTROL_WORD and DIGITAL_OUT RPDO FAILED on node {}", NodeID);
131134
return false;
132135
}
133136

134-
// Calculate COB_ID. If RPDO:
135-
//int COB_ID = 0x100 * (PDO_Num+1) + NodeID;
136-
//spdlog::debug("Set up CONTROL_WORD RPDO on Node {}", NodeID);
137-
//int RPDO_Num = 1;
138-
//if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
139-
// spdlog::error("Set up CONTROL_WORD RPDO FAILED on node {}", NodeID);
140-
// return false;
141-
//}
142-
143137
spdlog::debug("Set up TARGET_POS RPDO on Node {}", NodeID);
144138
RPDO_Num = 2;
145139
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
@@ -156,7 +150,7 @@ bool KincoDrive::initPDOs() {
156150

157151
spdlog::debug("Set up TARGET_TOR RPDO on Node {}", NodeID);
158152
RPDO_Num = 4;
159-
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff, 0x08)) < 0) {
153+
if (sendSDOMessages(generateRPDOConfigSDO(RPDO_MappedObjects[RPDO_Num], RPDO_Num, RPDO_COBID[RPDO_Num] + NodeID, 0xff)) < 0) {
160154
spdlog::error("Set up TARGET_TOR RPDO FAILED on node {}", NodeID);
161155
return false;
162156
}
@@ -331,4 +325,4 @@ std::vector<std::string> KincoDrive::writeSDOMessage(int address, int value) {
331325
sstream.str(std::string());
332326

333327
return CANCommands;
334-
}
328+
}

0 commit comments

Comments
 (0)