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).

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers maxonEPOS2.cpp Source File

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 }