Yeseong Jeong
/
HydraulicControlBoard_Start
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 38:118df027d851, committed 2019-11-28
- Comitter:
- Lightvalve
- Date:
- Thu Nov 28 09:18:03 2019 +0000
- Parent:
- 37:60a116154383
- Child:
- 39:e961db3b93f8
- Child:
- 40:3f2c0619c8c4
- Commit message:
- 191128
Changed in this revision
--- a/CAN/function_CAN.cpp Mon Nov 25 05:04:12 2019 +0000
+++ b/CAN/function_CAN.cpp Thu Nov 28 09:18:03 2019 +0000
@@ -693,9 +693,14 @@
//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);
}
-
break;
}
+ case CRX_ASK_VALVE_MAX_MIN_POS: {
+ CAN_TX_DDV_VALVE_DEADZONE();
+ CAN_TX_DDV_VALVE_MAX_MIN_POS();
+ break;
+ }
+
default:
break;
}
@@ -1240,6 +1245,22 @@
can.write(temp_msg);
}
+void CAN_TX_DDV_VALVE_MAX_MIN_POS(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 7;
+ temp_msg.data[0] = (uint8_t) CTX_VALVE_MAX_MIN_POS;
+ temp_msg.data[1] = (uint8_t) VALVE_MAX_POS;
+ temp_msg.data[2] = (uint8_t) ((int) (VALVE_MAX_POS) >> 8);
+ temp_msg.data[3] = (uint8_t) (VALVE_MIN_POS);
+ temp_msg.data[4] = (uint8_t) ((int) (VALVE_MIN_POS) >> 8);
+ temp_msg.data[5] = (uint8_t) (DDV_CENTER);
+ temp_msg.data[6] = (uint8_t) ((int) (DDV_CENTER) >> 8);
+
+ can.write(temp_msg);
+}
+
/******************************************************************************
Sensor & State Transmission Functions
*******************************************************************************/
@@ -1315,3 +1336,4 @@
+
--- a/CAN/function_CAN.h Mon Nov 25 05:04:12 2019 +0000
+++ b/CAN/function_CAN.h Thu Nov 28 09:18:03 2019 +0000
@@ -94,6 +94,7 @@
#define CRX_ASK_VALVE_PWM_VS_VALVE_POS 43
#define CRX_ASK_VALVE_POS_VS_FLOWRATE 44
#define CRX_ASK_VALVE_POS_NUM 45
+#define CRX_ASK_VALVE_MAX_MIN_POS 46
#define CRX_SET_ERR_CLEAR 150
#define CRX_SET_ROM 146
#define CRX_SET_VALVE_CENTER_OFFSET 147
@@ -140,7 +141,7 @@
#define CTX_VALVE_PWM_VS_VALVE_POS 43
#define CTX_VALVE_POS_VS_FLOWRATE 44
#define CTX_VALVE_POS_NUM 45
-
+#define CTX_VALVE_MAX_MIN_POS 46
// Sensor & State Transmission
void CAN_TX_POSITION(int32_t t_pos, int32_t t_vel);
void CAN_TX_TORQUE(int16_t t_torque);
@@ -184,6 +185,7 @@
void CAN_TX_DDV_VALVE_DEADZONE(void);
void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t canindex);
void CAN_TX_VALVE_POS_NUM(void);
+void CAN_TX_DDV_VALVE_MAX_MIN_POS(void);
class State
{
--- a/function_utilities/function_utilities.cpp Mon Nov 25 05:04:12 2019 +0000 +++ b/function_utilities/function_utilities.cpp Thu Nov 28 09:18:03 2019 +0000 @@ -223,7 +223,7 @@ float CUR_PRES_B_mean = 0.0f; float CUR_TORQUE_sum = 0.0f; float CUR_TORQUE_mean = 0.0f; -float PRES_A_NULL = 1.0f; +float PRES_A_NULL = 2048.0f; float PRES_B_NULL = 1.0f; float TORQUE_NULL = 3900.0f;
--- a/main.cpp Mon Nov 25 05:04:12 2019 +0000
+++ b/main.cpp Thu Nov 28 09:18:03 2019 +0000
@@ -206,15 +206,15 @@
make_delay();
//DAC init
- dac_1 = PRES_A_VREF / 3.3;
- dac_2 = PRES_B_VREF / 3.3;
+ dac_1 = PRES_A_VREF / 3.3f;
+ dac_2 = PRES_B_VREF / 3.3f;
make_delay();
for (int i=0; i<50; i++) {
if(i%2==0)
- ID_index_array[i] = - i * 0.5;
+ ID_index_array[i] = - i * 0.5f;
else
- ID_index_array[i] = (i+1) * 0.5;
+ ID_index_array[i] = (i+1) * 0.5f;
}
/************************************
@@ -246,9 +246,17 @@
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 = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
+ if(JOINT_VEL[i+1] == JOINT_VEL[i]){
+ Ref_Valve_Pos_FF = DDV_CENTER;
+ }else{
+ 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 = ((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]);
+ if(JOINT_VEL[i+1] == JOINT_VEL[i-1]){
+ Ref_Valve_Pos_FF = DDV_CENTER;
+ }else{
+ 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;
}
@@ -271,6 +279,12 @@
{
int i = 0;
+ if(REF_VALVE_POS > VALVE_MAX_POS) {
+ REF_VALVE_POS = VALVE_MAX_POS;
+ } else if(REF_VALVE_POS < VALVE_MIN_POS) {
+ REF_VALVE_POS = VALVE_MIN_POS;
+ }
+
valve_pos_err = REF_VALVE_POS - value;
valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
valve_pos_err_old = valve_pos_err;
@@ -280,11 +294,7 @@
VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff;
- if(REF_VALVE_POS > VALVE_MAX_POS) {
- REF_VALVE_POS = VALVE_MAX_POS;
- } else if(REF_VALVE_POS < VALVE_MIN_POS) {
- REF_VALVE_POS = VALVE_MIN_POS;
- }
+
// if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0])
@@ -399,16 +409,17 @@
ADC1->CR2 |= 0x40000000; // adc _ 12bit
//while((ADC1->SR & 0b10));
float alpha_update_pres_A = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
- float pres_A_new = ((float)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR;
+// float pres_A_new = ((float)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR;
+ float pres_A_new = ((float)ADC1->DR);
pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+ torq.sen = - (pres_A.sen-2048.0f)*10000.0f / 2048.0f; //N
+
//Pressure sensor 1B
- //ADC2->CR2 |= 0x40000000; // adc _ 12bit
- //while((ADC2->SR & 0b10));
- float alpha_update_pres_B = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
- float pres_B_new = ((float)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR;
- pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
- torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
+ //float alpha_update_pres_B = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
+ //float pres_B_new = ((float)ADC2->DR);
+ //pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+ //torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
@@ -526,19 +537,17 @@
// if(Ref_Joint_Vel_Act > 0) K_ff = 0.75f; // close
// VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50f;
- // torque feedback
- // torq.err = torq.ref - torq.sen;
- // torq.err_diff = torq.err - torq.err_old;
- // torq.err_old = torq.err;
- // torq.err_sum += torq.err;
- // if (torq.err_sum > 1000) torq.err_sum = 1000;
- // if (torq.err_sum<-1000) torq.err_sum = -1000;
- // VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
- // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
- VALVE_POS_RAW_FORCE_FB = 0.0f;
+ //torque feedback
+ torq.err = - torq.ref + torq.sen;
+ torq.err_diff = torq.err - torq.err_old;
+ torq.err_old = torq.err;
+ torq.err_sum += torq.err;
+ if (torq.err_sum > 1000) torq.err_sum = 1000;
+ if (torq.err_sum<-1000) torq.err_sum = -1000;
+ VALVE_POS_RAW_FORCE_FB = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+ VALVE_POS_RAW_FORCE_FB = VALVE_POS_RAW_FORCE_FB * 0.01f;
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) {
valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
@@ -1116,9 +1125,9 @@
CUR_PRES_A_sum = 0;
CUR_PRES_B_sum = 0;
- float VREF_NullingGain = 0.003f;
- PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
- PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
+ float VREF_NullingGain = 0.0003f;
+ PRES_A_VREF = PRES_A_VREF - VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
+ PRES_B_VREF = PRES_B_VREF - VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
@@ -1685,8 +1694,8 @@
if (flag_data_request[2] == HIGH) {
//pressure A and B
- //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
- CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+ CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF * 1000.0)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+ //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
// CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
// CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
// CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);