Sungwoo Kim
rainbow
Diff: main.cpp
- Revision:
- 243:30896263bd8b
- Parent:
- 242:b235d67d25ba
- Child:
- 244:e9c5ec04e378
diff -r b235d67d25ba -r 30896263bd8b main.cpp
--- a/main.cpp Thu Apr 07 06:18:38 2022 +0000
+++ b/main.cpp Mon Jun 13 08:48:55 2022 +0000
@@ -1,20 +1,12 @@
-//Hydraulic Control Board
+//Hydraulic Control Board Rainbow
//distributed by Sungwoo Kim
-// 2020/12/28
-//revised by Buyoun Cho
-// 2021/04/20
-
-// 유의사항
-// 소수 적을때 뒤에 f 꼭 붙이기
-// CAN 선은 ground까지 있는 3상 선으로 써야함.
-// 전원은 12~24V 인가.
+// 2022/05/31
#include "mbed.h"
#include "FastPWM.h"
#include "INIT_HW.h"
#include "function_CAN.h"
#include "SPI_EEP_ENC.h"
-#include "I2C_AS5510.h"
#include "setting.h"
#include "function_utilities.h"
#include "stm32f4xx_flash.h"
@@ -23,39 +15,34 @@
#include <iostream>
#include <cmath>
-using namespace std;
-Timer t;
-// dac & check ///////////////////////////////////////////
-DigitalOut check(PC_2);
-DigitalOut check_2(PC_3);
+// DAC ///////////////////////////////////////////
AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f
AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
-AnalogIn adc1(PC_4); //pressure_1
-AnalogIn adc2(PB_0); //pressure_2
-AnalogIn adc3(PC_1); //current
+
+// ADC ///////////////////////////////////////////
+//AnalogIn adc1(PC_4); //pressure_1
+//AnalogIn adc2(PC_5); //pressure_2
+//AnalogIn adc3(PC_1); //current
+//AnalogIn adc4(PB_0); //LVDT
// PWM ///////////////////////////////////////////
float dtc_v=0.0f;
float dtc_w=0.0f;
-DigitalOut LVDT_H(PB_4); //PWM_H
-DigitalOut LVDT_L(PB_5); //PWM_L
-// I2C ///////////////////////////////////////////
-I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
-const int i2c_slave_addr1 = 0x56; // AS5510 address
-unsigned int value; // 10bit output of reading sensor AS5510
+// LVDT ///////////////////////////////////////////
+//DigitalOut LVDT_H(PB_6);
+//DigitalOut LVDT_L(PB_7);
// SPI ///////////////////////////////////////////
SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
DigitalOut eeprom_cs(PB_12);
SPI enc(PC_12,PC_11,PC_10);
DigitalOut enc_cs(PD_2);
+
+// LED ///////////////////////////////////////////
DigitalOut LED(PA_15);
-// UART ///////////////////////////////////////////
-Serial pc(PA_9,PA_10); // _ UART
-
// CAN ///////////////////////////////////////////
CAN can(PB_8, PB_9, 1000000);
CANMessage msg;
@@ -77,11 +64,6 @@
State valve_pos;
State valve_pos_raw;
-State INIT_Vout;
-State INIT_Valve_Pos;
-State INIT_Pos;
-State INIT_torq;
-
extern int CID_RX_CMD;
extern int CID_RX_REF_POSITION;
extern int CID_RX_REF_OPENLOOP;
@@ -95,12 +77,6 @@
extern int CID_TX_VALVE_POSITION;
extern int CID_TX_SOMETHING;
-float temp_P_GAIN = 0.0f;
-float temp_I_GAIN = 0.0f;
-int temp_VELOCITY_COMP_GAIN = 0;
-int logging = 0;
-float valve_pos_pulse_can = 0.0f;
-
inline float tanh_inv(float y)
{
if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
@@ -167,12 +143,9 @@
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_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
// RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
@@ -202,36 +175,26 @@
}
+
int main()
{
- /*********************************
- *** Initialization
- *********************************/
-
+
HAL_Init();
SystemClock_Config();
LED = 0;
- pc.baud(9600);
- // i2c init
- i2c.frequency(400 * 1000); // 0.4 mHz
- wait_ms(2); // Power Up wait
- look_for_hardware_i2c(); // Hardware present
- init_as5510(i2c_slave_addr1);
- make_delay();
-
- // spi init
+ // SPI INIT
eeprom_cs = 1;
eeprom.format(8,3);
eeprom.frequency(5000000); //5M
eeprom_cs = 0;
make_delay();
- enc_cs = 1; //sw add
+ enc_cs = 1;
enc.format(8,0);
enc.frequency(5000000); //10M
- enc_cs = 0; //sw add
+ enc_cs = 0;
make_delay();
@@ -239,7 +202,7 @@
spi_enc_set_init();
make_delay();
- ////// bno rom
+ // bno rom
spi_eeprom_write(RID_BNO, (int16_t) 0);
make_delay();
////////
@@ -263,25 +226,41 @@
make_delay();
//can.reset();
-// can.filter(msg.id, 0xFFFFF000, CANStandard);
-// can.filter(0x050, 0xFFFFFFFF, CANStandard);
- can.filter(0b100000000, 0b100000010, CANStandard); //CAN ID 100~400번대 통과하게
+ can.filter(msg.id, 0xFFFFF000, CANStandard);
+// can.filter(0b100000000, 0b100000010, CANStandard); //CAN ID 100~400번대 통과하게
// TMR3 init
Init_TMR3();
TIM3->CR1 ^= TIM_CR1_UDIS;
make_delay();
- // TMR5 init
+ // TMR2 init
Init_TMR2();
TIM2->CR1 ^= TIM_CR1_UDIS;
make_delay();
+ // TMR1 init
+ Init_TMR1();
+ TIM1->CR1 ^= TIM_CR1_UDIS;
+ make_delay();
+
//Timer priority
NVIC_SetPriority(TIM3_IRQn, 2);
NVIC_SetPriority(TIM4_IRQn, 3);
NVIC_SetPriority(TIM2_IRQn, 4);
+// HAL_NVIC_SetPriority(TIM2_IRQn, 4, 0);
+// HAL_NVIC_EnableIRQ(TIM2_IRQn);
+// /* TIM3_IRQn interrupt configuration */
+// HAL_NVIC_SetPriority(TIM3_IRQn, 2, 0);
+// HAL_NVIC_EnableIRQ(TIM3_IRQn);
+// /* TIM4_IRQn interrupt configuration */
+// HAL_NVIC_SetPriority(TIM4_IRQn, 3, 0);
+// HAL_NVIC_EnableIRQ(TIM4_IRQn);
+// /* CAN1_RX0_IRQn interrupt configuration */
+// HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 14, 0);
+// HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
+
//DAC init
if (SENSING_MODE == 0) {
@@ -305,29 +284,16 @@
ID_index_array[i] = (i+1) * 0.5f;
}
- //pwm
- TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
- TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
- LVDT_H = 0;
- LVDT_L = 0;
-
/************************************
*** Program is operating!
*************************************/
while(1) {
+
+// if (LED > 0) LED = 0;
+// else LED = 1;
+ TIM1->CCR1 = (TMR1_COUNT)*(0.7f);
- // UART example
-// if(timer_while==100000) {
-// timer_while = 0;
-// pc.printf("%f\n", value);
-// }
-// timer_while ++;
- //i2c for SW valve
-// if(OPERATING_MODE == 5) {
-// read_field(i2c_slave_addr1);
-// if(DIR_VALVE_ENC < 0) value = 1023 - value;
-// }
}
}
@@ -375,7 +341,7 @@
} else if(REF_VALVE_POS < VALVE_MIN_POS) {
REF_VALVE_POS = VALVE_MIN_POS;
}
- valve_pos_err = (float) (REF_VALVE_POS - value);
+ valve_pos_err = (float) (REF_VALVE_POS - valve_pos.sen);
valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
valve_pos_err_old = valve_pos_err;
valve_pos_err_sum += valve_pos_err;
@@ -473,7 +439,6 @@
}
}
}
-
return PWM_duty;
}
@@ -483,43 +448,65 @@
TIMER INTERRUPT
*******************************************************************************/
+
+//------------------------------------------------
+// TMR3 : LVDT 1kHz
+//-----------------------------------------------
float LVDT_new = 0.0f;
float LVDT_old = 0.0f;
float LVDT_f_cut = 1000.0f;
float LVDT_LPF = 0.0f;
float LVDT_sum = 0.0f;
-float LVDT_can[100] = {0.0f};
extern "C" void TIM3_IRQHandler(void)
{
if (TIM3->SR & TIM_SR_UIF ) {
-// if (LED > 0) LED = 0;
-// else LED = 1;
+//// if (LED > 0) LED = 0;
+//// else LED = 1;
+//
+// LVDT_sum = 0.0f;
+//
+//
+// LVDT_L = 0;
+// LVDT_H = 1;
+//
+//// LED = 1;
+//
+// for (int ij = 0; ij<150; ij++) {
+// ADC1->CR2 |= 0x40000000;
+// LVDT_new = ((float)ADC1->DR) - 2047.5f;
+// LVDT_sum = LVDT_sum + LVDT_new;
+// }
+//
+//// LED = 0;
+//
+// LVDT_H = 0;
+// LVDT_L = 1;
- LVDT_sum = 0.0f;
-
- LVDT_H = 1;
- LVDT_L = 0;
- for (int ij = 0; ij<200; ij++) {
-// LED = 1;
- ADC1->CR2 |= 0x40000000;
- LVDT_new = ((float)ADC1->DR) - 2047.5f;
- LVDT_sum = LVDT_sum + LVDT_new;
-// LED = 0;
- }
+
+
+// LVDT_new = LVDT_sum * 0.01f*2.0f;
+//
+// float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
+// LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
+// valve_pos.sen = LVDT_LPF;
+// if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
- LVDT_H = 0;
- LVDT_L = 0;
+// TIM4->CCR2 = (PWM_ARR)*(0.95f);
+// TIM4->CCR1 = (PWM_ARR)*(0.975f);
+
+// TIM8->CCR1 = (TMR8_COUNT)*(0.95f);
+// TIM8->CCR2 = (TMR8_COUNT)*(0.975f);
+
-// LED = 0;
- LVDT_new = LVDT_sum * 0.005f;
+// PWM_H2 = 0;
+// PWM_L2 = 1;
- float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0f*PI*300.0f));
- LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
- valve_pos.sen = LVDT_LPF;
- if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
+// TIM3->CCR2 = (TMR3_COUNT)*(0.95f);
+// TIM3->CCR1 = (TMR3_COUNT)*(0.975f);
+
}
TIM3->SR = 0x0; // reset the status register
@@ -538,9 +525,6 @@
{
if (TIM4->SR & TIM_SR_UIF ) {
-// if (LED > 0) LED = 0;
-// else LED = 1;
-
// Current ===================================================
//ADC3->CR2 |= 0x40000000; // adc _ 12bit
@@ -578,6 +562,51 @@
}
*/
+// //FORWARD
+// if(CNT_TMR4 < 2)
+// {
+// PWM_L2 = 0;
+// PWM_H1 = 0;
+//
+// PWM_H2 = 1;
+// PWM_L1 = 1;
+// } else if(CNT_TMR4 < 50)
+// {
+// PWM_H2 = 0;
+// PWM_H1 = 0;
+//
+// PWM_L2 = 1;
+// PWM_L1 = 1;
+// } else
+// {
+// CNT_TMR4 = 0;
+// }
+
+
+// //BACKWARD
+// if(CNT_TMR4 < 2)
+// {
+// PWM_H2 = 0;
+// PWM_L1 = 0;
+//
+// PWM_L2 = 1;
+// PWM_H1 = 1;
+// } else if(CNT_TMR4 < 50)
+// {
+// PWM_H2 = 0;
+// PWM_H1 = 0;
+//
+// PWM_L2 = 1;
+// PWM_L1 = 1;
+// } else
+// {
+// CNT_TMR4 = 0;
+// }
+
+
+
+
+
CNT_TMR4++;
}
TIM4->SR = 0x0; // reset the status register
@@ -588,7 +617,6 @@
float FREQ_TMR5 = (float)FREQ_5k;
float DT_TMR5 = (float)DT_5k;
int cnt_trans = 0;
-double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
int can_rest =0;
float force_ref_act_can = 0.0f;
@@ -828,7 +856,6 @@
data_num = 0;
} else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
data_num = data_num + 1;
-// VALVE_POS_TMP = VALVE_POS_TMP + value;
VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
} else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
Vout.ref = 0.0f;
@@ -884,7 +911,7 @@
} else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
data_num = data_num + 1;
- VALVE_POS_TMP = VALVE_POS_TMP + value;
+ VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
} else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
DDV_POS_AVG = VALVE_POS_TMP / data_num;
@@ -923,7 +950,7 @@
}
} else {
if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
- if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) {
+ if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)*(float) 50.0f;
} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
START_POS = pos.sen;
@@ -1096,18 +1123,18 @@
valve_pos_ref = 10000.0f;
}
if(valve_pos_ref >= 0) {
- valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+ valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
} else {
- valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+ valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
}
VALVE_POS_CONTROL(valve_pos_raw.ref);
ref_array[cnt_step_test] = valve_pos_ref;
- if(value>=(float) VALVE_ELECTRIC_CENTER) {
- pos_array[cnt_step_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER);
+ if(valve_pos.sen >= (float) VALVE_ELECTRIC_CENTER) {
+ pos_array[cnt_step_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
} else {
- pos_array[cnt_step_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER);
+ pos_array[cnt_step_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
}
CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
@@ -1119,17 +1146,10 @@
CONTROL_UTILITY_MODE = MODE_SEND_OVER;
CONTROL_MODE = MODE_NO_ACT;
}
-// if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k))
-// {
-// CONTROL_UTILITY_MODE = MODE_NO_ACT;
-// CONTROL_MODE = MODE_NO_ACT;
-// CAN_TX_PWM((int16_t) (1)); //1300
-// }
break;
}
case MODE_SEND_OVER: {
- CAN_TX_TORQUE((int16_t) (buffer_data_size)); //1300
CONTROL_UTILITY_MODE = MODE_NO_ACT;
CONTROL_MODE = MODE_NO_ACT;
break;
@@ -1138,18 +1158,21 @@
case MODE_FREQ_TEST: {
float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5);
if(valve_pos_ref >= 0) {
- valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER + (double)valve_pos_ref * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+ valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
} else {
- valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (double)valve_pos_ref * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f;
+ valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
}
VALVE_POS_CONTROL(valve_pos_raw.ref);
ref_array[cnt_freq_test] = valve_pos_ref;
- if(value>=(float) VALVE_ELECTRIC_CENTER) {
- pos_array[cnt_freq_test] = 10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_ELECTRIC_CENTER);
+// if(value>=(float) VALVE_ELECTRIC_CENTER) {
+ if(valve_pos.sen>=(float) VALVE_ELECTRIC_CENTER) {
+// pos_array[cnt_freq_test] = 10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
+ pos_array[cnt_freq_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
} else {
- pos_array[cnt_freq_test] = -10000.0f*((double)value - (double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_ELECTRIC_CENTER);
+// pos_array[cnt_freq_test] = -10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
+ pos_array[cnt_freq_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
}
CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
@@ -1162,7 +1185,6 @@
if (freq_test_valve_ref >= 400) {
CONTROL_UTILITY_MODE = MODE_NO_ACT;
CONTROL_MODE = MODE_NO_ACT;
- CAN_TX_VOUT((int16_t) (1)); //1300
}
CONTROL_MODE = MODE_NO_ACT;
CONTROL_UTILITY_MODE = MODE_SEND_OVER;
@@ -1281,7 +1303,6 @@
float I_MAX = 10.0f; // Maximum Current : 10mA
// Anti-windup for FT
- // if (I_GAIN_JOINT_TORQUE != 0.0f) {
if (I_GAIN_JOINT_TORQUE > 0.001f) {
float Ka = 2.0f;
if (I_REF > I_MAX) {
@@ -1305,7 +1326,6 @@
} else { //SW valve
float Valve_pos_MAX = 10000.0f; // Maximum Valve Pos : 10000
// Anti-windup for FT
- // if (I_GAIN_JOINT_TORQUE != 0.0f) {
if (I_GAIN_JOINT_TORQUE > 0.001f) {
float Ka = 2.0f;
if (valve_pos_pulse > Valve_pos_MAX) {
@@ -1332,7 +1352,6 @@
// valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
// }
VALVE_POS_CONTROL_DZ(valve_pos_pulse);
- valve_pos_pulse_can = valve_pos_pulse;
V_out = Vout.ref;
}
break;
@@ -1343,79 +1362,6 @@
break;
}
-// case MODE_JOINT_ADAPTIVE_BACKSTEPPING: {
-//
-// float Va = (1256.6f + Amm * pos.sen/(float)(ENC_PULSE_PER_POSITION)) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * x, unit : m^3
-// float Vb = (1256.6f + Amm * (79.0f - pos.sen/(float)(ENC_PULSE_PER_POSITION))) * 0.000000001f; // 4mm pipe * 100mm + (25mm Cylinder 18mm Rod) * (79.0mm-x), unit : m^3
-//
-// V_adapt = 1.0f / (1.0f/Va + 1.0f/Vb); //initial 0.0000053f
-//
-// //float f3 = -Amm*Amm*beta*0.000001f*0.000001f/V_adapt * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
-// float f3_hat = -a_hat * vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f; // unit : N/s //xdot=10mm/s일때 -137076
-//
-// float g3_prime = 0.0f;
-// if (torq.sen > Amm*(Ps-Pt)*0.000001f) {
-// g3_prime = 1.0f;
-// } else if (torq.sen < -Amm*(Ps-Pt)*0.000001f) {
-// g3_prime = -1.0f;
-// } else {
-// if ((value-VALVE_CENTER) > 0) {
-// g3_prime = sqrt(Ps-Pt-torq.sen/Amm*1000000.0f);
-//// g3_prime = sqrt(Ps-Pt);
-// } else {
-// g3_prime = sqrt(Ps-Pt+torq.sen/Amm*1000000.0f);
-//// g3_prime = sqrt(Ps-Pt);
-// }
-// }
-// float tau = 0.01f;
-// float K_valve = 0.0004f;
-//
-// float x_v = 0.0f; //x_v : -1~1
-// if(value>=VALVE_CENTER) {
-// x_v = 1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
-// } else {
-// x_v = -1.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
-// }
-// float f4 = -x_v/tau;
-// float g4 = K_valve/tau;
-//
-// float torq_ref_dot = torq.ref_diff * 500.0f;
-//
-// pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
-// vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
-// pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
-//
-// torq.err = torq.ref - torq.sen; //[N]
-// torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
-//
-// float k3 = 2000.0f; //2000 //20000
-// float k4 = 10.0f;
-// float rho3 = 3.2f;
-// float rho4 = 10000000.0f; //25000000.0f;
-// float x_4_des = (-f3_hat + torq_ref_dot - k3*(-torq.err))/(gamma_hat*g3_prime);
-// if (x_4_des > 1) x_4_des = 1;
-// else if (x_4_des < -1) x_4_des = -1;
-//
-// if (x_4_des > 0) {
-// valve_pos.ref = x_4_des * (float)(VALVE_MAX_POS - VALVE_CENTER) + (float) VALVE_CENTER;
-// } else {
-// valve_pos.ref = x_4_des * (float)(VALVE_CENTER - VALVE_MIN_POS) + (float) VALVE_CENTER;
-// }
-//
-// float x_4_des_dot = (x_4_des - x_4_des_old)*(float) TMR_FREQ_5k;
-// x_4_des_old = x_4_des;
-// V_out = (-f4 + x_4_des_dot - k4*(x_v-x_4_des)- rho3/rho4*gamma_hat*g3_prime*(-torq.err))/g4;
-//
-// float rho_a = 0.00001f;
-// float a_hat_dot = -rho3/rho_a*vel.sen/(float)(ENC_PULSE_PER_POSITION)*0.001f*(-torq.err);
-// a_hat = a_hat + a_hat_dot / (float) TMR_FREQ_5k;
-//
-// if(a_hat > -3000000.0f) a_hat = -3000000.0f;
-// else if(a_hat < -30000000.0f) a_hat = -30000000.0f;
-//
-// break;
-// }
-
default:
break;
}
@@ -1437,7 +1383,6 @@
I_ERR = I_REF_fil_DZ - (double)cur.sen;
I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
-
// Moog Valve Current Control Gain
double R_model = 500.0f; // ohm
double L_model = 1.2f;
@@ -1455,7 +1400,6 @@
}
double FF_gain = 1.0f;
-
VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old;
I_REF_fil_old = I_REF_fil_DZ;
@@ -1527,54 +1471,38 @@
dtc_w=0.0f;
}
- //pwm
- TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
- TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
////////////////////////////////////////////////////////////////////////////
////////////////////// Data transmission through CAN //////////////////////
////////////////////////////////////////////////////////////////////////////
- if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+// if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
+ if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/500) == 0) {
// Position, Velocity, and Torque (ID:1200)
-// if (flag_data_request[0] == LOW) {
+ if (flag_data_request[0] == HIGH) {
-// if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-// CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-// CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+ if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
+ CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
-// } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-// CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
- CAN_TX_POSITION_FT((int16_t) (7.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (valve_pos.sen));
-// }
-// }
+ } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
+ CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+ }
+ }
// Valve Position (ID:1300)
if (flag_data_request[1] == HIGH) {
- CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
-// CAN_TX_PWM((int16_t)(TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
+ if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
+ CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
+ } else {
+ CAN_TX_PWM((int16_t)(valve_pos.sen));
+ }
}
- // Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (ID:1400)
+ // Others : SW (ID:1400)
if (flag_data_request[2] == HIGH) {
- float valve_pos_can = 0.0f;
- if(value >= VALVE_ELECTRIC_CENTER) {
- valve_pos_can = 10000.0f*((float)value-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
- } else {
- valve_pos_can = -10000.0f*((float)value -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
- }
- float valve_pos_ref_can = 0.0f;
- if(valve_pos.ref >= VALVE_ELECTRIC_CENTER) {
- valve_pos_ref_can = 10000.0f*((float)valve_pos.ref-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
- } else {
- valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
- }
-
- valve_pos_ref_can = (float)valve_pos.ref;
-
- CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_pulse_can);
+ CAN_TX_CURRENT((int16_t) valve_pos.sen, (int16_t) valve_pos.ref);
}
TMR2_COUNT_CAN_TX = 0;
@@ -1583,5 +1511,4 @@
}
TIM2->SR = 0x0; // reset the status register
-
-}
\ No newline at end of file
+}