AECL 601 / 4WOK_8dir_1m_PI_p2pcontrol

123

dxl_hal.cpp@0:e3a3ccc3b0f8, 2017-09-12 (annotated)

Committer:
peter16688
Date:
Tue Sep 12 13:25:29 2017 +0000
Revision:
0:e3a3ccc3b0f8
4W_8dir_1m_p2pcontrol

Who changed what in which revision?

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