123
dxl_hal.cpp@0:a49e4e666e2d, 2017-09-12 (annotated)
- Committer:
- peter16688
- Date:
- Tue Sep 12 13:22:08 2017 +0000
- Revision:
- 0:a49e4e666e2d
3WOK_8dir_1m_PD_p2pcontrol
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
peter16688 | 0:a49e4e666e2d | 1 | // Dynamixel SDK platform dependent source |
peter16688 | 0:a49e4e666e2d | 2 | #include "dxl_hal.h" |
peter16688 | 0:a49e4e666e2d | 3 | #include "mbed.h" |
peter16688 | 0:a49e4e666e2d | 4 | #include "Serial.h" |
peter16688 | 0:a49e4e666e2d | 5 | |
peter16688 | 0:a49e4e666e2d | 6 | #define DEF_TX_EN 1 |
peter16688 | 0:a49e4e666e2d | 7 | #define DEF_RX_EN 0 |
peter16688 | 0:a49e4e666e2d | 8 | |
peter16688 | 0:a49e4e666e2d | 9 | |
peter16688 | 0:a49e4e666e2d | 10 | |
peter16688 | 0:a49e4e666e2d | 11 | static DigitalOut gDir(D3); |
peter16688 | 0:a49e4e666e2d | 12 | Serial dxl(PA_9, PA_10);//UART1 TX, RX F446RE |
peter16688 | 0:a49e4e666e2d | 13 | extern Serial pc; |
peter16688 | 0:a49e4e666e2d | 14 | |
peter16688 | 0:a49e4e666e2d | 15 | extern DigitalOut myled; |
peter16688 | 0:a49e4e666e2d | 16 | |
peter16688 | 0:a49e4e666e2d | 17 | |
peter16688 | 0:a49e4e666e2d | 18 | #define DEF_US_PER_BYTE_1MBPS (20) //roughly 20us/byte for1Mbps use stanley |
peter16688 | 0:a49e4e666e2d | 19 | |
peter16688 | 0:a49e4e666e2d | 20 | int dxl_hal_open( int devIndex, float baudrate ) |
peter16688 | 0:a49e4e666e2d | 21 | { |
peter16688 | 0:a49e4e666e2d | 22 | // Opening device |
peter16688 | 0:a49e4e666e2d | 23 | // devIndex: Device index |
peter16688 | 0:a49e4e666e2d | 24 | // baudrate: Real baudrate (ex> 115200, 57600, 38400...) |
peter16688 | 0:a49e4e666e2d | 25 | // Return: 0(Failed), 1(Succeed) |
peter16688 | 0:a49e4e666e2d | 26 | |
peter16688 | 0:a49e4e666e2d | 27 | gDir=DEF_RX_EN; |
peter16688 | 0:a49e4e666e2d | 28 | dxl.baud((int)baudrate); |
peter16688 | 0:a49e4e666e2d | 29 | |
peter16688 | 0:a49e4e666e2d | 30 | return 1; |
peter16688 | 0:a49e4e666e2d | 31 | } |
peter16688 | 0:a49e4e666e2d | 32 | |
peter16688 | 0:a49e4e666e2d | 33 | void dxl_hal_close() |
peter16688 | 0:a49e4e666e2d | 34 | { |
peter16688 | 0:a49e4e666e2d | 35 | // Closing device |
peter16688 | 0:a49e4e666e2d | 36 | |
peter16688 | 0:a49e4e666e2d | 37 | //is there a close method for Serial class? |
peter16688 | 0:a49e4e666e2d | 38 | |
peter16688 | 0:a49e4e666e2d | 39 | } |
peter16688 | 0:a49e4e666e2d | 40 | |
peter16688 | 0:a49e4e666e2d | 41 | void dxl_hal_clear(void) |
peter16688 | 0:a49e4e666e2d | 42 | { |
peter16688 | 0:a49e4e666e2d | 43 | // Clear communication buffer |
peter16688 | 0:a49e4e666e2d | 44 | //char dumpster=0; |
peter16688 | 0:a49e4e666e2d | 45 | |
peter16688 | 0:a49e4e666e2d | 46 | gDir=DEF_RX_EN; |
peter16688 | 0:a49e4e666e2d | 47 | while (dxl.readable()) |
peter16688 | 0:a49e4e666e2d | 48 | { |
peter16688 | 0:a49e4e666e2d | 49 | dxl.getc(); |
peter16688 | 0:a49e4e666e2d | 50 | } |
peter16688 | 0:a49e4e666e2d | 51 | |
peter16688 | 0:a49e4e666e2d | 52 | //Wait_timer.reset(); // i think they expect a reset on a dxl_hal_clear() |
peter16688 | 0:a49e4e666e2d | 53 | //Wait_timer.stop(); |
peter16688 | 0:a49e4e666e2d | 54 | |
peter16688 | 0:a49e4e666e2d | 55 | } |
peter16688 | 0:a49e4e666e2d | 56 | |
peter16688 | 0:a49e4e666e2d | 57 | int dxl_hal_tx( unsigned char *pPacket, int numPacket ) |
peter16688 | 0:a49e4e666e2d | 58 | { |
peter16688 | 0:a49e4e666e2d | 59 | // Transmiting date |
peter16688 | 0:a49e4e666e2d | 60 | // *pPacket: data array pointer |
peter16688 | 0:a49e4e666e2d | 61 | // numPacket: number of data array |
peter16688 | 0:a49e4e666e2d | 62 | // Return: number of data transmitted. -1 is error. |
peter16688 | 0:a49e4e666e2d | 63 | |
peter16688 | 0:a49e4e666e2d | 64 | int i=0; |
peter16688 | 0:a49e4e666e2d | 65 | |
peter16688 | 0:a49e4e666e2d | 66 | gDir=DEF_TX_EN; |
peter16688 | 0:a49e4e666e2d | 67 | for (i=0; i<numPacket; i++) |
peter16688 | 0:a49e4e666e2d | 68 | { |
peter16688 | 0:a49e4e666e2d | 69 | while ( !dxl.writeable() ); // wait until you can write, may want to add timeout here |
peter16688 | 0:a49e4e666e2d | 70 | dxl.putc(pPacket[i]); //write |
peter16688 | 0:a49e4e666e2d | 71 | } |
peter16688 | 0:a49e4e666e2d | 72 | |
peter16688 | 0:a49e4e666e2d | 73 | wait_us(DEF_US_PER_BYTE_1MBPS*numPacket); |
peter16688 | 0:a49e4e666e2d | 74 | gDir=DEF_RX_EN; |
peter16688 | 0:a49e4e666e2d | 75 | |
peter16688 | 0:a49e4e666e2d | 76 | |
peter16688 | 0:a49e4e666e2d | 77 | //pc.printf("dxl_hal_tx numPacket=%d\n",numPacket);//stanley |
peter16688 | 0:a49e4e666e2d | 78 | |
peter16688 | 0:a49e4e666e2d | 79 | return numPacket; |
peter16688 | 0:a49e4e666e2d | 80 | } |
peter16688 | 0:a49e4e666e2d | 81 | |
peter16688 | 0:a49e4e666e2d | 82 | int dxl_hal_rx( unsigned char *pPacket, int numPacket ) |
peter16688 | 0:a49e4e666e2d | 83 | { |
peter16688 | 0:a49e4e666e2d | 84 | // Recieving date |
peter16688 | 0:a49e4e666e2d | 85 | // *pPacket: data array pointer |
peter16688 | 0:a49e4e666e2d | 86 | // numPacket: number of data array |
peter16688 | 0:a49e4e666e2d | 87 | // Return: number of data recieved. -1 is error. |
peter16688 | 0:a49e4e666e2d | 88 | |
peter16688 | 0:a49e4e666e2d | 89 | int i=0; |
peter16688 | 0:a49e4e666e2d | 90 | |
peter16688 | 0:a49e4e666e2d | 91 | while(dxl.readable()) |
peter16688 | 0:a49e4e666e2d | 92 | { |
peter16688 | 0:a49e4e666e2d | 93 | pPacket[i] = dxl.getc(); |
peter16688 | 0:a49e4e666e2d | 94 | i++; |
peter16688 | 0:a49e4e666e2d | 95 | } |
peter16688 | 0:a49e4e666e2d | 96 | |
peter16688 | 0:a49e4e666e2d | 97 | return i; |
peter16688 | 0:a49e4e666e2d | 98 | |
peter16688 | 0:a49e4e666e2d | 99 | //gDir=DEF_RX_EN; |
peter16688 | 0:a49e4e666e2d | 100 | //pc.printf("gDir=%d\n",gDir); |
peter16688 | 0:a49e4e666e2d | 101 | //for (i=0; i<numPacket; i++) |
peter16688 | 0:a49e4e666e2d | 102 | //{ |
peter16688 | 0:a49e4e666e2d | 103 | // while ( !dxl.readable() ); // wait until you can read, may want to add timeout here |
peter16688 | 0:a49e4e666e2d | 104 | // |
peter16688 | 0:a49e4e666e2d | 105 | // //temp= dxl.readable();//stanley |
peter16688 | 0:a49e4e666e2d | 106 | // //pc.printf("dxl.readable=%d\n",temp); |
peter16688 | 0:a49e4e666e2d | 107 | // pPacket[i] = dxl.getc(); // read |
peter16688 | 0:a49e4e666e2d | 108 | // |
peter16688 | 0:a49e4e666e2d | 109 | //} |
peter16688 | 0:a49e4e666e2d | 110 | |
peter16688 | 0:a49e4e666e2d | 111 | } |
peter16688 | 0:a49e4e666e2d | 112 | |
peter16688 | 0:a49e4e666e2d | 113 | |
peter16688 | 0:a49e4e666e2d | 114 | |
peter16688 | 0:a49e4e666e2d | 115 | Timer Wait_timer; |
peter16688 | 0:a49e4e666e2d | 116 | |
peter16688 | 0:a49e4e666e2d | 117 | float gfRcvWaitTime = 0.0f; |
peter16688 | 0:a49e4e666e2d | 118 | |
peter16688 | 0:a49e4e666e2d | 119 | void dxl_hal_set_timeout( int NumRcvByte ) |
peter16688 | 0:a49e4e666e2d | 120 | { |
peter16688 | 0:a49e4e666e2d | 121 | // Start stop watch |
peter16688 | 0:a49e4e666e2d | 122 | // NumRcvByte: number of recieving data(to calculate maximum waiting time) |
peter16688 | 0:a49e4e666e2d | 123 | |
peter16688 | 0:a49e4e666e2d | 124 | Wait_timer.reset(); |
peter16688 | 0:a49e4e666e2d | 125 | Wait_timer.start(); //start timer |
peter16688 | 0:a49e4e666e2d | 126 | gfRcvWaitTime = 900.0f+NumRcvByte*DEF_US_PER_BYTE_1MBPS; //900+NumRcvByte*20 |
peter16688 | 0:a49e4e666e2d | 127 | |
peter16688 | 0:a49e4e666e2d | 128 | } |
peter16688 | 0:a49e4e666e2d | 129 | |
peter16688 | 0:a49e4e666e2d | 130 | int dxl_hal_timeout(void) |
peter16688 | 0:a49e4e666e2d | 131 | { |
peter16688 | 0:a49e4e666e2d | 132 | // Check timeout |
peter16688 | 0:a49e4e666e2d | 133 | // Return: 0 is false (no timeout), 1 is true(timeout occurred) |
peter16688 | 0:a49e4e666e2d | 134 | |
peter16688 | 0:a49e4e666e2d | 135 | if(Wait_timer.read_us() > gfRcvWaitTime) |
peter16688 | 0:a49e4e666e2d | 136 | { //I'm assuming wait gfRcvWaitTime and gfByteTransTime are in units of (us) |
peter16688 | 0:a49e4e666e2d | 137 | return 1; // timeout! |
peter16688 | 0:a49e4e666e2d | 138 | } |
peter16688 | 0:a49e4e666e2d | 139 | else |
peter16688 | 0:a49e4e666e2d | 140 | return 0; |
peter16688 | 0:a49e4e666e2d | 141 | } |