Revision 2:2330ad8b1baa, committed 2016-06-15

Comitter:

katrijnverhasselt

Date:

Wed Jun 15 10:53:32 2016 +0000

Parent:

1:8ba039abd9b8

Commit message:

Finished programming. This program now simply extract only the info it needs and sends all of it immediately through its I2C port.

Changed in this revision

--- a/MCP4725.lib	Wed May 18 11:35:45 2016 +0000
+++ b/MCP4725.lib	Wed Jun 15 10:53:32 2016 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/users/katrijnverhasselt/code/MCP4725/#e83ec147be06
+https://developer.mbed.org/users/katrijnverhasselt/code/MCP4725/#05ccdd6cebf8

--- a/Reader.cpp	Wed May 18 11:35:45 2016 +0000
+++ b/Reader.cpp	Wed Jun 15 10:53:32 2016 +0000
@@ -9,8 +9,6 @@
     _buffer(),
     _bufferSize(0),
     _lastChar(0),
-    _isMessageStarted(false),
-    _charsToRead(-1),
     _index(0)
 {
     // Attach read function to pc
@@ -32,7 +30,7 @@
     return _lastMessageInfo;
 }
 
-const int8_t* Reader::GetLastMessage() const {
+const char* Reader::GetLastMessage() const {
     return _lastMessage;
 }
 
@@ -43,65 +41,41 @@
 void Reader::pcCallback() {
     // Get the sent character
     _lastChar = pc.getc();
-    if (_lastChar == Translator::Rules::StartChar) {
-        _index = 0;
-    }
-    // Add character to buffer
-    _buffer[_index] = _lastChar;
-    // Check if valid message by translating it
-    Translator::MessageInfo info;
-    if (!Translator::Translate(_buffer, _index + 1, &info)) {
-        _index++;
-        return;
-    }
     
-    // Translation succesful, copy to _info
-    _lastMessageInfo = info;
-    // Notify user of new message
-    _isNewMessageReceived = true;
-    // Reset counter
-    _index = 0;
-    return;
-
-    
-    
+    // If message not started and StartChar was read
+    //if (!_isMessageStarted && _lastChar == Translator::Rules::StartChar) {
+    //    // Reset and signal start
+    //    _index = 0;
+    //    _isMessageStarted = true;
+    //}
     
-    //printf("'%c' (%i) ; ", _lastChar, (int)_lastChar);
-    // If no ongoing message and starting character was received
-    if (_lastChar == Translator::Rules::StartChar) {
-        //printf("First char: %c", _lastChar);
-        // Get ready to receive the message
-        _isMessageStarted = true;
-        _index = 0;
-        _charsToRead = -1; // value < 0 indicates no length was read yet
-    } // If message was started but no length was read yet
-    else if (_charsToRead < 0) {
-        // Get ready to receive the command
-        _charsToRead = (int)_lastChar;
-        //printf("CharsToRead: %i", _charsToRead);
-        //_buffer = new int8_t[_charsToRead];
-        _index = 0;
-    } // If message was started and size is known
-    else if (_charsToRead > 0) {
-        //printf("Storing '%c' (%i)", _lastChar, (int)_lastChar);
-        // Read _charsToRead characters and store in _buffer
-        if (_index < _charsToRead) {
-            _buffer[_index] = _lastChar;
+    // If ongoing message
+    //if (_isMessageStarted) {
+        
+        // Add character to buffer
+        _buffer[_index] = _lastChar;
+        // Check if valid message by translating it
+        Translator::MessageInfo info;
+        
+        switch (Translator::Translate(_buffer, _index + 1, &info)) {
+        case Translator::INCOMPLETE:
+            // Keep adding chars
             _index++;
-            
-            // If message was fully received
-            if (_index == _charsToRead) {
-                // Copy _buffer to _lastMessage (see Utility.h)
-                DeepCopy(_buffer, _charsToRead, _lastMessage);
-                _lastMessageSize = _charsToRead;
-                // Reset values
-                _isMessageStarted = false;
-                _charsToRead = -1;
-                _index = 0;
-                // Let user know that a new message was received
-                _isNewMessageReceived = true;
-            }
+            break;;
+        case Translator::SUCCES:
+            // Copy message info
+            _lastMessageInfo = info;
+            // Notify user
+            _isNewMessageReceived = true;
+            // Reset counter
+            _index = 0;
+            break;
+        case Translator::INVALID:
+        default:
+            // Reset buffer
+            _index = 0;
+            break;
         }
-    }
-    //printf(" / ");
+    //}
+    return;
 }
\ No newline at end of file

--- a/Reader.h	Wed May 18 11:35:45 2016 +0000
+++ b/Reader.h	Wed Jun 15 10:53:32 2016 +0000
@@ -11,32 +11,32 @@
     // PC interaction object
     Serial pc;
     
-    static const int _maxSize = 255+2;
-    bool _isNewMessageReceived; // set to true if a complete message was read
-    Translator::MessageInfo _lastMessageInfo;
-    int8_t _lastMessage[_maxSize];       // last complete message received
+    static const int _maxSize = 256;          // Limitation of MCP4725, NOT as described according to protocol
+    bool _isNewMessageReceived;               // True if a complete message was read
+    Translator::MessageInfo _lastMessageInfo; // Contains formatted message info
+    char _lastMessage[_maxSize];// last complete message received
     int _lastMessageSize;       // size of last complete message received
-    int8_t _buffer[_maxSize];            // temporary storage while reading a message
-    int _bufferSize;            // size of the buffer
+    char _buffer[_maxSize]; // temporary storage while reading a message
+    int _bufferSize;        // size of the buffer
     
-    int8_t _lastChar;       // last byte read
-    bool _isMessageStarted; // starting character was received
-    int _charsToRead;       // number of characters to read
+    char _lastChar;         // last byte read
     int _index;             // current number of characters read
     
 public:
     // Default constructor, attaches a callback to pc
     Reader();
     
-    // Returns true once if a complete message was read
+    // Returns true only once when a complete message was read
     bool IsNewMessageReceived();
     Translator::MessageInfo GetLastMessageInfo() const;
     // Returns the last message (without header)
-    const int8_t* GetLastMessage() const;
+    const char* GetLastMessage() const;
     // Returns the size of the last message
     int GetLastMessageSize() const;
     
 private:
     // Function to be attached to pc
     void pcCallback();
+    // Resets the buffer
+    void Reset();
 };
\ No newline at end of file

--- a/Translator.cpp	Wed May 18 11:35:45 2016 +0000
+++ b/Translator.cpp	Wed Jun 15 10:53:32 2016 +0000
@@ -1,10 +1,8 @@
 #include "Translator.h"
 
-Translator::MessageInfo::MessageInfo()
-    : CommandType(NONE),
-    Channel(0),
-    DacValue(0)
-{}
+Translator::MessageInfo::MessageInfo() {
+    Reset();
+}
 
 bool Translator::MessageInfo::IsNone() {
     return CommandType == NONE;
@@ -18,71 +16,46 @@
     return CommandType == WRITE;
 }
 
-bool Translator::MessageInfo::IsOption() {
-    return CommandType = OPTION;
+bool Translator::MessageInfo::IsError() {
+    return CommandType = ERROR;
+}
+
+void Translator::MessageInfo::Reset() {
+    CommandType = NONE;
+    Address = 0;
+    for (int i = 0; i < Rules::MaxDataSize; i++)
+        Data[i] = 0;
 }
 
-int    Translator::Rules::StartCharIndex = 0;
-int8_t Translator::Rules::StartChar = '!';
-int    Translator::Rules::CharsToReadIndex = 1;
-int    Translator::Rules::CharsToReadOffset = CharsToReadIndex+1;
-int    Translator::Rules::rwoIndex = 2;
-int8_t Translator::Rules::ReadChar = 'r';
-int8_t Translator::Rules::WriteChar = 'w';
-int8_t Translator::Rules::OptionChar = 'o';
-int8_t Translator::Rules::ErrorChar = 'e';
-//int    Translator::Rules::ChannelIndex = 3;
-int    Translator::Rules::DataIndex = 3;
-int    Translator::Rules::DataLength = 2;
-
-int    Translator::Rules::ReadCmdSize = rwoIndex + 1;
-int    Translator::Rules::WriteCmdSize = DataIndex + DataLength;
-int    Translator::Rules::ErrorCmdSize = rwoIndex + 1;
+Translator::Translator() {}
 
 
-Translator::Translator()
-    : _rawMessage(0),
-    _size(0)
-{}
-
-
-bool Translator::Translate(MessageInfo info, int8_t** cmd, int* const cmdSize) {
+bool Translator::Translate(MessageInfo info, char cmd[Rules::MaxCmdSize], int* const cmdSize) {
+    if (MessageInfo::NONE)
+        return false;
+    
+    cmd[Rules::StartCharIndex] = Rules::StartChar;
+    cmd[Rules::AddressIndex] = info.Address;
+    for (int i = 0; i < Rules::MaxDataSize; i++)
+        cmd[Rules::DataIndex+i] = info.Data[i];
+    
     switch (info.CommandType) {
         case MessageInfo::NONE:
             return false;
         case MessageInfo::READ:
             *cmdSize = Rules::ReadCmdSize;
-            // Add extra character for '\0'
-            *cmd = new int8_t[Rules::ReadCmdSize+1];
-            (*cmd)[Rules::StartCharIndex] = Rules::StartChar;
-            (*cmd)[Rules::CharsToReadIndex] = Rules::ReadCmdSize-Rules::CharsToReadOffset;
-            (*cmd)[Rules::rwoIndex] = Rules::ReadChar;
-            //(*cmd)[Rules::ChannelIndex] = info.Channel;
-            (*cmd)[Rules::ReadCmdSize] = '\0';
+            cmd[Rules::CharsToReadIndex] = Rules::ReadCmdSize-Rules::PrefixSize;
+            cmd[Rules::CommandIndex] = Rules::ReadChar;
             break;
         case MessageInfo::WRITE:
             *cmdSize = Rules::WriteCmdSize;
-            // Add extra character for '\0'
-            *cmd = new int8_t[Rules::WriteCmdSize+1];
-            (*cmd)[Rules::StartCharIndex] = Rules::StartChar;
-            (*cmd)[Rules::CharsToReadIndex] = Rules::WriteCmdSize - Rules::CharsToReadOffset;
-            (*cmd)[Rules::rwoIndex] = Rules::WriteChar;
-            //(*cmd)[Rules::ChannelIndex] = info.Channel;
-            for (int i = 0; i < Rules::DataLength; i++)
-                (*cmd)[Rules::DataIndex + i] = (info.DacValue >> (Rules::DataLength - i - 1) * 8) & 0xFF;
-            (*cmd)[Rules::WriteCmdSize] = '\0';
+            cmd[Rules::CharsToReadIndex] = Rules::WriteCmdSize - Rules::PrefixSize;
+            cmd[Rules::CommandIndex] = Rules::WriteChar;
             break;
-        case MessageInfo::OPTION:
-            // Not implemented yet
-            return false;
         case MessageInfo::ERROR:
-            *cmdSize = Rules::ErrorCmdSize;
-            // Add extra character for '\0'
-            *cmd = new int8_t[Rules::ErrorCmdSize+1];
-            (*cmd)[Rules::StartCharIndex] = Rules::StartChar;
-            (*cmd)[Rules::CharsToReadIndex] = Rules::ErrorCmdSize - Rules::CharsToReadOffset;
-            (*cmd)[Rules::rwoIndex] = Rules::ErrorChar;
-            (*cmd)[Rules::ErrorCmdSize] = '\0';
+            *cmdSize = Rules::MaxCmdSize;
+            cmd[Rules::CharsToReadIndex] = Rules::MaxCmdSize - Rules::PrefixSize;
+            cmd[Rules::CommandIndex] = Rules::ErrorChar;
             break;
         default:
             return false;
@@ -92,43 +65,45 @@
 }
 
 
-bool Translator::Translate(const int8_t* const cmd, int cmdSize, MessageInfo* const info) {
+Translator::EErrorCode Translator::Translate(const char* const cmd, int cmdSize, MessageInfo* const info) {
     // Check starting character
     if (cmd[Rules::StartCharIndex] != Rules::StartChar) {
-        return false;
+        return INVALID;
     }
     // Check length of command
-    if (cmd[Rules::CharsToReadIndex] + Rules::CharsToReadOffset != cmdSize) {
-        return false;
-    }
+    if (cmdSize < Rules::PrefixSize || (int)(unsigned char)cmd[Rules::CharsToReadIndex] > cmdSize - Rules::PrefixSize)
+        return INCOMPLETE;
+    if ((int)(unsigned char)cmd[Rules::CharsToReadIndex] < cmdSize - Rules::PrefixSize)
+        return INVALID;
+        
+    // If complete message was read, check if correct size with respect to default read/write length
+    if ((cmd[Rules::CommandIndex] == 'r' && cmdSize != Rules::ReadCmdSize) || (cmd[Rules::CommandIndex] == 'w' && cmdSize != Rules::WriteCmdSize))
+        return INVALID;
+    
+    // Message should be correct
+    info->Reset();
 
     // Populate info
-    switch (cmd[Rules::rwoIndex]) {
+    info->Address = (int)(unsigned char)cmd[Rules::AddressIndex];
+    switch (cmd[Rules::CommandIndex]) {
     case 'r':
         info->CommandType = MessageInfo::READ;
+        for (int i = 0; i < Rules::ReadDataSize; i++)
+            info->Data[i] = cmd[Rules::DataIndex+i];
         break;
     case 'w':
         info->CommandType = MessageInfo::WRITE;
+        for (int i = 0; i < Rules::WriteDataSize; i++)
+            info->Data[i] = cmd[Rules::DataIndex+i];
         break;
-    case 'o':
-        info->CommandType = MessageInfo::OPTION;
+    case 'e':
+        info->CommandType = MessageInfo::ERROR;
         break;
     default:
-        return false;
+        return INVALID;
     }
-    //info->Channel = cmd[Rules::ChannelIndex];
-    info->DacValue = ByteUnshift(cmd+Rules::DataIndex, Rules::DataLength);
-    return true;
-}
-
-
-void Translator::ResetMessageInfo() {
-    _message.CommandType = MessageInfo::NONE;
-    _message.Channel = 0;
-    _message.DacValue = 0;
-}
-
-void Translator::InvalidateMessage() {
-    _rawMessage = 0;
-    _size = 0;
+    for (int i = 0; i < cmdSize-Rules::DataIndex; i++)
+        info->Data[i] = cmd[Rules::DataIndex+i];
+    
+    return SUCCES;
 }
\ No newline at end of file

--- a/Translator.h	Wed May 18 11:35:45 2016 +0000
+++ b/Translator.h	Wed Jun 15 10:53:32 2016 +0000
@@ -1,17 +1,51 @@
 #pragma once
 
+#define MAX(a,b) (((a) > (b)) ? (a) : (b))
+
 #include <string>
 #include "mbed.h"
-#include "Utility.h"
 
 // Implements the Data-Link layer, translating a raw message to MessageInfo and back
 class Translator {
+    
 public:
+    enum EErrorCode {
+        SUCCES,
+        INCOMPLETE,
+        INVALID
+    };
+    
+    // Rules over how the command is structured, indexes are per byte
+    struct Rules {
+        static const int StartCharIndex = 0;   // Location of starting char
+        static const int CharsToReadIndex = 1; // Location of chars to read value
+        static const int AddressIndex = 2;     // Location of pin address
+        static const int CommandIndex = 3;     // Location of command type char
+        static const int DataIndex = 4;        // Location of data start
+
+        static const char StartChar = '!'; // Starting character
+        static const char ReadChar = 'r';  // Character to indicate read command
+        static const char WriteChar = 'w'; // Character to indicate write command
+        static const char OtherChar = 'o'; // Character to indicate other command
+        static const char ErrorChar = 'e'; // Character to indicate an error
+
+        static const int PrefixSize = CharsToReadIndex + 1; // Number of chars in the prefix
+        static const int ReadDataSize = 5;   // Number of chars used in read data
+        static const int WriteDataSize = 3;   // Number of chars used in write data
+        static const int MaxDataSize = MAX(ReadDataSize, WriteDataSize); // Maximum size of data array
+        static const int EmptyDataCmdSize = DataIndex; // Number of chars in command if no data
+        static const int ReadCmdSize = EmptyDataCmdSize + ReadDataSize;  // Size of a read command
+        static const int WriteCmdSize = EmptyDataCmdSize + WriteDataSize; // Size of a write command
+        static const int ErrorCmdSize = EmptyDataCmdSize; // Size of an error command
+
+        static const int MaxCmdSize = MAX(ReadCmdSize, WriteCmdSize); // Maximum total command size
+    };
+    
     // Contains easy retreivable message info
     struct MessageInfo {
-        enum ECommandType { NONE, READ, WRITE, OPTION, ERROR } CommandType;
-        int Channel; //Not used
-        int DacValue;
+        enum ECommandType { NONE, READ, WRITE, ERROR } CommandType;
+        int Address;
+        char Data[Rules::MaxCmdSize];
         
         MessageInfo();
         
@@ -19,36 +53,10 @@
         bool IsNone();
         bool IsRead();
         bool IsWrite();
-        bool IsOption();
-    };
-    
-private:
-    // Contains the raw message in bytes
-    const int8_t* _rawMessage;
-    // Size of the raw message
-    int _size;
-    // Message info, is populated in SetRawMessage
-    MessageInfo _message;
-    
-public:
-    // Contains rules about how the raw message is formatted
-    struct Rules {
-        static int    StartCharIndex;
-        static int8_t StartChar;
-        static int    CharsToReadIndex;
-        static int    CharsToReadOffset;
-        static int    rwoIndex;
-        static int8_t ReadChar;
-        static int8_t WriteChar;
-        static int8_t OptionChar;
-        static int8_t ErrorChar;
-        //static int    ChannelIndex;
-        static int    DataIndex;
-        static int    DataLength;
+        bool IsError();
 
-        static int ReadCmdSize;
-        static int WriteCmdSize;
-        static int ErrorCmdSize;
+        // Resets all values to default
+        void Reset();
     };
 
 private:
@@ -62,19 +70,14 @@
     //  cmdSize geeft de grootte van de pointer ZONDER het null karakter zodat het zowel veilig
     //  kan gebruikt worden in printf en andere functies die geen null karakter vereisen (en waarbij
     //  dus het NULL karakter niet zal ingelezen worden>
-    // Translate MessageInfo to raw int8_t* (NULL terminated)
-    static bool Translate(MessageInfo info, int8_t** const cmd, int* const cmdSize);
+    // Translate MessageInfo to raw char* (NULL terminated)
+    static bool Translate(MessageInfo info, char cmd[Rules::MaxCmdSize], int* const cmdSize);
     // <Hier bijvoorbeeld is het NULL karakter niet vereist omdat de grootte wordt opgevraagt
-    //  Indien de int8_t* van vorige functie hier terug wordt in gezet volstaat het om ook zijn
+    //  Indien de char* van vorige functie hier terug wordt in gezet volstaat het om ook zijn
     //  cmdSize te gebruiken: het NULL karakter valt hier buiten en wordt daarom dus niet gelezen
     //  Dit zal ik toevoegen aan de ondervonden problemen.>
-    // Translate raw int8_t* message to MessageInfo
-    static bool Translate(const int8_t* const cmd, int cmdSize, MessageInfo* const info);
+    // Translate raw char* message to MessageInfo
+    // Returns 0 if succesful, -1 if message too short, 1 for faulty/unrecoverable message
+    static EErrorCode Translate(const char* const cmd, int cmdSize, MessageInfo* const info);
 
-    
-private:
-    void SetMessageInfo();
-    void ResetMessageInfo();
-    void InvalidateMessage();
-    
 };
\ No newline at end of file

--- a/Utility.h	Wed May 18 11:35:45 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,26 +0,0 @@
-#pragma once
-
-#include "mbed.h"
-
-// Splits a number into a set number of bytes
-inline void ByteShift(int num, int numBytes, int8_t** bytes, bool lowToHigh =false) {
-    *bytes = new int8_t[numBytes];
-    for (int i = 0; i < numBytes; i++) {
-        (*bytes)[i] = (num >> 8*(lowToHigh ? i : numBytes-i-1)) & 0xFF;
-    }
-}
-
-// Unsplits a number of bytes back to a number
-inline int ByteUnshift(const int8_t* bytes, int numBytes, bool lowToHigh =false) {
-    int result = 0;
-    for (int i = 0; i < numBytes; i++)
-        result += (bytes[i] << 8*(lowToHigh ? i : numBytes-i-1));
-    return result;
-}
-
-// Copies an array deeply
-inline void DeepCopy(const int8_t* source, int size, int8_t* dest) {
-    //*dest = new int8_t[size];
-    for (int i = 0; i < size; i++)
-        dest[i] = source[i];
-}
\ No newline at end of file

--- a/Writer.cpp	Wed May 18 11:35:45 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,50 +0,0 @@
-#include "Writer.h"
-
-bool Writer::Send(const Translator::MessageInfo message, int dacValue) {
-    // Copy msg to adapt it
-    Translator::MessageInfo msg = message;
-    
-    // Check Read/Write/Option
-    switch (msg.CommandType) {
-    case Translator::MessageInfo::READ:
-    case Translator::MessageInfo::WRITE:
-        // Copy dacValue in msg
-        msg.DacValue = dacValue;
-        // Translate to raw message
-        int8_t* rawMsg;
-        int rawMsgSize;
-        if (!Translator::Translate(msg, &rawMsg, &rawMsgSize))
-            return false;
-        // <Zie Translator.h voor de eerste uitleg. Dit is het andere voorbeeld.
-        //  Hier wordt rawMsg gegeven zonder een grootte. De functie printf zal nu
-        //  simpelweg alle karakters uitlezen en stoppen wanneer het '\0' tegenkomt.
-        //  Het werkt dus ook naar behoren omdat Translator::Translate dit karakter
-        //  erbij zet.>
-        // Send it
-        printf("%s", rawMsg);
-        break;
-    
-    case Translator::MessageInfo::OPTION:
-        // Not yet implemented
-        SendError(msg);
-        break;
-        
-    default:
-        return false;
-    }
-    return true;
-}
-
-bool Writer::SendError(const Translator::MessageInfo message) {
-    // Copy msg to adapt it
-    Translator::MessageInfo msg = message;
-    msg.CommandType = Translator::MessageInfo::ERROR;
-    // Translate to raw message
-    int rawMsgSize;
-    int8_t* rawMsg;
-    if (!Translator::Translate(msg, &rawMsg, &rawMsgSize))
-        return false;
-    // Send it
-    printf("%s", rawMsg);
-    return true;
-}
\ No newline at end of file

--- a/Writer.h	Wed May 18 11:35:45 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,18 +0,0 @@
-#pragma once
-
-#include "mbed.h"
-#include <string>
-#include "Translator.h"
-
-// Static class that will write messages to 
-class Writer {
-private:
-    // Private constructor, no instance can be created
-    Writer();
-    
-public:
-    // Sends a message to the computer using the given info and used dacValue as DAC value
-    static bool Send(const Translator::MessageInfo msg, int dacValue);
-    // Sends an error message to the computer using the given info, also makes the LED red!
-    static bool SendError(const Translator::MessageInfo msg);
-};
\ No newline at end of file

--- a/main.cpp	Wed May 18 11:35:45 2016 +0000
+++ b/main.cpp	Wed Jun 15 10:53:32 2016 +0000
@@ -25,14 +25,28 @@
 #include "mcp4725.h"
 #include "Translator.h"
 #include "Reader.h"
-#include "Writer.h"
 
 // Initialize LEDs
 PwmOut rled(LED1);
 PwmOut gled(LED2);
 PwmOut bled(LED3);
 
-Serial pc(USBTX, USBRX);
+//Serial pc(USBTX, USBRX);
+
+// Sends the message to the pc
+bool Send(Translator::MessageInfo info);
+void SendError();
+void SendError(int address);
+void SendError(char* data, int dataSize);
+void SendError(int address, char* data, int dataSize);
+void SendError(Translator::MessageInfo info);
+
+// Turns LED blue
+void SignalWaiting();
+// Turns LED green
+void SignalReceived();
+// Turns LED red
+void SignalError();
 
 int main(void) {
     // <SDA en SCL zijn de pins hierboven gedefinieerd, wss hardware eigenschappen dat je mag negeren>
@@ -51,54 +65,124 @@
     
     // Start infinite loop
     while (true) {
-        rled = 1.0f;
-        gled = 1.0f;
-        bled = 0.0f;
+        SignalWaiting();
         if (!reader.IsNewMessageReceived())
             continue; // Do nothing as long as no new complete message was received
-        rled = 1.0f;
-        gled = 0.0f;
-        bled = 1.0f;
+        SignalReceived();
+        
         
-        // Translate the raw message
+        
+        // Get the formatted message
         Translator::MessageInfo info = reader.GetLastMessageInfo();
-        //if (!Translator::Translate(reader.GetLastMessage(), reader.GetLastMessageSize(), &info)) {
-        //    printf("I have failed you...");
-        //    continue;
-        //}
         
-        int dacValue;
-        if (info.IsRead()) { // If read command
-            //printf("Reading ");
-            // Read the current DAC value
-            if (dac.getDACvalue(dacValue) == 0) {
-                //printf("succes!");
-                // Succesful: send back the current DAC value
-                Writer::Send(info, dacValue);
-            } else {
-                //printf("failed");
-                // Failed: send error message
-                Writer::SendError(info);
+        // If write was specified
+        if (info.IsWrite()) {
+            // Issue write
+            if (i2cdev->write(info.Address, info.Data, Translator::Rules::WriteDataSize, false) != 0) {
+            //if (dac.setDACvalue((int)(unsigned char)info.Data[1]<<4+(int)(unsigned char)info.Data[2]>>4, 0) != 0) {
+                SendError(info);
+                continue;
+            }
+            // Issue also a read
+            //if (i2cdev->read(info.Address, info.Data, Translator::Rules::ReadDataSize, false) != 0) {
+            //    //SendError(info);
+            //    continue;
+            //}
+            if (!Send(info)) {
+                SendError(info);
             }
-        } else if (info.IsWrite()) { // If write command
-            //printf("Writing ");
-            // Set the given DAC value
-            if (dac.setDACvalue(info.DacValue) == 0) {
-                //printf("succes!");
-                // Succesful: Get the DAC value
-                dac.getDACvalue(dacValue);
-                // Send new value
-                Writer::Send(info, dacValue);
-            } else {
-                //printf("failed");
-                // Failed: send error message
-                Writer::SendError(info);
+        }
+        // If read was specified
+        else if (info.IsRead()) {
+            // read from DAC
+            if (i2cdev->read(info.Address, info.Data, sizeof(info.Data)/sizeof(info.Data[0]), false) != 0) {
+                SendError(info);
+                continue;
             }
-        } else if (info.IsOption()) {
-            //printf("Option failed");
-            // Not implemented yet
-            Writer::SendError(info);
+            // send read values to pc
+            if (!Send(info))
+                SendError();
         }
+        // If error was specified
+        else if (info.IsError()) {
+            // Send the error back
+            if (!Send(info))
+                SendError();
+        }
+        
+        // Don't overdo it
+        wait_ms(500);
     }
 }
 
+
+bool Send(Translator::MessageInfo info) {
+    // Create buffer
+    int bufferSize;
+    char buffer[Translator::Rules::MaxCmdSize];
+    // Translate to raw message
+    if (!Translator::Translate(info, buffer, &bufferSize))
+        return false;
+    // Send every character
+    for (int i = 0; i < bufferSize; i++)
+        printf("%c", buffer[i]);
+    return true;
+}
+
+void SendError() {
+    SendError(0,0,0);
+}
+
+void SendError(int address) {
+    SendError(address, 0, 0);
+}
+
+void SendError(char* data, int dataSize) {
+    SendError(0, data, dataSize);
+}
+
+void SendError(int address, char* data , int dataSize) {
+    // Create MessageInfo with error
+    Translator::MessageInfo info;
+    info.Address = address;
+    for (int i = 0; i < dataSize; i++)
+        info.Data[i] = data[i];
+    SendError(info);
+}
+
+void SendError(Translator::MessageInfo info) {
+    SignalError();
+    info.CommandType = Translator::MessageInfo::ERROR;
+    Send(info);
+}
+
+void SignalWaiting() {
+    // Blue light
+    rled = 1.0f;
+    gled = 1.0f;
+    bled = 0.0f;
+}
+
+void SignalReceived() {
+    // Green light
+    rled = 1.0f;
+    gled = 0.0f;
+    bled = 1.0f;
+}
+
+void SignalError() {
+    // Red light
+    rled = 0.0f;
+    gled = 1.0f;
+    bled = 1.0f;
+}
+
+
+
+
+
+
+
+
+
+