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