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