Diff: dxl_hal.cpp

Revision:

0:bf4774b25188

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/dxl_hal.cpp	Wed Feb 08 02:49:39 2017 +0000
@@ -0,0 +1,141 @@
+// Dynamixel SDK platform dependent source
+#include "dxl_hal.h"
+#include "mbed.h"
+#include "Serial.h"
+
+#define DEF_TX_EN 1
+#define DEF_RX_EN 0
+
+ 
+
+static DigitalOut gDir(D3);
+Serial dxl(PA_9, PA_10);//UART1 TX, RX F446RE
+extern Serial pc;
+
+extern DigitalOut myled;
+
+
+#define DEF_US_PER_BYTE_1MBPS (20) //roughly 20us/byte for1Mbps use   stanley 
+
+int dxl_hal_open( int devIndex, float baudrate )
+{
+    // Opening device
+    // devIndex: Device index
+    // baudrate: Real baudrate (ex> 115200, 57600, 38400...)
+    // Return: 0(Failed), 1(Succeed)
+   
+    gDir=DEF_RX_EN;
+    dxl.baud((int)baudrate);
+    
+    return 1;
+}
+ 
+void dxl_hal_close()
+{
+    // Closing device
+    
+    //is there a close method for Serial class?
+ 
+}
+ 
+void dxl_hal_clear(void)
+{
+    // Clear communication buffer
+    //char dumpster=0;
+    
+    gDir=DEF_RX_EN;
+    while (dxl.readable())
+    { 
+        dxl.getc(); 
+    } 
+    
+    //Wait_timer.reset(); // i think they expect a reset on a dxl_hal_clear()
+    //Wait_timer.stop();
+ 
+}
+ 
+int dxl_hal_tx( unsigned char *pPacket, int numPacket )
+{
+    // Transmiting date
+    // *pPacket: data array pointer
+    // numPacket: number of data array
+    // Return: number of data transmitted. -1 is error.
+    
+    int i=0;
+ 
+    gDir=DEF_TX_EN;
+    for (i=0; i<numPacket; i++) 
+    {
+       while ( !dxl.writeable() );  // wait until you can write, may want to add timeout here
+       dxl.putc(pPacket[i]);  //write
+    }   
+ 
+    wait_us(DEF_US_PER_BYTE_1MBPS*numPacket); 
+    gDir=DEF_RX_EN;
+    
+    
+    //pc.printf("dxl_hal_tx numPacket=%d\n",numPacket);//stanley
+ 
+    return numPacket;
+}
+ 
+int dxl_hal_rx( unsigned char *pPacket, int numPacket )
+{
+    // Recieving date
+    // *pPacket: data array pointer
+    // numPacket: number of data array
+    // Return: number of data recieved. -1 is error.
+    
+    int i=0;
+
+	while(dxl.readable())
+	{
+		pPacket[i] = dxl.getc();
+		i++;
+	}
+
+	return i;
+
+    //gDir=DEF_RX_EN;
+    //pc.printf("gDir=%d\n",gDir);
+    //for (i=0; i<numPacket; i++) 
+    //{
+    //    while ( !dxl.readable() );  // wait until you can read, may want to add timeout here
+    //    
+    //        //temp= dxl.readable();//stanley
+    //        //pc.printf("dxl.readable=%d\n",temp);
+    //     pPacket[i] = dxl.getc();  // read        
+    //    
+    //}
+    
+}
+
+
+  
+Timer Wait_timer;
+ 
+float   gfRcvWaitTime   = 0.0f;
+
+void dxl_hal_set_timeout( int NumRcvByte )
+{
+    // Start stop watch
+    // NumRcvByte: number of recieving data(to calculate maximum waiting time)
+    
+	Wait_timer.reset();
+	Wait_timer.start(); //start timer
+    gfRcvWaitTime = 900.0f+NumRcvByte*DEF_US_PER_BYTE_1MBPS; //900+NumRcvByte*20
+ 
+}
+ 
+int dxl_hal_timeout(void)
+{
+    // Check timeout
+    // Return: 0 is false (no timeout), 1 is true(timeout occurred)
+    
+    if(Wait_timer.read_us() > gfRcvWaitTime)
+	{ //I'm assuming wait gfRcvWaitTime and gfByteTransTime are in units of (us)
+        return 1; // timeout!
+    } 
+    else 
+		return 0;
+}
\ No newline at end of file