00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051 #ifndef FORCETORQUECTRL_INCLUDEDEF_H
00052 #define FORCETORQUECTRL_INCLUDEDEF_H
00053
00054 #include <iostream>
00055 #include <fstream>
00056 #include <Eigen/Core>
00057
00058
00059 #include <cob_generic_can/CanItf.h>
00060
00061 #define DEBUG 0
00062
00063
00064
00065 #define READ_SG 0x0
00066 #define READ_MATRIX 0x2
00067 #define READ_SERIALNR 0x5
00068 #define SET_CALIB 0x6
00069 #define READ_COUNTSPERU 0x7
00070 #define READ_UNITCODE 0x8
00071 #define READ_DIAGNOV 0X9
00072 #define RESET 0xC
00073 #define SET_BASEID 0xD
00074 #define SET_BAUD 0xE
00075 #define READ_FIRMWARE 0xF
00076
00077 #define ATI_CAN_BAUD_2M 0
00078 #define ATI_CAN_BAUD_1M 1
00079 #define ATI_CAN_BAUD_500K 3
00080 #define ATI_CAN_BAUD_250K 7
00081
00082 class ForceTorqueCtrl
00083 {
00084 public:
00085 ForceTorqueCtrl();
00086 ForceTorqueCtrl(int can_type, std::string can_path, int can_baudrate, int base_identifier);
00087 ~ForceTorqueCtrl();
00088
00089 bool Init();
00090 bool ReadFTSerialNumber();
00091 bool ReadCountsPerUnit();
00092 bool ReadUnitCodes();
00093 bool ReadDiagnosticADCVoltages(int index, short int& value);
00094 bool SetActiveCalibrationMatrix(int num);
00095 bool SetBaudRate(int value);
00096 bool SetBaseIdentifier(int identifier);
00097 bool Reset();
00098 bool ReadSGData(int statusCode, double& Fx, double& Fy, double& Fz, double& Tx, double& Ty, double& Tz);
00099 bool ReadFirmwareVersion();
00100 void ReadCalibrationMatrix();
00101
00102 void SetGaugeOffset(float sg0Off, float sg1Off, float sg2Off, float sg3Off, float sg4Off, float sg5Off);
00103 void SetGaugeGain(float gg0, float gg1, float gg2, float gg3, float gg4, float gg5);
00104 void SetFXGain(float fxg0, float fxg1, float fxg2, float fxg3, float fxg4, float fxg5);
00105 void SetFYGain(float fyg0, float fyg1, float fyg2, float fyg3, float fyg4, float fyg5);
00106 void SetFZGain(float fzg0, float fzg1, float fzg2, float fzg3, float fzg4, float fzg5);
00107 void SetTXGain(float txg0, float txg1, float txg2, float txg3, float txg4, float txg5);
00108 void SetTYGain(float tyg0, float tyg1, float tyg2, float tyg3, float tyg4, float tyg5);
00109 void SetTZGain(float tzg0, float tzg1, float tzg2, float tzg3, float tzg4, float tzg5);
00110
00111 void SetCalibMatrix();
00112 void CalcCalibMatrix();
00113 void StrainGaugeToForce(int& sg0, int& sg1, int& sg2, int& sg3, int& sg4, int& sg5);
00114
00115 protected:
00116 bool initCan();
00117
00118
00119 CanMsg m_CanMsgRec;
00120 bool m_bWatchdogErr;
00121
00122 private:
00123 CanMsg CMsg;
00124 CanItf* m_pCanCtrl;
00125
00126 int m_CanType;
00127 std::string m_CanDevice;
00128 int m_CanBaudrate;
00129 int m_CanBaseIdentifier;
00130
00131 unsigned int d_len;
00132 Eigen::VectorXf m_v3StrainGaigeOffset;
00133 Eigen::VectorXf m_v3GaugeGain;
00134 Eigen::VectorXf m_v3FXGain;
00135 Eigen::VectorXf m_v3FYGain;
00136 Eigen::VectorXf m_v3FZGain;
00137 Eigen::VectorXf m_v3TXGain;
00138 Eigen::VectorXf m_v3TYGain;
00139 Eigen::VectorXf m_v3TZGain;
00140 Eigen::MatrixXf m_mXCalibMatrix;
00141 Eigen::MatrixXf m_vForceData;
00142
00143
00144
00145 void ReadMatrix(int axis, Eigen::VectorXf& vec);
00146
00147 union
00148 {
00149 char bytes[2];
00150 short int value;
00151 } ibBuf;
00152
00153 union
00154 {
00155 char bytes[4];
00156 int value;
00157 } intbBuf;
00158
00159 union
00160 {
00161 char bytes[4];
00162 float value;
00163 } fbBuf;
00164
00165 std::ofstream out;
00166 };
00167
00168 #endif