Sungwoo Kim
2011
Diff: main.cpp
- Revision:
- 33:91b17819ec30
- Parent:
- 32:4b8c0fedaf2c
- Child:
- 34:bb2ca2fc2a8e
diff -r 4b8c0fedaf2c -r 91b17819ec30 main.cpp
--- a/main.cpp Mon Nov 11 09:48:00 2019 +0000
+++ b/main.cpp Tue Nov 12 11:29:36 2019 +0000
@@ -238,28 +238,28 @@
}
}
-int DDV_JOINT_POS_FF(float REF_JOINT_VEL)
+float DDV_JOINT_POS_FF(float REF_JOINT_VEL)
{
int i = 0;
- int Ref_Valve_Pos_FF = DDV_CENTER;
+ float Ref_Valve_Pos_FF = DDV_CENTER;
for(i=0; i<VALVE_POS_NUM; i++) {
if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) {
if(i==0) {
- Ref_Valve_Pos_FF = ((int) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
+ Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
} else {
- Ref_Valve_Pos_FF = ((int) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + 10*ID_index_array[i-1];
+ Ref_Valve_Pos_FF = ((float) 10*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + (float) (10*ID_index_array[i-1]);
}
break;
}
}
if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
- Ref_Valve_Pos_FF = VALVE_MAX_POS;
+ Ref_Valve_Pos_FF = (float) VALVE_MAX_POS;
} else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
- Ref_Valve_Pos_FF = VALVE_MIN_POS;
+ Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
}
- Ref_Valve_Pos_FF = (int) ( (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + (float) DDV_CENTER);
+ Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER) + DDV_CENTER;
return Ref_Valve_Pos_FF;
}
@@ -535,7 +535,7 @@
// VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
VALVE_POS_RAW_FORCE_FB = 0.0f;
- valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB + VALVE_CENTER_OFFSET; // valve center offset = - 2.0f;
+ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
//valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
if (valve_pos.ref > DDV_CENTER)
@@ -1402,17 +1402,13 @@
DZ_index= DZ_index *2;
if(DZ_index >= 128) {
SECOND_DZ = valve_pos.ref;
- DDV_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
+ DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ);
first_check = 0;
- VALVE_DEADZONE_MINUS = FIRST_DZ;
- VALVE_DEADZONE_PLUS = SECOND_DZ;
+ VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
+ VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
- //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
- //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
- //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
+
CONTROL_MODE = MODE_NO_ACT;
DZ_index = 1;
}
@@ -1501,17 +1497,11 @@
DZ_index= DZ_index *2;
if(DZ_index >= 128) {
SECOND_DZ = valve_pos.ref;
- DDV_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
+ DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ);
first_check = 0;
- VALVE_DEADZONE_MINUS = FIRST_DZ;
- //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
- VALVE_DEADZONE_PLUS = SECOND_DZ;
+ VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
+ VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
- //ROM_RESET_DATA();
-
- //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
- //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
- //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
CONTROL_MODE = MODE_NO_ACT;
DZ_index = 1;
}
@@ -1551,12 +1541,12 @@
V_out = (float) P_GAIN_JOINT_POSITION * 0.01f* (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
} else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
data_num = 0;
- valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER;
+ valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
START_POS = pos.sen;
} else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
- valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER;
+ valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
VALVE_POS_CONTROL(valve_pos.ref);
data_num = data_num + 1;
if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) {
@@ -1717,9 +1707,7 @@
if (flag_data_request[4] == HIGH) {
//valve position
CAN_TX_VALVE_POSITION((int16_t) (value), (int16_t) valve_pos.ref, (int16_t) V_out);
- //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
- //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
-
+ //CAN_TX_VALVE_POSITION((int16_t) (DDV_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
//CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
// CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));