123

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers dxl_hal.cpp Source File

dxl_hal.cpp

00001 // Dynamixel SDK platform dependent source
00002 #include "dxl_hal.h"
00003 #include "mbed.h"
00004 #include "Serial.h"
00005 
00006 #define DEF_TX_EN 1
00007 #define DEF_RX_EN 0
00008 
00009  
00010 
00011 static DigitalOut gDir(D3);
00012 Serial dxl(PA_9, PA_10);//UART1 TX, RX F446RE
00013 extern Serial pc;
00014 
00015 extern DigitalOut myled;
00016 
00017 
00018 #define DEF_US_PER_BYTE_1MBPS (20) //roughly 20us/byte for1Mbps use   stanley 
00019 
00020 int dxl_hal_open( int devIndex, float baudrate )
00021 {
00022     // Opening device
00023     // devIndex: Device index
00024     // baudrate: Real baudrate (ex> 115200, 57600, 38400...)
00025     // Return: 0(Failed), 1(Succeed)
00026    
00027     gDir=DEF_RX_EN;
00028     dxl.baud((int)baudrate);
00029     
00030     return 1;
00031 }
00032  
00033 void dxl_hal_close()
00034 {
00035     // Closing device
00036     
00037     //is there a close method for Serial class?
00038  
00039 }
00040  
00041 void dxl_hal_clear(void)
00042 {
00043     // Clear communication buffer
00044     //char dumpster=0;
00045     
00046     gDir=DEF_RX_EN;
00047     while (dxl.readable())
00048     { 
00049         dxl.getc(); 
00050     } 
00051     
00052     //Wait_timer.reset(); // i think they expect a reset on a dxl_hal_clear()
00053     //Wait_timer.stop();
00054  
00055 }
00056  
00057 int dxl_hal_tx( unsigned char *pPacket, int numPacket )
00058 {
00059     // Transmiting date
00060     // *pPacket: data array pointer
00061     // numPacket: number of data array
00062     // Return: number of data transmitted. -1 is error.
00063     
00064     int i=0;
00065  
00066     gDir=DEF_TX_EN;
00067     for (i=0; i<numPacket; i++) 
00068     {
00069        while ( !dxl.writeable() );  // wait until you can write, may want to add timeout here
00070        dxl.putc(pPacket[i]);  //write
00071     }   
00072  
00073     wait_us(DEF_US_PER_BYTE_1MBPS*numPacket); 
00074     gDir=DEF_RX_EN;
00075     
00076     
00077     //pc.printf("dxl_hal_tx numPacket=%d\n",numPacket);//stanley
00078  
00079     return numPacket;
00080 }
00081  
00082 int dxl_hal_rx( unsigned char *pPacket, int numPacket )
00083 {
00084     // Recieving date
00085     // *pPacket: data array pointer
00086     // numPacket: number of data array
00087     // Return: number of data recieved. -1 is error.
00088     
00089     int i=0;
00090 
00091     while(dxl.readable())
00092     {
00093         pPacket[i] = dxl.getc();
00094         i++;
00095     }
00096 
00097     return i;
00098 
00099     //gDir=DEF_RX_EN;
00100     //pc.printf("gDir=%d\n",gDir);
00101     //for (i=0; i<numPacket; i++) 
00102     //{
00103     //    while ( !dxl.readable() );  // wait until you can read, may want to add timeout here
00104     //    
00105     //        //temp= dxl.readable();//stanley
00106     //        //pc.printf("dxl.readable=%d\n",temp);
00107     //     pPacket[i] = dxl.getc();  // read        
00108     //    
00109     //}
00110     
00111 }
00112 
00113 
00114   
00115 Timer Wait_timer;
00116  
00117 float   gfRcvWaitTime   = 0.0f;
00118 
00119 void dxl_hal_set_timeout( int NumRcvByte )
00120 {
00121     // Start stop watch
00122     // NumRcvByte: number of recieving data(to calculate maximum waiting time)
00123     
00124     Wait_timer.reset();
00125     Wait_timer.start(); //start timer
00126     gfRcvWaitTime = 900.0f+NumRcvByte*DEF_US_PER_BYTE_1MBPS; //900+NumRcvByte*20
00127  
00128 }
00129  
00130 int dxl_hal_timeout(void)
00131 {
00132     // Check timeout
00133     // Return: 0 is false (no timeout), 1 is true(timeout occurred)
00134     
00135     if(Wait_timer.read_us() > gfRcvWaitTime)
00136     { //I'm assuming wait gfRcvWaitTime and gfByteTransTime are in units of (us)
00137         return 1; // timeout!
00138     } 
00139     else 
00140         return 0;
00141 }