TheRobotStudio ROSA / Mbed 2 deprecated trs_slave

Code for the mbed NXP LPC1768 To be used on The Robot Studio Slave Boards License : Simplified BSD

Dependencies:   mbed

src/eposCmd.cpp@6:8a0250a4877a, 2015-02-11 (annotated)

Committer:
therobotstudio
Date:
Wed Feb 11 16:18:15 2015 +0000
Revision:
6:8a0250a4877a
Parent:
5:52e3137c2831 
v1.0.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
therobotstudio 6:8a0250a4877a 1 /*
therobotstudio 6:8a0250a4877a 2 * Copyright (c) 2013, The Robot Studio
therobotstudio 6:8a0250a4877a 3 * All rights reserved.
therobotstudio 6:8a0250a4877a 4 *
therobotstudio 6:8a0250a4877a 5 * Redistribution and use in source and binary forms, with or without
therobotstudio 6:8a0250a4877a 6 * modification, are permitted provided that the following conditions are met:
therobotstudio 6:8a0250a4877a 7 *
therobotstudio 6:8a0250a4877a 8 * * Redistributions of source code must retain the above copyright notice, this
therobotstudio 6:8a0250a4877a 9 * list of conditions and the following disclaimer.
therobotstudio 6:8a0250a4877a 10 *
therobotstudio 6:8a0250a4877a 11 * * Redistributions in binary form must reproduce the above copyright notice,
therobotstudio 6:8a0250a4877a 12 * this list of conditions and the following disclaimer in the documentation
therobotstudio 6:8a0250a4877a 13 * and/or other materials provided with the distribution.
therobotstudio 6:8a0250a4877a 14 *
therobotstudio 6:8a0250a4877a 15 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
therobotstudio 6:8a0250a4877a 16 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
therobotstudio 6:8a0250a4877a 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
therobotstudio 6:8a0250a4877a 18 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
therobotstudio 6:8a0250a4877a 19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
therobotstudio 6:8a0250a4877a 20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
therobotstudio 6:8a0250a4877a 21 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
therobotstudio 6:8a0250a4877a 22 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
therobotstudio 6:8a0250a4877a 23 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
therobotstudio 6:8a0250a4877a 24 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
therobotstudio 6:8a0250a4877a 25 *
therobotstudio 6:8a0250a4877a 26 * Created on: Feb 27, 2013
therobotstudio 6:8a0250a4877a 27 * Author: Cyril Jourdan (cyril.jourdan@therobotstudio.com)
therobotstudio 6:8a0250a4877a 28 */
therobotstudio 6:8a0250a4877a 29
rrknight 0:18d7499b82f3 30 #include "../include/eposCmd.h"
rrknight 0:18d7499b82f3 31
rrknight 0:18d7499b82f3 32 CAN cantoepos(p9, p10);
rrknight 0:18d7499b82f3 33 //Serial pc(p28, p27);
rrknight 0:18d7499b82f3 34
rrknight 0:18d7499b82f3 35 Serial pc(USBTX, USBRX);
rrknight 0:18d7499b82f3 36 DigitalOut ledchain[] = {(LED1), (LED2), (LED3), (LED4)}; //used for debugging
rrknight 0:18d7499b82f3 37 char data[8];
rrknight 0:18d7499b82f3 38 //unsigned int count[12] = {0};
rrknight 3:c16d726670b2 39 ActivatedMode activMode[NUMBER_MAX_EPOS2_PER_SLAVE];
rrknight 0:18d7499b82f3 40
rrknight 0:18d7499b82f3 41 //ADC variables
rrknight 0:18d7499b82f3 42 AnalogIn an1(p15);
rrknight 0:18d7499b82f3 43 AnalogIn an2(p16);
rrknight 0:18d7499b82f3 44 AnalogIn an3(p17);
rrknight 0:18d7499b82f3 45 AnalogIn an4(p18);
rrknight 0:18d7499b82f3 46 AnalogIn an5(p19);
rrknight 0:18d7499b82f3 47 AnalogIn an6(p20);
rrknight 0:18d7499b82f3 48
rrknight 0:18d7499b82f3 49 //sensor variables
rrknight 3:c16d726670b2 50 int32_t encPosition[NUMBER_MAX_EPOS2_PER_SLAVE] = {0};
rrknight 3:c16d726670b2 51 int16_t potiPosArray[NUMBER_MAX_EPOS2_PER_SLAVE] = {0};
rrknight 3:c16d726670b2 52 int16_t medPotiPosition[NUMBER_MAX_EPOS2_PER_SLAVE] = {0};
rrknight 3:c16d726670b2 53 int16_t avgCurrent[NUMBER_MAX_EPOS2_PER_SLAVE] = {0};
rrknight 3:c16d726670b2 54 int16_t medForce[NUMBER_MAX_EPOS2_PER_SLAVE] = {0};
rrknight 3:c16d726670b2 55 int32_t velocity[NUMBER_MAX_EPOS2_PER_SLAVE] = {0};
rrknight 0:18d7499b82f3 56
rrknight 3:c16d726670b2 57 int8_t boardStatus[NUMBER_MAX_EPOS2_PER_SLAVE] = {0}; //0 = OK, 1 = fault
rrknight 0:18d7499b82f3 58
rrknight 0:18d7499b82f3 59 //Multiplexer address lines
rrknight 4:396433e30e64 60 DigitalOut A0(p24);
rrknight 0:18d7499b82f3 61 DigitalOut A1(p22);
rrknight 0:18d7499b82f3 62 DigitalOut A2(p21);
rrknight 0:18d7499b82f3 63
rrknight 0:18d7499b82f3 64 void setMultiplexerChannel(const int8_t nodeID) //for force values
rrknight 0:18d7499b82f3 65 {
rrknight 0:18d7499b82f3 66 switch (nodeID)
rrknight 0:18d7499b82f3 67 {
rrknight 0:18d7499b82f3 68 //Analog 1
rrknight 0:18d7499b82f3 69 case 2: //Supra
rrknight 0:18d7499b82f3 70 A0 = 1;
rrknight 0:18d7499b82f3 71 A1 = 0;
rrknight 0:18d7499b82f3 72 A2 = 0;
rrknight 0:18d7499b82f3 73 break;
rrknight 0:18d7499b82f3 74 case 3: //Infra
rrknight 0:18d7499b82f3 75 A0 = 0;
rrknight 0:18d7499b82f3 76 A1 = 1;
rrknight 0:18d7499b82f3 77 A2 = 0;
rrknight 0:18d7499b82f3 78 break;
rrknight 0:18d7499b82f3 79 case 4: //Tmin
rrknight 0:18d7499b82f3 80 A0 = 1;
rrknight 0:18d7499b82f3 81 A1 = 1;
rrknight 0:18d7499b82f3 82 A2 = 0;
rrknight 0:18d7499b82f3 83 break;
rrknight 0:18d7499b82f3 84 case 6: //Subscap
rrknight 0:18d7499b82f3 85 A0 = 1;
rrknight 0:18d7499b82f3 86 A1 = 0;
rrknight 0:18d7499b82f3 87 A2 = 1;
rrknight 0:18d7499b82f3 88 break;
rrknight 0:18d7499b82f3 89 case 7: //Ant Delt
rrknight 0:18d7499b82f3 90 A0 = 0;
rrknight 0:18d7499b82f3 91 A1 = 1;
rrknight 0:18d7499b82f3 92 A2 = 1;
rrknight 0:18d7499b82f3 93 break;
rrknight 0:18d7499b82f3 94 case 8: //Lat Delt
rrknight 0:18d7499b82f3 95 A0 = 1;
rrknight 0:18d7499b82f3 96 A1 = 1;
rrknight 0:18d7499b82f3 97 A2 = 1;
rrknight 0:18d7499b82f3 98 break;
rrknight 0:18d7499b82f3 99 //Analog 3
rrknight 0:18d7499b82f3 100 case 9: //Post Delt nb AN3
rrknight 0:18d7499b82f3 101 A0 = 1;
rrknight 0:18d7499b82f3 102 A1 = 0;
rrknight 0:18d7499b82f3 103 A2 = 1;
rrknight 0:18d7499b82f3 104 break;
rrknight 0:18d7499b82f3 105 default:
rrknight 0:18d7499b82f3 106 //pc.printf("invalid node");
rrknight 0:18d7499b82f3 107 break;
rrknight 0:18d7499b82f3 108 }
rrknight 0:18d7499b82f3 109 }
rrknight 0:18d7499b82f3 110
rrknight 0:18d7499b82f3 111 void setNMT(const int8_t nodeID, uint8_t cs)
rrknight 0:18d7499b82f3 112 {
rrknight 0:18d7499b82f3 113 //Firmware Spec 7.3
rrknight 0:18d7499b82f3 114 //CANMessage canmsg; //test
rrknight 0:18d7499b82f3 115 data[0] = cs;
rrknight 0:18d7499b82f3 116 data[1] = nodeID;
rrknight 0:18d7499b82f3 117 cantoepos.write(CANMessage(CAN_NMT_ID, data, 2));
rrknight 0:18d7499b82f3 118 /*
rrknight 0:18d7499b82f3 119 //test
rrknight 0:18d7499b82f3 120 if(cs == CS_RESET_COMMUNICATION)
rrknight 0:18d7499b82f3 121 {
rrknight 0:18d7499b82f3 122 while (!(cantoepos.read(canmsg))) //wainting for the boot-up message
rrknight 0:18d7499b82f3 123 {
rrknight 0:18d7499b82f3 124 wait_us(1);
rrknight 0:18d7499b82f3 125 }
rrknight 0:18d7499b82f3 126 }
rrknight 0:18d7499b82f3 127 //fin test
rrknight 0:18d7499b82f3 128 */
rrknight 0:18d7499b82f3 129 wait_ms(PAUSE);
rrknight 0:18d7499b82f3 130 }
rrknight 0:18d7499b82f3 131
rrknight 0:18d7499b82f3 132 int8_t setObjectSDO(const int8_t nodeID, const int32_t object, int32_t value)
rrknight 0:18d7499b82f3 133 {
rrknight 0:18d7499b82f3 134 CANMessage canmsg;
rrknight 0:18d7499b82f3 135 uint8_t nbByteObject;
rrknight 0:18d7499b82f3 136 uint8_t nbByte = 0;
rrknight 3:c16d726670b2 137 uint32_t timeout = 0;
rrknight 0:18d7499b82f3 138
rrknight 0:18d7499b82f3 139 //nbByteObject = object & 0x000000FF;
rrknight 0:18d7499b82f3 140 nbByteObject = (uint8_t)object;
rrknight 0:18d7499b82f3 141
rrknight 0:18d7499b82f3 142 switch(nbByteObject)
rrknight 0:18d7499b82f3 143 {
rrknight 0:18d7499b82f3 144 case 0x08 :
rrknight 0:18d7499b82f3 145 nbByteObject = WRITING_OBJECT_1_BYTE;
rrknight 0:18d7499b82f3 146 nbByte = 1;
rrknight 0:18d7499b82f3 147 break;
rrknight 0:18d7499b82f3 148
rrknight 0:18d7499b82f3 149 case 0x10 :
rrknight 0:18d7499b82f3 150 nbByteObject = WRITING_OBJECT_2_BYTE;
rrknight 0:18d7499b82f3 151 nbByte = 2;
rrknight 0:18d7499b82f3 152 break;
rrknight 0:18d7499b82f3 153
rrknight 0:18d7499b82f3 154 case 0x20 :
rrknight 0:18d7499b82f3 155 nbByteObject = WRITING_OBJECT_4_BYTE;
rrknight 0:18d7499b82f3 156 nbByte = 4;
rrknight 0:18d7499b82f3 157 break;
rrknight 0:18d7499b82f3 158
rrknight 0:18d7499b82f3 159 default :
rrknight 0:18d7499b82f3 160 return EPOS2_ERROR;
rrknight 0:18d7499b82f3 161 }
rrknight 0:18d7499b82f3 162
rrknight 0:18d7499b82f3 163 data[0] = nbByteObject;
rrknight 0:18d7499b82f3 164 data[1] = (uint8_t)(object >> 16);
rrknight 0:18d7499b82f3 165 data[2] = (uint8_t)(object >> 24);
rrknight 0:18d7499b82f3 166 data[3] = (uint8_t)(object >> 8);
rrknight 0:18d7499b82f3 167 data[4] = (uint8_t)value;
rrknight 0:18d7499b82f3 168 data[5] = (uint8_t)(value >> 8);
rrknight 0:18d7499b82f3 169 data[6] = (uint8_t)(value >> 16);
rrknight 0:18d7499b82f3 170 data[7] = (uint8_t)(value >> 24);
rrknight 0:18d7499b82f3 171 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 4 + nbByte));
rrknight 0:18d7499b82f3 172 while (!(cantoepos.read(canmsg)))
rrknight 0:18d7499b82f3 173 {
rrknight 3:c16d726670b2 174 wait_us(1); //use a timeout instead
rrknight 3:c16d726670b2 175
rrknight 3:c16d726670b2 176 if(timeout >= TIMEOUT)
rrknight 3:c16d726670b2 177 {
rrknight 3:c16d726670b2 178 return EPOS2_ERROR;
rrknight 3:c16d726670b2 179 }
rrknight 3:c16d726670b2 180
rrknight 3:c16d726670b2 181 timeout++;
rrknight 0:18d7499b82f3 182 }
rrknight 0:18d7499b82f3 183 wait_ms(PAUSE);
rrknight 0:18d7499b82f3 184
rrknight 0:18d7499b82f3 185 //pc.printf("setSDO[%d] [%02X %02X %02X %02X %02X %02X %02X %02X]\n\r", nodeID, data[7], data[6], data[5], data[4], data[3], data[2], data[1], data[0]);
rrknight 0:18d7499b82f3 186
rrknight 0:18d7499b82f3 187 return EPOS2_OK;
rrknight 0:18d7499b82f3 188 }
rrknight 0:18d7499b82f3 189
rrknight 0:18d7499b82f3 190 int8_t getObjectSDO(const int8_t nodeID, const int32_t object, int32_t value)
rrknight 0:18d7499b82f3 191 {
rrknight 0:18d7499b82f3 192 //todo
rrknight 0:18d7499b82f3 193 return EPOS2_OK;
rrknight 0:18d7499b82f3 194 }
rrknight 0:18d7499b82f3 195
rrknight 0:18d7499b82f3 196 int8_t setPDO(const int8_t nodeID, uint16_t pdoIdx, uint8_t subIdx, uint32_t value, uint8_t nbByte)
rrknight 0:18d7499b82f3 197 {
rrknight 0:18d7499b82f3 198 CANMessage canmsg;
rrknight 0:18d7499b82f3 199 uint8_t nbByteObject;
rrknight 0:18d7499b82f3 200
rrknight 0:18d7499b82f3 201 switch(nbByte)
rrknight 0:18d7499b82f3 202 {
rrknight 0:18d7499b82f3 203 case 1 :
rrknight 0:18d7499b82f3 204 nbByteObject = WRITING_OBJECT_1_BYTE;
rrknight 0:18d7499b82f3 205 break;
rrknight 0:18d7499b82f3 206
rrknight 0:18d7499b82f3 207 case 2 :
rrknight 0:18d7499b82f3 208 nbByteObject = WRITING_OBJECT_2_BYTE;
rrknight 0:18d7499b82f3 209 break;
rrknight 0:18d7499b82f3 210
rrknight 0:18d7499b82f3 211 case 4 :
rrknight 0:18d7499b82f3 212 nbByteObject = WRITING_OBJECT_4_BYTE;
rrknight 0:18d7499b82f3 213 break;
rrknight 0:18d7499b82f3 214
rrknight 0:18d7499b82f3 215 default :
rrknight 0:18d7499b82f3 216 return EPOS2_ERROR;
rrknight 0:18d7499b82f3 217 }
rrknight 0:18d7499b82f3 218
rrknight 0:18d7499b82f3 219 data[0] = nbByteObject;
rrknight 0:18d7499b82f3 220 data[1] = (uint8_t)pdoIdx;
rrknight 0:18d7499b82f3 221 data[2] = (uint8_t)(pdoIdx >> 8);
rrknight 0:18d7499b82f3 222 data[3] = subIdx;
rrknight 0:18d7499b82f3 223 data[4] = (uint8_t)value;
rrknight 0:18d7499b82f3 224 data[5] = (uint8_t)(value >> 8);
rrknight 0:18d7499b82f3 225 data[6] = (uint8_t)(value >> 16);
rrknight 0:18d7499b82f3 226 data[7] = (uint8_t)(value >> 24);
rrknight 0:18d7499b82f3 227 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 4 + nbByte));
rrknight 0:18d7499b82f3 228 while (!(cantoepos.read(canmsg)))
rrknight 0:18d7499b82f3 229 {
rrknight 0:18d7499b82f3 230 wait_us(1);
rrknight 0:18d7499b82f3 231 }
rrknight 0:18d7499b82f3 232 wait_ms(PAUSE);
rrknight 0:18d7499b82f3 233
rrknight 0:18d7499b82f3 234 return EPOS2_OK;
rrknight 0:18d7499b82f3 235 }
rrknight 0:18d7499b82f3 236
rrknight 0:18d7499b82f3 237 int8_t setModeOfOperation(const int8_t nodeID, uint8_t mode)
rrknight 0:18d7499b82f3 238 {
rrknight 0:18d7499b82f3 239 //Switch Mode of Operation (Firmware Spec 8.2.89)
rrknight 0:18d7499b82f3 240
rrknight 0:18d7499b82f3 241 CANMessage canmsg;
rrknight 0:18d7499b82f3 242
rrknight 0:18d7499b82f3 243 data[0] = WRITING_OBJECT_1_BYTE;
rrknight 0:18d7499b82f3 244 data[1] = (uint8_t)OBJECT_MODE_OF_OPERATION_INDEX;
rrknight 0:18d7499b82f3 245 data[2] = (uint8_t)(OBJECT_MODE_OF_OPERATION_INDEX >> 8);
rrknight 0:18d7499b82f3 246 data[3] = OBJECT_MODE_OF_OPERATION_SUBINDEX;
rrknight 0:18d7499b82f3 247 data[4] = mode;
rrknight 0:18d7499b82f3 248 data[5] = 0x00;
rrknight 0:18d7499b82f3 249 data[6] = 0x00;
rrknight 0:18d7499b82f3 250 data[7] = 0x00;
rrknight 0:18d7499b82f3 251
rrknight 0:18d7499b82f3 252 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 5));
rrknight 0:18d7499b82f3 253 while (!(cantoepos.read(canmsg)))
rrknight 0:18d7499b82f3 254 {
rrknight 0:18d7499b82f3 255 wait_us(1);
rrknight 0:18d7499b82f3 256 }
rrknight 0:18d7499b82f3 257
rrknight 0:18d7499b82f3 258 //set activMode
rrknight 0:18d7499b82f3 259 switch(mode)
rrknight 0:18d7499b82f3 260 {
rrknight 0:18d7499b82f3 261 case VALUE_PROFILE_POSITION_MODE :
rrknight 0:18d7499b82f3 262 activMode[nodeID-1] = PROFILE_POSITION;
rrknight 0:18d7499b82f3 263 break;
rrknight 0:18d7499b82f3 264
rrknight 0:18d7499b82f3 265 case VALUE_POSITION_MODE :
rrknight 0:18d7499b82f3 266 activMode[nodeID-1] = POSITION;
rrknight 0:18d7499b82f3 267 break;
rrknight 0:18d7499b82f3 268
rrknight 0:18d7499b82f3 269 case VALUE_CURRENT_MODE :
rrknight 0:18d7499b82f3 270 activMode[nodeID-1] = CURRENT;
rrknight 0:18d7499b82f3 271 break;
rrknight 0:18d7499b82f3 272
rrknight 0:18d7499b82f3 273 case VALUE_VELOCITY_MODE :
rrknight 0:18d7499b82f3 274 activMode[nodeID-1] = VELOCITY;
rrknight 0:18d7499b82f3 275 break;
rrknight 0:18d7499b82f3 276
rrknight 0:18d7499b82f3 277 default :
rrknight 0:18d7499b82f3 278 return EPOS2_ERROR;
rrknight 0:18d7499b82f3 279 }
rrknight 0:18d7499b82f3 280
rrknight 0:18d7499b82f3 281 wait_ms(PAUSE);
rrknight 0:18d7499b82f3 282
rrknight 0:18d7499b82f3 283 return EPOS2_OK;
rrknight 0:18d7499b82f3 284 }
rrknight 0:18d7499b82f3 285
rrknight 0:18d7499b82f3 286 void setModeOfOperationPDO(const int8_t nodeID, uint8_t mode)
rrknight 0:18d7499b82f3 287 {
rrknight 0:18d7499b82f3 288 data[0] = mode;
rrknight 0:18d7499b82f3 289 cantoepos.write(CANMessage(COB_ID_RECEIVE_PDO_4_ENABLE + nodeID, data, 1)); //RxPDO 4 for Modes of Operation
rrknight 0:18d7499b82f3 290
rrknight 0:18d7499b82f3 291 //set activMode
rrknight 0:18d7499b82f3 292 switch(mode)
rrknight 0:18d7499b82f3 293 {
rrknight 0:18d7499b82f3 294 case VALUE_PROFILE_POSITION_MODE :
rrknight 0:18d7499b82f3 295 activMode[nodeID-1] = PROFILE_POSITION;
rrknight 0:18d7499b82f3 296 break;
rrknight 0:18d7499b82f3 297
rrknight 0:18d7499b82f3 298 case VALUE_POSITION_MODE :
rrknight 0:18d7499b82f3 299 activMode[nodeID-1] = POSITION;
rrknight 0:18d7499b82f3 300 break;
rrknight 0:18d7499b82f3 301
rrknight 0:18d7499b82f3 302 case VALUE_CURRENT_MODE :
rrknight 0:18d7499b82f3 303 activMode[nodeID-1] = CURRENT;
rrknight 0:18d7499b82f3 304 break;
rrknight 0:18d7499b82f3 305
rrknight 0:18d7499b82f3 306 case VALUE_VELOCITY_MODE :
rrknight 0:18d7499b82f3 307 activMode[nodeID-1] = VELOCITY;
rrknight 0:18d7499b82f3 308 break;
rrknight 0:18d7499b82f3 309
rrknight 0:18d7499b82f3 310 default :
rrknight 0:18d7499b82f3 311 }
rrknight 0:18d7499b82f3 312
rrknight 0:18d7499b82f3 313 wait_us(100);
rrknight 0:18d7499b82f3 314 }
rrknight 0:18d7499b82f3 315
rrknight 0:18d7499b82f3 316 void shutdownControlword(const int8_t nodeID)
rrknight 0:18d7499b82f3 317 {
rrknight 0:18d7499b82f3 318 CANMessage canmsg;
rrknight 0:18d7499b82f3 319
rrknight 0:18d7499b82f3 320 //Shutdown Controlword Firmware Spec 8.2.84 bit wise 0xxx x110 so 0x0006
rrknight 0:18d7499b82f3 321 data[0] = WRITING_OBJECT_2_BYTE;
rrknight 0:18d7499b82f3 322 data[1] = 0x40;
rrknight 0:18d7499b82f3 323 data[2] = 0x60;
rrknight 0:18d7499b82f3 324 data[3] = 0x00;
rrknight 0:18d7499b82f3 325 data[4] = 0x06; //Low Byte
rrknight 0:18d7499b82f3 326 data[5] = 0x00; //High Byte
rrknight 0:18d7499b82f3 327 data[6] = 0x00;
rrknight 0:18d7499b82f3 328 data[7] = 0x00;
rrknight 0:18d7499b82f3 329 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 6));
rrknight 0:18d7499b82f3 330 while (!(cantoepos.read(canmsg)))
rrknight 0:18d7499b82f3 331 {
rrknight 0:18d7499b82f3 332 wait_us(1);
rrknight 0:18d7499b82f3 333 }
rrknight 0:18d7499b82f3 334 wait_ms(PAUSE);
rrknight 0:18d7499b82f3 335 }
rrknight 0:18d7499b82f3 336
rrknight 0:18d7499b82f3 337 void shutdownControlwordIT(const int8_t nodeID)
rrknight 0:18d7499b82f3 338 {
rrknight 0:18d7499b82f3 339 CANMessage canmsg;
rrknight 0:18d7499b82f3 340
rrknight 0:18d7499b82f3 341 //Shutdown Controlword Firmware Spec 8.2.84 bit wise 0xxx x110 so 0x0006
rrknight 0:18d7499b82f3 342 data[0] = WRITING_OBJECT_2_BYTE;
rrknight 0:18d7499b82f3 343 data[1] = 0x40;
rrknight 0:18d7499b82f3 344 data[2] = 0x60;
rrknight 0:18d7499b82f3 345 data[3] = 0x00;
rrknight 0:18d7499b82f3 346 data[4] = 0x06; //Low Byte
rrknight 0:18d7499b82f3 347 data[5] = 0x00; //High Byte
rrknight 0:18d7499b82f3 348 data[6] = 0x00;
rrknight 0:18d7499b82f3 349 data[7] = 0x00;
rrknight 0:18d7499b82f3 350 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 6));
rrknight 0:18d7499b82f3 351 }
rrknight 0:18d7499b82f3 352
rrknight 0:18d7499b82f3 353 void switchOnEnableOperationControlword(const int8_t nodeID)
rrknight 0:18d7499b82f3 354 {
rrknight 0:18d7499b82f3 355 CANMessage canmsg;
rrknight 0:18d7499b82f3 356
rrknight 0:18d7499b82f3 357 //Switch On & Enable Operation Controlword Firmware Spec 8.2.84 bit wise 0xxx 1111 so 0x000F
rrknight 0:18d7499b82f3 358 data[0] = WRITING_OBJECT_2_BYTE;
rrknight 0:18d7499b82f3 359 data[1] = 0x40;
rrknight 0:18d7499b82f3 360 data[2] = 0x60;
rrknight 0:18d7499b82f3 361 data[3] = 0x00;
rrknight 0:18d7499b82f3 362 data[4] = 0x0F;
rrknight 0:18d7499b82f3 363 data[5] = 0x00;
rrknight 0:18d7499b82f3 364 data[6] = 0x00;
rrknight 0:18d7499b82f3 365 data[7] = 0x00;
rrknight 0:18d7499b82f3 366 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 6));
rrknight 0:18d7499b82f3 367 while (!(cantoepos.read(canmsg)))
rrknight 0:18d7499b82f3 368 {
rrknight 0:18d7499b82f3 369 wait_us(1);
rrknight 0:18d7499b82f3 370 }
rrknight 0:18d7499b82f3 371 wait_ms(126); //Tested 5ms not enough, 6ms seems ok
rrknight 0:18d7499b82f3 372 }
rrknight 0:18d7499b82f3 373
rrknight 0:18d7499b82f3 374 void switchOnEnableOperationControlwordIT(const int8_t nodeID)
rrknight 0:18d7499b82f3 375 {
rrknight 0:18d7499b82f3 376 CANMessage canmsg;
rrknight 0:18d7499b82f3 377
rrknight 0:18d7499b82f3 378 //Switch On & Enable Operation Controlword Firmware Spec 8.2.84 bit wise 0xxx 1111 so 0x000F
rrknight 0:18d7499b82f3 379 data[0] = WRITING_OBJECT_2_BYTE;
rrknight 0:18d7499b82f3 380 data[1] = 0x40;
rrknight 0:18d7499b82f3 381 data[2] = 0x60;
rrknight 0:18d7499b82f3 382 data[3] = 0x00;
rrknight 0:18d7499b82f3 383 data[4] = 0x0F;
rrknight 0:18d7499b82f3 384 data[5] = 0x00;
rrknight 0:18d7499b82f3 385 data[6] = 0x00;
rrknight 0:18d7499b82f3 386 data[7] = 0x00;
rrknight 0:18d7499b82f3 387 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 6));
rrknight 0:18d7499b82f3 388 }
rrknight 0:18d7499b82f3 389
rrknight 0:18d7499b82f3 390 void faultResetControlword(const int8_t nodeID)
rrknight 0:18d7499b82f3 391 {
rrknight 0:18d7499b82f3 392 CANMessage canmsg;
rrknight 0:18d7499b82f3 393
rrknight 0:18d7499b82f3 394 //Fault reset Controlword Firmware Spec 8.2.84 bit wise 0xxx x110 so 0x0006
rrknight 0:18d7499b82f3 395 data[0] = WRITING_OBJECT_2_BYTE;
rrknight 0:18d7499b82f3 396 data[1] = 0x40;
rrknight 0:18d7499b82f3 397 data[2] = 0x60;
rrknight 0:18d7499b82f3 398 data[3] = 0x00;
rrknight 0:18d7499b82f3 399 data[4] = 0x80; //Low Byte
rrknight 0:18d7499b82f3 400 data[5] = 0x00; //High Byte
rrknight 0:18d7499b82f3 401 data[6] = 0x00;
rrknight 0:18d7499b82f3 402 data[7] = 0x00;
rrknight 0:18d7499b82f3 403 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 6));
rrknight 0:18d7499b82f3 404 }
rrknight 0:18d7499b82f3 405
rrknight 0:18d7499b82f3 406 int8_t initEposBoard(const int8_t nodeID)
rrknight 0:18d7499b82f3 407 {
rrknight 0:18d7499b82f3 408 CANMessage canmsg;
rrknight 0:18d7499b82f3 409
rrknight 0:18d7499b82f3 410 //if(nodeID ==1) setNMT(nodeID, CS_RESET_NODE);
rrknight 0:18d7499b82f3 411 pc.printf("\t- initialise board ID = %d...", nodeID);
rrknight 0:18d7499b82f3 412
rrknight 0:18d7499b82f3 413 if(cantoepos.frequency(1000000) != 1) return EPOS2_ERROR;
rrknight 0:18d7499b82f3 414
rrknight 5:52e3137c2831 415 //First reset the node, that also clears the red LED that sometimes appears at the end of the CAN bus
therobotstudio 6:8a0250a4877a 416 //if(nodeID == 1) setNMT(nodeID, CS_RESET_COMMUNICATION);
therobotstudio 6:8a0250a4877a 417 //wait_ms(100);
therobotstudio 6:8a0250a4877a 418 //if(nodeID ==1) setNMT(nodeID, CS_RESET_NODE);
therobotstudio 6:8a0250a4877a 419 //wait_ms(100);
rrknight 5:52e3137c2831 420
rrknight 3:c16d726670b2 421 //setObjectSDO(nodeID, OBJECT_MAXIMAL_FOLLOWING_ERROR, 0xFFFFFFFA);
rrknight 3:c16d726670b2 422 setNMT(nodeID, CS_ENTER_PRE_OPERATIONAL);
rrknight 0:18d7499b82f3 423 wait_ms(100);
rrknight 0:18d7499b82f3 424
rrknight 3:c16d726670b2 425 //set parameters
rrknight 3:c16d726670b2 426 if(setObjectSDO(nodeID, OBJECT_MOTOR_TYPE, BRUSHED_DC_MOTOR) == EPOS2_OK) //do the first as a test if the nodeID exist and board is responding
rrknight 3:c16d726670b2 427 {
rrknight 3:c16d726670b2 428 setObjectSDO(nodeID, OBJECT_POLE_PAIR_NUMBER, 0x01);
rrknight 4:396433e30e64 429 setObjectSDO(nodeID, OBJECT_MAXIMAL_MOTOR_SPEED, 0x000030D4);
rrknight 4:396433e30e64 430 setObjectSDO(nodeID, OBJECT_THERMAL_TIME_CONSTANT_WINDING, 0x00B5);
rrknight 3:c16d726670b2 431
therobotstudio 6:8a0250a4877a 432 //Miscellaneous configuration : Set bit 0 to 1 to Disable sensor supervision by software, to prevent Position Sensor Breach Error
rrknight 4:396433e30e64 433 setObjectSDO(nodeID, OBJECT_MISCELLANEOUS_CONFIGURATION, 0x0000);
rrknight 0:18d7499b82f3 434
rrknight 3:c16d726670b2 435 //set Position Mode parameters
rrknight 4:396433e30e64 436 setObjectSDO(nodeID, OBJECT_MAXIMAL_FOLLOWING_ERROR, 0x0000C350);
rrknight 3:c16d726670b2 437 //setObjectSDO(nodeID, OBJECT_MAXIMAL_PROFILE_VELOCITY, 0x000061A8); //out of range ??
rrknight 5:52e3137c2831 438 setObjectSDO(nodeID, OBJECT_MAXIMAL_ACCELERATION, 0x00001332); //0x00000FFF ; 0x00002FFF
rrknight 0:18d7499b82f3 439
rrknight 3:c16d726670b2 440 //save all the parameters
rrknight 3:c16d726670b2 441 setObjectSDO(nodeID, OBJECT_STORE_PARAMETERS, SAVE_ALL);
rrknight 0:18d7499b82f3 442
rrknight 3:c16d726670b2 443 setNMT(nodeID, CS_RESET_COMMUNICATION);
rrknight 0:18d7499b82f3 444
rrknight 3:c16d726670b2 445 //setObjectSDO(nodeID, OBJECT_PROFILE_VELOCITY, 0x000003E8);
rrknight 3:c16d726670b2 446 //setObjectSDO(nodeID, OBJECT_PROFILE_ACCELERATION, 0x00002710);
rrknight 3:c16d726670b2 447 //setObjectSDO(nodeID, OBJECT_PROFILE_DECELERATION, 0x00002710);
rrknight 3:c16d726670b2 448 //setObjectSDO(nodeID, OBJECT_QUICKSTOP_DECELERATION, 0x00002710);
rrknight 3:c16d726670b2 449 //setObjectSDO(nodeID, OBJECT_MOTION_PROFILE_TYPE, 0x0001); //0= trapezoidal, 1 = sinusoidal
rrknight 3:c16d726670b2 450
rrknight 3:c16d726670b2 451 setNMT(nodeID, CS_ENTER_PRE_OPERATIONAL);
rrknight 3:c16d726670b2 452
rrknight 3:c16d726670b2 453 //set RxPDO 1 for position mode
rrknight 3:c16d726670b2 454 setPDO(nodeID, RECEIVE_PDO_1_PARAMETER_INDEX, COB_ID_RECEIVE_PDO_1_SUBINDEX, COB_ID_RECEIVE_PDO_1_DISABLE + nodeID, 4);
rrknight 3:c16d726670b2 455 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_1_INDEX, NUMBER_OBJECTS_RECEIVE_PDO_SUBINDEX, 0x00, 1);
rrknight 3:c16d726670b2 456 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_1_INDEX, MAPPED_OBJECT_1_RECEIVE_PDO_SUBINDEX, OBJECT_POSITION_MODE_SETTING_VALUE, 4);
rrknight 3:c16d726670b2 457 //setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_1_INDEX, MAPPED_OBJECT_1_RECEIVE_PDO_SUBINDEX, OBJECT_TARGET_POSITION, 4);
rrknight 3:c16d726670b2 458 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_1_INDEX, NUMBER_OBJECTS_RECEIVE_PDO_SUBINDEX, 0x01, 1); //enable RxPDO 1 with 1 object
rrknight 3:c16d726670b2 459 setPDO(nodeID, RECEIVE_PDO_1_PARAMETER_INDEX, COB_ID_RECEIVE_PDO_1_SUBINDEX, COB_ID_RECEIVE_PDO_1_ENABLE + nodeID, 4);
rrknight 3:c16d726670b2 460
rrknight 3:c16d726670b2 461 //set RxPDO 2 for current mode
rrknight 3:c16d726670b2 462 setPDO(nodeID, RECEIVE_PDO_2_PARAMETER_INDEX, COB_ID_RECEIVE_PDO_2_SUBINDEX, COB_ID_RECEIVE_PDO_2_DISABLE + nodeID, 4);
rrknight 3:c16d726670b2 463 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_2_INDEX, NUMBER_OBJECTS_RECEIVE_PDO_SUBINDEX, 0x00, 1);
rrknight 3:c16d726670b2 464 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_2_INDEX, MAPPED_OBJECT_1_RECEIVE_PDO_SUBINDEX, OBJECT_CURRENT_MODE_SETTING_VALUE, 4);
rrknight 3:c16d726670b2 465 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_2_INDEX, NUMBER_OBJECTS_RECEIVE_PDO_SUBINDEX, 0x01, 1); //enable RxPDO 2 with 1 object
rrknight 3:c16d726670b2 466 setPDO(nodeID, RECEIVE_PDO_2_PARAMETER_INDEX, COB_ID_RECEIVE_PDO_2_SUBINDEX, COB_ID_RECEIVE_PDO_2_ENABLE + nodeID, 4);
rrknight 3:c16d726670b2 467
rrknight 3:c16d726670b2 468 //set RxPDO 3 for velocity mode
rrknight 3:c16d726670b2 469 setPDO(nodeID, RECEIVE_PDO_3_PARAMETER_INDEX, COB_ID_RECEIVE_PDO_3_SUBINDEX, COB_ID_RECEIVE_PDO_3_DISABLE + nodeID, 4);
rrknight 3:c16d726670b2 470 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_3_INDEX, NUMBER_OBJECTS_RECEIVE_PDO_SUBINDEX, 0x00, 1);
rrknight 3:c16d726670b2 471 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_3_INDEX, MAPPED_OBJECT_1_RECEIVE_PDO_SUBINDEX, OBJECT_VELOCITY_MODE_SETTING_VALUE, 4);
rrknight 3:c16d726670b2 472 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_3_INDEX, NUMBER_OBJECTS_RECEIVE_PDO_SUBINDEX, 0x01, 1); //enable RxPDO 3 with 1 object
rrknight 3:c16d726670b2 473 setPDO(nodeID, RECEIVE_PDO_3_PARAMETER_INDEX, COB_ID_RECEIVE_PDO_3_SUBINDEX, COB_ID_RECEIVE_PDO_3_ENABLE + nodeID, 4);
rrknight 3:c16d726670b2 474
rrknight 3:c16d726670b2 475 //TODO set RxPDO 4 for fast Mode change
rrknight 3:c16d726670b2 476 setPDO(nodeID, RECEIVE_PDO_4_PARAMETER_INDEX, COB_ID_RECEIVE_PDO_4_SUBINDEX, COB_ID_RECEIVE_PDO_4_DISABLE + nodeID, 4);
rrknight 3:c16d726670b2 477 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_4_INDEX, NUMBER_OBJECTS_RECEIVE_PDO_SUBINDEX, 0x00, 1);
rrknight 3:c16d726670b2 478 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_4_INDEX, MAPPED_OBJECT_1_RECEIVE_PDO_SUBINDEX, OBJECT_MODE_OF_OPERATION, 4);
rrknight 3:c16d726670b2 479 setPDO(nodeID, MAPPED_OBJECT_RECEIVE_PDO_4_INDEX, NUMBER_OBJECTS_RECEIVE_PDO_SUBINDEX, 0x01, 1); //enable RxPDO 4 with 1 object
rrknight 3:c16d726670b2 480 setPDO(nodeID, RECEIVE_PDO_4_PARAMETER_INDEX, COB_ID_RECEIVE_PDO_4_SUBINDEX, COB_ID_RECEIVE_PDO_4_ENABLE + nodeID, 4);
rrknight 3:c16d726670b2 481
rrknight 3:c16d726670b2 482 //set TxPDO 1 to get position
rrknight 3:c16d726670b2 483 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_1_INDEX, NUMBER_OBJECTS_TRANSMIT_PDO_SUBINDEX, 0x00, 1);
rrknight 3:c16d726670b2 484 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_1_INDEX, MAPPED_OBJECT_1_TRANSMIT_PDO_SUBINDEX, OBJECT_POSITION_ACTUAL_VALUE, 4);
rrknight 3:c16d726670b2 485 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_1_INDEX, NUMBER_OBJECTS_TRANSMIT_PDO_SUBINDEX, 0x01, 1);
rrknight 3:c16d726670b2 486
rrknight 3:c16d726670b2 487 //set TxPDO 2 to get current
rrknight 3:c16d726670b2 488 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_2_INDEX, NUMBER_OBJECTS_TRANSMIT_PDO_SUBINDEX, 0x00, 1);
rrknight 3:c16d726670b2 489 //setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_2_INDEX, MAPPED_OBJECT_1_TRANSMIT_PDO_SUBINDEX, OBJECT_CURRENT_ACTUAL_VALUE, 4);
rrknight 3:c16d726670b2 490 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_2_INDEX, MAPPED_OBJECT_1_TRANSMIT_PDO_SUBINDEX, OBJECT_CURRENT_ACTUAL_VALUE_AVERAGED, 4);
rrknight 3:c16d726670b2 491 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_2_INDEX, NUMBER_OBJECTS_TRANSMIT_PDO_SUBINDEX, 0x01, 1);
rrknight 3:c16d726670b2 492
rrknight 3:c16d726670b2 493 //set TxPDO 3 to get velocity
rrknight 3:c16d726670b2 494 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_3_INDEX, NUMBER_OBJECTS_TRANSMIT_PDO_SUBINDEX, 0x00, 1);
rrknight 3:c16d726670b2 495 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_3_INDEX, MAPPED_OBJECT_1_TRANSMIT_PDO_SUBINDEX, OBJECT_VELOCITY_ACTUAL_VALUE_AVERAGED, 4);
rrknight 3:c16d726670b2 496 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_3_INDEX, NUMBER_OBJECTS_TRANSMIT_PDO_SUBINDEX, 0x01, 1);
rrknight 3:c16d726670b2 497
rrknight 3:c16d726670b2 498 //set TxPDO 4 to get poti position (analog input 1)
rrknight 3:c16d726670b2 499 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_4_INDEX, NUMBER_OBJECTS_TRANSMIT_PDO_SUBINDEX, 0x00, 1);
rrknight 3:c16d726670b2 500 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_4_INDEX, MAPPED_OBJECT_1_TRANSMIT_PDO_SUBINDEX, OBJECT_ANALOG_INPUT_1, 4);
rrknight 3:c16d726670b2 501 setPDO(nodeID, MAPPED_OBJECT_TRANSMIT_PDO_4_INDEX, NUMBER_OBJECTS_TRANSMIT_PDO_SUBINDEX, 0x01, 1);
rrknight 3:c16d726670b2 502
rrknight 3:c16d726670b2 503 //set TxPDO 1 to RTR mode (request only)
rrknight 3:c16d726670b2 504 setPDO(nodeID, TRANSMIT_PDO_1_PARAMETER_INDEX, COB_ID_TRANSMIT_PDO_1_SUBINDEX, COB_ID_TRANSMIT_PDO_1_DISABLE + nodeID, 4);
rrknight 3:c16d726670b2 505 setPDO(nodeID, TRANSMIT_PDO_1_PARAMETER_INDEX, TRANSMISSION_TYPE_TRANSMIT_PDO_1_SUBINDEX, PARAM_TRANSMISSION_TYPE_RTR, 1);
rrknight 3:c16d726670b2 506 setPDO(nodeID, TRANSMIT_PDO_1_PARAMETER_INDEX, COB_ID_TRANSMIT_PDO_1_SUBINDEX, COB_ID_TRANSMIT_PDO_1_ENABLE + nodeID, 4);
rrknight 3:c16d726670b2 507
rrknight 3:c16d726670b2 508 //set TxPDO 2 to RTR mode (request only)
rrknight 3:c16d726670b2 509 setPDO(nodeID, TRANSMIT_PDO_2_PARAMETER_INDEX, COB_ID_TRANSMIT_PDO_2_SUBINDEX, COB_ID_TRANSMIT_PDO_2_DISABLE + nodeID, 4);
rrknight 3:c16d726670b2 510 setPDO(nodeID, TRANSMIT_PDO_2_PARAMETER_INDEX, TRANSMISSION_TYPE_TRANSMIT_PDO_2_SUBINDEX, PARAM_TRANSMISSION_TYPE_RTR, 1);
rrknight 3:c16d726670b2 511 setPDO(nodeID, TRANSMIT_PDO_2_PARAMETER_INDEX, COB_ID_TRANSMIT_PDO_2_SUBINDEX, COB_ID_TRANSMIT_PDO_2_ENABLE + nodeID, 4);
rrknight 3:c16d726670b2 512
rrknight 3:c16d726670b2 513 //set TxPDO 3 to RTR mode (request only)
rrknight 3:c16d726670b2 514 setPDO(nodeID, TRANSMIT_PDO_3_PARAMETER_INDEX, COB_ID_TRANSMIT_PDO_3_SUBINDEX, COB_ID_TRANSMIT_PDO_3_DISABLE + nodeID, 4);
rrknight 3:c16d726670b2 515 setPDO(nodeID, TRANSMIT_PDO_3_PARAMETER_INDEX, TRANSMISSION_TYPE_TRANSMIT_PDO_3_SUBINDEX, PARAM_TRANSMISSION_TYPE_RTR, 1);
rrknight 3:c16d726670b2 516 setPDO(nodeID, TRANSMIT_PDO_3_PARAMETER_INDEX, COB_ID_TRANSMIT_PDO_3_SUBINDEX, COB_ID_TRANSMIT_PDO_3_ENABLE + nodeID, 4);
rrknight 3:c16d726670b2 517
rrknight 3:c16d726670b2 518 //set TxPDO 4 to RTR mode (request only)
rrknight 3:c16d726670b2 519 setPDO(nodeID, TRANSMIT_PDO_4_PARAMETER_INDEX, COB_ID_TRANSMIT_PDO_4_SUBINDEX, COB_ID_TRANSMIT_PDO_4_DISABLE + nodeID, 4);
rrknight 3:c16d726670b2 520 setPDO(nodeID, TRANSMIT_PDO_4_PARAMETER_INDEX, TRANSMISSION_TYPE_TRANSMIT_PDO_4_SUBINDEX, PARAM_TRANSMISSION_TYPE_RTR, 1);
rrknight 3:c16d726670b2 521 setPDO(nodeID, TRANSMIT_PDO_4_PARAMETER_INDEX, COB_ID_TRANSMIT_PDO_4_SUBINDEX, COB_ID_TRANSMIT_PDO_4_ENABLE + nodeID, 4);
rrknight 0:18d7499b82f3 522
rrknight 3:c16d726670b2 523 //setNMT(nodeID, CS_RESET_COMMUNICATION);
rrknight 3:c16d726670b2 524 //setObjectSDO(nodeID, OBJECT_MAXIMAL_FOLLOWING_ERROR, 0xFFFFFFFD);
rrknight 3:c16d726670b2 525 setNMT(nodeID, CS_START_REMOTE_NODE);
rrknight 3:c16d726670b2 526 //setObjectSDO(nodeID, OBJECT_MAXIMAL_FOLLOWING_ERROR, 0xFFFFFFFC);
rrknight 3:c16d726670b2 527
rrknight 3:c16d726670b2 528 setModeOfOperation(nodeID, VALUE_POSITION_MODE);//VALUE_PROFILE_POSITION_MODE); //this also initialise activMode variable
rrknight 3:c16d726670b2 529
rrknight 3:c16d726670b2 530 shutdownControlword(nodeID);
rrknight 3:c16d726670b2 531 switchOnEnableOperationControlword(nodeID);
rrknight 3:c16d726670b2 532 //setObjectSDO(nodeID, OBJECT_MAXIMAL_FOLLOWING_ERROR, 0xFFFFFFFB);
rrknight 3:c16d726670b2 533 /*
rrknight 3:c16d726670b2 534 //test
rrknight 3:c16d726670b2 535 setObjectSDO(nodeID, OBJECT_MAXIMAL_FOLLOWING_ERROR, 0xFFFFFFFD); //try here
rrknight 3:c16d726670b2 536 //save all the parameters
rrknight 3:c16d726670b2 537 setObjectSDO(nodeID, STORE_PARAMETERS, SAVE_ALL);
rrknight 3:c16d726670b2 538 //test end
rrknight 3:c16d726670b2 539 */
rrknight 3:c16d726670b2 540 pc.printf("...OK\n\r");
rrknight 3:c16d726670b2 541 }
rrknight 3:c16d726670b2 542 else
rrknight 3:c16d726670b2 543 {
rrknight 3:c16d726670b2 544 pc.printf("...NOT RESPONDING\n\r");
rrknight 0:18d7499b82f3 545
rrknight 3:c16d726670b2 546 return EPOS2_ERROR;
rrknight 3:c16d726670b2 547 }
rrknight 0:18d7499b82f3 548
rrknight 0:18d7499b82f3 549 return EPOS2_OK;
rrknight 0:18d7499b82f3 550 }
rrknight 0:18d7499b82f3 551
rrknight 0:18d7499b82f3 552 void setPosition(const int8_t nodeID, const int32_t position)
rrknight 0:18d7499b82f3 553 {
rrknight 0:18d7499b82f3 554 /*
rrknight 0:18d7499b82f3 555 if(activMode[nodeID-1] != POSITION)
rrknight 0:18d7499b82f3 556 {
rrknight 0:18d7499b82f3 557 setModeOfOperationPDO(nodeID, VALUE_POSITION_MODE);
rrknight 0:18d7499b82f3 558 }
rrknight 0:18d7499b82f3 559 */
rrknight 0:18d7499b82f3 560 data[0] = (position & 0x000000FF); //LSB
rrknight 0:18d7499b82f3 561 data[1] = (position & 0x0000FF00) >> 8;
rrknight 0:18d7499b82f3 562 data[2] = (position & 0x00FF0000) >> 16;
rrknight 0:18d7499b82f3 563 data[3] = (position & 0xFF000000) >> 24;
rrknight 0:18d7499b82f3 564 cantoepos.write(CANMessage(COB_ID_RECEIVE_PDO_1_ENABLE + nodeID, data, 4));
rrknight 0:18d7499b82f3 565 wait_us(100);
rrknight 0:18d7499b82f3 566 }
rrknight 0:18d7499b82f3 567
rrknight 0:18d7499b82f3 568 void setCurrent(const int8_t nodeID, const int16_t current)
rrknight 0:18d7499b82f3 569 {
rrknight 0:18d7499b82f3 570 /*
rrknight 0:18d7499b82f3 571 if(activMode[nodeID-1] != CURRENT)
rrknight 0:18d7499b82f3 572 {
rrknight 0:18d7499b82f3 573 setModeOfOperationPDO(nodeID, VALUE_CURRENT_MODE);
rrknight 0:18d7499b82f3 574 }
rrknight 0:18d7499b82f3 575 */
rrknight 0:18d7499b82f3 576 data[0] = (current & 0x00FF);
rrknight 0:18d7499b82f3 577 data[1] = current >> 8;
rrknight 0:18d7499b82f3 578
rrknight 0:18d7499b82f3 579 cantoepos.write(CANMessage(COB_ID_RECEIVE_PDO_2_ENABLE + nodeID, data, 2)); //RxPDO 2 for Current
rrknight 0:18d7499b82f3 580 wait_us(100);
rrknight 0:18d7499b82f3 581 }
rrknight 0:18d7499b82f3 582
rrknight 0:18d7499b82f3 583 void setVelocity(const int8_t nodeID, const int32_t velocity)
rrknight 0:18d7499b82f3 584 {
rrknight 0:18d7499b82f3 585 /*
rrknight 0:18d7499b82f3 586 if(activMode[nodeID-1] != VELOCITY)
rrknight 0:18d7499b82f3 587 {
rrknight 0:18d7499b82f3 588 setModeOfOperationPDO(nodeID, VALUE_VELOCITY_MODE);
rrknight 0:18d7499b82f3 589 }
rrknight 0:18d7499b82f3 590 */
rrknight 0:18d7499b82f3 591 data[0] = (velocity & 0x000000FF); //LSB
rrknight 0:18d7499b82f3 592 data[1] = (velocity & 0x0000FF00) >> 8;
rrknight 0:18d7499b82f3 593 data[2] = (velocity & 0x00FF0000) >> 16;
rrknight 0:18d7499b82f3 594 data[3] = (velocity & 0xFF000000) >> 24;
rrknight 0:18d7499b82f3 595 cantoepos.write(CANMessage(COB_ID_RECEIVE_PDO_3_ENABLE + nodeID, data, 4));
rrknight 0:18d7499b82f3 596 wait_us(100);
rrknight 0:18d7499b82f3 597 }
rrknight 0:18d7499b82f3 598
rrknight 0:18d7499b82f3 599 void setForce(const int8_t nodeID, const int16_t force)
rrknight 0:18d7499b82f3 600 {
rrknight 0:18d7499b82f3 601 //TODO
rrknight 0:18d7499b82f3 602 }
rrknight 0:18d7499b82f3 603
rrknight 0:18d7499b82f3 604 void getPosition(const int8_t nodeID)
rrknight 0:18d7499b82f3 605 {
rrknight 0:18d7499b82f3 606 cantoepos.write(CANMessage(COB_ID_TRANSMIT_PDO_1_ENABLE + nodeID)); //the answer frame is caught by interrupt
rrknight 0:18d7499b82f3 607 }
rrknight 0:18d7499b82f3 608
rrknight 0:18d7499b82f3 609 void getCurrent(const int8_t nodeID)
rrknight 0:18d7499b82f3 610 {
rrknight 0:18d7499b82f3 611 cantoepos.write(CANMessage(COB_ID_TRANSMIT_PDO_2_ENABLE + nodeID));
rrknight 0:18d7499b82f3 612 }
rrknight 0:18d7499b82f3 613
rrknight 0:18d7499b82f3 614 void getVelocity(const int8_t nodeID)
rrknight 0:18d7499b82f3 615 {
rrknight 0:18d7499b82f3 616 cantoepos.write(CANMessage(COB_ID_TRANSMIT_PDO_3_ENABLE + nodeID));
rrknight 0:18d7499b82f3 617 }
rrknight 0:18d7499b82f3 618
rrknight 0:18d7499b82f3 619 int16_t getForce(const int8_t nodeID)
rrknight 0:18d7499b82f3 620 {
rrknight 0:18d7499b82f3 621 //unsigned short force = 0x0000; //unsigned short is 16 bits
rrknight 0:18d7499b82f3 622 int16_t force = 0x0000;
rrknight 0:18d7499b82f3 623 float force_f = 0.0;
rrknight 0:18d7499b82f3 624
rrknight 5:52e3137c2831 625 //setMultiplexerChannel(nodeID);
rrknight 5:52e3137c2831 626 /*
rrknight 0:18d7499b82f3 627 if((nodeID > 0) && (nodeID <= 8))
rrknight 0:18d7499b82f3 628 {
rrknight 0:18d7499b82f3 629 force_f = an1.read();
rrknight 0:18d7499b82f3 630 }
rrknight 0:18d7499b82f3 631 else if((nodeID > 8) && (nodeID <= NUMBER_EPOS2_BOARDS))
rrknight 0:18d7499b82f3 632 {
rrknight 0:18d7499b82f3 633 force_f = an3.read();
rrknight 0:18d7499b82f3 634 }
rrknight 5:52e3137c2831 635 */
rrknight 0:18d7499b82f3 636
rrknight 5:52e3137c2831 637 force_f = an6.read();
rrknight 0:18d7499b82f3 638 force_f = force_f*10000;
rrknight 0:18d7499b82f3 639 force = (int16_t)force_f;
rrknight 5:52e3137c2831 640 //pc.printf("%d\n", force);
rrknight 0:18d7499b82f3 641
rrknight 0:18d7499b82f3 642 return force;
rrknight 0:18d7499b82f3 643 }
rrknight 0:18d7499b82f3 644
rrknight 0:18d7499b82f3 645 void getPotiPosition(const int8_t nodeID)
rrknight 0:18d7499b82f3 646 {
rrknight 0:18d7499b82f3 647 cantoepos.write(CANMessage(COB_ID_TRANSMIT_PDO_4_ENABLE + nodeID)); //TODO change it to get analog input on epos2
rrknight 0:18d7499b82f3 648 }
rrknight 0:18d7499b82f3 649
rrknight 0:18d7499b82f3 650 void getIncEnc1CntIdxPls(const int8_t nodeID)
rrknight 0:18d7499b82f3 651 {
rrknight 0:18d7499b82f3 652 //Get Incremental Encoder 1 Counter at Index Pulse Firmware Spec 8.2.45 Motor
rrknight 0:18d7499b82f3 653 data[0] = 0x40; //or 0x00 //read, so no data
rrknight 0:18d7499b82f3 654 data[1] = 0x21;
rrknight 0:18d7499b82f3 655 data[2] = 0x20;
rrknight 0:18d7499b82f3 656 data[3] = 0x00;
rrknight 0:18d7499b82f3 657 data[4] = 0x00; //null data
rrknight 0:18d7499b82f3 658 data[5] = 0x00;
rrknight 0:18d7499b82f3 659 data[6] = 0x00;
rrknight 0:18d7499b82f3 660 data[7] = 0x00;
rrknight 0:18d7499b82f3 661 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 4));
rrknight 0:18d7499b82f3 662 }
rrknight 0:18d7499b82f3 663
rrknight 0:18d7499b82f3 664 void getStatusword(const int8_t nodeID)
rrknight 0:18d7499b82f3 665 {
rrknight 0:18d7499b82f3 666 //Get Incremental Encoder 1 Counter at Index Pulse Firmware Spec 8.2.45 Motor
rrknight 0:18d7499b82f3 667 data[0] = 0x40; //receiving data : Byte 0 is 4B : see App Notes 10.4.2
rrknight 0:18d7499b82f3 668 data[1] = 0x41;
rrknight 0:18d7499b82f3 669 data[2] = 0x60;
rrknight 0:18d7499b82f3 670 data[3] = 0x00;
rrknight 0:18d7499b82f3 671 data[4] = 0x00; //null data
rrknight 0:18d7499b82f3 672 data[5] = 0x00;
rrknight 0:18d7499b82f3 673 data[6] = 0x00;
rrknight 0:18d7499b82f3 674 data[7] = 0x00;
rrknight 0:18d7499b82f3 675 cantoepos.write(CANMessage(COB_ID_SDO_CLIENT_TO_SERVER_DEFAULT + nodeID, data, 4));
rrknight 0:18d7499b82f3 676 }