diff --git a/docs/readmeCanOpenSocket.md b/docs/readmeCanOpenSocket.md
index cabc6f5..1461016 100644
--- a/docs/readmeCanOpenSocket.md
+++ b/docs/readmeCanOpenSocket.md
@@ -272,3 +272,36 @@ r 0x280 [0]
 w 0x701 [1] 0x05
 w 0x080 [0]
 w 0x7E5 [0]
+
+
+
+
+# SDO Write:
+################################## TEST WRITE SDO#############
+WRITEVALUE!!
+STATE = WRITE_REQ_TRANSFER
+STATE = WRITE_WAIT_FOR_CONF
+w 0x603 [8] 0x21 0x90 0x26 0x01 0x07 0x00 0x00 0x00
+
+STATE = WRITE_DATA
+r 0x583 [8] 0x60 0x90 0x26 0x01 0x00 0x00 0x00 0x00
+w 0x603 [8] 0x20 0x08 0x74 0xE8 0xBE 0xA8 0x5B 0x6B
+
+
+
+Example of error frame!!
+r 0x583 [8] 0x80 0x08 0x74 0xE8 0x00 0x00 0x02 0x06
+
+
+
+
+
+
+################################## TEST WRITE SDO#############
+
+w 0x603 [8] 0x21 0x90 0x26 0x01 0x07 0x00 0x00 0x00 # request upload/write
+r 0x583 [8] 0x60 0x90 0x26 0x01 0x00 0x00 0x00 0x00 # Ok from slave
+w 0x603 [8] 0x60 0x08 0xF4 0xA9 0xBE 0xC0 0xCB 0xE1 # Send data
+r 0x583 [8] 0x01 0x18 0x00 0x24 0x00 0x0E 0x00 0x0A # ???
+
+
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp
index 49a1a0f..e214b40 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp
@@ -108,7 +108,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   readSlaveConfFrame_.data[7] = ODLengthBytes_.byte3;
 
   // Request write to slave
-  writeReqTransferFrame_.can_id  = cobIdTx;
+  writeReqTransferFrame_.can_id  = cobIdRx;
   writeReqTransferFrame_.can_dlc = 8;     // data length
   writeReqTransferFrame_.data[0] = 0x21;  // Write cmd
   writeReqTransferFrame_.data[1] = ODIndexBytes_.byte0;
@@ -120,7 +120,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   writeReqTransferFrame_.data[7] = ODLengthBytes_.byte3;
 
   // Slave write confirm frame
-  writeSlaveConfCmdFrame_.can_id  = cobIdRx;
+  writeSlaveConfCmdFrame_.can_id  = cobIdTx;
   writeSlaveConfCmdFrame_.can_dlc = 8;     // data length
   writeSlaveConfCmdFrame_.data[0] = 0x60;  // confirm frame for write
   writeSlaveConfCmdFrame_.data[1] = ODIndexBytes_.byte0;
@@ -132,7 +132,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   writeSlaveConfCmdFrame_.data[7] = 0;
 
   // Data frame base
-  writeDataFrame_.can_id  = cobIdTx;
+  writeDataFrame_.can_id  = cobIdRx;
   writeDataFrame_.can_dlc = 8;     // data length
   writeDataFrame_.data[0] = 0;     // need to toggle here
   writeDataFrame_.data[1] = 0;
@@ -144,7 +144,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   writeDataFrame_.data[7] = 0;
 
   // Slave write confirm frame TG0
-  writeConfReqFrameTg0_.can_id  = cobIdRx;
+  writeConfReqFrameTg0_.can_id  = cobIdTx;
   writeConfReqFrameTg0_.can_dlc = 8;     // data length
   writeConfReqFrameTg0_.data[0] = 0x20;  // Toggle 0
   writeConfReqFrameTg0_.data[1] = 0;
@@ -156,7 +156,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
   writeConfReqFrameTg0_.data[7] = 0;
 
   // Slave write confirm frame TG1
-  writeConfReqFrameTg1_.can_id  = cobIdRx;
+  writeConfReqFrameTg1_.can_id  = cobIdTx;
   writeConfReqFrameTg1_.can_dlc = 8;     // data length
   writeConfReqFrameTg1_.data[0] = 0x30;  // Toggle 1
   writeConfReqFrameTg1_.data[1] = 0;
@@ -279,6 +279,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
 }
 
 int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
+  printf("writeDataStateMachine\n");
   int bytes = 0;
   switch(writeStates_) {
     case WRITE_WAIT_FOR_CONF:
@@ -305,10 +306,7 @@ int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
           return 0;
       }
 
-      // next frame use the other toggle option
-      useTg1Frame_ = !useTg1Frame_;
-      bytes = writeNextDataToSlave(useTg1Frame_);
-
+      // Check if write was done already or if more frames are needed!
       if(writtenBytes_ >= ODSize_) {
         writeStates_ = WRITE_IDLE;
         useTg1Frame_ = 0;
@@ -320,6 +318,11 @@ int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
         }
         return 0;
       }
+
+      // next frame use the other toggle option
+      useTg1Frame_ = !useTg1Frame_;
+      bytes = writeNextDataToSlave(useTg1Frame_);
+
       break;
     default:
       return 0;
