hsu han-lin / DXL_SDK_For_F446RE

DXL_SDK_For_F446RE. Now test with MX64AR, MX28AR with 1Mbps

Dependents:   RoboticArm DXL_SDK_Porting_Test

dxl_hal.cpp@5:edccfcb47ab8, 2017-02-10 (annotated)

Committer:
stanley1228
Date:
Fri Feb 10 22:55:30 2017 +0800
Revision:
5:edccfcb47ab8
Parent:
0:bf4774b25188 
1.Add read pos

2.Add Index_Axis gMapAxisNo gMapAxisID

Who changed what in which revision?

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