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