M-LVDS high-speed serial communication control method in the mechanical arm system
Technical field
What the present invention relates to is the technical field of high-speed serial communication.
Background technology
The collapsible robot system of HIT four-degree-of-freedom is made up of mechanical arm and terminal HIT-DLR Dextrous Hand, and overall weight is about 25kg, and the straight configuration height is about 1.3m.Mechanical arm has four rotary joints, and the design in each joint adopts big center pit permanent-magnet brushless DC electric machine to drive, and realizes big moment output by 160: 1 harmonic speed reducer.All cablings all pass from center pit in the system, compact conformation, and avoid producing in the joint coiling phenomenon.By this robot, can replace the people under dangerous, rugged environment, to finish the complex operations task that common unit institute can not finish, will play a significant role in fields such as the nuclear industry environment of abominable space environment, danger and high-level service robots.
Adopt the upper and lower computer structure on the mechanical arm electrical design: slave computer is made up of four joints and Dextrous Hand, and control circuit is integrated in joint and Dextrous Hand inside, and its microprocessor all adopts FPGA; The host computer circuit design is integrated in the principal computer by pci bus, and its microprocessor uses DSP/FPGA, and wherein FPGA mainly realizes the transmission-receiving function of data, is the important component part of system bus.Upper and lower computer adopts two high-speed serial bus, and with mechanical arm and Dextrous Hand separate communications, to realize grand little coordinated manipulation of real-time high-precision, wherein Dextrous Hand adopts point-to-point PPSeCo communication (patent No. 200410013732.0).In addition, host computer DSP and principal computer real-time Communication for Power, DSP receives the operation signal of principal computer, and principal computer is by the state variable of DSP feedback mechanical arm.
Can only carry out fixed-point arithmetic for host computer DSP, lower computer FPGA, the slow and internal microprocessor resource-constrained of arithmetic speed is unfavorable for realizing computings such as system kinematics, dynamics, trajectory planning and dynamic compensation.In order to improve robot system dynamic and stability, slave computer should upgrade joint angles expectation value, velocity amplitude, accekeration and gravity that this moment, the joint was born from host computer with the fastest speed.
Shanghai Communications University has applied for " based on the patent of the individual mechanical arm system (publication number 1434391) of USB (universal serial bus); usb bus is applied to the solid mechanical arm is connected with personal computer; make mechanical arm have plug-and-play feature, but real system still is subjected to the restriction of the cable length and the traffic rate of defined in the usb bus agreement.
Summary of the invention
The present invention is for the traffic rate of communication bus between host computer that overcomes the existing machinery hand and the slave computer is too low, cause existing slave computer can not from host computer, upgrade fast joint angles expectation value, velocity amplitude, accekeration and this moment the gravimetric data that bear in the joint problem.And M-LVDS high-speed serial communication control method in a kind of mechanical arm system has been proposed.
Realize that device of the present invention is made up of host node 2, a plurality of partial node 3, universal serial bus 4, resistance R T1, resistance R T2;
Host node 2 drives transceiver 2-2 by main fpga logic device 2-1, main M-LVDS and forms;
The communication data input/output bus end of fpga logic device 2-1 connects the communication data input/output bus end of the DSP digital signal processor 1 in the host computer, and the M-LVDS Data Control output input bus end of fpga logic device 2-1 connects the Data Control output input bus end that main M-LVDS drives transceiver 2-2;
Each partial node 3 all drives transceiver 3-1, fpga logic device 3-2 by M-LVDS and forms;
M-LVDS drives the M-LVDS Data Control output input bus end of the Data Control output input bus end connection fpga logic device 3-2 of transceiver 3-1, and the communication data input/output bus end of fpga logic device 3-2 is the external data input/output terminal;
Main M-LVDS in the host node 2 drives the serial communication end of transceiver 2-2, the serial communication end that the M-LVDS in each partial node 3 drives transceiver 3-1 all is connected in turn on the universal serial bus 4, between two lines at universal serial bus 4 two ends respectively cross-over connection resistance R T1, resistance R T2 are arranged.
The step of the M-LVDS high-speed serial communication control method in its mechanical arm system is:
Step 1, start, System self-test;
Fpga logic device 3-2 in step 2, each partial node 3 reads the address date of oneself; Address date comprises the address that receives bus address and send to the main fpga logic device 2-1 in the host node 2;
Step 3, data transmit-receive process time T is set, partial node quantity N is provided with each partial node 3 and sends data time T1;
Receiver module among the main fpga logic device 2-1 of step 4, host node 2 is in holding state, the transceiver module of fpga logic device 3-2 in each partial node 3 is in holding state and receives the data that transmit on the universal serial bus 4 in real time, and the sending module of the main fpga logic device 2-1 in the host node 2 begins the timing interruption that the unlatching cycle is T;
The sending module of the main fpga logic device 2-1 of step 5, host node 2 judges whether to enter T and regularly interrupts, and judged result is not for, then standby, and judged result is for being then to enter T timing interrupt procedure; In T timer interrupt routine process, each partial node 3 data that at first will receive are merged into to wrap according to partial node 3 orders and pass the DSP digital signal processor 1 that sends in the host computer, (first cycle data is 0) passes the data of issuing each partial node 3 in the DSP digital signal processor 1 in the host computer then down; Open single partial node 3 data of T1 and send regularly interruption, and open the transmission zone bit, withdraw from the T interrupt procedure and enter holding state;
Step 6, under holding state, judge whether to enter that T1 interrupts and send whether zone bit is 1, judged result is not for, then standby, judged result is for being then to enter T1 timing interrupt procedure; In T1 timing interrupt procedure, at first send single partial node 3 data; Whether the data of judging last partial node 3 then send end, the result then enters holding state and waits for that next T1 interrupts sending the data of next partial node 3 for not, and judged result is for being, then close T1 and regularly interrupt and will send the zone bit zero clearing, continue standby;
Step 7, when the sending module of the main fpga logic device 2-1 of host node 2 sends each partial node 3 data successively, the fpga logic device 3-2 transceiver module in each partial node 3 is in holding state; Under holding state, the fpga logic device 3-2 in each partial node 3 receives on the universal serial bus 4 data that transmit in real time, at first judges whether to detect the SOFO data, and judged result then continues standby for not, and the result is for being, then the receiver address segment data; After receiving the address field data, the fpga logic device 3-2 in the partial node 3 carries out matching addresses and detects, and judged result is for denying, then this partial node 3 is not issued in explanation, continues to hold machine, and judged result is for being, then receive total data, and data are deposited in the corresponding memory; Whether after receiving data, data are carried out cyclic redundancy check (CRC), be used for specified data and wrap in transmission course and make mistakes; Judged result then makes an explanation and deal with data for being, judged result then abandons the data that receive for not; After the cyclic redundancy checking finishes, start at once and interrupt sending this partial node 3 data; After interruption sends self partial node 3 data, continue standby;
Step 8, when having data to return on the universal serial bus 4, the receiver module of the main fpga logic device 2-1 of host node 2 is in holding state; Under holding state, the main fpga logic device 2-1 of host node 2 receives the data of uploading on the universal serial bus 4 in real time, at first judges whether to detect the SOFO data, and judged result then continues standby for not, and judged result is for being, then the receiver address segment data; After receiving the address field data, the main fpga logic device 2-1 of host node 2 carries out matching addresses and detects, judged result is for denying, illustrate that then data send the wrong address field data that do not belong to, continue to hold machine, judged result then judges it is the data which partial node 3 is sent for being, and the reception total data, it is stored in the corresponding memory; Whether partial node 3 Data Receiving will be carried out cyclic redundancy check (CRC) after finishing, be used for specified data and wrap in transmission course and make mistakes; Judged result then makes an explanation and deal with data for being, judged result then abandons the data that receive for not; After the cyclic redundancy checking finishes, continue standby;
Step 9, return operating procedure three.
The present invention can reach the transmission speed of 500Mbps (megabyte/second), in actual applications data transmission rate is set to 25Mbps, has just satisfied the mechanical arm requirement of control in real time.The M-LVDS signal used ± differential signal of 350mV, therefore has minimum electromagnetic interference (EMI), very strong antijamming capability, this is for extremely important sensor data acquisition, and extremely low electromagnetic interference (EMI) makes that high-precision sensor data acquisition is easy to realize more.And has with low cost a, advantage of simple structure of cost.
Description of drawings
Fig. 1 is an integrated circuit structural representation of the present invention.
Embodiment
Embodiment one: in conjunction with Fig. 1 present embodiment is described, realizes that the device of the M-LVDS high-speed serial communication control method in the present embodiment mechanical arm system is made up of host node 2, a plurality of partial node 3, universal serial bus 4, resistance R T1, resistance R T2;
Host node 2 drives transceiver 2-2 by main fpga logic device 2-1, main M-LVDS and forms; The communication data input/output bus end of fpga logic device 2-1 connects the communication data input/output bus end of the DSP digital signal processor 1 in the host computer, and the M-LVDS Data Control output input bus end of fpga logic device 2-1 connects the Data Control output input bus end that main M-LVDS drives transceiver 2-2;
Each partial node 3 all drives transceiver 3-1, fpga logic device 3-2 by M-LVDS and forms; M-LVDS drives the M-LVDS Data Control output input bus end of the Data Control output input bus end connection fpga logic device 3-2 of transceiver 3-1, and the communication data input/output bus end of fpga logic device 3-2 is the external data input/output terminal;
Main M-LVDS in the host node 2 drives the serial communication end of transceiver 2-2, the serial communication end that the M-LVDS in each partial node 3 drives transceiver 3-1 all is connected in turn on the universal serial bus 4, between two lines at universal serial bus 4 two ends respectively cross-over connection resistance R T1, resistance R T2 are arranged.
The step of the M-LVDS high-speed serial communication control method in its mechanical arm system is:
Step 1, start, System self-test;
Fpga logic device 3-2 in step 2, each partial node 3 reads the address date of oneself; Address date comprises the address that receives bus address and send to the main fpga logic device 2-1 in the host node 2;
Step 3, data transmit-receive process time T is set, partial node quantity N is provided with each partial node 3 and sends data time T1;
Receiver module among the main fpga logic device 2-1 of step 4, host node 2 is in holding state, the transceiver module of fpga logic device 3-2 in each partial node 3 is in holding state and receives the data that transmit on the universal serial bus 4 in real time, and the sending module of the main fpga logic device 2-1 in the host node 2 begins the timing interruption that the unlatching cycle is T;
The sending module of the main fpga logic device 2-1 of step 5, host node 2 judges whether to enter T and regularly interrupts, and judged result is not for, then standby, and judged result is for being then to enter T timing interrupt procedure; In T timer interrupt routine process, each partial node 3 data that at first will receive are merged into to wrap according to partial node 3 orders and pass the DSP digital signal processor 1 that sends in the host computer, (first cycle data is 0) passes the data of issuing each partial node 3 in the DSP digital signal processor 1 in the host computer then down; Open single partial node 3 data of T1 and send regularly interruption, and open the transmission zone bit, withdraw from the T interrupt procedure and enter holding state;
Step 6, under holding state, judge whether to enter that T1 interrupts and send whether zone bit is 1, judged result is not for, then standby, judged result is for being then to enter T1 timing interrupt procedure; In T1 timing interrupt procedure, at first send single partial node 3 data; Whether the data of judging last partial node 3 then send end, the result then enters holding state and waits for that next T1 interrupts sending the data of next partial node 3 for not, and judged result is for being, then close T1 and regularly interrupt and will send the zone bit zero clearing, continue standby;
Step 7, when the sending module of the main fpga logic device 2-1 of host node 2 sends each partial node 3 data successively, the fpga logic device 3-2 transceiver module in each partial node 3 is in holding state; Under holding state, the fpga logic device 3-2 in each partial node 3 receives on the universal serial bus 4 data that transmit in real time, at first judges whether to detect the SOFO data, and judged result then continues standby for not, and the result is for being, then the receiver address segment data; After receiving the address field data, the fpga logic device 3-2 in the partial node 3 carries out matching addresses and detects, and judged result is for denying, then this partial node 3 is not issued in explanation, continues to hold machine, and judged result is for being, then receive total data, and data are deposited in the corresponding memory; Whether after receiving data, data are carried out cyclic redundancy check (CRC), be used for specified data and wrap in transmission course and make mistakes; Judged result then makes an explanation and deal with data for being, judged result then abandons the data that receive for not; After the cyclic redundancy checking finishes, start at once and interrupt sending this partial node 3 data; After interruption sends self partial node 3 data, continue standby;
Step 8, when having data to return on the universal serial bus 4, the receiver module of the main fpga logic device 2-1 of host node 2 is in holding state; Under holding state, the main fpga logic device 2-1 of host node 2 receives the data of uploading on the universal serial bus 4 in real time, at first judges whether to detect the SOFO data, and judged result then continues standby for not, and judged result is for being, then the receiver address segment data; After receiving the address field data, the main fpga logic device 2-1 of host node 2 carries out matching addresses and detects, judged result is for denying, illustrate that then data send the wrong address field data that do not belong to, continue to hold machine, judged result then judges it is the data which partial node 3 is sent for being, and the reception total data, it is stored in the corresponding memory; Whether partial node 3 Data Receiving will be carried out cyclic redundancy check (CRC) after finishing, be used for specified data and wrap in transmission course and make mistakes; Judged result then makes an explanation and deal with data for being, judged result then abandons the data that receive for not; After the cyclic redundancy checking finishes, continue standby;
Step 9, return operating procedure three.
In the said method step: T is chosen as 200us, and N is chosen as 7 partial nodes, and each node time spent T1 is chosen as 28us.
The resistance that described resistance R T1, resistance R T2 select for use all is 100 ohm, the model that main fpga logic device 2-1, fpga logic device 3-2 select for use all is an ALTERA Cyclone EP1C20F32417 programmable gate array, the model that main M-LVDS driving transceiver 2-2, M-LVDS driving transceiver 3-1 select for use is MLVDS204 all, and the model that the DSP digital signal processor 1 in the host computer is selected for use is that TMS320C6713, universal serial bus 4 are twisted-pair feeder.