Sungwoo Kim
Original Version of STM Board
Diff: CAN/function_CAN.cpp
- Revision:
- 224:985dba42f261
- Parent:
- 209:ebc69d6ee6f1
- Child:
- 225:9c0becc196ba
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)
{