123
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sun Aug 7 2022 21:58:09 by
1.7.2