asdgas
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 62:456f87e3124e
- Parent:
- 61:8d36b4042096
- Child:
- 63:376a20d2fbfd
diff -r 8d36b4042096 -r 456f87e3124e main.cpp --- 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