AECL 601 / 3W_8Dir_p2pcontrol

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?

UserRevisionLine numberNew 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 }