Revision 0:0c88691e3904, committed 2015-03-01

Comitter:

soulx

Date:

Sun Mar 01 06:28:01 2015 +0000

Child:

1:1f6864549b92

Commit message:

create lib communication for robot

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Utilities.cpp	Sun Mar 01 06:28:01 2015 +0000
@@ -0,0 +1,75 @@
+/* Copyright (c) 2012 Georgios Petrou, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
+ * and associated documentation files (the "Software"), to deal in the Software without restriction, 
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or 
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+ 
+#include "Utilities.h"
+ 
+uint8_t Utilities::GetCheckSum(const uint8_t *data, uint8_t length)
+{
+    uint8_t checkSum = 0;
+
+    for(int i = 0; i < length; i++)
+        checkSum += data[i];
+
+    checkSum = ~checkSum;
+    
+    return checkSum;
+}
+
+void Utilities::ConvertUInt16ToUInt8Array(uint16_t value, uint8_t* data)
+{
+    data[0] = value;
+    data[1] = value >> 8;
+}        
+      
+void Utilities::ConvertInt16ToUInt8Array(int16_t value, uint8_t* data)
+{
+    data[0] = value;
+    data[1] = value >> 8;
+}           
+  
+void Utilities::ConvertUInt32ToUInt8Array(uint32_t value, uint8_t* data)
+{
+    data[0] = value;
+    data[1] = value >> 8;
+    data[2] = value >> 16;
+    data[3] = value >> 24;
+} 
+      
+void Utilities::ConvertInt32ToUInt8Array(int32_t value, uint8_t* data)
+{
+    data[0] = value;
+    data[1] = value >> 8;
+    data[2] = value >> 16;
+    data[3] = value >> 24;
+} 
+
+uint16_t Utilities::ConvertUInt8ArrayToUInt16(uint8_t* data)
+{
+    return (((uint16_t)data[1]) << 8) | ((uint16_t)data[0]);
+}
+
+int16_t Utilities::ConvertUInt8ArrayToInt16(uint8_t* data)
+{
+    return (((int16_t)data[1]) << 8) | ((int16_t)data[0]);
+}
+
+int32_t Utilities::ConvertUInt8ArrayToInt32(uint8_t* data)
+{
+    return  (((int32_t)data[3]) << 24) | (((int32_t)data[2]) << 16) | (((int32_t)data[1]) << 8) | ((int32_t)data[0]);
+}
+ 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Utilities.h	Sun Mar 01 06:28:01 2015 +0000
@@ -0,0 +1,88 @@
+/* Copyright (c) 2012 Georgios Petrou, MIT License
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
+ * and associated documentation files (the "Software"), to deal in the Software without restriction, 
+ * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
+ * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all copies or 
+ * substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
+ * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+ 
+#ifndef MX28_UTILITIES_H
+#define MX28_UTILITIES_H
+
+#include "mbed.h"
+
+/** MX28 utility functions class
+*/
+class Utilities
+{
+    public:
+        /** Get the checksum for a packet.
+        *        
+        * @param data The array of bytes.
+        * @param length The size of the array.
+        * @return checksum
+        */
+        static uint8_t GetCheckSum(const uint8_t *data, uint8_t length);
+        
+        /** Convert a UInt16 number to array of chars.
+        *        
+        * @param value The number value.
+        * @param data The array to store the conversion.
+        */
+        static void ConvertUInt16ToUInt8Array(uint16_t value, uint8_t* data);
+
+        /** Convert an Int16 number to array of chars.
+        *        
+        * @param value The number value.
+        * @param data The array to store the conversion.
+        */
+        static void ConvertInt16ToUInt8Array(int16_t value, uint8_t* data);
+        
+        /** Convert a UInt32 number to array of chars.
+        *        
+        * @param value The number value.
+        * @param data The array to store the conversion.
+        */
+        static void ConvertUInt32ToUInt8Array(uint32_t value, uint8_t* data);
+
+        /** Convert an Int32 number to array of chars.
+        *        
+        * @param value The number value.
+        * @param data The array to store the conversion.
+        */
+        static void ConvertInt32ToUInt8Array(int32_t value, uint8_t* data);
+        
+        /** Convert an array of char to UInt16.
+        *        
+        * @param data The array containing the chars.
+        * @return UInt16 converted number.
+        */
+        static uint16_t ConvertUInt8ArrayToUInt16(uint8_t* data);
+        
+        /** Convert an array of char to Int16.
+        *        
+        * @param data The array containing the chars.
+        * @return Int16 converted number.
+        */
+        static int16_t ConvertUInt8ArrayToInt16(uint8_t* data);
+        
+        /** Convert an array of char to Int32.
+        *        
+        * @param data The array containing the chars.
+        * @return Int32 number.
+        */
+        static int32_t ConvertUInt8ArrayToInt32(uint8_t* data);
+};
+
+#endif // MX28_UTILITIES_H 
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/communication.cpp	Sun Mar 01 06:28:01 2015 +0000
@@ -0,0 +1,157 @@
+#include "mbed.h"
+#include "communication.h"
+
+uint8_t COMMUNICATION::CommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet)
+{
+    uint8_t currentParameter = 0;
+    bool isWholePacket = false;
+
+    currentParameter = 0;
+    isWholePacket = false;
+    uint8_t decoderState = WAIT_ON_HEADER_0;
+
+    Timer timer;
+    timer.start();
+
+    while((timer.read_ms() < ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS) && (!isWholePacket)) {
+        if(serialCom->readable()) {
+            switch(decoderState) {
+                case WAIT_ON_HEADER_0: {
+
+                    uint8_t mx28ProtocolHeader0 = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("Read: 0x%02X ", mx28ProtocolHeader0);
+#endif
+
+                    decoderState = WAIT_ON_HEADER_1;
+
+                    break;
+                }
+                case WAIT_ON_HEADER_1: {
+
+                    uint8_t mx28ProtocolHeader1 = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", mx28ProtocolHeader1);
+#endif
+
+                    decoderState = WAIT_ON_ROBOT_ID;
+
+                    break;
+                }
+                case WAIT_ON_ROBOT_ID: {
+
+
+                    packet->robotId = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->servoId);
+#endif
+
+                    decoderState = WAIT_ON_LENGTH;
+
+                    break;
+                }
+                case WAIT_ON_LENGTH: {
+
+                    packet->length = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->length);
+#endif
+
+                    decoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
+
+                    break;
+                }
+                case WAIT_ON_INSTRUCTION_ERROR_ID: {
+
+                    packet->instructionErrorId = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", packet->instructionErrorId);
+#endif
+
+                    if(packet->length > 2)
+                        decoderState = WAIT_ON_PARAMETER;
+                    else
+                        decoderState = WAIT_ON_CHECK_SUM;
+
+                    break;
+                }
+                case WAIT_ON_PARAMETER: {
+
+                    uint8_t parameter = serialCom->getc();
+                    packet->parameter[currentParameter] = parameter;
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("0x%02X ", parameter);
+#endif
+
+                    if(++currentParameter == packet->length - 2)
+                        decoderState = WAIT_ON_CHECK_SUM;
+
+                    break;
+                }
+                case WAIT_ON_CHECK_SUM: {
+
+                    packet->checkSum = serialCom->getc();
+
+#ifdef ANDANTE_DEBUG
+                    pc->printf("sum =0x%02X\r\n", packet->checkSum);
+#endif
+
+                    decoderState = WAIT_ON_HEADER_0;
+                    isWholePacket = true;
+
+                    break;
+                }
+            }
+        }
+    }
+
+#ifdef ANDANTE_DEBUG
+    pc->printf("Timer: %d ms\r\n", timer.read_ms());
+#endif
+
+    timer.stop();
+
+    if(!isWholePacket) {
+#ifdef ANDANTE_DEBUG
+        pc->printf("Error: Read response timeout\r\n");
+#endif
+
+        return ANDANTE_ERRBIT_READ_TIMEOUT;
+    }
+
+    timer.reset();
+    timer.start();
+
+}
+
+COMMUNICATION::COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff, uint16_t rx_buff )
+{
+
+
+#ifdef ANDANTE_DEBUG
+    pc = new Serial(USBTX, USBRX);
+    pc->baud(115200);
+    pc->printf("\033[2J");
+#endif
+
+    serialCom = new iSerial(tx, rx,NULL,tx_buff,rx_buff);
+    serialCom->baud(baudRate);
+
+}
+
+COMMUNICATION::~COMMUNICATION()
+{
+#ifdef ANDANTE_DEBUG
+    if(pc != NULL)
+        delete pc;
+#endif
+
+    if(serialCom != NULL)
+        delete serialCom;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/communication.h	Sun Mar 01 06:28:01 2015 +0000
@@ -0,0 +1,36 @@
+#ifndef ANDANTE_COMMUNICATION_H
+#define ANDANTE_COMMUNICATION_H
+
+#include "mbed.h"
+#include "iSerial.h"
+#include "protocol.h"
+#include "Utilities.h"
+
+class COMMUNICATION
+{
+private:
+    iSerial *serialCom;
+
+public:
+    /** Send the MX28 packet over the serial half duplex connection.
+    *
+    * @param packet The MX28 packet.
+    * @return MX28_ERRBIT_NONE if succeeded, error code otherwise.
+    */
+    uint8_t CommunicatePacket(ANDANTE_PROTOCOL_PACKET *packet);
+
+    /** Create an MX28 servo object connected to the specified serial half duplex pins,
+    *   with the specified baudrate.
+    *
+    * @param tx Send pin.
+    * @param rx Receive pin.
+    * @param baudrate The bus speed.
+    */
+    COMMUNICATION(PinName tx, PinName rx, uint32_t baudRate, uint16_t tx_buff=1000, uint16_t rx_buff=1000 );
+
+    /** Destroy an MX28 servo object
+    */
+    ~COMMUNICATION();
+};
+
+#endif // MX28_H
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/protocol.h	Sun Mar 01 06:28:01 2015 +0000
@@ -0,0 +1,49 @@
+#ifndef ANDANTE_PROTOCOL_H
+#define ANDANTE_PROTOCOL_H
+
+#include "mbed.h"
+
+#define ANDANTE_BUFFER_SIZE            0x8F
+
+#define ANDANTE_PROTOCOL_COMMAND_RESPONSE_TIMEOUT_MS   20
+
+#define ANDANTE_PROTOCOL_HEADER_0      0xEF
+#define ANDANTE_PROTOCOL_HEADER_1      0x0F
+
+// Errors
+#define ANDANTE_ERRBIT_NONE            0x00
+#define ANDANTE_ERRBIT_VOLTAGE         0x01
+#define ANDANTE_ERRBIT_ANGLE           0x02
+#define ANDANTE_ERRBIT_OVERHEAT        0x04
+#define ANDANTE_ERRBIT_RANGE           0x08
+#define ANDANTE_ERRBIT_CHECKSUM        0x10
+#define ANDANTE_ERRBIT_OVERLOAD        0x20
+#define ANDANTE_ERRBIT_INSTRUCTION     0x40
+
+// Extra errors
+#define ANDANTE_ERRBIT_WRITE_TIMEOUT   0xFD
+#define ANDANTE_ERRBIT_READ_TIMEOUT    0xFE
+#define ANDANTE_ERRBIT_MASTER_CHECKSUM 0xFF
+
+
+struct ANDANTE_PROTOCOL_PACKET
+{
+    uint8_t robotId;
+    uint8_t length;    
+    uint8_t instructionErrorId;   
+    uint8_t parameter[ANDANTE_BUFFER_SIZE];
+    uint8_t checkSum;
+};
+
+enum ANDANTE_PROTOCOL_ENCODER_DECODER_STATE
+{
+    WAIT_ON_HEADER_0,
+    WAIT_ON_HEADER_1,
+    WAIT_ON_ROBOT_ID,   
+    WAIT_ON_LENGTH,
+    WAIT_ON_INSTRUCTION_ERROR_ID,
+    WAIT_ON_PARAMETER,
+    WAIT_ON_CHECK_SUM
+};
+
+#endif // ANDANTE_PROTOCOL_H
\ No newline at end of file