@@ -339,15 +342,28 @@ int ecmcCANOpenSDO::writeNextDataToSlave(int useToggle) {
   if (bytesToWrite<=0) {
     return 0;
   }
+
+  // seems byte 0 should be: 000tnnnc
+  // t toggle bit
+  // nnn bytes that do NOT contain data
+  // c for last write, Then no more communication
+  
+  writeCmdByte temp;
+  temp.notused=0;
+  temp.nnn = 7-bytesToWrite;
+  temp.c = writtenBytes_+bytesToWrite >= ODSize_;
+  temp.t = useToggle;
+  memcpy(&(writeDataFrame_.data[0]),&temp,1);
+
   //Copy data to frame (start at element 1 since toggle is at data 0)
   memcpy(&(writeDataFrame_.data[1]),dataBuffer_ + writtenBytes_,bytesToWrite);
   writeDataFrame_.can_dlc = bytesToWrite + 1; // need to include the toggle byte
   // add toggle byte (start with toggle 0)
-  if(useToggle) {
-    writeDataFrame_.data[0] = 0x30; // toggle 1
-  } else {
-    writeDataFrame_.data[0] = 0x20; // toggle 0
-  }
+  //if(useToggle) {
+  //  writeDataFrame_.data[0] = 0x70; // toggle 1
+  //} else {
+  //  writeDataFrame_.data[0] = 0x60; // toggle 0
+  //}
 
   writeBuffer_->addWriteCAN(&writeDataFrame_);  // Send first data frame
   writtenBytes_ += bytesToWrite;
@@ -411,6 +427,7 @@ void ecmcCANOpenSDO::setValue(uint8_t *data, size_t bytes) {
 
 int ecmcCANOpenSDO::writeValue() {
   // Busy right now!
+  printf("WRITEVALUE!!\n");
   if(busy_ || writeStates_ != WRITE_IDLE ) {
     return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
   }
@@ -427,6 +444,7 @@ int ecmcCANOpenSDO::writeValue() {
   if(dbgMode_) {
     printf("STATE = WRITE_WAIT_FOR_CONF\n");
   }
+  return 0;
   // State machine is now in rx frame()
 }
 
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h
index 9ffd080..3572a53 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h
@@ -27,6 +27,7 @@
 #define ECMC_CAN_ERROR_SDO_WRITE_BUSY 110
 
 
+
 class ecmcCANOpenSDO {
  public:
   ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
index 2e2764b..3a39d76 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
@@ -82,6 +82,8 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
   lssPdo_      = NULL;
   syncPdo_     = NULL;
   heartPdo_    = NULL;
+  basicConfSdo_ = NULL;
+  cycleCounter_ = 0;
 
   exeSampleTimeMs_ = exeSampleTimeMs;
   
@@ -130,6 +132,18 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
   heartPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x701,DIR_WRITE,1,0,1000,exeSampleTimeMs_, cfgDbgMode_);
   heartPdo_->setValue(5);
 
+  basicConfSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_WRITE,0x2690,1,7,0,exeSampleTimeMs_, cfgDbgMode_);
+  //byte0 = 0
+  //byte1 = 0
+  //byte2 = 0
+  //byte 3,4 = 5000
+  //byte 5 =0
+  //byte 6 =0
+  //byte 7 =0
+  // => 0x1388000
+  uint64_t tempVal = 0x1388000;
+  uint8_t * val = (uint8_t*)&tempVal;
+  basicConfSdo_->setValue(val,7);
   initAsyn();
 }
 
@@ -228,9 +242,9 @@ void ecmcSocketCAN::doReadWorker() {
 
     // TODO MUST CHECK RETRUN VALUE OF READ!!!!!  
     read(socketId_, &rxmsg_, sizeof(rxmsg_));
-    if(testSdo_) {
-      testSdo_->newRxFrame(&rxmsg_);
-    }
+    //if(testSdo_) {
+    //  testSdo_->newRxFrame(&rxmsg_);
+    //}
     if(testPdo_) {
       testPdo_->newRxFrame(&rxmsg_);
     }
@@ -245,6 +259,10 @@ void ecmcSocketCAN::doReadWorker() {
       heartPdo_->newRxFrame(&rxmsg_);
     }    
 
+    if(basicConfSdo_) {
+      basicConfSdo_->newRxFrame(&rxmsg_);
+    }
+ 
     if(cfgDbgMode_) {
       // Simulate candump printout
       printf("r 0x%03X", rxmsg_.can_id);
@@ -310,9 +328,9 @@ int ecmcSocketCAN::addWriteCAN(uint32_t canId,
   
 void  ecmcSocketCAN::execute() {
 
-  if(testSdo_) {
-    testSdo_->execute();
-  }
+  //if(testSdo_) {
+  //  testSdo_->execute();
+ // }
 
   if(testPdo_) {
     testPdo_->execute();
@@ -330,6 +348,18 @@ void  ecmcSocketCAN::execute() {
     heartPdo_->execute();
   }
 
+  cycleCounter_++;
+  if(basicConfSdo_) {
+    basicConfSdo_->execute();
+    if(cycleCounter_ > 10000) {
+      cycleCounter_ = 0;
+      printf("################################### TEST WRITE SDO#############\n");
+      basicConfSdo_->writeValue();
+    }
+    
+  }
+
+
   return;
 }
 
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
index 7d59cb9..f1eb8af 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
@@ -110,6 +110,8 @@ class ecmcSocketCAN : public asynPortDriver {
   ecmcCANOpenPDO *lssPdo_;
   ecmcCANOpenPDO *syncPdo_;
   ecmcCANOpenPDO *heartPdo_;
+  ecmcCANOpenSDO *basicConfSdo_;
+  int cycleCounter_;
 };
 
 #endif  /* ECMC_SOCKETCAN_H_ */
diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h
index 0a7616f..1c32569 100644
--- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h
+++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h
@@ -48,6 +48,13 @@ struct ODLegthBytes {
     char byte3:8;
 };
 
+struct writeCmdByte {
+    char c:1;
+    char nnn:3;
+    char t:1;
+    char notused:3;
+};
+
 /** Just one error code in "c" part of plugin 
 (error handled with exceptions i c++ part) */
 #define ECMC_PLUGIN_SOCKETCAN_ERROR_CODE 1