Used to communicate with the Maxon EPOS 24/2 motor controller using the ST Nucleo micro controller. Can be easily modified to work with other micro controllers as well. Only limited functionality (velocity mode and current reading).
maxonEPOS2.cpp
00001 /** 00002 * 00003 * maxonEPOS2.cpp 00004 * 00005 * Created on: Oct 5, 2016 00006 * Author: Nadun Wijesinghe 00007 */ 00008 00009 #include "maxonEPOS2.h" 00010 00011 00012 BufferedSerial maxonEPOS2::getSerialPC() 00013 { 00014 return BufferedSerial (USBTX, USBRX, 256, 4); 00015 } 00016 00017 BufferedSerial maxonEPOS2::getSerialEPOS(int serialID) 00018 { 00019 switch (serialID) { 00020 case 2: 00021 return BufferedSerial (PD_5, PD_6, 256, 4); 00022 00023 case 3: 00024 return BufferedSerial (PB_10, PB_11, 256, 4); 00025 00026 case 5: 00027 return BufferedSerial (PC_12, PD_2, 256, 4); 00028 00029 case 7: 00030 default: 00031 return BufferedSerial (PE_8, PE_7, 256, 4); 00032 } 00033 if (serialID == 0) return BufferedSerial (PD_5, PD_6, 256, 4); 00034 else return BufferedSerial (PC_12, PD_2, 256, 4); 00035 } 00036 00037 /************************************************************************************ 00038 * 00039 * The getObject function uses arguments to transmitten the required bytes to receive 00040 * the intended information from the object dictionary. 00041 * 00042 * Technically can work with any transmission but has not been tested. 00043 * 00044 * comm[] - the pointer to the array of bytes that are to be sent 00045 * commsSize - size of comm[] 00046 * talkative - Activate debugging 00047 * 00048 * Additional Info: 00049 * - Adapts to send any byte array size 00050 * - Can recieve up to a 20 byte response 00051 * - Does a decent job at filtering out any issues such as: 00052 * - Reading null bytes, which are seen as 255 00053 * - Dealing with speed of checksum 00054 * - This is probably the only part of the program people would ever care about. 00055 * 00056 **********************************************************************************/ 00057 int maxonEPOS2::getObject(unsigned char comms[], int commsSize) 00058 { 00059 // Declare and initialize variables 00060 receivedArraySize = 0; 00061 for (int i = 0; i < TX_RX_ARRAY_SIZE; i++) receivedArray[i] = 0xFF; 00062 00063 //debug 00064 if (DEBUG_LL) { 00065 pc.printf("\nAttempting to get this object:\n"); 00066 for (int i = 0; i < commsSize; i++) { 00067 pc.printf("%X, ", comms[i]); 00068 } 00069 pc.printf("\n"); 00070 } 00071 00072 // Clear the buffer (to remove any remnants of the old transmission) 00073 clearBuffer(); 00074 if (DEBUG_LL) pc.printf("Sending op code.\n"); 00075 // Send Op code 00076 epos.putc(comms[0]); 00077 00078 // Small wait_ms for response 00079 wait_ms(10); 00080 00081 // Read Response 00082 char firstConfirm = 0; 00083 if (epos.readable()) firstConfirm = epos.getc(); 00084 00085 // If response was 'O' as in OK then op code handshake was confirmed 00086 if (firstConfirm == 'O') { 00087 //debug 00088 if (DEBUG_LL) { 00089 pc.printf("Op code recieved correctly, sending data.\n"); 00090 } 00091 00092 // Set reponse to 255 to filter for an answer 00093 //clearBuffer(); //-------------------------------------------------- 00094 // Send data of the message 00095 for (int i = 1; i < commsSize; i++) { 00096 epos.putc(comms[i]); 00097 wait_us(100); 00098 } 00099 00100 // Wait for maxon to calculate checksum and send answer op code 00101 wait_ms(10); 00102 00103 // Filter for an answer 00104 char secondConfirm = 255; 00105 while ((secondConfirm == 255) && (epos.readable() >= 1)) { 00106 //if (DEBUG_LL) pc.printf("Waiting for secondConfirm.\n"); 00107 secondConfirm = epos.getc(); 00108 } 00109 00110 // Check if response is confirmed again 00111 if (secondConfirm == 'O') { 00112 //debug 00113 if (DEBUG_LL) { 00114 pc.printf("Data was recieved correctly, waiting for response.\n"); 00115 } 00116 00117 // Filter for response again 00118 00119 /* 00120 int timeout = 10; int count = 0; 00121 while ((response[0] == 255) && (count < timeout)) 00122 { 00123 //pc.printf("Trying to read response[0].\n"); 00124 if (epos.readable() >= 1) response[0] = epos.getc(); 00125 //count++; 00126 wait_ms(100); 00127 } 00128 */ 00129 while ((receivedArray[0] == 255) && (epos.readable() >= 1)) { 00130 //if (DEBUG_LL) pc.printf("Reading receivedArray[0].\n"); 00131 receivedArray[0] = epos.getc(); 00132 wait_us(10); 00133 } 00134 00135 //epos.putc(79); 00136 00137 // If the response was received and confirmed to be an answer of 0 00138 // (which would be correct) 00139 if (receivedArray[0] == 0) 00140 //if (1) 00141 { 00142 // Send acknowledgement (the character 'O') 00143 //clearBuffer(); 00144 epos.putc(79); 00145 00146 //int count = 0; 00147 // Receive responses 00148 for (int i = 1; i < TX_RX_ARRAY_SIZE; i++) { 00149 wait_us(240); 00150 if (epos.readable()) { 00151 receivedArray[i] = epos.getc(); 00152 //count++; 00153 } 00154 //else i--; 00155 00156 //if (count > 20) break; 00157 } 00158 wait_ms(10); 00159 // Send confirmation of answer, without checking CRC 00160 epos.putc('O'); 00161 00162 //debug 00163 if (DEBUG_LL) { 00164 pc.printf("\nAnswer recieved \n"); 00165 00166 for (int i = 0; i < TX_RX_ARRAY_SIZE; i++) { 00167 if (receivedArray[i] != 255) { 00168 pc.printf("\n"); 00169 pc.printf("%d", (i + 1)); 00170 pc.printf(": "); 00171 pc.printf("%X", receivedArray[i]); 00172 } 00173 } 00174 } 00175 } 00176 // If receivedArray was incorrect 00177 else { 00178 if (DEBUG_LL) pc.printf("receivedArray[0] was: %d\n", receivedArray[0]); 00179 return responseError; 00180 } 00181 } 00182 // If the second confirm was not an 'O' 00183 else if (secondConfirm == 'F') { 00184 if (DEBUG_LL) pc.printf("Got an F for secondConfirm\n"); 00185 return secondConfirmFail; 00186 } else { 00187 if (DEBUG_LL) pc.printf("Invalid secondConfirm: %d\n", secondConfirm); 00188 return secondConfirmError; 00189 } 00190 } 00191 // If the first confirm was not an 'O' 00192 else if (firstConfirm == 'F') { 00193 if (DEBUG_LL) pc.printf("Got an F for firstConfirm\n"); 00194 return firstConfirmFail; 00195 } else { 00196 if (DEBUG_LL) pc.printf("Invalid firstConfirm: %d\n", firstConfirm); 00197 return firstConfirmError; 00198 } 00199 00200 receivedArraySize = (receivedArray[1] + 1) * 2 + 4; 00201 return noError; 00202 } 00203 00204 void maxonEPOS2::debug(const char* s) 00205 { 00206 pc.printf(s); 00207 } 00208 00209 void maxonEPOS2::init() 00210 { 00211 // Establish connections 00212 // Serial to PC 00213 pc.baud(9600); 00214 pc.format(8, SerialBase::None, 1); 00215 if (DEBUG_HL) pc.printf("\n=======================================\n"); 00216 if (DEBUG_HL) pc.printf("Connected to PC.\n"); 00217 00218 00219 // Serial to Maxon EPOS 24/2 00220 epos.baud(115200); 00221 epos.format(8, SerialBase::None, 1); 00222 if (DEBUG_HL) pc.printf("Connected to EPOS.\n"); 00223 00224 // Initialize arrays and variables 00225 for (int i = 0; i < TX_RX_ARRAY_SIZE; i++) { 00226 command[i] = 0; 00227 transmitArray[i] = 0; 00228 receivedArray[i] = 0; 00229 } 00230 transmitArraySize = 0; 00231 receivedArraySize = 0; 00232 } 00233 00234 /* Build the serial command based on input parameters. 00235 * @param op-code, index, node ID, sub index, data, size of the data array 00236 * @returns size of the serial command 00237 */ 00238 void maxonEPOS2::buildSerialCommand(unsigned char opCode, unsigned int index, 00239 unsigned char nodeID, unsigned char subIndex, unsigned char* data, 00240 unsigned int dataSize) 00241 { 00242 // Increase size of the array by 8 bytes to account for: 00243 // Op code - 1 00244 // Len-1 - 1 00245 // Index - 2 00246 // Sub index - 1 00247 // Node ID - 1 00248 // CRC - 2 00249 if (DEBUG_LL) pc.printf("Building command with op code %X\n", opCode); 00250 00251 transmitArraySize = dataSize + 8; 00252 00253 // Create array to store the command and calculate lenMinusOne value 00254 unsigned char lenMinusOne = 2 + dataSize / 2 - 1; 00255 00256 // Insert the first few parameters 00257 command[0] = opCode; 00258 command[1] = lenMinusOne; 00259 command[2] = (index & HIGH_BYTE_MASK) >> 8; 00260 command[3] = index & LOW_BYTE_MASK; 00261 command[4] = nodeID; 00262 command[5] = subIndex; 00263 00264 // Insert the data bytes 00265 for (int i = 0; i < dataSize; i++) { 00266 command[6 + i] = data[i]; 00267 } 00268 00269 // Calculate CRC and insert it 00270 unsigned int crc = calc_crc(command, transmitArraySize - 2); 00271 command[6 + dataSize] = (crc & HIGH_BYTE_MASK) >> 8; 00272 command[7 + dataSize] = crc & LOW_BYTE_MASK; 00273 } 00274 00275 /* Clear the buffer of any remnant data from the last transmission. 00276 * @param none 00277 * @returns none 00278 */ 00279 void maxonEPOS2::clearBuffer () 00280 { 00281 //clears Serial read buffer after each pass just as a debugging measure 00282 if (DEBUG_LL) pc.printf("Clearing buffer.\n"); 00283 char char1 = 0; 00284 while (epos.readable()) { 00285 char1 = epos.getc(); 00286 if (DEBUG_LL) pc.printf("Byte 0x%X removed from buffer.\n", char1); 00287 } 00288 } 00289 00290 /* Build the transmit array based on the serial commend (since some bytes are 00291 * out-of-order when it's being transmitted). 00292 * @param byte array from buildSerialCommand, size of that array 00293 * @returns nothing 00294 */ 00295 void maxonEPOS2::buildTransmitArray() 00296 { 00297 if (DEBUG_LL) pc.printf("Building transmit array.\n"); 00298 00299 // The op-code and len-1 bytes stay the same 00300 transmitArray[0] = command[0]; 00301 transmitArray[1] = command[1]; 00302 00303 // Other bytes have to be rearranged 00304 for (int i = 2; i < transmitArraySize; i++) { 00305 if (i % 2 == 0) transmitArray[i] = command[i + 1]; 00306 else transmitArray[i] = command[i - 1]; 00307 } 00308 } 00309 00310 long maxonEPOS2::faultReset (unsigned char nodeID) 00311 { 00312 // Turn off motor 00313 // 11 02 6040 0100 0080 B3C8 00314 00315 // Initialize 00316 transmitArraySize = 0; 00317 unsigned int dataSize = 0; 00318 long receivedErrorCode = noError; 00319 00320 unsigned char data[] = d_FaultReset; 00321 dataSize = sizeof(data) / sizeof(unsigned char); 00322 00323 // Build the serial command 00324 buildSerialCommand(opCode_Write, i_ControlWord, nodeID, 00325 subI_ControlWord, data, dataSize); 00326 if (DEBUG_HL) printArray (command, transmitArraySize, "Clear faults", "totalSize", 1); 00327 // Rearrange to create the array to transmit 00328 buildTransmitArray(); 00329 // Transmit the array and get the transmission result (as the error code) 00330 receivedErrorCode = transmitCommand(); 00331 wait_ms(COMMAND_DELAY); 00332 00333 return receivedErrorCode; 00334 } 00335 00336 long maxonEPOS2::motorShutdown (unsigned char nodeID) 00337 { 00338 // Turn off motor 00339 // 11 02 6040 0100 0006 4286 00340 00341 // Initialize 00342 transmitArraySize = 0; 00343 unsigned int dataSize = 0; 00344 long receivedErrorCode = noError; 00345 00346 unsigned char data[] = d_Shutdown; 00347 dataSize = sizeof(data) / sizeof(unsigned char); 00348 00349 // Build the serial command 00350 buildSerialCommand(opCode_Write, i_ControlWord, nodeID, 00351 subI_ControlWord, data, dataSize); 00352 if (DEBUG_HL) printArray (command, transmitArraySize, "Motor shut down", "totalSize", 1); 00353 // Rearrange to create the array to transmit 00354 buildTransmitArray(); 00355 // Transmit the array and get the transmission result (as the error code) 00356 receivedErrorCode = transmitCommand(); 00357 wait_ms(COMMAND_DELAY); 00358 00359 return receivedErrorCode; 00360 } 00361 00362 long maxonEPOS2::motorSwitchOn (unsigned char nodeID) 00363 { 00364 // Turn on motor 00365 // 11 02 6040 0100 000F D3AF 00366 00367 // Initialize 00368 transmitArraySize = 0; 00369 unsigned int dataSize = 0; 00370 long receivedErrorCode = noError; 00371 00372 unsigned char data[] = d_SwitchOn; 00373 dataSize = sizeof(data) / sizeof(unsigned char); 00374 00375 // Build the serial command 00376 buildSerialCommand(opCode_Write, i_ControlWord, nodeID, 00377 subI_ControlWord, data, dataSize); 00378 if (DEBUG_HL) printArray (command, transmitArraySize, "Motor switch on", "totalSize", 1); 00379 // Rearrange to create the array to transmit 00380 buildTransmitArray(); 00381 // Transmit the array 00382 receivedErrorCode = transmitCommand(); 00383 wait_ms(COMMAND_DELAY); 00384 00385 return receivedErrorCode; 00386 } 00387 00388 long maxonEPOS2::motorVelocityModeOn (unsigned char nodeID) 00389 { 00390 // Turn on velocity mode 00391 //11 02 6060 0100 FFFE 27DA 00392 00393 // Initialize 00394 transmitArraySize = 0; 00395 unsigned int dataSize = 0; 00396 long receivedErrorCode = noError; 00397 00398 unsigned char data[] = d_VelocityMode; 00399 dataSize = sizeof(data) / sizeof(unsigned char); 00400 00401 // Build the serial command 00402 buildSerialCommand(opCode_Write, i_ModesOfOperation, nodeID, 00403 subI_ModesOfOperation, data, dataSize); 00404 if (DEBUG_HL) printArray (command, transmitArraySize, "Turn on velocity mode", "totalSize", 1); 00405 // Rearrange to create the array to transmit 00406 buildTransmitArray(); 00407 // Transmit the array 00408 receivedErrorCode = transmitCommand(); 00409 wait_ms(COMMAND_DELAY); 00410 00411 return receivedErrorCode; 00412 } 00413 00414 long maxonEPOS2::motorSetVelocity (unsigned char nodeID, long motorSpeed) 00415 { 00416 // Set velocity value 00417 //11 03 206B 0100 1F40 0000 C28E 00418 00419 // Initialize 00420 transmitArraySize = 0; 00421 unsigned int dataSize = 0; 00422 long receivedErrorCode = noError; 00423 00424 // Convert the speed to a byte array 00425 unsigned char data[4]; 00426 numberToByteArray (motorSpeed, data); 00427 dataSize = sizeof(data) / sizeof(unsigned char); 00428 00429 // Build the serial command 00430 buildSerialCommand(opCode_Write, i_VelocityModeSettingValue, nodeID, 00431 subI_VelocityModeSettingValue, data, dataSize); 00432 if (DEBUG_HL) printArray (command, transmitArraySize, "Set velocity value", "totalSize", 1); 00433 // Rearrange to create the array to transmit 00434 buildTransmitArray(); 00435 // Transmit the array 00436 receivedErrorCode = transmitCommand(); 00437 wait_ms(COMMAND_DELAY); 00438 00439 return receivedErrorCode; 00440 } 00441 00442 long maxonEPOS2::motorGetCurrent (unsigned char nodeID) 00443 { 00444 // Turn off motor 00445 // 11 02 6040 0100 0006 4286 00446 00447 // Initialize 00448 transmitArraySize = 0; 00449 unsigned int dataSize = 0; 00450 long receivedErrorCode = noError; 00451 00452 unsigned char data[1]; 00453 dataSize = 0; 00454 00455 // Build the serial command 00456 buildSerialCommand(opCode_Read, i_CurrentActualValueAveraged, nodeID, 00457 subI_CurrentActualValueAveraged, data, dataSize); 00458 if (DEBUG_HL) printArray (command, transmitArraySize, "Get motor current", "totalSize", 1); 00459 // Rearrange to create the array to transmit 00460 buildTransmitArray(); 00461 // Transmit the array 00462 receivedErrorCode = transmitCommand(); 00463 wait_ms(COMMAND_DELAY); 00464 00465 return receivedErrorCode; 00466 } 00467 00468 /* Parse the error code bytes in received array to determine the error value 00469 * @param result after transmitting 00470 * @returns error code (if present) or error type (as defined in the header file) 00471 */ 00472 long maxonEPOS2::parseErrorCode (int transmitResult) 00473 { 00474 long error = 0; 00475 // Debug 00476 if (transmitResult == firstConfirmFail) { 00477 if (DEBUG_HL) pc.printf("First confirm failed.\n"); 00478 } else if (transmitResult == firstConfirmError) { 00479 if (DEBUG_HL) pc.printf("First confirm error.\n"); 00480 } else if (transmitResult == secondConfirmFail) { 00481 if (DEBUG_HL) pc.printf("Second confirm failed.\n"); 00482 } else if (transmitResult == secondConfirmError) { 00483 if (DEBUG_HL) pc.printf("Second confirm error.\n"); 00484 } else if (transmitResult == responseError) { 00485 if (DEBUG_HL) pc.printf("Response error: receivedArray[0] was 0x%X\n", receivedArray[0]); 00486 } else { 00487 // Check the error bytes (op code is already checked in the getObject function 00488 // and the length byte is not important) 00489 if (receivedArray[2] != 0 || receivedArray[3] != 0 || receivedArray[4] != 0 00490 || receivedArray[5] != 0) { 00491 if (DEBUG_HL) pc.printf ("Error code received: "); 00492 if (DEBUG_HL) pc.printf ("%X", receivedArray[5]); 00493 if (DEBUG_HL) pc.printf (" "); 00494 if (DEBUG_HL) pc.printf ("%X", receivedArray[4]); 00495 if (DEBUG_HL) pc.printf (" "); 00496 if (DEBUG_HL) pc.printf ("%X", receivedArray[3]); 00497 if (DEBUG_HL) pc.printf (" "); 00498 if (DEBUG_HL) pc.printf ("%X", receivedArray[2]); 00499 if (DEBUG_HL) pc.printf ("\n"); 00500 // Create the error value and return it 00501 error = (receivedArray[5] << 24) || (receivedArray[4] << 16) 00502 || (receivedArray[3] << 8) || receivedArray[2]; 00503 return error; 00504 } else { 00505 if (DEBUG_HL) pc.printf("Command sent successfully.\n"); 00506 return noError; 00507 } 00508 } 00509 // Return the error type (as defined in the header file) 00510 return transmitResult; 00511 } 00512 00513 /* Print an array to the serial interface 00514 * @param array to print, size of the array, name of the array, 00515 * name of the size variable 00516 * @returns none 00517 */ 00518 void maxonEPOS2::printArray (unsigned char* array, unsigned int arraySize, 00519 const char* arrayName, const char* sizeName, char printHeader) 00520 { 00521 if (!DEBUG_HL) return; 00522 00523 if (printHeader) pc.printf("\n---------------------------------------\n"); 00524 // Exit if the size is invalid 00525 if (arraySize <= 0) { 00526 pc.printf ("\nInvalid size "); 00527 pc.printf ("%d", arraySize); 00528 pc.printf (" in variable "); 00529 pc.printf ("%s\n", sizeName); 00530 return; 00531 } 00532 00533 // Print the headers 00534 pc.printf ("%s (%s : %d)\n", arrayName, sizeName, arraySize); 00535 00536 // Print the array 00537 for (int i = 0; i < arraySize; i++) { 00538 pc.printf("%X", array[i]); 00539 pc.printf(" "); 00540 } 00541 pc.printf ("\n"); 00542 } 00543 00544 /* Convert a number to a 4-byte array (used when setting velocity) 00545 * @param number to convert, array that stores the converted data 00546 * @returns none 00547 */ 00548 void maxonEPOS2::numberToByteArray (long number, unsigned char* data) 00549 { 00550 // Build up the array by shifting and modulus operation 00551 data[1] = number % 0x0100; 00552 data[0] = (number >> 8) % 0x0100; 00553 data[3] = (number >> 16) % 0x0100; 00554 data[2] = (number >> 24) % 0x0100; 00555 } 00556 00557 /* Parse the data that have been received from the EPOS after a read command 00558 * @param data array (NOT the whole received array) 00559 * @returns the data being read (as a number) 00560 */ 00561 long maxonEPOS2::parseData () 00562 { 00563 unsigned char* data = &receivedArray[6]; 00564 00565 // Shift and add to create the number 00566 long number = data[0] | (data[1] << 8) | (data[2] << 16) | (data[3] << 24); 00567 00568 if (DEBUG_HL) pc.printf("Data : %d\n", number); 00569 00570 return number; 00571 } 00572 00573 /* Transmit the command to the EPOS 00574 * @param array to transmit (from the buildSerialArray function), size of the array 00575 * @returns error code / error type after communication (0 if no error) 00576 */ 00577 long maxonEPOS2::transmitCommand () 00578 { 00579 if (DEBUG_LL) pc.printf("Transmitting command.\n"); 00580 00581 // Initialize 00582 unsigned int transmitCount = 0; 00583 int transmitResult = noError; 00584 long receivedErrorCode = noError; 00585 00586 // Transmit up to MAX_TRANSMIT_COUNT times 00587 while (transmitCount < MAX_TRANSMIT_COUNT) { 00588 // Transmit the command and get the result 00589 transmitResult = getObject(transmitArray, transmitArraySize); 00590 // If the transmission was a success, exit loop 00591 receivedErrorCode = parseErrorCode(transmitResult); 00592 if (receivedErrorCode == noError) { 00593 if (DEBUG_HL) printArray (receivedArray, receivedArraySize, "Received array", 00594 "receivedArraySize", 0); 00595 return receivedErrorCode; 00596 } 00597 // If it was a failure, continue 00598 transmitCount++; 00599 if (DEBUG_HL) pc.printf("\nRe-transmitting.\n"); 00600 wait_ms(500); 00601 } 00602 00603 if (DEBUG_HL) pc.printf("Communication error, the command was not sent.\n"); 00604 return receivedErrorCode; 00605 } 00606 00607 /* Calculate the CRC value of a byte array 00608 * @param byte array to calculate CRC of, size of the array 00609 * @returns CRC of the array 00610 */ 00611 uint16_t maxonEPOS2::calc_crc(unsigned char *msg,int n) 00612 { 00613 uint16_t x = 0; 00614 00615 while(n--) { 00616 x = crc_xmodem_update(x, (uint16_t)*msg++); 00617 } 00618 00619 return(x); 00620 } 00621 00622 00623 /* Supportive function to calculate the CRC value 00624 */ 00625 uint16_t maxonEPOS2::crc_xmodem_update (uint16_t crc, uint8_t data) 00626 { 00627 int i; 00628 00629 crc = crc ^ ((uint16_t)data << 8); 00630 for (i=0; i<8; i++) { 00631 if (crc & 0x8000) 00632 crc = (crc << 1) ^ 0x1021; //(polynomial = 0x1021) 00633 else 00634 crc <<= 1; 00635 } 00636 return crc; 00637 }
Generated on Tue Jul 26 2022 01:55:06 by
1.7.2