![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
for learning
Diff: CAN/function_CAN.cpp
- Revision:
- 224:985dba42f261
- Parent:
- 209:ebc69d6ee6f1
diff -r e26830fbaffd -r 985dba42f261 CAN/function_CAN.cpp --- a/CAN/function_CAN.cpp Mon Dec 28 12:10:50 2020 +0000 +++ b/CAN/function_CAN.cpp Mon Dec 28 14:27:11 2020 +0000 @@ -62,8 +62,6 @@ } case CRX_SET_BNO: { BNO = (int16_t) msg.data[1]; - - //ROM_RESET_DATA(); spi_eeprom_write(RID_BNO, (int16_t) BNO); CAN_ID_INIT(); // can id init break; @@ -77,7 +75,6 @@ SENSING_MODE = (uint8_t) msg.data[2]; CURRENT_CONTROL_MODE = (uint8_t) msg.data[3]; FLAG_VALVE_DEADZONE = (uint8_t) msg.data[4]; -// ROM_RESET_DATA(); spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE); spi_eeprom_write(RID_SENSING_MODE, (int16_t) SENSING_MODE); spi_eeprom_write(RID_CURRENT_CONTROL_MODE, (int16_t) CURRENT_CONTROL_MODE); @@ -113,7 +110,6 @@ case CRX_SET_CAN_FREQ: { CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8); - ROM_RESET_DATA(); spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ); break; } @@ -125,7 +121,6 @@ } case CRX_SET_CONTROL_MODE: { - //CONTROL_MODE = (int16_t) (msg.data[1]); CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]); if (CONTROL_MODE == 22) { //MODE_FIND_HOME FLAG_FIND_HOME = true; @@ -136,7 +131,6 @@ case CRX_SET_DATA_REQUEST: { int request_type = msg.data[2]; flag_data_request[request_type] = msg.data[1]; - //pc.printf("can middle %d\n", request_type); // if (flag_data_request[1] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC //if (flag_data_request[1] == HIGH) dac_1 = PRES_A_VREF/3.3; @@ -159,8 +153,6 @@ else DIR_JOINT_ENC = -1; -// ROM_RESET_DATA(); - spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC); break; @@ -179,8 +171,6 @@ else DIR_VALVE = -1; -// ROM_RESET_DATA(); - spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE); break; @@ -199,8 +189,6 @@ else DIR_VALVE_ENC = -1; -// ROM_RESET_DATA(); - spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC); break; @@ -215,7 +203,6 @@ case CRX_SET_VOLTAGE_SUPPLY: { SUPPLY_VOLTAGE = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f; -// ROM_RESET_DATA(); spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.0f)); @@ -231,8 +218,6 @@ case CRX_SET_VOLTAGE_VALVE: { VALVE_VOLTAGE_LIMIT = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f; -// ROM_RESET_DATA(); - spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f)); @@ -240,7 +225,6 @@ } case CRX_SET_HOMEPOS: { - //CONTROL_MODE = 22; CONTROL_UTILITY_MODE = 22; break; } @@ -257,7 +241,6 @@ I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8); D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION); spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION); spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION); @@ -267,7 +250,6 @@ I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8); D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION); spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION); spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION); @@ -277,7 +259,6 @@ I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8); D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE); spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE); spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE); @@ -286,7 +267,6 @@ K_SPRING = (float) (((float) ((int16_t) (msg.data[2] | msg.data[3] << 8))) * 0.1f); D_DAMPER = (float) (((float) ((int16_t) (msg.data[4] | msg.data[5] << 8))) * 0.01f); -// ROM_RESET_DATA(); //For Real-time changing // spi_eeprom_write(RID_K_SPRING, (int16_t) K_SPRING); // spi_eeprom_write(RID_D_DAMPER, (int16_t) D_DAMPER); @@ -295,7 +275,6 @@ I_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[4] | msg.data[5] << 8); D_GAIN_JOINT_TORQUE_FF = (int16_t) (msg.data[6] | msg.data[7] << 8); -// ROM_RESET_DATA(); } break; @@ -312,7 +291,6 @@ VALVE_DEADZONE_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8); VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER); spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f)); spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f)); @@ -329,7 +307,6 @@ case CRX_SET_VELOCITY_COMP_GAIN: { VELOCITY_COMP_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN); @@ -345,7 +322,6 @@ case CRX_SET_COMPLIANCE_GAIN: { COMPLIANCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN); @@ -361,8 +337,6 @@ case CRX_SET_VALVE_FF: { VALVE_FF = (int16_t) (msg.data[1] | msg.data[2] << 8); -// ROM_RESET_DATA(); - spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF); break; @@ -376,9 +350,6 @@ case CRX_SET_BULK_MODULUS: { BULK_MODULUS = (int16_t) (msg.data[1] | msg.data[2] << 8); - -// ROM_RESET_DATA(); - spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS); break; @@ -394,8 +365,6 @@ CHAMBER_VOLUME_A = (int16_t) (msg.data[1] | msg.data[2] << 8); CHAMBER_VOLUME_B = (int16_t) (msg.data[3] | msg.data[4] << 8); -// ROM_RESET_DATA(); - spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A); spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B); @@ -414,8 +383,6 @@ PISTON_AREA_alpha = (double)PISTON_AREA_A/(double)PISTON_AREA_B; alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha; -// ROM_RESET_DATA(); - spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A); spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B); break; @@ -433,7 +400,6 @@ case CRX_SET_PRES: { PRES_SUPPLY = (int16_t) (msg.data[1] | msg.data[2] << 8); PRES_RETURN = (int16_t) (msg.data[3] | msg.data[4] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY); spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN); @@ -450,7 +416,6 @@ case CRX_SET_ENC_LIMIT: { ENC_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8); ENC_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS); spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS); @@ -464,7 +429,6 @@ case CRX_SET_STROKE: { STROKE = (int16_t) (msg.data[1] | msg.data[2] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_STROKE, (int16_t) STROKE); break; @@ -479,7 +443,6 @@ case CRX_SET_VALVE_LIMIT: { VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8); VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS); spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS); @@ -495,7 +458,6 @@ case CRX_SET_ENC_PULSE_PER_POSITION: { ENC_PULSE_PER_POSITION = (int16_t) (msg.data[1] | msg.data[2] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) (ENC_PULSE_PER_POSITION)); break; @@ -509,7 +471,6 @@ case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: { TORQUE_SENSOR_PULSE_PER_TORQUE = (float) ((int16_t) (msg.data[1] | msg.data[2] << 8) * 0.0001f); -// ROM_RESET_DATA(); spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) (TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f)); break; @@ -524,7 +485,6 @@ case CRX_SET_PRES_SENSOR_PULSE_PER_PRES: { PRES_SENSOR_A_PULSE_PER_BAR = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) * 0.01f; PRES_SENSOR_B_PULSE_PER_BAR = (double) ((int16_t) (msg.data[3] | msg.data[4] << 8)) * 0.01f; -// ROM_RESET_DATA(); spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f)); spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f)); @@ -539,7 +499,6 @@ case CRX_SET_FRICTION: { FRICTION = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.0f; -// ROM_RESET_DATA(); spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.0f)); break; @@ -556,7 +515,6 @@ VALVE_GAIN_LPM_PER_V[4] = (double) msg.data[3] / 50.0f; VALVE_GAIN_LPM_PER_V[6] = (double) msg.data[4] / 50.0f; VALVE_GAIN_LPM_PER_V[8] = (double) msg.data[5] / 50.0f; -// ROM_RESET_DATA(); spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.0f)); spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.0f)); spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.0f)); @@ -577,7 +535,6 @@ VALVE_GAIN_LPM_PER_V[5] = (double) msg.data[3] / 50.0f; VALVE_GAIN_LPM_PER_V[7] = (double) msg.data[4] / 50.0f; VALVE_GAIN_LPM_PER_V[9] = (double) msg.data[5] / 50.0f; -// ROM_RESET_DATA(); spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.0f)); spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.0f)); spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.0f)); @@ -628,7 +585,6 @@ } case CRX_SET_HOMEPOS_OFFSET: { HOMEPOS_OFFSET = (int16_t) (msg.data[1] | msg.data[2] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET); break; } @@ -639,7 +595,6 @@ } case CRX_SET_HOMEPOS_VALVE_OPENING: { HOMEPOS_VALVE_OPENING = (int16_t) (msg.data[1] | msg.data[2] << 8); -// ROM_RESET_DATA(); spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING); break; } @@ -660,13 +615,11 @@ } case CRX_SET_ROM: { - ROM_RESET_DATA(); break; } case CRX_SET_VALVE_CENTER_OFFSET: { VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f; VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET; -// ROM_RESET_DATA(); spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER); break; @@ -674,7 +627,6 @@ case CRX_SET_VALVE_DZ_MINUS_OFFSET: { VALVE_DZ_MINUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f; VALVE_DEADZONE_MINUS = VALVE_DEADZONE_MINUS + VALVE_DZ_MINUS_OFFSET; -// ROM_RESET_DATA(); spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f)); break; @@ -682,7 +634,6 @@ case CRX_SET_VALVE_DZ_PLUS_OFFSET: { VALVE_DZ_PLUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f; VALVE_DEADZONE_PLUS = VALVE_DEADZONE_PLUS + VALVE_DZ_PLUS_OFFSET; -// ROM_RESET_DATA(); spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f)); break; } @@ -691,21 +642,13 @@ P_GAIN_VALVE_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8); I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8); D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8); - -// ROM_RESET_DATA(); - //spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION); //spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION); //spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION); - - } else if (msg.data[1] == 1) { P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8); I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8); D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8); - -// ROM_RESET_DATA(); - //spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION); //spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION); //spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION); @@ -713,9 +656,6 @@ P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8); I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8); D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8); - -// ROM_RESET_DATA(); - //spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE); //spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE); //spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE); @@ -723,7 +663,6 @@ break; } case CRX_ASK_VALVE_MAX_MIN_POS: { - //CAN_TX_DDV_VALVE_DEADZONE(); CAN_TX_DDV_VALVE_MAX_MIN_POS(); break; } @@ -789,44 +728,9 @@ torq.ref = (double)temp_torq * 0.1f / TORQUE_SENSOR_PULSE_PER_TORQUE; //N torq.ref_diff = torq.ref - torq.ref_old; - torq_dot.sen = torq.sen-totq_sen_past; + torq_dot.sen = torq.sen-torq_sen_past; torq.ref_old = torq.ref; - totq_sen_past = torq.sen; - - ///////////////Make Data/////////////////// - for(int i=0; i<num_array_u_past-1; i++) { - u_past[i] = u_past[i+1]; - } - u_past[num_array_u_past-1] = output_normalized; - - for(int i=0; i<num_array_x_past-1; i++) { - x_past[i] = x_past[i+1]; - } - x_past[num_array_x_past-1] = pos.sen / ENC_PULSE_PER_POSITION; //mm - -// for(int i=0;i<num_array_x_future-1;i++){ -// x_future[i] = x_future[i+1]; -// } -// x_future[num_array_x_future-1] = pos.sen / ENC_PULSE_PER_POSITION; //mm - - for(int i=0; i<num_array_f_past-1; i++) { - f_past[i] = f_past[i+1]; - } - f_past[num_array_f_past-1] = torq.sen; //N - - f_future[0] = torq.sen; //N - - if(flag_every_reference == 1) { - for(int i=1; i<num_array_f_future-1; i++) { - f_future[i] = 500.0f; - } - f_future[num_array_f_future-1] = 500.0f; //N - } else { - for(int i=1; i<num_array_f_future-1; i++) { - f_future[i] = f_future[i+1]; - } - f_future[num_array_f_future-1] = torq.ref; //N - } + torq_sen_past = torq.sen; } else if(address==CID_RX_REF_VALVE_POS) { @@ -1271,8 +1175,6 @@ float temp_valve_deadzone_plus = 0.0f; float temp_ddv_center = 0.0f; - //temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER); - //temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER); temp_valve_deadzone_plus = (double)VALVE_DEADZONE_PLUS; temp_valve_deadzone_minus = (double)VALVE_DEADZONE_MINUS; temp_ddv_center = (double)VALVE_CENTER; @@ -1356,7 +1258,6 @@ { CANMessage temp_msg; int16_t valve_pos_vs_pwm; -// valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]); if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) { valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER); @@ -1504,18 +1405,6 @@ can.write(temp_msg); } -//void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout) { -// CANMessage temp_msg; -// -// temp_msg.id = CID_TX_TORQUE; -// temp_msg.len = 4; -// temp_msg.data[0] = (uint8_t) t_valve_pos; -// temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8); -// temp_msg.data[2] = (uint8_t) t_vout; -// temp_msg.data[3] = (uint8_t) (t_vout >> 8); -// -// can.write(temp_msg); -//} void CAN_TX_TORQUE(int16_t t_valve_pos) {