Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 244:e9c5ec04e378, committed 2022-06-17
- Comitter:
- Lightvalve
- Date:
- Fri Jun 17 06:06:53 2022 +0000
- Parent:
- 243:30896263bd8b
- Child:
- 245:071785d74ad0
- Commit message:
- 220617
Changed in this revision
--- a/CAN/function_CAN.cpp Mon Jun 13 08:48:55 2022 +0000 +++ b/CAN/function_CAN.cpp Fri Jun 17 06:06:53 2022 +0000 @@ -25,12 +25,7 @@ extern DigitalOut LED; -extern float u_past[]; -extern float x_past[]; -extern float x_future[]; -extern float f_past[]; -extern float f_future[]; -extern float input_NN[]; +int can_test = 0; /******************************************************************************* * State Class functions @@ -674,10 +669,6 @@ void CAN_RX_HANDLER() { - -// if (LED > 0) LED = 0; -// else LED = 1; - can.read(msg); unsigned int address = msg.id;
--- a/INIT_HW/INIT_HW.cpp Mon Jun 13 08:48:55 2022 +0000 +++ b/INIT_HW/INIT_HW.cpp Fri Jun 17 06:06:53 2022 +0000 @@ -4,142 +4,184 @@ extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc2; +extern ADC_HandleTypeDef hadc3; -//void Init_ADC(void){ -// // ADC Setup -// RCC->APB2ENR |= RCC_APB2ENR_ADC3EN; // clock for ADC3 -// RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2 -// RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1 -// -// RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // Enable clock for GPIOC -// RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // Enable clock for GPIOA -// -// ADC->CCR = 0x00000016; // Regular simultaneous mode only -// ADC1->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON -// ADC1->SQR3 = 0x0000000E; //channel // use PC_4 as input- ADC1_IN14 -// ADC2->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC2 ON -// ADC2->SQR3 = 0x00000008; // use PB_0 as input - ADC2_IN8 -// ADC3->CR2 |= ADC_CR2_ADON; // ADC3 ON -// ADC3->SQR3 = 0x0000000B; // use PC_1, - ADC3_IN11 -// GPIOC->MODER |= 0b1100001100; //each channel // PC_4, PC_1 are analog inputs -// GPIOB->MODER |= 0x3; // PB_0 as analog input -// -// ADC1->SMPR1 |= 0x00001000; // 15 cycles on CH_14, 0b0001000000000000 -// ADC2->SMPR2 |= 0x01000000; // 15 cycles on CH_8, 0b0000000100000000<<16 -// ADC3->SMPR1 |= 0x00000008; // 15 cycles on CH_11, 0b0000000000001000 -//} - - -void Init_ADC(void) +void Init_ADC1(void) { +/* // ADC Setup // RCC->APB2ENR |= RCC_APB2ENR_ADC3EN; // clock for ADC3 // RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2 RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC1 - -// RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // Enable clock for GPIOC - RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // Enable clock for GPIOB - - ADC->CCR = 0x00000016; // Regular simultaneous mode only + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // Enable clock for GPIOC +// RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // Enable clock for GPIOB ADC1->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC1 ON - ADC1->SQR3 = 0x00000008; // use PB_0 as input - ADC1_IN8 -// ADC1->SQR3 = 0x0000000E; //channel // use PC_4 as input- ADC1_IN14 +// ADC->CCR = 0x00000016; // Regular simultaneous mode only + ADC1->CR1 |= 0x800; // Discontinuous mode + ADC1->CR1 |= 0x100; // Scan mode + ADC1->CR2 |= 0x400; // EOC + ADC1->SQR1 |= 0x100000; // 2 conversions +// ADC1->SQR3 = 0x00000008; // use PB_0 as input - ADC1_IN8 + ADC1->SQR3 |= 0x0000000E; //channel // use PC_4 as input - ADC1_IN14 + ADC1->SQR3 |= 0x000001E0; // use PC_5 as input - ADC1_IN15 0b0000 0000 0000 0000 0000 0001 1110 0000 // ADC2->CR2 |= ADC_CR2_ADON;//0x00000001; // ADC2 ON // ADC2->SQR3 = 0x00000008; // use PB_0 as input - ADC2_IN8 -// ADC3->CR2 |= ADC_CR2_ADON; // ADC3 ON +// ADC3->CR2 |= ADC_CR2_ADON; // ADC3 ON00000000000 // ADC3->SQR3 = 0x0000000B; // use PC_1, - ADC3_IN11 -// GPIOC->MODER |= 0b1100001100; //each channel // PC_4, PC_1 are analog inputs - GPIOB->MODER |= 0x3; // PB_0 as analog input - - ADC1->SMPR2 |= 0x01000000; // 15 cycles on CH_8, 0b0000000100000000<<16 -// ADC1->SMPR1 |= 0x00001000; // 15 cycles on CH_14, 0b0001000000000000 + GPIOC->MODER |= 0b111100000000; //each channel // PC_4, PC_5 are analog inputs +// GPIOB->MODER |= 0x3; // PB_0 as analog input +// ADC1->SMPR2 |= 0x01000000; // 15 cycles on CH_8, 0b0000000100000000<<16 +// ADC1->SMPR1 |= 0x00001000; // 15 cycles on CH_14 +// ADC1->SMPR1 |= 0x00008000; // 15 cycles on CH_15 // ADC2->SMPR2 |= 0x01000000; // 15 cycles on CH_8, 0b0000000100000000<<16 // ADC3->SMPR1 |= 0x00000008; // 15 cycles on CH_11, 0b0000000000001000 + +*/ + + //////////////////////////////////////////////////ADC1//////////////////////////////////////////////// + ADC_ChannelConfTypeDef sConfig = {0}; + + hadc1.Instance = ADC1; + hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc1.Init.Resolution = ADC_RESOLUTION_12B; + hadc1.Init.ScanConvMode = ENABLE; + hadc1.Init.ContinuousConvMode = DISABLE; + hadc1.Init.DiscontinuousConvMode = ENABLE; + hadc1.Init.NbrOfDiscConversion = 1; + hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc1.Init.NbrOfConversion = 2; + hadc1.Init.DMAContinuousRequests = DISABLE; + hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if (HAL_ADC_Init(&hadc1) != HAL_OK) + { +// Error_Handler(); + } + sConfig.Channel = ADC_CHANNEL_14; + sConfig.Rank = 1; + sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { +// Error_Handler(); + } + sConfig.Channel = ADC_CHANNEL_15; + sConfig.Rank = 2; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { +// Error_Handler(); + } + hadc1.Instance->CR2 |= ADC_CR2_ADON; } - - - +void Init_ADC2(void) +{ + + //////////////////////////////////////////////////ADC2//////////////////////////////////////////////// + ADC_ChannelConfTypeDef sConfig = {0}; + + hadc2.Instance = ADC2; + hadc2.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc2.Init.Resolution = ADC_RESOLUTION_12B; + hadc2.Init.ScanConvMode = ENABLE; + hadc2.Init.ContinuousConvMode = DISABLE; + hadc2.Init.DiscontinuousConvMode = ENABLE; + hadc2.Init.NbrOfDiscConversion = 1; + hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc2.Init.NbrOfConversion = 2; + hadc2.Init.DMAContinuousRequests = DISABLE; + hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if (HAL_ADC_Init(&hadc2) != HAL_OK) + { +// Error_Handler(); + } + sConfig.Channel = ADC_CHANNEL_11; + sConfig.Rank = 1; + sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES; + if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) + { +// Error_Handler(); + } + sConfig.Channel = ADC_CHANNEL_9; + sConfig.Rank = 2; + if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) + { +// Error_Handler(); + } + hadc2.Instance->CR2 |= ADC_CR2_ADON; +} -void Init_PWM() +void Init_ADC3(void) { + //////////////////////////////////////////////////ADC3//////////////////////////////////////////////// + ADC_ChannelConfTypeDef sConfig = {0}; + + hadc3.Instance = ADC3; + hadc3.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc3.Init.Resolution = ADC_RESOLUTION_12B; + hadc3.Init.ScanConvMode = DISABLE; + hadc3.Init.ContinuousConvMode = DISABLE; + hadc3.Init.DiscontinuousConvMode = DISABLE; + hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc3.Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc3.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc3.Init.NbrOfConversion = 1; + hadc3.Init.DMAContinuousRequests = DISABLE; + hadc3.Init.EOCSelection = ADC_EOC_SEQ_CONV; + if (HAL_ADC_Init(&hadc3) != HAL_OK) + { +// Error_Handler(); + } + sConfig.Channel = ADC_CHANNEL_1; + sConfig.Rank = 1; + sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES; + if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK) + { +// Error_Handler(); + } + hadc3.Instance->CR2 |= ADC_CR2_ADON; +} +void Init_TIM4() +{ RCC->APB1ENR |= RCC_APB1ENR_TIM4EN; // enable TIM4 clock -// FastPWM pwm_v(PIN_V); -// FastPWM pwm_w(PIN_W); - - //ISR Setup - - NVIC_EnableIRQ(TIM4_IRQn); //Enable TIM4 IRQ - +// FastPWM pwm_H3(PIN_H3); +// FastPWM pwm_L3(PIN_L3); + + NVIC_EnableIRQ(TIM4_IRQn); //Enable TIM4 IRQ + TIM4->DIER |= TIM_DIER_UIE; // enable update interrupt TIM4->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode -// TIM4->CR1 = 0x10; TIM4->CR1 |= TIM_CR1_UDIS; -// TIM4->CR1 |= TIM_CR1_ARPE; // autoreload on, TIM4->RCR |= 0x001; // update event once per up/down count of TIM4 TIM4->EGR |= TIM_EGR_UG; //PWM Setup - TIM4->PSC = 0x0; // no prescaler, timer counts up in sync with the peripheral clock -// TIM4->PSC = 10-1; // no prescaler, timer counts up in sync with the peripheral clock +// TIM4->PSC = 0x0; + TIM4->PSC = 10-1; // 10 prescaler, timer counts up in sync with the peripheral clock TIM4->ARR = PWM_ARR-1; // set auto reload TIM4->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on. TIM4->CR1 |= TIM_CR1_CEN; // enable TIM4 - - TIM4->CCMR1 |= 0x7060; - +// TIM4->CCMR1 |= 0x7060; } - -//void Init_TMR3() -//{ -// RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // enable TIM3 clock -//// FastPWM pwm_v(PIN_V); -//// FastPWM pwm_w(PIN_W); -// //ISR Setup -// -// NVIC_EnableIRQ(TIM3_IRQn); //Enable TIM3 IRQ -// -// TIM3->DIER |= TIM_DIER_UIE; // enable update interrupt -//// TIM3->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode -// TIM3->CR1 = 0x10; -// TIM3->CR1 |= TIM_CR1_UDIS; -//// TIM3->CR1 |= TIM_CR1_ARPE; // autoreload on, -// TIM3->RCR |= 0x001; // update event once per up/down count of TIM3 -// TIM3->EGR |= TIM_EGR_UG; -// -//// TIM3->PSC = 0x00; // no prescaler, timer counts up in sync with the peripheral clock -// TIM3->PSC = 0x01; -// TIM3->ARR = TMR3_COUNT-1; // set auto reload, 5 khz -// TIM3->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on. -// TIM3->CR1 |= TIM_CR1_CEN; // enable TIM4 -//} - void Init_TMR3() { RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // enable TIM3 clock -// FastPWM pwm_v(PIN_V); -// FastPWM pwm_w(PIN_W); - //ISR Setup - NVIC_EnableIRQ(TIM3_IRQn); //Enable TIM3 IRQ TIM3->DIER |= TIM_DIER_UIE; // enable update interrupt TIM3->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode -// TIM3->CR1 = 0x10; TIM3->CR1 |= TIM_CR1_UDIS; -// TIM3->CR1 |= TIM_CR1_ARPE; // autoreload on, TIM3->RCR |= 0x001; // update event once per up/down count of TIM3 TIM3->EGR |= TIM_EGR_UG; TIM3->PSC = 0x00; // no prescaler, timer counts up in sync with the peripheral clock -// TIM3->PSC = 0x01; TIM3->ARR = TMR3_COUNT-1; // set auto reload, 5 khz TIM3->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on. TIM3->CR1 |= TIM_CR1_CEN; // enable TIM4 - - TIM3->CCMR1 |= 0x7060; } void Init_TMR2() @@ -152,7 +194,6 @@ TIM2->DIER |= TIM_DIER_UIE; // enable update interrupt TIM2->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode TIM2->CR1 |= TIM_CR1_UDIS; -// TIM2->CR1 |= TIM_CR1_ARPE; // autoreload on, TIM2->RCR |= 0x001; // update event once per up/down count of TIM2 TIM2->EGR |= TIM_EGR_UG; @@ -164,11 +205,12 @@ void Init_TMR1() { - RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock -// RCC->APB2ENR |= 0x02; // enable TIM8 clock - FastPWM pwm_v(PIN_V); - FastPWM pwm_w(PIN_W); + + FastPWM pwm_H1(PIN_H1); + FastPWM pwm_L1(PIN_L1); + FastPWM pwm_H2(PIN_H2); + FastPWM pwm_L2(PIN_L2); TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt TIM1->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode @@ -177,34 +219,16 @@ TIM1->RCR |= 0x001; // update event once per up/down count of TIM8 TIM1->EGR |= TIM_EGR_UG; + TIM1->CCMR1 |= 0x60; //CH1 - PWM mode 1 + TIM1->CCMR1 |= 0x6000; //CH2 - PWM mode 1 - TIM1->CCMR1 |= 0x60; -// TIM1->CCMR1 |= TIM_CCMR1_OC1PE; -// TIM1->CCMR1 &= ~TIM_CCMR1_OC1FE; -// TIM1->CCMR1 |= TIM_CCMR1_OC2PE; -// TIM1->CCMR1 &= ~TIM_CCMR1_OC2FE; - - TIM1->PSC = 0x00; // no prescaler, timer counts up in sync with the peripheral clock - TIM1->ARR = TMR1_COUNT-1; // set auto reload, 5 khz -// TIM1->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on. + TIM1->PSC = 0x00; // no prescaler, timer counts up in sync with the peripheral clock + TIM1->ARR = TMR1_COUNT-1; // set auto reload, 20 khz TIM1->CCER |= 0x05; // CC1E = 1, CC1P = 0, CC1NE = 1, CC1NP = 0 + TIM1->CCER |= 0x50; // CC2E = 1, CC2P = 0, CC2NE = 1, CC2NP = 0 TIM1->BDTR |= 0x8000; // MOE = 1; TIM1->BDTR |= 0x7F; // Dead-time 7Ftick - TIM1->CR1 |= TIM_CR1_CEN; // enable TIM8 - - - -// __HAL_RCC_GPIOA_CLK_ENABLE(); -// -// GPIO_InitTypeDef GPIO_InitStruct = {0}; -// -// GPIO_InitStruct.Pin = GPIO_PIN_7|GPIO_PIN_6; -// GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; -// GPIO_InitStruct.Pull = GPIO_NOPULL; -// GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; -// GPIO_InitStruct.Alternate = GPIO_AF3_TIM1; -// HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - + TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1 } \ No newline at end of file
--- a/INIT_HW/INIT_HW.h Mon Jun 13 08:48:55 2022 +0000 +++ b/INIT_HW/INIT_HW.h Fri Jun 17 06:06:53 2022 +0000 @@ -4,11 +4,12 @@ #include "mbed.h" #include "FastPWM.h" -void Init_ADC(void); -void Init_PWM(); +void Init_ADC1(void); +void Init_ADC2(void); +void Init_ADC3(void); +void Init_TMR1(); +void Init_TMR2(); void Init_TMR3(); -void Init_TMR2(); -void Init_TMR1(); -//void Init_TMR5(); +void Init_TIM4(); #endif
--- a/function_utilities/function_utilities.cpp Mon Jun 13 08:48:55 2022 +0000 +++ b/function_utilities/function_utilities.cpp Fri Jun 17 06:06:53 2022 +0000 @@ -21,7 +21,7 @@ uint8_t FLAG_VALVE_DEADZONE = 0; uint8_t REFERENCE_MODE = 1; int16_t CAN_FREQ = 500; -int16_t DIR_JOINT_ENC = 0; +int16_t DIR_JOINT_ENC = 1; int16_t DIR_VALVE = 0; int16_t DIR_VALVE_ENC = 0; @@ -46,21 +46,10 @@ int16_t flag_delay_test = 0; -//float P_GAIN_VALVE_POSITION_OPP = 0.0f; -//float I_GAIN_VALVE_POSITION_OPP= 0.0f; -//float D_GAIN_VALVE_POSITION_OPP= 0.0f; -//float P_GAIN_JOINT_POSITION_OPP = 0.0f; -//float I_GAIN_JOINT_POSITION_OPP = 0.0f; -//float D_GAIN_JOINT_POSITION_OPP = 0.0f; -//float P_GAIN_JOINT_TORQUE_OPP = 0.0f; -//float I_GAIN_JOINT_TORQUE_OPP = 0.0; -//float D_GAIN_JOINT_TORQUE_OPP = 0.0; - int16_t VALVE_DEADZONE_PLUS; int16_t VALVE_DEADZONE_MINUS; int16_t VELOCITY_COMP_GAIN; -//int16_t COMPLIANCE_GAIN; int16_t VALVE_CENTER; @@ -94,9 +83,6 @@ float V_adapt = 0.0000053f; // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2 float x_4_des_old = 0.0f; -//int16_t VALVE_LIMIT_PLUS; -//int16_t VALVE_LIMIT_MINUS; - float ENC_PULSE_PER_POSITION = 1.0f; float TORQUE_SENSOR_PULSE_PER_TORQUE = 1.0f; float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f; @@ -348,10 +334,7 @@ float K_LPF = 0.0f; float D_LPF = 0.0f; -float torq_sen_past = 0.0f; -float torq_ref_past = 0.0f; -float output_normalized = 0.0f; - +float V_EXI = 0.0f; /******************************************************************************* * General math functions
--- a/main.cpp Mon Jun 13 08:48:55 2022 +0000 +++ b/main.cpp Fri Jun 17 06:06:53 2022 +0000 @@ -21,18 +21,18 @@ AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f // ADC /////////////////////////////////////////// -//AnalogIn adc1(PC_4); //pressure_1 -//AnalogIn adc2(PC_5); //pressure_2 -//AnalogIn adc3(PC_1); //current -//AnalogIn adc4(PB_0); //LVDT +ADC_HandleTypeDef hadc1; +ADC_HandleTypeDef hadc2; +ADC_HandleTypeDef hadc3; + +AnalogIn adc1(PC_4); //pressure_1 +AnalogIn adc2(PC_5); //pressure_2 +AnalogIn adc3(PC_1); //current +AnalogIn adc4(PB_1); //V_EXI +AnalogIn adc5(PA_1); //LVDT // PWM /////////////////////////////////////////// -float dtc_v=0.0f; -float dtc_w=0.0f; - -// LVDT /////////////////////////////////////////// -//DigitalOut LVDT_H(PB_6); -//DigitalOut LVDT_L(PB_7); +float PWM_duty = 0.0f; // SPI /////////////////////////////////////////// SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK); @@ -43,6 +43,13 @@ // LED /////////////////////////////////////////// DigitalOut LED(PA_15); +// LVDT /////////////////////////////////////////// +DigitalOut LVDT_H(PB_6); +DigitalOut LVDT_L(PB_7); + +// MOTOR_ENA /////////////////////////////////////////// +DigitalOut M_ENABLE(PA_2); + // CAN /////////////////////////////////////////// CAN can(PB_8, PB_9, 1000000); CANMessage msg; @@ -159,13 +166,9 @@ 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; @@ -178,7 +181,7 @@ int main() { - + HAL_Init(); SystemClock_Config(); @@ -191,7 +194,7 @@ eeprom_cs = 0; make_delay(); - enc_cs = 1; + enc_cs = 1; enc.format(8,0); enc.frequency(5000000); //10M enc_cs = 0; @@ -211,13 +214,20 @@ ROM_CALL_DATA(); make_delay(); + + // ADC init - Init_ADC(); - make_delay(); + RCC->APB2ENR |= RCC_APB2ENR_ADC1EN; // clock for ADC3 + RCC->APB2ENR |= RCC_APB2ENR_ADC2EN; // clock for ADC2 + RCC->APB2ENR |= RCC_APB2ENR_ADC3EN; // clock for ADC1 - // Pwm init - Init_PWM(); - TIM4->CR1 ^= TIM_CR1_UDIS; + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN; // Enable clock for GPIOC + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOBEN; // Enable clock for GPIOB + RCC->AHB1ENR |= RCC_AHB1ENR_GPIOCEN; // Enable clock for GPIOB + + Init_ADC1(); + Init_ADC2(); + Init_ADC3(); make_delay(); // CAN @@ -229,38 +239,31 @@ 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(); - - // TMR2 init - Init_TMR2(); - TIM2->CR1 ^= TIM_CR1_UDIS; - make_delay(); - - // TMR1 init + // TMR1 init (PWM) 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); + // TMR2 init (Control) + Init_TMR2(); + TIM2->CR1 ^= TIM_CR1_UDIS; + make_delay(); + + // TMR3 init (Sensors) + Init_TMR3(); + TIM3->CR1 ^= TIM_CR1_UDIS; + make_delay(); -// 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); + // TIM4 init (LVDT) + Init_TIM4(); + TIM4->CR1 ^= TIM_CR1_UDIS; + make_delay(); + //Timer priority + NVIC_SetPriority(TIM3_IRQn, 3); + NVIC_SetPriority(TIM2_IRQn, 4); + NVIC_SetPriority(TIM4_IRQn, 2); + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 14, 0); //DAC init if (SENSING_MODE == 0) { @@ -275,6 +278,10 @@ // dac_2 = PRES_A_VREF / 3.3f; // } } + + dac_1 = 1.0f/3.3f; + dac_2 = 1.0f/3.3f; + make_delay(); for (int i=0; i<50; i++) { @@ -284,16 +291,15 @@ ID_index_array[i] = (i+1) * 0.5f; } + M_ENABLE = 1; + /************************************ *** Program is operating! *************************************/ while(1) { - -// if (LED > 0) LED = 0; -// else LED = 1; - TIM1->CCR1 = (TMR1_COUNT)*(0.7f); - +// if (LED > 0) LED = 0; +// else LED = 1; } } @@ -450,64 +456,78 @@ //------------------------------------------------ -// TMR3 : LVDT 1kHz -//----------------------------------------------- +// TMR3 : PWM, Sensor 20kHz +//------------------------------------------------ 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 FREQ_TMR3 = (float)FREQ_20k; +long CNT_TMR3 = 0; +float DT_TMR3 = (float)DT_20k; extern "C" void TIM3_IRQHandler(void) { if (TIM3->SR & TIM_SR_UIF ) { -//// 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; + if (LED > 0) LED = 0; + else LED = 1; + + float PSEN1 = 0.0f; + float PSEN2 = 0.0f; + float CURRENT_SEN = 0.0f; + + /////////////////////////Current//////////////////////////////////////////////////////////////////////////// + HAL_ADC_Start(&hadc2); + HAL_ADC_PollForConversion(&hadc2, 1); + CURRENT_SEN = (float) HAL_ADC_GetValue(&hadc2); + cur.UpdateSen(((float)CURRENT_SEN-2047.5f)/2047.5f*10.0f, FREQ_TMR3, 500.0f); // unit : mA + + /////////////////////////V_EXI//////////////////////////////////////////////////////////////////////////// + HAL_ADC_Start(&hadc2); + HAL_ADC_PollForConversion(&hadc2, 1); + V_EXI = (float) HAL_ADC_GetValue(&hadc2); + + /////////////////////////Encoder//////////////////////////////////////////////////////////////////////////// +// if (CNT_TMR1 % 2) == 0) { + ENC_UPDATE(); // } -// -//// LED = 0; -// -// LVDT_H = 0; -// LVDT_L = 1; - - + /////////////////////////Force or Pressure////////////////////////////////////////////////////////////////// + if (SENSING_MODE == 0) { // Force sensing + + HAL_ADC_Start(&hadc1); + HAL_ADC_PollForConversion(&hadc1, 1); + PSEN1 = (float) HAL_ADC_GetValue(&hadc1); + force.UpdateSen((((float)PSEN1) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR3, 100.0f); // unit : N -// 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; + } else if (SENSING_MODE == 1) { // Pressure sensing -// TIM4->CCR2 = (PWM_ARR)*(0.95f); -// TIM4->CCR1 = (PWM_ARR)*(0.975f); + HAL_ADC_Start(&hadc1); + HAL_ADC_PollForConversion(&hadc1, 1); + PSEN1 = (float) HAL_ADC_GetValue(&hadc1); -// TIM8->CCR1 = (TMR8_COUNT)*(0.95f); -// TIM8->CCR2 = (TMR8_COUNT)*(0.975f); + HAL_ADC_Start(&hadc1); + HAL_ADC_PollForConversion(&hadc1, 1); + PSEN2 = (float) HAL_ADC_GetValue(&hadc1); + float pres_A_new, pres_B_new; + pres_A_new = (((float)PSEN1) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar + pres_B_new = (((float)PSEN2) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; -// PWM_H2 = 0; -// PWM_L2 = 1; + pres_A.UpdateSen(pres_A_new,FREQ_TMR3,200.0f); + pres_B.UpdateSen(pres_B_new,FREQ_TMR3,200.0f); -// TIM3->CCR2 = (TMR3_COUNT)*(0.95f); -// TIM3->CCR1 = (TMR3_COUNT)*(0.975f); - - + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator + float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm + torq.UpdateSen(torq_new,FREQ_TMR3,1000.0f); // unit : Nm + } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator + float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N + force.UpdateSen(force_new,FREQ_TMR3,1000.0f); // unit : N + } + } + CNT_TMR3++; } TIM3->SR = 0x0; // reset the status register } @@ -515,8 +535,8 @@ //------------------------------------------------ -// TMR4 : Sensor Read & Data Handling -//----------------------------------------------- +// TMR4 : LVDT 1kHz +//------------------------------------------------ float FREQ_TMR4 = (float)FREQ_20k; float DT_TMR4 = (float)DT_20k; long CNT_TMR4 = 0; @@ -525,123 +545,59 @@ { if (TIM4->SR & TIM_SR_UIF ) { - // Current =================================================== - //ADC3->CR2 |= 0x40000000; // adc _ 12bit + float LVDT_OUT = 0.0f; + LVDT_sum = 0.0f; + + LVDT_L = 0; + LVDT_H = 1; - cur.UpdateSen(((float)ADC3->DR-2047.5f)/2047.5f*10.0f, FREQ_TMR4, 500.0f); // unit : mA + for (int ij = 0; ij<150; ij++) { - // Encoder =================================================== - if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) { - ENC_UPDATE(); + ADC3->CR2 |= 0x40000000; + LVDT_new = ((float)ADC3->DR) - 2047.5f; + LVDT_sum = LVDT_sum + LVDT_new; } - /* - // Force or Pressure Transducer ============================================= - ADC1->CR2 |= 0x40000000; - if (SENSING_MODE == 0) { // Force sensing - force.UpdateSen((((float)ADC1->DR) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR4, 100.0f); // unit : N - } else if (SENSING_MODE == 1) { // Pressure sensing - float pres_A_new, pres_B_new; - // if (DIR_VALVE_ENC > 0) { - pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar - pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; - // } else { - // pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar - // pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR; - // } - pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f); - pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f); + LVDT_H = 0; + LVDT_L = 1; - if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator - float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm - torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f); // unit : Nm - } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator - float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N - force.UpdateSen(force_new,FREQ_TMR4,1000.0f); // unit : N - } - } - */ + LVDT_new = LVDT_sum * 0.01f*2.0f; -// //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; -// } - - - - + 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; CNT_TMR4++; } TIM4->SR = 0x0; // reset the status register } - -int j =0; -float FREQ_TMR5 = (float)FREQ_5k; -float DT_TMR5 = (float)DT_5k; +//------------------------------------------------ +// TMR2 : Control 5kHz +//------------------------------------------------ +float FREQ_TMR2 = (float)FREQ_5k; +float DT_TMR2 = (float)DT_5k; int cnt_trans = 0; -int can_rest =0; -float force_ref_act_can = 0.0f; - extern "C" void TIM2_IRQHandler(void) { if (TIM2->SR & TIM_SR_UIF ) { -// if (LED > 0) LED = 0; -// else LED = 1; - if(MODE_POS_FT_TRANS == 1) { if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2; - alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR5 /3.0f))/2.0f; + alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f; cnt_trans++; torq.err_int = 0.0f; force.err_int = 0.0f; - if((float)cnt_trans * DT_TMR5 > 3.0f) + if((float)cnt_trans * DT_TMR2 > 3.0f) MODE_POS_FT_TRANS = 2; } else if(MODE_POS_FT_TRANS == 3) { if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0; - alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR5 /3.0f))/2.0f; + alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f; cnt_trans++; torq.err_int = 0.0f; force.err_int = 0.0f; - if((float) cnt_trans * DT_TMR5 > 3.0f ) + if((float) cnt_trans * DT_TMR2 > 3.0f ) MODE_POS_FT_TRANS = 0; } else if(MODE_POS_FT_TRANS == 2) { alpha_trans = 1.0f; @@ -1131,7 +1087,7 @@ VALVE_POS_CONTROL(valve_pos_raw.ref); ref_array[cnt_step_test] = valve_pos_ref; - if(valve_pos.sen >= (float) 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*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER); @@ -1156,7 +1112,7 @@ } 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); + float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR2); if(valve_pos_ref >= 0) { valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f; } else { @@ -1167,7 +1123,7 @@ ref_array[cnt_freq_test] = valve_pos_ref; // if(value>=(float) VALVE_ELECTRIC_CENTER) { - if(valve_pos.sen>=(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 { @@ -1177,7 +1133,7 @@ CONTROL_MODE = MODE_VALVE_OPEN_LOOP; cnt_freq_test++; - if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR5 > 2) { + if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR2 > 2) { buffer_data_size = cnt_freq_test; cnt_freq_test = 0; cnt_send_buffer = 0; @@ -1259,7 +1215,6 @@ // float alpha_torque_ref = 1.0f/(1.0f+TMR_FREQ_5k/(2.0f*PI*1.0f)); // force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act; //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - force_ref_act_can = force_ref_act; force.err = force_ref_act - force.sen; force.err_int += force.err/((float)TMR_FREQ_5k); temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s @@ -1296,9 +1251,6 @@ } } - - - if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve float I_MAX = 10.0f; // Maximum Current : 10mA @@ -1464,31 +1416,29 @@ else if (PWM_out < -1.0f) PWM_out=-1.0f; if (PWM_out>0.0f) { - dtc_v=0.0f; - dtc_w=PWM_out; + TIM1->CCR1 = (TMR1_COUNT)*(PWM_out); + TIM1->CCR2 = (TMR1_COUNT)*(0.0f); } else { - dtc_v=-PWM_out; - dtc_w=0.0f; + TIM1->CCR1 = (TMR1_COUNT)*(0.0f); + TIM1->CCR2 = 0.0f -(TMR1_COUNT)*(PWM_out); } - //////////////////////////////////////////////////////////////////////////// ////////////////////// 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/500) == 0) { - // Position, Velocity, and Torque (ID:1200) - 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)); - - } 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)); - } + if (flag_data_request[0] == LOW) { + CAN_TX_POSITION_FT((int16_t) (cur.sen*1000.0f), (int16_t) (V_EXI), (int16_t) (valve_pos.sen)); +// 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)); +// } } // Valve Position (ID:1300) @@ -1508,7 +1458,6 @@ TMR2_COUNT_CAN_TX = 0; } TMR2_COUNT_CAN_TX++; - } TIM2->SR = 0x0; // reset the status register }
--- a/setting.h Mon Jun 13 08:48:55 2022 +0000 +++ b/setting.h Fri Jun 17 06:06:53 2022 +0000 @@ -2,18 +2,17 @@ #include "FastPWM.h" // pwm -//#define PIN_V PB_4 -//#define PIN_W PB_5 -//#define PIN_V PC_6 -//#define PIN_W PC_7 -#define PIN_V PA_8 -#define PIN_W PA_7 -//#define PIN_V PB_6 -//#define PIN_W PB_7 +#define PIN_H1 PA_8 +#define PIN_L1 PA_7 +#define PIN_H2 PA_9 +#define PIN_L2 PB_0 +//#define PIN_H3 PB_6 +//#define PIN_L3 PB_7 + //#define PWM_ARR 0x465 // loop 80k, pwm 40k //#define PWM_ARR 0x8CA // loop 40k, pwm 20k -#define PWM_ARR 0x1194 // loop 20k, pwm 10k 원래이거 -//#define PWM_ARR 0x2328 // loop 10k, pwm 5k +//#define PWM_ARR 0x1194 // loop 20k, pwm 10k +#define PWM_ARR 0x2328 // loop 10k, pwm 5k //#define PWM_ARR 0xAFC8 // loop 2 k, pwm 1k //#define TMR3_COUNT 0xAFC8 // loop 2 k, pwm 1k @@ -42,8 +41,7 @@ extern AnalogOut dac_1; extern AnalogOut dac_2; -extern float dtc_v; -extern float dtc_w; +extern float PWM_duty; // SPI extern SPI eeprom; //(SPI_MOSI, SPI_MISO, SPI_SCK); @@ -58,6 +56,7 @@ extern CAN can; extern CANMessage msg; + // Board Information @@ -445,9 +444,7 @@ extern float K_LPF; extern float D_LPF; -extern float torq_sen_past; -extern float torq_ref_past; -extern float output_normalized; +extern float V_EXI; @@ -457,6 +454,3 @@ - - -