function_CAN.cpp

00001 #include "function_CAN.h"
00002 #include "setting.h"
00003 #include "function_utilities.h"
00004 #include "SPI_EEP_ENC.h"
00005 #include "stm32f4xx_flash.h"
00006 #include "FlashWriter.h"
00007 
00008 // CAN ID Setting Variables
00009 int CID_RX_CMD = 100;
00010 int CID_RX_REF_POSITION = 200;
00011 int CID_RX_REF_OPENLOOP = 300;
00012 int CID_RX_REF_PWM = 400;
00013 
00014 int CID_TX_INFO = 1100;
00015 int CID_TX_POS_VEL_TORQ = 1200;
00016 int CID_TX_PWM = 1300;
00017 int CID_TX_SOMETHING = 1400;
00018 
00019 int CID_TX_CURRENT = 1400;
00020 int CID_TX_VOUT = 1500;
00021 int CID_TX_VALVE_POSITION = 1600;
00022 
00023 // variables
00024 uint8_t can_index = 0;
00025 
00026 extern DigitalOut LED;
00027 
00028 extern float u_past[];
00029 extern float x_past[];
00030 extern float x_future[];
00031 extern float f_past[];
00032 extern float f_future[];
00033 extern float input_NN[];
00034 
00035 /*******************************************************************************
00036  * State Class functions
00037  ******************************************************************************/
00038 void State::UpdateSen(float sen_new, float Freq_update, float f_cut) {
00039     if(f_cut<=0.0f) f_cut=0.001f;
00040     
00041     this->sen_diff = (sen_new-this->sen)*Freq_update;
00042     float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz
00043     this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new;
00044 }
00045 
00046 void State::UpdateRef(float ref_new) {
00047     this->ref = ref_new;
00048 }
00049 
00050 void State::Reset() {
00051     this->sen = 0.0f;
00052     this->sen_diff = 0.0f;
00053     this->ref = 0.0f;
00054     this->err = 0.0f;
00055     this->err_int = 0.0f;
00056     this->err_old = 0.0f;
00057     this->err_diff = 0.0f;
00058 }
00059 
00060 /*******************************************************************************
00061  * CAN functions
00062  ******************************************************************************/
00063 void CAN_ID_INIT(void)
00064 {
00065 
00066     CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
00067     CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
00068     CID_RX_REF_OPENLOOP = (int) (BNO + INIT_CID_RX_REF_OPENLOOP);
00069     CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM);
00070 
00071     CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO);
00072     CID_TX_POS_VEL_TORQ = (int) (BNO + INIT_CID_TX_POS_VEL_TORQ);
00073     CID_TX_PWM = (int) (BNO + INIT_CID_TX_PWM);
00074     CID_TX_CURRENT = (int) (BNO + INIT_CID_TX_CURRENT);
00075     CID_TX_VOUT = (int) (BNO + INIT_CID_TX_VOUT);
00076     CID_TX_VALVE_POSITION = (int) (BNO + INIT_CID_TX_VALVE_POSITION);
00077     CID_TX_SOMETHING = (int) (BNO + INIT_CID_TX_SOMETHING);
00078 }
00079 
00080 void ReadCMD(int16_t CMD)
00081 {
00082     switch(CMD) {
00083         case CRX_ASK_INFO: {
00084             CAN_TX_INFO();
00085             break;
00086         }
00087         case CRX_ASK_BNO: {
00088             CAN_TX_BNO();
00089             break;
00090         }
00091         case CRX_SET_BNO: {
00092             BNO = (int16_t) msg.data[1];
00093             spi_eeprom_write(RID_BNO, (int16_t) BNO);
00094             CAN_ID_INIT(); // can id init
00095             break;
00096         }
00097         case CRX_ASK_OPERATING_MODE: {
00098             CAN_TX_OPERATING_MODE();
00099             break;
00100         }
00101         case CRX_SET_OPERATING_MODE: {
00102             OPERATING_MODE = (uint8_t) msg.data[1];
00103             SENSING_MODE = (uint8_t) msg.data[2];
00104             CURRENT_CONTROL_MODE = (uint8_t) msg.data[3];
00105             FLAG_VALVE_DEADZONE = (uint8_t) msg.data[4];
00106             spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE);
00107             spi_eeprom_write(RID_SENSING_MODE, (int16_t) SENSING_MODE);
00108             spi_eeprom_write(RID_CURRENT_CONTROL_MODE, (int16_t) CURRENT_CONTROL_MODE);
00109             spi_eeprom_write(RID_FLAG_VALVE_DEADZONE, (int16_t) FLAG_VALVE_DEADZONE);
00110             break;
00111         }
00112         case CRX_SET_ENC_ZERO: {
00113             ENC_SET_ZERO();
00114 
00115             break;
00116         }
00117         case CRX_SET_FET_ON: {
00118 
00119             break;
00120         }
00121 
00122         case CRX_SET_POS_TORQ_TRANS: {
00123             MODE_POS_FT_TRANS = (int16_t) msg.data[1];
00124             /*
00125             MODE_POS_FT_TRANS == 0 : Position Control
00126             MODE_POS_FT_TRANS == 1 : Trasition(Position->Torque)
00127             MODE_POS_FT_TRANS == 2 : Torque Control (Convert to 2 automatically 3sec after transition)
00128             MODE_POS_FT_TRANS == 3 : Transition(Toque->Position)
00129             */
00130             break;
00131         }
00132 
00133         case CRX_ASK_CAN_FREQ: {
00134             CAN_TX_CAN_FREQ();
00135 
00136             break;
00137         }
00138 
00139         case CRX_SET_CAN_FREQ: {
00140             CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8);
00141             spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
00142             break;
00143         }
00144 
00145         case CRX_ASK_CONTROL_MODE: {
00146             CAN_TX_CONTROL_MODE();
00147 
00148             break;
00149         }
00150 
00151         case CRX_SET_CONTROL_MODE: {
00152             CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]);
00153             if (CONTROL_MODE == 22) {    //MODE_FIND_HOME
00154                 FLAG_FIND_HOME = true;
00155                 FINDHOME_STAGE = FINDHOME_INIT;
00156             }
00157             break;
00158         }
00159 
00160         case CRX_SET_DATA_REQUEST: {
00161             int request_type = msg.data[2];
00162             flag_data_request[request_type] = msg.data[1];
00163 
00164 //            if (flag_data_request[1] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
00165             //if (flag_data_request[1] == HIGH) dac_1 = PRES_A_VREF/3.3;
00166 //            if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
00167             //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3;
00168 
00169             
00170             break;
00171         }
00172 
00173         case CRX_ASK_JOINT_ENC_DIR: {
00174             CAN_TX_JOINT_ENC_DIR();
00175 
00176             break;
00177         }
00178 
00179         case CRX_SET_JOINT_ENC_DIR: {
00180             DIR_JOINT_ENC = (int16_t) (msg.data[1] | msg.data[2] << 8);
00181             if (DIR_JOINT_ENC >= 0)
00182                 DIR_JOINT_ENC = 1;
00183             else
00184                 DIR_JOINT_ENC = -1;
00185 
00186             spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
00187 
00188             break;
00189         }
00190 
00191         case CRX_ASK_VALVE_DIR: {
00192             CAN_TX_VALVE_DIR();
00193 
00194             break;
00195         }
00196 
00197         case CRX_SET_VALVE_DIR: {
00198             DIR_VALVE = (int16_t) (msg.data[1] | msg.data[2] << 8);
00199             if (DIR_VALVE >= 0)
00200                 DIR_VALVE = 1;
00201             else
00202                 DIR_VALVE = -1;
00203 
00204             spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
00205 
00206             break;
00207         }
00208 
00209         case CRX_ASK_VALVE_ENC_DIR: {
00210             CAN_TX_VALVE_ENC_DIR();
00211 
00212             break;
00213         }
00214 
00215         case CRX_SET_VALVE_ENC_DIR: {
00216             DIR_VALVE_ENC = (int16_t) (msg.data[1] | msg.data[2] << 8);
00217             if (DIR_VALVE_ENC >= 0)
00218                 DIR_VALVE_ENC = 1;
00219             else
00220                 DIR_VALVE_ENC = -1;
00221 
00222             spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
00223 
00224             break;
00225         }
00226 
00227         case CRX_ASK_VOLTAGE_SUPPLY: {
00228             CAN_TX_VOLTAGE_SUPPLY();
00229 
00230             break;
00231         }
00232 
00233         case CRX_SET_VOLTAGE_SUPPLY: {
00234             SUPPLY_VOLTAGE = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
00235 
00236 
00237             spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.0f));
00238 
00239             break;
00240         }
00241 
00242         case CRX_ASK_VOLTAGE_VALVE: {
00243             CAN_TX_VOLTAGE_VALVE();
00244 
00245             break;
00246         }
00247 
00248         case CRX_SET_VOLTAGE_VALVE: {
00249             VALVE_VOLTAGE_LIMIT = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
00250 
00251             spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f));
00252 
00253 
00254             break;
00255         }
00256 
00257         case CRX_SET_HOMEPOS: {
00258             CONTROL_UTILITY_MODE = 22;
00259             break;
00260         }
00261         
00262         case CRX_ASK_VARIABLE_SUPPLY:
00263         {
00264             CAN_TX_VARIABLE_SUPPLY_ONOFF();
00265             break;
00266         }                
00267         
00268         case CRX_SET_VARIABLE_SUPPLY:
00269         {
00270             SUPPLY_PRESSURE_UPDATE = msg.data[1];
00271             break;
00272         }
00273 
00274         case CRX_ASK_PID_GAIN: {
00275             CAN_TX_PID_GAIN(msg.data[1]);
00276 
00277             break;
00278         }
00279 
00280         case CRX_SET_PID_GAIN: {
00281             if (msg.data[1] == 0) {
00282                 P_GAIN_VALVE_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
00283                 I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
00284                 D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
00285 
00286                 spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
00287                 spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
00288                 spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
00289 
00290             } else if (msg.data[1] == 1) {
00291                 P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
00292                 I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
00293                 D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
00294 
00295                 spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
00296                 spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
00297                 spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
00298 
00299             } else if (msg.data[1] == 2) {
00300                 P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8);
00301                 I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
00302                 D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8);
00303 
00304                 spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
00305                 spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
00306                 spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
00307 
00308             } else if (msg.data[1] == 3) {
00309                 K_SPRING = (float) (((float) ((int16_t) (msg.data[2] | msg.data[3] << 8))) * 0.1f);
00310                 D_DAMPER = (float) (((float) ((int16_t) (msg.data[4] | msg.data[5] << 8))) * 0.01f);
00311 
00312 //                spi_eeprom_write(RID_K_SPRING, (int16_t) K_SPRING);
00313 //                spi_eeprom_write(RID_D_DAMPER, (int16_t) D_DAMPER);
00314 
00315             } else if (msg.data[1] == 4) {
00316                 P_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[2] | msg.data[3] << 8);
00317                 I_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[4] | msg.data[5] << 8);
00318                 D_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[6] | msg.data[7] << 8);
00319 
00320             }
00321 
00322             break;
00323         }
00324 
00325         case CRX_ASK_VALVE_DEADZONE: {
00326             CAN_TX_VALVE_DEADZONE();
00327 
00328             break;
00329         }
00330 
00331         case CRX_SET_VALVE_DEADZONE: {
00332             VALVE_CENTER = (int16_t) (msg.data[1] | msg.data[2] << 8);
00333             VALVE_DEADZONE_PLUS = (int16_t)(msg.data[3] | msg.data[4] << 8);
00334             VALVE_DEADZONE_MINUS = (int16_t)(msg.data[5] | msg.data[6] << 8);
00335 
00336             spi_eeprom_write(RID_VALVE_CNETER, VALVE_CENTER);
00337             spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
00338             spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
00339 
00340             break;
00341         }
00342 
00343         case CRX_ASK_VELOCITY_COMP_GAIN: {
00344             CAN_TX_VELOCITY_COMP_GAIN();
00345 
00346             break;
00347         }
00348 
00349         case CRX_SET_VELOCITY_COMP_GAIN: {
00350             VELOCITY_COMP_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
00351 
00352 
00353             spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
00354 
00355             break;
00356         }
00357 
00358         case CRX_ASK_VALVE_ELECTRIC_CENTER: {
00359             CAN_TX_VALVE_ELECTRIC_CENTER();
00360 
00361             break;
00362         }
00363 
00364         case CRX_SET_VALVE_ELECTRIC_CENTER: {
00365             VALVE_ELECTRIC_CENTER = (int16_t) (msg.data[1] | msg.data[2] << 8);
00366 
00367 
00368             spi_eeprom_write(RID_VALVE_ELECTRIC_CENTER, (int16_t) VALVE_ELECTRIC_CENTER);
00369 
00370             break;
00371         }
00372 
00373         case CRX_ASK_VALVE_FF: {
00374             CAN_TX_VALVE_FF();
00375 
00376             break;
00377         }
00378 
00379         case CRX_SET_VALVE_FF: {
00380             VALVE_FF = (int16_t) (msg.data[1] | msg.data[2] << 8);
00381 
00382             spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
00383 
00384             break;
00385         }
00386 
00387         case CRX_ASK_BULK_MODULUS: {
00388             CAN_TX_BULK_MODULUS();
00389 
00390             break;
00391         }
00392 
00393         case CRX_SET_BULK_MODULUS: {
00394             BULK_MODULUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
00395             spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
00396 
00397             break;
00398         }
00399 
00400         case CRX_ASK_CHAMBER_VOLUME: {
00401             CAN_TX_CHAMBER_VOLUME();
00402 
00403             break;
00404         }
00405 
00406         case CRX_SET_CHAMBER_VOLUME: {
00407             CHAMBER_VOLUME_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
00408             CHAMBER_VOLUME_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
00409 
00410             spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
00411             spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
00412 
00413             break;
00414         }
00415 
00416         case CRX_ASK_PISTON_AREA: {
00417             CAN_TX_PISTON_AREA();
00418 
00419             break;
00420         }
00421 
00422         case CRX_SET_PISTON_AREA: {
00423             PISTON_AREA_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
00424             PISTON_AREA_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
00425             PISTON_AREA_alpha = (double)PISTON_AREA_A/(double)PISTON_AREA_B;
00426             alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
00427 
00428             spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
00429             spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
00430             break;
00431         }
00432 
00433         case CRX_ASK_SUP_PRES: {
00434             CAN_TX_SUP_PRES();
00435             break;
00436         }
00437 
00438         case CRX_SET_SUP_PRES: {
00439             int16_t temp = (int16_t) (msg.data[1] | msg.data[2] << 8);
00440             spi_eeprom_write(RID_PRES_SUPPLY, temp);
00441             PRES_SUPPLY_NOM = (float)temp;
00442             PRES_SUPPLY = PRES_SUPPLY_NOM;
00443             break;
00444         }
00445 
00446         case CRX_ASK_ENC_LIMIT: {
00447             CAN_TX_ENC_LIMIT();
00448 
00449             break;
00450         }
00451 
00452         case CRX_SET_ENC_LIMIT: {
00453             ENC_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
00454             ENC_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
00455             spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
00456             spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
00457 
00458             break;
00459         }
00460 
00461         case CRX_ASK_STROKE: {
00462             CAN_TX_STROKE();
00463             break;
00464         }
00465 
00466         case CRX_SET_STROKE: {
00467             STROKE = (int16_t) (msg.data[1] | msg.data[2] << 8);
00468             spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
00469 
00470             break;
00471         }
00472 
00473         case CRX_ASK_VALVE_LIMIT: {
00474             CAN_TX_VALVE_LIMIT();
00475 
00476             break;
00477         }
00478 
00479         case CRX_SET_VALVE_LIMIT: {
00480             VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8);
00481             VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8);
00482 
00483             spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
00484             spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
00485 
00486             break;
00487         }
00488 
00489         case CRX_ASK_ENC_PULSE_PER_POSITION: {
00490             CAN_TX_ENC_PULSE_PER_POSITION();
00491 
00492             break;
00493         }
00494 
00495         case CRX_SET_ENC_PULSE_PER_POSITION: {
00496             ENC_PULSE_PER_POSITION = (int16_t) (msg.data[1] | msg.data[2] << 8);
00497             spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) (ENC_PULSE_PER_POSITION));
00498 
00499             break;
00500         }
00501 
00502         case CRX_ASK_TORQUE_SENSOR_PULSE_PER_TORQUE: {
00503             CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE();
00504 
00505             break;
00506         }
00507 
00508         case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
00509 //            TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.01f);
00510             TORQUE_SENSOR_PULSE_PER_TORQUE = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8)))*0.001f;
00511             spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*1000.0f));
00512 
00513             break;
00514         }
00515 
00516         case CRX_ASK_PRES_SENSOR_PULSE_PER_PRES: {
00517             CAN_TX_PRES_SENSOR_PULSE_PER_PRES();
00518 
00519             break;
00520         }
00521 
00522         case CRX_SET_PRES_SENSOR_PULSE_PER_PRES: {
00523             PRES_SENSOR_A_PULSE_PER_BAR = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) * 0.01f;
00524             PRES_SENSOR_B_PULSE_PER_BAR = (double) ((int16_t) (msg.data[3] | msg.data[4] << 8)) * 0.01f;
00525             spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
00526             spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
00527 
00528             break;
00529         }
00530 
00531         case CRX_ASK_FRICTION: {
00532             CAN_TX_FRICTION();
00533 
00534             break;
00535         }
00536 
00537         case CRX_SET_FRICTION: {
00538             FRICTION = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f;
00539             spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.0f));
00540 
00541             break;
00542         }
00543 
00544         case CRX_ASK_VALVE_GAIN_PLUS: {
00545             CAN_TX_VALVE_GAIN_PLUS();
00546 
00547             break;
00548         }
00549         case CRX_SET_VALVE_GAIN_PLUS: {
00550             VALVE_GAIN_LPM_PER_V[0] = (double) msg.data[1] / 50.0f;
00551             VALVE_GAIN_LPM_PER_V[2] = (double) msg.data[2] / 50.0f;
00552             VALVE_GAIN_LPM_PER_V[4] = (double) msg.data[3] / 50.0f;
00553             VALVE_GAIN_LPM_PER_V[6] = (double) msg.data[4] / 50.0f;
00554             VALVE_GAIN_LPM_PER_V[8] = (double) msg.data[5] / 50.0f;
00555             spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.0f));
00556             spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.0f));
00557             spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.0f));
00558             spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.0f));
00559             spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.0f));
00560 
00561             break;
00562         }
00563 
00564         case CRX_ASK_VALVE_GAIN_MINUS: {
00565             CAN_TX_VALVE_GAIN_MINUS();
00566 
00567             break;
00568         }
00569         case CRX_SET_VALVE_GAIN_MINUS: {
00570             VALVE_GAIN_LPM_PER_V[1] = (double) msg.data[1] / 50.0f;
00571             VALVE_GAIN_LPM_PER_V[3] = (double) msg.data[2] / 50.0f;
00572             VALVE_GAIN_LPM_PER_V[5] = (double) msg.data[3] / 50.0f;
00573             VALVE_GAIN_LPM_PER_V[7] = (double) msg.data[4] / 50.0f;
00574             VALVE_GAIN_LPM_PER_V[9] = (double) msg.data[5] / 50.0f;
00575             spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.0f));
00576             spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.0f));
00577             spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.0f));
00578             spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.0f));
00579             spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.0f));
00580 
00581             break;
00582         }
00583 
00584         case CRX_LOW_REF: {
00585             REFERENCE_MODE = msg.data[1];
00586 
00587             REF_NUM = msg.data[2];
00588             REF_PERIOD = (double) ((int16_t) (msg.data[3] | msg.data[4] << 8)) / 100.0f;
00589             if (REF_PERIOD <= 0.0f) REF_MOVE_TIME_5k = TMR_FREQ_5k / CAN_FREQ;
00590             else REF_MOVE_TIME_5k = (int) (REF_PERIOD * (double) TMR_FREQ_5k);
00591             REF_MAG = (double) ((int16_t) (msg.data[5] | msg.data[6] << 8)) / 100.0f;
00592 
00593             break;
00594         }
00595 
00596         case CRX_JUMP_STATUS: {
00597             MODE_JUMP_STATUS = msg.data[1];
00598 
00599             break;
00600         }
00601 
00602         case CRX_SET_ERR_CLEAR: {
00603 
00604             for (int i = 0; i < num_err; i++) {
00605                 flag_err[i] = FALSE;
00606                 flag_err_old[i] = FALSE;
00607             }
00608 
00609             flag_err_rt = FALSE;
00610 
00611 
00612             break;
00613         }
00614 
00615         case CRX_ASK_HOMEPOS_OFFSET: {
00616             CAN_TX_HOMEPOS_OFFSET();
00617             break;
00618         }
00619         case CRX_SET_HOMEPOS_OFFSET: {
00620             HOMEPOS_OFFSET = (int16_t)(msg.data[1] | msg.data[2] << 8);
00621             spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
00622             break;
00623         }
00624 
00625         case CRX_ASK_HOMEPOS_VALVE_OPENING: {
00626             CAN_TX_HOMPOS_VALVE_OPENING();
00627             break;
00628         }
00629         case CRX_SET_HOMEPOS_VALVE_OPENING: {
00630             HOMEPOS_VALVE_OPENING = (int16_t) (msg.data[1] | msg.data[2] << 8);
00631             spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
00632             break;
00633         }
00634 
00635         case CRX_ASK_VALVE_PWM_VS_VALVE_POS: {
00636             can_index = (int16_t) msg.data[1];
00637             CAN_TX_VALVE_PWM_VS_VALVE_POS(can_index);
00638             break;
00639         }
00640         case CRX_ASK_VALVE_POS_VS_FLOWRATE: {
00641             can_index = (int16_t) msg.data[1];
00642             CAN_TX_VALVE_POS_VS_FLOWRATE(can_index);
00643             break;
00644         }
00645         case CRX_ASK_VALVE_POS_NUM: {
00646             CAN_TX_VALVE_POS_NUM();
00647             break;
00648         }
00649 
00650         case CRX_SET_ROM: {
00651             break;
00652         }
00653         
00654         case CRX_ASK_BUFFER: {
00655             cnt_buffer = (int16_t) (msg.data[1] | msg.data[2] << 8);
00656             CAN_TX_BUFFER(cnt_buffer);
00657             break;
00658         }
00659         case CRX_SET_STEP_TEST: {
00660             cnt_step_test = 0;
00661             CONTROL_UTILITY_MODE = 37;
00662             break;
00663         }
00664         case CRX_SET_FREQ_TEST: {
00665             cnt_freq_test = 0;
00666             CONTROL_UTILITY_MODE = 34;
00667             break;
00668         }
00669 
00670         default:
00671             break;
00672     }
00673 }
00674 
00675 void CAN_RX_HANDLER()
00676 {
00677 
00678     can.read(msg);
00679     unsigned int address = msg.id;
00680     if(address==CID_RX_CMD) {
00681         unsigned int CMD = msg.data[0];
00682         ReadCMD(CMD);
00683 
00684     } else if(address==CID_RX_REF_POSITION) {
00685 
00686         int16_t temp_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); 
00687         int16_t temp_vel = (int16_t) (msg.data[2] | msg.data[3] << 8);
00688         int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
00689 
00690         if((OPERATING_MODE&0b001)==0) { // Rotary Actuator
00691             REF_POSITION = (float)temp_pos / 200.0f;
00692             REF_VELOCITY = (float)temp_vel / 20.0f;
00693             REF_TORQUE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;  // pulse >> Nm
00694         } else { //Linear Actuator
00695             REF_POSITION = (float)temp_pos / 200.0f;
00696             REF_VELOCITY = (float)temp_vel / 20.0f;
00697             REF_FORCE = (float)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE;  // pulse >> N
00698         }
00699 
00700         if(SUPPLY_PRESSURE_UPDATE == 1) {
00701             int16_t temp_REF_Ps = (int16_t) (msg.data[6] | msg.data[7] << 8);
00702             PRES_SUPPLY = ((float)temp_REF_Ps) / 100.0; 
00703             if(PRES_SUPPLY<35.0f) PRES_SUPPLY = 35.0f;
00704             else if(PRES_SUPPLY>210.0f) PRES_SUPPLY = 210.0f;
00705         } else {
00706             PRES_SUPPLY = PRES_SUPPLY_NOM;
00707         }
00708 
00709         if(CAN_FREQ == -1) {
00710 
00711             // Position, Velocity, and Torque (ID:1200)
00712             if (flag_data_request[0] == HIGH) {
00713                 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
00714                     CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
00715                 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
00716                     CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
00717                 }
00718             }
00719 
00720             // Valve Position (ID:1300)
00721             if (flag_data_request[1] == HIGH) {
00722                 CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); 
00723             }
00724 
00725             // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
00726             if (flag_data_request[2] == HIGH) {
00727                 CAN_TX_SOMETHING((int16_t)(pres_A.sen*100.0f), (int16_t)(pres_B.sen*100.0f), (int16_t) (0), (int16_t) (0)); 
00728             }
00729         }
00730 
00731     } else if(address==CID_RX_REF_OPENLOOP) {
00732         int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8);
00733 
00734         if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve
00735             valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~10000)
00736         } else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve
00737             valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~30000)
00738         } else { //SW Valve
00739             if(temp_ref_valve_pos >= 0) {
00740 //                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
00741                 valve_pos.ref = (double)temp_ref_valve_pos; // -10000~10000
00742             } else {
00743 //                valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
00744                 valve_pos.ref = (double)temp_ref_valve_pos; // -10000~10000
00745             }
00746         }
00747     } else if(address==CID_RX_REF_PWM) {
00748         int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
00749         Vout.ref = (double) temp_ref_pwm;
00750     }
00751 
00752 
00753 }
00754 
00755 /******************************************************************************
00756  Information Transmission Functions
00757 *******************************************************************************/
00758 
00759 void CAN_TX_INFO(void)
00760 {
00761     CANMessage temp_msg;
00762 
00763     temp_msg.id = CID_TX_INFO;
00764     temp_msg.len = 7;
00765     temp_msg.data[0] = (uint8_t) CTX_SEND_INFO;
00766     temp_msg.data[1] = (uint8_t) BNO;
00767     temp_msg.data[2] = (uint8_t) CAN_FREQ;
00768     temp_msg.data[3] = (uint8_t) (CAN_FREQ >> 8);
00769     temp_msg.data[4] = (uint8_t) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]);
00770     temp_msg.data[5] = (uint8_t) CONTROL_UTILITY_MODE;
00771     temp_msg.data[6] = (uint8_t) OPERATING_MODE;
00772 
00773     can.write(temp_msg);
00774 }
00775 
00776 void CAN_TX_BNO(void)
00777 {
00778     CANMessage temp_msg;
00779 
00780     temp_msg.id = CID_TX_INFO;
00781     //temp_msg.len = 2;
00782     temp_msg.len = 2;
00783     temp_msg.data[0] = (uint8_t) CTX_SEND_BNO;
00784     temp_msg.data[1] = (uint8_t) BNO;
00785 
00786     can.write(temp_msg);
00787 }
00788 
00789 void CAN_TX_OPERATING_MODE(void)
00790 {
00791     CANMessage temp_msg;
00792 
00793     temp_msg.id = CID_TX_INFO;
00794     temp_msg.len = 5;
00795     temp_msg.data[0] = (uint8_t) CTX_SEND_OPERATING_MODE;
00796     temp_msg.data[1] = (uint8_t) OPERATING_MODE;
00797     temp_msg.data[2] = (uint8_t) SENSING_MODE;
00798     temp_msg.data[3] = (uint8_t) CURRENT_CONTROL_MODE;
00799     temp_msg.data[4] = (uint8_t) FLAG_VALVE_DEADZONE;
00800 
00801     can.write(temp_msg);
00802 }
00803 
00804 void CAN_TX_CAN_FREQ(void)
00805 {
00806     CANMessage temp_msg;
00807 
00808     temp_msg.id = CID_TX_INFO;
00809     temp_msg.len = 3;
00810     temp_msg.data[0] = (uint8_t) CTX_SEND_CAN_FREQ;
00811     temp_msg.data[1] = (uint8_t) CAN_FREQ;
00812     temp_msg.data[2] = (uint8_t) (CAN_FREQ >> 8);
00813 
00814     can.write(temp_msg);
00815 }
00816 
00817 void CAN_TX_CONTROL_MODE(void)
00818 {
00819     CANMessage temp_msg;
00820 
00821     temp_msg.id = CID_TX_INFO;
00822     temp_msg.len = 2;
00823     temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE;
00824     temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE;
00825 
00826     can.write(temp_msg);
00827 }
00828 
00829 void CAN_TX_JOINT_ENC_DIR(void)
00830 {
00831     CANMessage temp_msg;
00832 
00833     temp_msg.id = CID_TX_INFO;
00834     temp_msg.len = 3;
00835     temp_msg.data[0] = (uint8_t) CTX_SEND_JOINT_ENC_DIR;
00836     temp_msg.data[1] = (uint8_t) DIR_JOINT_ENC;
00837     temp_msg.data[2] = (uint8_t) (DIR_JOINT_ENC >> 8);
00838 
00839     can.write(temp_msg);
00840 }
00841 
00842 void CAN_TX_VALVE_DIR(void)
00843 {
00844     CANMessage temp_msg;
00845 
00846     temp_msg.id = CID_TX_INFO;
00847     temp_msg.len = 3;
00848     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DIR;
00849     temp_msg.data[1] = (uint8_t) DIR_VALVE;
00850     temp_msg.data[2] = (uint8_t) (DIR_VALVE >> 8);
00851 
00852     can.write(temp_msg);
00853 }
00854 
00855 void CAN_TX_VALVE_ENC_DIR(void)
00856 {
00857     CANMessage temp_msg;
00858 
00859     temp_msg.id = CID_TX_INFO;
00860     temp_msg.len = 3;
00861     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_ENC_DIR;
00862     temp_msg.data[1] = (uint8_t) DIR_VALVE_ENC;
00863     temp_msg.data[2] = (uint8_t) (DIR_VALVE_ENC >> 8);
00864 
00865     can.write(temp_msg);
00866 }
00867 
00868 void CAN_TX_VOLTAGE_SUPPLY(void)
00869 {
00870     int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10.0f);
00871 
00872     CANMessage temp_msg;
00873 
00874     temp_msg.id = CID_TX_INFO;
00875     temp_msg.len = 3;
00876     temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_SUPPLY;
00877     temp_msg.data[1] = (uint8_t) (send_voltage_supply);
00878     temp_msg.data[2] = (uint8_t) (send_voltage_supply >> 8);
00879 
00880     can.write(temp_msg);
00881 }
00882 
00883 void CAN_TX_VOLTAGE_VALVE(void)
00884 {
00885     int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f);
00886 
00887     CANMessage temp_msg;
00888 
00889     temp_msg.id = CID_TX_INFO;
00890     temp_msg.len = 3;
00891     temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_VALVE;
00892     temp_msg.data[1] = (uint8_t) send_voltage_valve;
00893     temp_msg.data[2] = (uint8_t) (send_voltage_valve >> 8);
00894 
00895     can.write(temp_msg);
00896 }
00897 
00898 void CAN_TX_VARIABLE_SUPPLY_ONOFF(void)
00899 {
00900     CANMessage temp_msg;
00901     temp_msg.id = CID_TX_INFO;
00902     temp_msg.len = 2;
00903     temp_msg.data[0] = (uint8_t) CTX_SEND_VARIABLE_SUPPLY;
00904     temp_msg.data[1] = (uint8_t) SUPPLY_PRESSURE_UPDATE;
00905 
00906     can.write(temp_msg);
00907 }
00908 
00909 void CAN_TX_PID_GAIN(int t_type)
00910 {
00911     // t_type = 0 : valve position control gain
00912     // t_type = 1 : joint position control gain
00913     // t_type = 2 : joint torque control gain
00914 
00915     int16_t sendPgain=0, sendIgain=0, sendDgain=0;
00916     if (t_type == 0) {
00917         sendPgain = (int16_t) (P_GAIN_VALVE_POSITION);
00918         sendIgain = (int16_t) (I_GAIN_VALVE_POSITION);
00919         sendDgain = (int16_t) (D_GAIN_VALVE_POSITION);
00920     } else if (t_type == 1) {
00921         sendPgain = (int16_t) (P_GAIN_JOINT_POSITION);
00922         sendIgain = (int16_t) (I_GAIN_JOINT_POSITION);
00923         sendDgain = (int16_t) (D_GAIN_JOINT_POSITION);
00924     } else if (t_type == 2) {
00925         sendPgain = (int16_t) (P_GAIN_JOINT_TORQUE);
00926         sendIgain = (int16_t) (I_GAIN_JOINT_TORQUE);
00927         sendDgain = (int16_t) (D_GAIN_JOINT_TORQUE);
00928     } else if (t_type == 3) {
00929         sendPgain = (int16_t) (K_SPRING * 10.0f);
00930         sendIgain = (int16_t) (D_DAMPER * 100.0f);
00931     }
00932 
00933     CANMessage temp_msg;
00934 
00935     temp_msg.id = CID_TX_INFO;
00936     temp_msg.len = 8;
00937     temp_msg.data[0] = (uint8_t) CTX_SEND_PID_GAIN;
00938     temp_msg.data[1] = (uint8_t) t_type;
00939     temp_msg.data[2] = (uint8_t) sendPgain;
00940     temp_msg.data[3] = (uint8_t) (sendPgain >> 8);
00941     temp_msg.data[4] = (uint8_t) sendIgain;
00942     temp_msg.data[5] = (uint8_t) (sendIgain >> 8);
00943     temp_msg.data[6] = (uint8_t) sendDgain;
00944     temp_msg.data[7] = (uint8_t) (sendDgain >> 8);
00945 
00946     can.write(temp_msg);
00947 }
00948 
00949 
00950 void CAN_TX_VALVE_DEADZONE(void)
00951 {
00952     CANMessage temp_msg;
00953 
00954     temp_msg.id = CID_TX_INFO;
00955     temp_msg.len = 7;
00956     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE;
00957     temp_msg.data[1] = (uint8_t) VALVE_CENTER;
00958     temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8);
00959     temp_msg.data[3] = (uint8_t) VALVE_DEADZONE_PLUS;
00960     temp_msg.data[4] = (uint8_t) (VALVE_DEADZONE_PLUS >> 8);
00961     temp_msg.data[5] = (uint8_t) VALVE_DEADZONE_MINUS;
00962     temp_msg.data[6] = (uint8_t) (VALVE_DEADZONE_MINUS >> 8);
00963 
00964     can.write(temp_msg);
00965 }
00966 
00967 void CAN_TX_VELOCITY_COMP_GAIN(void)
00968 {
00969     CANMessage temp_msg;
00970 
00971     temp_msg.id = CID_TX_INFO;
00972     temp_msg.len = 3;
00973     temp_msg.data[0] = (uint8_t) CTX_SEND_VELOCITY_COMP_GAIN;
00974     temp_msg.data[1] = (uint8_t) VELOCITY_COMP_GAIN;
00975     temp_msg.data[2] = (uint8_t) (VELOCITY_COMP_GAIN >> 8);
00976 
00977     can.write(temp_msg);
00978 }
00979 
00980 void CAN_TX_VALVE_ELECTRIC_CENTER(void)
00981 {
00982     CANMessage temp_msg;
00983 
00984     temp_msg.id = CID_TX_INFO;
00985     temp_msg.len = 3;
00986     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_ELECTRIC_CENTER;
00987     temp_msg.data[1] = (uint8_t) VALVE_ELECTRIC_CENTER;
00988     temp_msg.data[2] = (uint8_t) (VALVE_ELECTRIC_CENTER >> 8);
00989 
00990     can.write(temp_msg);
00991 }
00992 
00993 void CAN_TX_VALVE_FF(void)
00994 {
00995     CANMessage temp_msg;
00996 
00997     temp_msg.id = CID_TX_INFO;
00998     temp_msg.len = 3;
00999     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_FF;
01000     temp_msg.data[1] = (uint8_t) VALVE_FF;
01001     temp_msg.data[2] = (uint8_t) (VALVE_FF >> 8);
01002 
01003     can.write(temp_msg);
01004 }
01005 
01006 void CAN_TX_BULK_MODULUS(void)
01007 {
01008     CANMessage temp_msg;
01009 
01010     temp_msg.id = CID_TX_INFO;
01011     temp_msg.len = 3;
01012     temp_msg.data[0] = (uint8_t) CTX_SEND_BULK_MODULUS;
01013     temp_msg.data[1] = (uint8_t) BULK_MODULUS;
01014     temp_msg.data[2] = (uint8_t) (BULK_MODULUS >> 8);
01015 
01016     can.write(temp_msg);
01017 }
01018 
01019 void CAN_TX_CHAMBER_VOLUME(void)
01020 {
01021     CANMessage temp_msg;
01022 
01023     temp_msg.id = CID_TX_INFO;
01024     temp_msg.len = 5;
01025     temp_msg.data[0] = (uint8_t) CTX_SEND_CHAMBER_VOLUME;
01026     temp_msg.data[1] = (uint8_t) CHAMBER_VOLUME_A;
01027     temp_msg.data[2] = (uint8_t) (CHAMBER_VOLUME_A >> 8);
01028     temp_msg.data[3] = (uint8_t) CHAMBER_VOLUME_B;
01029     temp_msg.data[4] = (uint8_t) (CHAMBER_VOLUME_B >> 8);
01030 
01031     can.write(temp_msg);
01032 }
01033 
01034 void CAN_TX_PISTON_AREA(void)
01035 {
01036     CANMessage temp_msg;
01037 
01038     temp_msg.id = CID_TX_INFO;
01039     temp_msg.len = 5;
01040     temp_msg.data[0] = (uint8_t) CTX_SEND_PISTON_AREA;
01041     temp_msg.data[1] = (uint8_t) PISTON_AREA_A;
01042     temp_msg.data[2] = (uint8_t) (PISTON_AREA_A >> 8);
01043     temp_msg.data[3] = (uint8_t) PISTON_AREA_B;
01044     temp_msg.data[4] = (uint8_t) (PISTON_AREA_B >> 8);
01045 
01046     can.write(temp_msg);
01047 }
01048 
01049 void CAN_TX_SUP_PRES(void)
01050 {
01051     CANMessage temp_msg;
01052 
01053     int16_t temp_PRES_SUPPLY = (int16_t) (PRES_SUPPLY);
01054     temp_msg.id = CID_TX_INFO;
01055     temp_msg.len = 5;
01056     temp_msg.data[0] = (uint8_t) CTX_SEND_SUP_PRES;
01057     temp_msg.data[1] = (uint8_t) temp_PRES_SUPPLY;
01058     temp_msg.data[2] = (uint8_t) (temp_PRES_SUPPLY >> 8);
01059     temp_msg.data[3] = 0;
01060     temp_msg.data[4] = 0;
01061 
01062     can.write(temp_msg);
01063 }
01064 
01065 void CAN_TX_ENC_LIMIT(void)
01066 {
01067     CANMessage temp_msg;
01068 
01069     temp_msg.id = CID_TX_INFO;
01070     temp_msg.len = 5;
01071     temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_LIMIT;
01072     temp_msg.data[1] = (uint8_t) ENC_LIMIT_MINUS;
01073     temp_msg.data[2] = (uint8_t) (ENC_LIMIT_MINUS >> 8);
01074     temp_msg.data[3] = (uint8_t) ENC_LIMIT_PLUS;
01075     temp_msg.data[4] = (uint8_t) (ENC_LIMIT_PLUS >> 8);
01076 
01077     can.write(temp_msg);
01078 }
01079 
01080 void CAN_TX_STROKE(void)
01081 {
01082     CANMessage temp_msg;
01083 
01084     temp_msg.id = CID_TX_INFO;
01085     temp_msg.len = 3;
01086     temp_msg.data[0] = (uint8_t) CTX_SEND_STROKE;
01087     temp_msg.data[1] = (uint8_t) STROKE;
01088     temp_msg.data[2] = (uint8_t) (STROKE >> 8);
01089 
01090     can.write(temp_msg);
01091 }
01092 
01093 void CAN_TX_VALVE_LIMIT(void)
01094 {
01095     CANMessage temp_msg;
01096 
01097     temp_msg.id = CID_TX_INFO;
01098     temp_msg.len = 5;
01099     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT;
01100     temp_msg.data[1] = (uint8_t) VALVE_MIN_POS;
01101     temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8);
01102     temp_msg.data[3] = (uint8_t) VALVE_MAX_POS;
01103     temp_msg.data[4] = (uint8_t) (VALVE_MAX_POS >> 8);
01104 
01105     can.write(temp_msg);
01106 }
01107 
01108 void CAN_TX_ENC_PULSE_PER_POSITION(void)
01109 {
01110     CANMessage temp_msg;
01111 
01112     temp_msg.id = CID_TX_INFO;
01113     temp_msg.len = 3;
01114     temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION;
01115     int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION);
01116     temp_msg.data[1] = (uint8_t) temp_enc_pulse_per_position;
01117     temp_msg.data[2] = (uint8_t) (temp_enc_pulse_per_position >> 8);
01118 
01119     can.write(temp_msg);
01120 }
01121 
01122 void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void)
01123 {
01124     CANMessage temp_msg;
01125 
01126     temp_msg.id = CID_TX_INFO;
01127     temp_msg.len = 3;
01128     temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
01129     int16_t temp_torque_sensor_pulse_per_torque = (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE * 1000.0f);
01130     temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque;
01131     temp_msg.data[2] = (uint8_t) (temp_torque_sensor_pulse_per_torque >> 8);
01132 
01133     can.write(temp_msg);
01134 }
01135 
01136 void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void)
01137 {
01138     CANMessage temp_msg;
01139 
01140     temp_msg.id = CID_TX_INFO;
01141     temp_msg.len = 5;
01142     temp_msg.data[0] = (uint8_t) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
01143     temp_msg.data[1] = (uint8_t) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f);
01144     temp_msg.data[2] = (uint8_t) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f) >> 8);
01145     temp_msg.data[3] = (uint8_t) (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f);
01146     temp_msg.data[4] = (uint8_t) ((int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f) >> 8);
01147 
01148     can.write(temp_msg);
01149 }
01150 
01151 void CAN_TX_FRICTION(void)
01152 {
01153     CANMessage temp_msg;
01154     int16_t send_friction;
01155     send_friction = (int16_t) (FRICTION * 10.0f);
01156 
01157     temp_msg.id = CID_TX_INFO;
01158     temp_msg.len = 3;
01159     temp_msg.data[0] = (uint8_t) CTX_SEND_FRICTION;
01160     temp_msg.data[1] = (uint8_t) send_friction;
01161     temp_msg.data[2] = (uint8_t) (send_friction >> 8);
01162 
01163     can.write(temp_msg);
01164 }
01165 
01166 void CAN_TX_VALVE_GAIN_PLUS(void)
01167 {
01168     CANMessage temp_msg;
01169 
01170     temp_msg.id = CID_TX_INFO;
01171     temp_msg.len = 6;
01172     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_PLUS;
01173     temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[0] * 50.0f);
01174     temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[2] * 50.0f);
01175     temp_msg.data[3] = (uint8_t) (VALVE_GAIN_LPM_PER_V[4] * 50.0f);
01176     temp_msg.data[4] = (uint8_t) (VALVE_GAIN_LPM_PER_V[6] * 50.0f);
01177     temp_msg.data[5] = (uint8_t) (VALVE_GAIN_LPM_PER_V[8] * 50.0f);
01178 
01179     can.write(temp_msg);
01180 }
01181 
01182 
01183 void CAN_TX_VALVE_GAIN_MINUS(void)
01184 {
01185     CANMessage temp_msg;
01186 
01187     temp_msg.id = CID_TX_INFO;
01188     temp_msg.len = 6;
01189     temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_MINUS;
01190     temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[1] * 50.0f);
01191     temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[3] * 50.0f);
01192     temp_msg.data[3] = (uint8_t) (VALVE_GAIN_LPM_PER_V[5] * 50.0f);
01193     temp_msg.data[4] = (uint8_t) (VALVE_GAIN_LPM_PER_V[7] * 50.0f);
01194     temp_msg.data[5] = (uint8_t) (VALVE_GAIN_LPM_PER_V[9] * 50.0f);
01195 
01196     can.write(temp_msg);
01197 }
01198 
01199 void CAN_TX_REFENCE_MODE(void)
01200 {
01201     CANMessage temp_msg;
01202 
01203     temp_msg.id = CID_TX_INFO;
01204     temp_msg.len = 6;
01205     temp_msg.data[0] = (uint8_t) CTX_SEND_REFENCE_MODE;
01206     temp_msg.data[1] = (uint8_t) REFERENCE_MODE;
01207     temp_msg.data[2] = (uint8_t) (int) (REFERENCE_FREQ * 100.0f);
01208     temp_msg.data[3] = (uint8_t) ((int) (REFERENCE_FREQ * 100.0f) >> 8);
01209     temp_msg.data[4] = (uint8_t) (int) (REFERENCE_MAG * 100.0f);
01210     temp_msg.data[5] = (uint8_t) ((int) (REFERENCE_MAG * 100.0f) >> 8);
01211 
01212     can.write(temp_msg);
01213 }
01214 
01215 void CAN_TX_HOMEPOS_OFFSET(void)
01216 {
01217     CANMessage temp_msg;
01218     int16_t send_homepos_offset;
01219     send_homepos_offset = (int16_t)HOMEPOS_OFFSET;
01220 
01221     temp_msg.id = CID_TX_INFO;
01222     temp_msg.len = 3;
01223     temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_OFFSET;
01224     temp_msg.data[1] = (uint8_t) send_homepos_offset;
01225     temp_msg.data[2] = (uint8_t) (send_homepos_offset >> 8);
01226 
01227     can.write(temp_msg);
01228 }
01229 
01230 void CAN_TX_HOMPOS_VALVE_OPENING(void)
01231 {
01232     CANMessage temp_msg;
01233     int16_t send_homepos_valve_opening;
01234     send_homepos_valve_opening = (int16_t) (HOMEPOS_VALVE_OPENING);
01235 
01236     temp_msg.id = CID_TX_INFO;
01237     temp_msg.len = 3;
01238     temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_VALVE_OPENING;
01239     temp_msg.data[1] = (uint8_t) send_homepos_valve_opening;
01240     temp_msg.data[2] = (uint8_t) (send_homepos_valve_opening >> 8);
01241 
01242     can.write(temp_msg);
01243 }
01244 
01245 void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex)
01246 {
01247     CANMessage temp_msg;
01248     int16_t valve_pos_vs_pwm;
01249 
01250     if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_ELECTRIC_CENTER) {
01251         valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER);
01252     } else {
01253         valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER);
01254     }
01255 
01256     int16_t PWM_VALVE_ID;
01257     PWM_VALVE_ID = ID_index_array[canindex] * 1000;
01258 
01259     temp_msg.id = CID_TX_INFO;
01260     temp_msg.len = 5;
01261     temp_msg.data[0] = (uint8_t) CTX_VALVE_PWM_VS_VALVE_POS;
01262     temp_msg.data[1] = (uint8_t) PWM_VALVE_ID;
01263     temp_msg.data[2] = (uint8_t) (PWM_VALVE_ID >> 8);
01264     temp_msg.data[3] = (uint8_t) valve_pos_vs_pwm;
01265     temp_msg.data[4] = (uint8_t) (valve_pos_vs_pwm >> 8);
01266 
01267     can.write(temp_msg);
01268 }
01269 
01270 void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t canindex)
01271 {
01272     CANMessage temp_msg;
01273     int32_t valve_pos_vs_flowrate;
01274     valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
01275 
01276     int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + VALVE_CENTER;
01277     int16_t temp_valve_pos = 0;
01278     if(VALVE_POS_VALVE_ID>=VALVE_CENTER) {
01279         temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER));
01280     } else {
01281         temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER));
01282     }
01283 
01284     temp_msg.id = CID_TX_INFO;
01285     temp_msg.len = 7;
01286     temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE;
01287     temp_msg.data[1] = (uint8_t) temp_valve_pos;
01288     temp_msg.data[2] = (uint8_t) (temp_valve_pos >> 8);
01289     temp_msg.data[3] = (uint8_t) valve_pos_vs_flowrate;
01290     temp_msg.data[4] = (uint8_t) (valve_pos_vs_flowrate >> 8);
01291     temp_msg.data[5] = (uint8_t) (valve_pos_vs_flowrate >> 16);
01292     temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 24);
01293 
01294     can.write(temp_msg);
01295 }
01296 
01297 void CAN_TX_VALVE_POS_NUM(void)
01298 {
01299     CANMessage temp_msg;
01300     int32_t valve_pos_num;
01301     valve_pos_num = (int16_t) VALVE_POS_NUM;
01302 
01303     temp_msg.id = CID_TX_INFO;
01304     temp_msg.len = 3;
01305     temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_NUM;
01306     temp_msg.data[1] = (uint8_t) valve_pos_num;
01307     temp_msg.data[2] = (uint8_t) (valve_pos_num >> 8);
01308 
01309     can.write(temp_msg);
01310 }
01311 
01312 void CAN_TX_DDV_VALVE_MAX_MIN_POS(void)
01313 {
01314     CANMessage temp_msg;
01315 
01316     float temp_valve_max_pos = 0.0f;
01317     float temp_valve_min_pos = 0.0f;
01318     float temp_ddv_center = 0.0f;
01319 
01320     temp_valve_max_pos = VALVE_MAX_POS;
01321     temp_valve_min_pos = VALVE_MIN_POS;
01322     temp_ddv_center = VALVE_CENTER;
01323 
01324     temp_msg.id = CID_TX_INFO;
01325     temp_msg.len = 7;
01326     temp_msg.data[0] = (uint8_t) CTX_VALVE_MAX_MIN_POS;
01327     temp_msg.data[1] = (uint8_t) temp_valve_max_pos;
01328     temp_msg.data[2] = (uint8_t) ((int) (temp_valve_max_pos) >> 8);
01329     temp_msg.data[3] = (uint8_t) (temp_valve_min_pos);
01330     temp_msg.data[4] = (uint8_t) ((int) (temp_valve_min_pos) >> 8);
01331     temp_msg.data[5] = (uint8_t) (temp_ddv_center);
01332     temp_msg.data[6] = (uint8_t) ((int) (temp_ddv_center) >> 8);
01333 
01334     can.write(temp_msg);
01335 }
01336 
01337 
01338 void CAN_TX_BUFFER(int16_t t_cnt_buffer)
01339 {
01340     CANMessage temp_msg;
01341     int16_t send_pos_array, send_ref_array;
01342     send_pos_array = (int16_t) (pos_array[t_cnt_buffer]);
01343     send_ref_array = (int16_t) (ref_array[t_cnt_buffer]);
01344 
01345     temp_msg.id = CID_TX_INFO;
01346     temp_msg.len = 5;
01347     temp_msg.data[0] = (uint8_t) CTX_SEND_BUFFER;
01348     temp_msg.data[1] = (uint8_t) send_pos_array;
01349     temp_msg.data[2] = (uint8_t) (send_pos_array >> 8);
01350     temp_msg.data[3] = (uint8_t) (send_ref_array);
01351     temp_msg.data[4] = (uint8_t) ((send_ref_array) >> 8);
01352 
01353     can.write(temp_msg);
01354 }
01355 
01356 
01357 /******************************************************************************
01358  Sensor & State Transmission Functions
01359 *******************************************************************************/
01360 
01361 void CAN_TX_POSITION_FT(int16_t t_pos, int16_t t_vel, int16_t t_torq)
01362 {
01363     CANMessage temp_msg;
01364 
01365     temp_msg.id = CID_TX_POS_VEL_TORQ;
01366     temp_msg.len = 6;
01367     temp_msg.data[0] = (uint8_t) t_pos;
01368     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
01369     temp_msg.data[2] = (uint8_t) t_vel;
01370     temp_msg.data[3] = (uint8_t) (t_vel >> 8);
01371     temp_msg.data[4] = (uint8_t) t_torq;
01372     temp_msg.data[5] = (uint8_t) (t_torq >> 8);
01373 
01374     can.write(temp_msg);
01375 }
01376 
01377 void CAN_TX_POSITION_PRESSURE(int16_t t_pos, int16_t t_vel, int16_t t_pa, int16_t t_pb)
01378 {
01379 
01380     CANMessage temp_msg;
01381 
01382     temp_msg.id = CID_TX_POS_VEL_TORQ;
01383     temp_msg.len = 8;
01384     temp_msg.data[0] = (uint8_t) t_pos;
01385     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
01386     temp_msg.data[2] = (uint8_t) t_vel;
01387     temp_msg.data[3] = (uint8_t) (t_vel >> 8);
01388     temp_msg.data[4] = (uint8_t) t_pa;
01389     temp_msg.data[5] = (uint8_t) (t_pa >> 8);
01390     temp_msg.data[6] = (uint8_t) t_pb;
01391     temp_msg.data[7] = (uint8_t) (t_pb >> 8);
01392 
01393     can.write(temp_msg);
01394 }
01395 
01396 void CAN_TX_CURRENT(int16_t t_pres_a, int16_t t_pres_b)
01397 {
01398     CANMessage temp_msg;
01399 
01400     temp_msg.id = CID_TX_CURRENT;
01401     temp_msg.len = 4;
01402     temp_msg.data[0] = (uint8_t) t_pres_a;
01403     temp_msg.data[1] = (uint8_t) (t_pres_a >> 8);
01404     temp_msg.data[2] = (uint8_t) t_pres_b;
01405     temp_msg.data[3] = (uint8_t) (t_pres_b >> 8);
01406 
01407     can.write(temp_msg);
01408 }
01409 
01410 void CAN_TX_TORQUE(int16_t t_valve_pos)
01411 {
01412     CANMessage temp_msg;
01413 
01414     temp_msg.id = CID_TX_PWM;
01415     temp_msg.len = 2;
01416     temp_msg.data[0] = (uint8_t) t_valve_pos;
01417     temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8);
01418 
01419     can.write(temp_msg);
01420 }
01421 
01422 void CAN_TX_PWM(int16_t t_pwm)
01423 {
01424     CANMessage temp_msg;
01425 
01426     temp_msg.id = CID_TX_PWM;
01427     temp_msg.len = 2;
01428     temp_msg.data[0] = (uint8_t) t_pwm;
01429     temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
01430 
01431     can.write(temp_msg);
01432 }
01433 
01434 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)
01435 {
01436     CANMessage temp_msg;
01437 
01438     temp_msg.id = CID_TX_VALVE_POSITION;
01439     temp_msg.len = 8;
01440     temp_msg.data[0] = (uint8_t) t_valve_pos_1;
01441     temp_msg.data[1] = (uint8_t) (t_valve_pos_1 >> 8);
01442     temp_msg.data[2] = (uint8_t) t_valve_pos_2;
01443     temp_msg.data[3] = (uint8_t) (t_valve_pos_2 >> 8);
01444     temp_msg.data[4] = (uint8_t) t_ref_valve_pos;
01445     temp_msg.data[5] = (uint8_t) (t_ref_valve_pos >> 8);
01446     temp_msg.data[6] = (uint8_t) t_pwm;
01447     temp_msg.data[7] = (uint8_t) (t_pwm >> 8);
01448 
01449 
01450     can.write(temp_msg);
01451 }
01452 
01453 void CAN_TX_SOMETHING(int16_t t_a, int16_t t_b, int16_t t_c, int16_t t_d)
01454 {
01455     CANMessage temp_msg;
01456 
01457     temp_msg.id = CID_TX_SOMETHING;
01458     temp_msg.len = 8;
01459     temp_msg.data[0] = (uint8_t) t_a;
01460     temp_msg.data[1] = (uint8_t) (t_a >> 8);
01461     temp_msg.data[2] = (uint8_t) t_b;
01462     temp_msg.data[3] = (uint8_t) (t_b >> 8);
01463     temp_msg.data[4] = (uint8_t) t_c;
01464     temp_msg.data[5] = (uint8_t) (t_c >> 8);
01465     temp_msg.data[6] = (uint8_t) t_d;
01466     temp_msg.data[7] = (uint8_t) (t_d >> 8);
01467 
01468 
01469     can.write(temp_msg);
01470 }
01471 
01472 void CAN_TX_VOUT(int16_t t_vout)
01473 {
01474     CANMessage temp_msg;
01475 
01476     temp_msg.id = CID_TX_VOUT;
01477     temp_msg.len = 2;
01478     temp_msg.data[0] = (uint8_t) t_vout;
01479     temp_msg.data[1] = (uint8_t) (t_vout >> 8);
01480 
01481     can.write(temp_msg);
01482 }