Sungwoo Kim
asdgas
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 62:456f87e3124e
- Parent:
- 61:8d36b4042096
- Child:
- 63:376a20d2fbfd
--- a/main.cpp Mon Apr 13 11:55:09 2020 +0000
+++ b/main.cpp Tue Apr 28 09:19:26 2020 +0000
@@ -131,13 +131,17 @@
MODE_SYSTEM_ID, //33
};
-
+void SystemClock_Config(void);
int main()
{
/*********************************
*** Initialization
*********************************/
+ HAL_Init();
+ SystemClock_Config();
+
+
LED = 1;
pc.baud(9600);
@@ -307,12 +311,12 @@
11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f,
25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f
}; // duty
-float LT_Voltage_Output[LT_MAX_IDX] = {-321.4f, -291.3f, -261.5f, -246.8f, -231.7f, -223.9f, -216.1f, -207.9f, -198.8f,
- -196.9f, -195.0f, -192.5f, -188.8f, -184.5f, -180.2f, -175.9f, -171.5f, -166.3f, -161.0f,
- -156.0f, -149.5f, -139.0f, -126.0f, -107.0f, -87.5f, -64.0f, -38.5f, -9.4f, 0.0f,
- 12.0f, 43.5f, 69.0f, 94.0f, 114.0f, 132.0f, 146.0f, 155.5f, 162.3f, 168.2f,
- 173.1f, 178.2f, 182.8f, 187.4f, 191.8f, 196.0f, 199.7f, 201.9f, 203.8f, 205.6f,
- 214.6f, 222.5f, 230.4f, 238.2f, 253.3f, 268.0f, 297.6f, 327.7f
+float LT_Voltage_Output[LT_MAX_IDX] = {-230.0f, -215.0f, -192.5f, -185.0f, -177.5f, -170.0f, -164.0f, -160.0f, -150.0f,
+ -150.0f, -145.0f, -145.0f, -145.0f, -135.0f, -135.0f, -135.0f, -127.5f, -127.5f, -115.0f,
+ -115.0f, -115.0F, -100.0f, -100.0f, -100.0f, -60.0f, -60.0f, -10.0f, -5.0f, 0.0f,
+ 7.5f, 14.0f, 14.0f, 14.0f, 42.5f, 42.5f, 42.5f, 80.0f, 80.0f, 105.0f,
+ 105.0f, 105.0f, 120.0f, 120.0f, 120.0f, 131.0f, 131.0f, 140.0f, 140.0f, 140.0f,
+ 155.0f, 160.0f, 170.0f, 174.0f, 182.0f, 191.0f, 212.0f, 230.0f
}; // mV
float PWM_duty_byLT(float Ref_V)
@@ -370,7 +374,15 @@
float pres_A_new = (((float) ADC1->DR) - 2047.5f);
double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 200Hz
pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
- torq.sen = pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
+ torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
+
+
+// float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.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.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+// torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047
+
} else if (SENSING_MODE == 1) {
// Pressure Sensing (0~210)bar =============================================
@@ -618,8 +630,8 @@
if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
//REFERENCE_MODE = MODE_REF_NO_ACT;
- if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 2.0f;
- else pos.ref = pos.ref - 2.0f;
+ if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 8.0f;
+ else pos.ref = pos.ref - 8.0f;
// pos.err = pos.ref_home_pos - pos.sen;
// float VALVE_POS_RAW_POS_FB = 0.0f;
@@ -1211,13 +1223,13 @@
}
case MODE_VALVE_POSITION_CONTROL: {
- if (((OPERATING_MODE&0b110)>>1) == 0) { //Moog Valve
- I_REF = valve_pos.ref;
- } else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve
- V_out = valve_pos.ref;
- } else { //SW Valve
+ if (OPERATING_MODE == 5) { //SW Valve
VALVE_POS_CONTROL(valve_pos.ref);
V_out = Vout.ref;
+ } else if (CURRENT_CONTROL_MODE == 0) { //PWM
+ V_out = valve_pos.ref;
+ } else {
+ I_REF = valve_pos.ref * 0.001f;
}
break;
@@ -1226,9 +1238,9 @@
case MODE_JOINT_CONTROL: {
double torq_ref = 0.0f;
pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
- vel.err = (vel.ref - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
+ vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
- torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
+ torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f - D_DAMPER * vel.sen * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
// torque feedback
torq.err = (torq_ref)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE) - torq.sen; //[N]
@@ -1303,7 +1315,7 @@
VALVE_POS_CONTROL(valve_pos.ref);
- Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err);
+// Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err);
V_out = (float) Vout.ref;
}
@@ -1327,15 +1339,15 @@
//////////////////////////// CURRENT CONTROL //////////////////////////////
////////////////////////////////////////////////////////////////////////////
if (CURRENT_CONTROL_MODE) {
- double alpha_update_Iref = 1.0f / (1.0f + TMR_FREQ_5k / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
+ double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
I_ERR = I_REF_fil - cur.sen;
- I_ERR_INT = I_ERR_INT + (I_ERR) * DT_TMR3;
+ I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
// Moog Valve Current Control Gain
- double R_model = 539.0f; // ohm
+ double R_model = 500.0f; // ohm
double L_model = 1.2f;
double w0 = 2.0f * 3.14f * 150.0f;
double KP_I = 0.1f * L_model*w0;
@@ -1352,24 +1364,25 @@
double FF_gain = 1.0f;
- VALVE_PWM_RAW = KP_I * I_ERR + KI_I * I_ERR_INT;
+ VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
// VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model*I_REF); // Unit : mV
I_REF_fil_diff = I_REF_fil - I_REF_fil_old;
I_REF_fil_old = I_REF_fil;
- VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * TMR_FREQ_5k); // Unit : mV
- double V_MAX = 12000.; // Maximum Voltage : 12V = 12000mV
+// VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV
+ VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil); // Unit : mV
+ double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
double Ka = 3.0f / KP_I;
if (VALVE_PWM_RAW > V_MAX) {
V_rem = VALVE_PWM_RAW - V_MAX;
V_rem = Ka*V_rem;
VALVE_PWM_RAW = V_MAX;
- I_ERR_INT = I_ERR_INT - V_rem * DT_TMR3;
+ I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
} else if (VALVE_PWM_RAW < -V_MAX) {
V_rem = VALVE_PWM_RAW - (-V_MAX);
V_rem = Ka*V_rem;
VALVE_PWM_RAW = -V_MAX;
- I_ERR_INT = I_ERR_INT - V_rem * DT_TMR3;
+ I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
}
Cur_Valve_Open_pulse = cur.sen / mA_PER_pulse;
} else {
@@ -1393,19 +1406,25 @@
// Output Voltage Linearization
double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV
- double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin);
+ double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000
// Dead Zone Cancellation (Electrical dead-zone)
- if (CUR_PWM_lin > 0) V_out = (int) CUR_PWM_lin + 140;
- else if (CUR_PWM_lin < 0) V_out = (int) CUR_PWM_lin - 140;
- else V_out = CUR_PWM_lin;
+ if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f);
+ else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
+ else V_out = (float) (CUR_PWM_lin);
}
-
-
-
+
+// if(V_out > 0.0f) V_out = (float) (V_out + 169.0f);
+// else if(V_out < 0.0f) V_out = (float) (V_out - 174.0f);
+// else V_out = V_out;
+
/*******************************************************
*** PWM
********************************************************/
+ if(DIR_VALVE<0){
+ V_out = -V_out;
+ }
+
if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) {
V_out = VALVE_VOLTAGE_LIMIT*1000.0f;
} else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
@@ -1436,26 +1455,26 @@
if (flag_data_request[0] == HIGH) {
if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.sen*10.0f));
+ CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
} else if (SENSING_MODE == 1) {
CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
}
} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
if (SENSING_MODE == 0) {
- CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen));
+ CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen));
} else if (SENSING_MODE == 1) {
- CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
+ CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
}
}
}
if (flag_data_request[1] == HIGH) {
//valve position
double t_value = 0;
-// if(value>=(float) VALVE_CENTER) {
-// t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-// } else {
-// t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-// }
+ if(valve_pos.ref>=(float) VALVE_CENTER) {
+ t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
+ } else {
+ t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
+ }
if(OPERATING_MODE==5) {
t_value = (double) value;
} else if(CURRENT_CONTROL_MODE==1) {
@@ -1463,7 +1482,8 @@
} else {
t_value = V_out;
}
- CAN_TX_TORQUE((int16_t) (t_value), (int16_t) (V_out)); //1300
+ //CAN_TX_TORQUE((int16_t) (value), (int16_t) (V_out)); //1300
+ CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300
}
@@ -1504,4 +1524,49 @@
}
TIM3->SR = 0x0; // reset the status register
+}
+
+void SystemClock_Config(void)
+{
+ RCC_OscInitTypeDef RCC_OscInitStruct = {0};
+ RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+
+ /** Configure the main internal regulator output voltage
+ */
+ __HAL_RCC_PWR_CLK_ENABLE();
+ __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
+ /** Initializes the CPU, AHB and APB busses clocks
+ */
+ RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
+ RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+ RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
+ RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
+ RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
+ RCC_OscInitStruct.PLL.PLLM = 8; //8
+ RCC_OscInitStruct.PLL.PLLN = 180;//180
+ RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
+ RCC_OscInitStruct.PLL.PLLQ = 2;
+ RCC_OscInitStruct.PLL.PLLR = 2;
+ if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
+ //Error_Handler();
+ }
+ /** Activate the Over-Drive mode
+ */
+ if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
+ //Error_Handler();
+ }
+ /** Initializes the CPU, AHB and APB busses clocks
+ */
+ RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
+ |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
+ RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+ RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
+ RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
+ RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
+
+ if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
+ //Error_Handler();
+ }
+ HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSI, RCC_MCODIV_1);
+ HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_SYSCLK, RCC_MCODIV_2);
}
\ No newline at end of file