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