function_CAN.h

00001 #ifndef _FUNCTION_CAN_H_
00002 #define _FUNCTION_CAN_H_
00003 
00004 #include "mbed.h"
00005 
00006 extern CAN can;
00007 extern CANMessage msg;
00008 
00009 // INIT_CID
00010 #define INIT_CID_RX_CMD                   100
00011 #define INIT_CID_RX_REF_POSITION          200
00012 #define INIT_CID_RX_REF_VALVE_POS       300
00013 #define INIT_CID_RX_REF_PWM            400
00014 
00015 #define INIT_CID_TX_INFO              1100
00016 #define INIT_CID_TX_POSITION          1200
00017 #define INIT_CID_TX_TORQUE            1300
00018 #define INIT_CID_TX_PRES              1400
00019 #define INIT_CID_TX_VOUT              1500
00020 #define INIT_CID_TX_VALVE_POSITION    1600
00021 
00022 
00023 // CID_RX_CMD - RX CMD type
00024 #define             CRX_ASK_INFO                    0
00025 #define             CRX_ASK_BNO                     1
00026 #define             CRX_SET_BNO                     101
00027 #define             CRX_ASK_OPERATING_MODE          2
00028 #define             CRX_SET_OPERATING_MODE          102
00029 #define             CRX_SET_ENC_ZERO                103
00030 #define             CRX_SET_FET_ON                  104
00031 #define             CRX_SET_POS_TORQ_TRANS          105
00032 #define             CRX_ASK_CAN_FREQ                6
00033 #define             CRX_SET_CAN_FREQ                106
00034 #define             CRX_ASK_CONTROL_MODE            7
00035 #define             CRX_SET_CONTROL_MODE            107
00036 #define             CRX_SET_DATA_REQUEST            108
00037 #define             CRX_ASK_JOINT_ENC_DIR           9
00038 #define             CRX_SET_JOINT_ENC_DIR           109
00039 #define             CRX_ASK_VALVE_DIR               10
00040 #define             CRX_SET_VALVE_DIR               110
00041 #define             CRX_ASK_VALVE_ENC_DIR           11
00042 #define             CRX_SET_VALVE_ENC_DIR           111
00043 #define             CRX_ASK_VOLTAGE_SUPPLY          12
00044 #define             CRX_SET_VOLTAGE_SUPPLY          112
00045 #define             CRX_ASK_VOLTAGE_VALVE           13
00046 #define             CRX_SET_VOLTAGE_VALVE           113
00047 #define             CRX_SET_HOMEPOS                 114      
00048 #define             CRX_ASK_PID_GAIN                20
00049 #define             CRX_SET_PID_GAIN                120
00050 #define             CRX_ASK_VALVE_DEADZONE          21
00051 #define             CRX_SET_VALVE_DEADZONE          121
00052 #define             CRX_ASK_VELOCITY_COMP_GAIN      22
00053 #define             CRX_SET_VELOCITY_COMP_GAIN      122
00054 #define             CRX_ASK_COMPLIANCE_GAIN         23
00055 #define             CRX_SET_COMPLIANCE_GAIN         123
00056 #define             CRX_ASK_VALVE_FF                25
00057 #define             CRX_SET_VALVE_FF                125
00058 #define             CRX_ASK_BULK_MODULUS            26
00059 #define             CRX_SET_BULK_MODULUS            126
00060 #define             CRX_ASK_CHAMBER_VOLUME          27
00061 #define             CRX_SET_CHAMBER_VOLUME          127
00062 #define             CRX_ASK_PISTON_AREA             28
00063 #define             CRX_SET_PISTON_AREA             128
00064 #define             CRX_ASK_PRES                    29
00065 #define             CRX_SET_PRES                    129
00066 #define             CRX_ASK_ENC_LIMIT               30
00067 #define             CRX_SET_ENC_LIMIT               130
00068 #define             CRX_ASK_STROKE                  31
00069 #define             CRX_SET_STROKE                  131
00070 #define             CRX_ASK_VALVE_LIMIT             32
00071 #define             CRX_SET_VALVE_LIMIT             132
00072 #define             CRX_ASK_ENC_PULSE_PER_POSITION     33
00073 #define             CRX_SET_ENC_PULSE_PER_POSITION     133
00074 #define             CRX_ASK_TORQUE_SENSOR_PULSE_PER_TORQUE     34
00075 #define             CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE     134
00076 #define             CRX_ASK_PRES_SENSOR_PULSE_PER_PRES      35
00077 #define             CRX_SET_PRES_SENSOR_PULSE_PER_PRES      135
00078 #define             CRX_ASK_FRICTION                36
00079 #define             CRX_SET_FRICTION                136
00080 #define             CRX_ASK_VALVE_GAIN_PLUS         37
00081 #define             CRX_SET_VALVE_GAIN_PLUS         137
00082 #define             CRX_ASK_VALVE_GAIN_MINUS        38
00083 #define             CRX_SET_VALVE_GAIN_MINUS        138
00084 #define             CRX_LOW_REF                     139
00085 #define             CRX_ASK_HOMEPOS_OFFSET          40
00086 #define             CRX_SET_HOMEPOS_OFFSET          140
00087 #define             CRX_ASK_HOMEPOS_VALVE_OPENING   41
00088 #define             CRX_SET_HOMEPOS_VALVE_OPENING   141
00089 #define             CRX_ASK_DDV_VALVE_DEADZONE     42
00090 #define             CRX_SET_DDV_VALVE_DEADZONE     142
00091 #define             CRX_ASK_VALVE_PWM_VS_VALVE_POS  43
00092 #define             CRX_ASK_VALVE_POS_VS_FLOWRATE   44
00093 #define             CRX_ASK_VALVE_POS_NUM           45
00094 #define             CRX_ASK_VALVE_MAX_MIN_POS       46
00095 #define             CRX_SET_ERR_CLEAR               150
00096 #define             CRX_SET_ROM                     146
00097 #define             CRX_SET_VALVE_CENTER_OFFSET     147
00098 #define             CRX_SET_VALVE_DZ_MINUS_OFFSET   148
00099 #define             CRX_SET_VALVE_DZ_PLUS_OFFSET    149
00100 #define             CRX_SET_PID_GAIN_OPP            152
00101 #define             CRX_DELAY_TEST                  153
00102 #define             CRX_JUMP_STATUS                 255
00103 
00104 // CID_TX_INFO - TX INFO type
00105 
00106 #define             CTX_SEND_INFO                               0
00107 #define             CTX_SEND_BNO                                1
00108 #define             CTX_SEND_OPERATING_MODE                     2
00109 #define             CTX_SEND_CAN_FREQ                           6
00110 #define             CTX_SEND_CONTROL_MODE                       7
00111 #define             CTX_SEND_JOINT_ENC_DIR                      9
00112 #define             CTX_SEND_VALVE_DIR                          10
00113 #define             CTX_SEND_VALVE_ENC_DIR                      11
00114 #define             CTX_SEND_VOLTAGE_SUPPLY                     12
00115 #define             CTX_SEND_VOLTAGE_VALVE                      13
00116 #define             CTX_SEND_PID_GAIN                           20
00117 #define             CTX_SEND_VALVE_DEADZONE                     21
00118 #define             CTX_SEND_VELOCITY_COMP_GAIN                 22
00119 #define             CTX_SEND_COMPLIANCE_GAIN                    23
00120 #define             CTX_SEND_VALVE_FF                           25
00121 #define             CTX_SEND_BULK_MODULUS                       26
00122 #define             CTX_SEND_CHAMBER_VOLUME                     27
00123 #define             CTX_SEND_PISTON_AREA                        28
00124 #define             CTX_SEND_PRES                               29
00125 #define             CTX_SEND_ENC_LIMIT                          30
00126 #define             CTX_SEND_STROKE                             31
00127 #define             CTX_SEND_VALVE_LIMIT                        32
00128 #define             CTX_SEND_ENC_PULSE_PER_POSITION             33
00129 #define             CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE     34
00130 #define             CTX_SEND_PRES_SENSOR_PULSE_PER_BAR          35
00131 #define             CTX_SEND_FRICTION                           36
00132 #define             CTX_SEND_VALVE_GAIN_PLUS                    37
00133 #define             CTX_SEND_VALVE_GAIN_MINUS                   38
00134 #define             CTX_SEND_REFENCE_MODE                       39
00135 #define             CTX_SEND_HOMEPOS_OFFSET                     40
00136 #define             CTX_SEND_HOMEPOS_VALVE_OPENING              41
00137 #define             CTX_SEND_DDV_VALVE_DEADZONE                 42
00138 #define             CTX_VALVE_PWM_VS_VALVE_POS                  43
00139 #define             CTX_VALVE_POS_VS_FLOWRATE                   44
00140 #define             CTX_VALVE_POS_NUM                           45
00141 #define             CTX_VALVE_MAX_MIN_POS                       46
00142 
00143 // Sensor & State Transmission
00144 void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq);
00145 void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb);
00146 //void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout);
00147 void CAN_TX_TORQUE(int16_t t_valve_pos);
00148 void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b);
00149 void CAN_TX_PWM(int16_t t_pwm);
00150 void CAN_TX_VALVE_POSITION(int16_t t_valve_pos, int16_t t_ref_valve_pos, int16_t t_pwm);
00151 
00152 // Information Transmission
00153 void CAN_TX_INFO(void);
00154 void CAN_TX_BNO(void);
00155 void CAN_TX_OPERATING_MODE(void);
00156 void CAN_TX_CAN_FREQ(void);
00157 void CAN_TX_CONTROL_MODE(void);
00158 void CAN_TX_JOINT_ENC_DIR(void);
00159 void CAN_TX_VALVE_DIR(void);
00160 void CAN_TX_VALVE_ENC_DIR(void);
00161 void CAN_TX_VOLTAGE_SUPPLY(void);
00162 void CAN_TX_VOLTAGE_VALVE(void);
00163 void CAN_TX_PID_GAIN(int t_type);
00164 void CAN_TX_VALVE_DEADZONE(void);
00165 void CAN_TX_VELOCITY_COMP_GAIN(void);
00166 void CAN_TX_COMPLIANCE_GAIN(void);
00167 void CAN_TX_VALVE_FF(void);
00168 void CAN_TX_BULK_MODULUS(void);
00169 void CAN_TX_CHAMBER_VOLUME(void);
00170 void CAN_TX_PISTON_AREA(void);
00171 void CAN_TX_PRES_A_AND_B(void);
00172 void CAN_TX_ENC_LIMIT(void);
00173 void CAN_TX_STROKE(void);
00174 void CAN_TX_VALVE_LIMIT(void);
00175 void CAN_TX_ENC_PULSE_PER_POSITION(void);
00176 void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void);
00177 void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void);
00178 void CAN_TX_FRICTION(void);
00179 void CAN_TX_VALVE_GAIN_PLUS(void);
00180 void CAN_TX_VALVE_GAIN_MINUS(void);
00181 void CAN_TX_REFENCE_MODE(void);
00182 void CAN_TX_HOMEPOS_OFFSET(void);
00183 void CAN_TX_HOMPOS_VALVE_OPENING(void);
00184 void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex);
00185 void CAN_TX_DDV_VALVE_DEADZONE(void);
00186 void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t canindex);
00187 void CAN_TX_VALVE_POS_NUM(void);
00188 void CAN_TX_DDV_VALVE_MAX_MIN_POS(void);
00189 
00190 class State 
00191 {
00192     public:
00193         float sen;
00194         float ref;
00195         float ref_old;
00196         float ref_diff;
00197         float ref_home_pos;
00198         float err;
00199         float err_int;
00200         float err_old;
00201         float err_diff;
00202         float err_sum;
00203     public:
00204         State(){
00205             sen = 0.0f;
00206             ref = 0.0f;
00207             ref_old = 0.0f;
00208             ref_diff = 0.0f;
00209             ref_home_pos = 0.0f;
00210             err = 0.0f;
00211             err_int = 0.0f;
00212             err_old = 0.0f;
00213             err_diff = 0.0f;
00214         }
00215 };
00216 
00217 extern State pos;
00218 extern State vel;
00219 extern State Vout;
00220 extern State torq;
00221 extern State pres_A;
00222 extern State pres_B;
00223 extern State cur;
00224 extern State valve_pos;
00225 
00226 extern State INIT_Vout;
00227 extern State INIT_Valve_Pos;
00228 extern State INIT_Pos;
00229 extern State INIT_torq;
00230 
00231 // CAN Receive Functions
00232 void ReadCMD();
00233 void CAN_RX_HANDLER();
00234 void CAN_ID_INIT(void);
00235 
00236 extern int ID_index_array[50];
00237 
00238 #endif
00239 
00240 
00241 
00242