Diff: MX28.cpp

Revision:

3:7929abcdc22f

Parent:

1:5f537df9dca8

--- a/MX28.cpp	Wed Apr 03 16:44:50 2013 +0000
+++ b/MX28.cpp	Fri Oct 24 08:57:38 2014 +0000
@@ -17,6 +17,12 @@
  */
  
 #include "MX28.h"
+#include "mbed.h"
+
+#define DELAY_DIR 90 // unit in us
+
+DigitalOut dirc_rs485(PIN_RS485_DIRC);
+
 
 uint8_t MX28::CommunicatePacket(MX28_PROTOCOL_PACKET *packet)
 {    
@@ -33,6 +39,7 @@
     { 
         if(servoSerialHalfDuplex->writeable()) 
         {
+
             switch(encoderState)
             {
                 case WAIT_ON_HEADER_0:
@@ -40,8 +47,13 @@
                     #ifdef MX28_DEBUG
                         pc->printf("Write: 0x%02X ", MX28_PROTOCOL_HEADER_0);
                     #endif
-                    
+                    dirc_rs485=1;
+                   // wait_us(DELAY_DIR);
+
                     servoSerialHalfDuplex->putc(MX28_PROTOCOL_HEADER_0);
+                 //   dirc_rs485=0;
+                //    wait_us(DELAY_DIR);                    
+                   
    
                     encoderState = WAIT_ON_HEADER_1;                
                     
@@ -52,8 +64,13 @@
                     #ifdef MX28_DEBUG
                         pc->printf("0x%02X ", MX28_PROTOCOL_HEADER_1);
                     #endif
-                          
+                    
+                    dirc_rs485=1;
+              //      wait_us(DELAY_DIR);
+                    
                     servoSerialHalfDuplex->putc(MX28_PROTOCOL_HEADER_1);
+              //      dirc_rs485=0;
+              //      wait_us(DELAY_DIR);
                     
                     encoderState = WAIT_ON_SERVO_ID;
                     
@@ -64,8 +81,12 @@
                     #ifdef MX28_DEBUG
                         pc->printf("0x%02X ", packet->servoId);
                     #endif
+                    dirc_rs485=1;
+              //      wait_us(DELAY_DIR);
                     
                     servoSerialHalfDuplex->putc(packet->servoId);
+              //      dirc_rs485=0;
+              //      wait_us(DELAY_DIR);
                     
                     encoderState = WAIT_ON_LENGTH;
                    
@@ -76,8 +97,10 @@
                     #ifdef MX28_DEBUG
                         pc->printf("0x%02X ", packet->length);
                     #endif
-                    
+                    dirc_rs485=1;
+              //      wait_us(DELAY_DIR);
                     servoSerialHalfDuplex->putc(packet->length);
+              //      dirc_rs485=0;
                     
                     encoderState = WAIT_ON_INSTRUCTION_ERROR_ID;
                     
@@ -88,6 +111,8 @@
                     #ifdef MX28_DEBUG
                         pc->printf("0x%02X ", packet->instructionErrorId);
                     #endif
+                    dirc_rs485=1;
+              //      wait_us(DELAY_DIR);
                     
                     servoSerialHalfDuplex->putc(packet->instructionErrorId);
                     
@@ -103,7 +128,9 @@
                     #ifdef MX28_DEBUG
                         pc->printf("0x%02X ", packet->parameter[currentParameter]);
                     #endif
-        
+                    dirc_rs485=1;
+              //      wait_us(DELAY_DIR);
+                    
                     servoSerialHalfDuplex->putc(packet->parameter[currentParameter]);
                                        
                     if(++currentParameter == packet->length - 2)                    
@@ -116,9 +143,12 @@
                     #ifdef MX28_DEBUG
                         pc->printf("0x%02X\r\n", packet->checkSum);
                     #endif
+                    dirc_rs485=1;
+               //     wait_us(DELAY_DIR);
                     
                     servoSerialHalfDuplex->putc(packet->checkSum);              
-                    
+                 
+
                     encoderState = WAIT_ON_HEADER_0; 
                     isWholePacket = true;
                     
@@ -128,12 +158,17 @@
         }    
     }
     
+    wait_us(DELAY_DIR);
+    dirc_rs485=0;  
+    
     #ifdef MX28_DEBUG
-        pc->printf("Timer: %d ms\r\n", timer.read_ms());
+        pc->printf("Timer: %d ms\r\n", timer.read_ms());                      
     #endif    
     
     timer.stop();
     
+
+    
     if(!isWholePacket)
     {
         #ifdef MX28_DEBUG
@@ -152,7 +187,7 @@
     currentParameter = 0; 
     isWholePacket = false;    
     uint8_t decoderState = WAIT_ON_HEADER_0;
-        
+    
     timer.reset();
     timer.start();
     
@@ -164,6 +199,8 @@
             {
                 case WAIT_ON_HEADER_0:
                 {   
+                    dirc_rs485=0;
+                   // wait_us(DELAY_DIR);
                     uint8_t mx28ProtocolHeader0 = servoSerialHalfDuplex->getc(); 
                     
                     #ifdef MX28_DEBUG
@@ -176,6 +213,8 @@
                 }
                 case WAIT_ON_HEADER_1:
                 {   
+                    dirc_rs485=0;
+                    //wait_us(DELAY_DIR);
                     uint8_t mx28ProtocolHeader1 = servoSerialHalfDuplex->getc(); 
                     
                     #ifdef MX28_DEBUG
@@ -188,6 +227,8 @@
                 }
                 case WAIT_ON_SERVO_ID:
                 {   
+                    dirc_rs485=0;
+                    //wait_us(DELAY_DIR);
                     packet->servoId = servoSerialHalfDuplex->getc(); 
                     
                     #ifdef MX28_DEBUG
@@ -200,6 +241,8 @@
                 }                
                 case WAIT_ON_LENGTH:
                 {
+                    dirc_rs485=0;
+                 //   wait_us(DELAY_DIR);
                     packet->length = servoSerialHalfDuplex->getc();
                     
                     #ifdef MX28_DEBUG
@@ -212,6 +255,8 @@
                 } 
                 case WAIT_ON_INSTRUCTION_ERROR_ID:
                 {
+                    dirc_rs485=0;
+                  //  wait_us(DELAY_DIR);
                     packet->instructionErrorId = servoSerialHalfDuplex->getc();
                     
                     #ifdef MX28_DEBUG
@@ -227,6 +272,8 @@
                 } 
                 case WAIT_ON_PARAMETER:
                 {    
+                    dirc_rs485=0;
+               //     wait_us(DELAY_DIR);
                     uint8_t parameter = servoSerialHalfDuplex->getc();               
                     packet->parameter[currentParameter] = parameter;
                     
@@ -241,10 +288,12 @@
                 }
                 case WAIT_ON_CHECK_SUM:
                 {
+                    dirc_rs485=0;
+               //     wait_us(DELAY_DIR);
                     packet->checkSum = servoSerialHalfDuplex->getc();
                     
                     #ifdef MX28_DEBUG
-                        pc->printf("0x%02X\r\n", packet->checkSum);
+                        pc->printf("sum =0x%02X\r\n", packet->checkSum);
                     #endif
                     
                     decoderState = WAIT_ON_HEADER_0; 
@@ -1488,14 +1537,20 @@
 
 MX28::MX28(PinName tx, PinName rx, int baudRate)
 {
+    
+    
     #ifdef MX28_DEBUG
         pc = new Serial(USBTX, USBRX);
         pc->baud(115200);
         pc->printf("\033[2J");
     #endif
-    
-    servoSerialHalfDuplex = new SerialHalfDuplex(tx, rx);
+    //pc->printf("baud1= %d\r\n",baudRate);
+    //servoSerialHalfDuplex = new SerialHalfDuplex(tx, rx);
+    servoSerialHalfDuplex = new iSerial(tx, rx,NULL,1000,100);
+    //pc->printf("baud2= %d\r\n",baudRate);
     servoSerialHalfDuplex->baud(baudRate);    
+    //servoSerialHalfDuplex->baud(1000000);    
+    //servoSerialHalfDuplex->printf("a");
 }
 
 MX28::~MX28()