Sungwoo Kim
rainbow
main.cpp
- Committer:
- Lightvalve
- Date:
- 2022-03-31
- Revision:
- 257:c93d3eabff75
- Parent:
- 239:8ac5c6162bc1
File content as of revision 257:c93d3eabff75:
//Hydraulic Control Board
//distributed by Sungwoo Kim
// 2020/12/28
//revised by Buyoun Cho
// 2021/04/20
// 유의사항
// 소수 적을때 뒤에 f 꼭 붙이기
// CAN 선은 ground까지 있는 3상 선으로 써야함.
// 전원은 12~24V 인가.
#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"
#include "FlashWriter.h"
#include <string>
#include <iostream>
#include <cmath>
using namespace std;
Timer t;
// dac & check ///////////////////////////////////////////
DigitalOut check(PC_2);
DigitalOut check_2(PC_3);
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
// PWM ///////////////////////////////////////////
float dtc_v=0.0f;
float dtc_w=0.0f;
// 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
// 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);
DigitalOut LED(PA_15);
// UART ///////////////////////////////////////////
Serial pc(PA_9,PA_10); // _ UART
// CAN ///////////////////////////////////////////
CAN can(PB_8, PB_9, 1000000);
CANMessage msg;
void onMsgReceived()
{
CAN_RX_HANDLER();
}
// Variables ///////////////////////////////////////////
State pos;
State vel;
State Vout;
State force;
State torq; // unit : N
State torq_dot;
State pres_A; // unit : bar
State pres_B;
State cur; // unit : mA
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;
extern int CID_RX_REF_PWM;
extern int CID_TX_INFO;
extern int CID_TX_POS_VEL_TORQ;
extern int CID_TX_PWM;
extern int CID_TX_CURRENT;
extern int CID_TX_VOUT;
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;
if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f;
return log(sqrt((1.0f+y)/(1.0f-y)));
}
/*******************************************************************************
* REFERENCE MODE
******************************************************************************/
enum _REFERENCE_MODE {
MODE_REF_NO_ACT = 0,
MODE_REF_DIRECT,
MODE_REF_FINDHOME
};
/*******************************************************************************
* CONTROL MODE
******************************************************************************/
enum _CONTROL_MODE {
//control mode
MODE_NO_ACT = 0, //0
MODE_VALVE_POSITION_CONTROL, //1
MODE_JOINT_CONTROL, //2
MODE_VALVE_OPEN_LOOP, //3
MODE_JOINT_ADAPTIVE_BACKSTEPPING, //4
MODE_RL, //5
MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
MODE_TEST_CURRENT_CONTROL, //9
MODE_TEST_PWM_CONTROL, //10
MODE_CURRENT_CONTROL, //11
MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12
MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13
MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //14
//utility
MODE_TORQUE_SENSOR_NULLING = 20, //20
MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
MODE_FIND_HOME, //22
MODE_VALVE_GAIN_SETTING, //23
MODE_PRESSURE_SENSOR_NULLING, //24
MODE_PRESSURE_SENSOR_CALIB, //25
MODE_ROTARY_FRICTION_TUNING, //26
MODE_DDV_POS_VS_PWM_ID = 30, //30
MODE_DDV_DEADZONE_AND_CENTER, //31
MODE_DDV_POS_VS_FLOWRATE, //32
MODE_SYSTEM_ID, //33
MODE_FREQ_TEST, //34
MODE_SEND_BUFFER, //35
MODE_SEND_OVER, //36
MODE_STEP_TEST, //37
};
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_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
// RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 1;//4
RCC_OscInitStruct.PLL.PLLN = 192; //96
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_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
//Error_Handler();
}
// HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
// HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
// HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
// HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}
//void SystemClock_Config(void) //External clock
//{
// RCC_OscInitTypeDef RCC_OscInitStruct;
// RCC_ClkInitTypeDef RCC_ClkInitStruct;
//
// /* Configure the main internal regulator output voltage
// */
// __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.HSIState = RCC_HSE_ON;
//// RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
// RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
// RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
// RCC_OscInitStruct.PLL.PLLM = 8;//8
// RCC_OscInitStruct.PLL.PLLN = 80; //180
// RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
// RCC_OscInitStruct.PLL.PLLQ = 4;
// HAL_RCC_OscConfig(&RCC_OscInitStruct);
//
// HAL_PWREx_ActivateOverDrive();
//// 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_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;
// HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
//
// HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
//
// HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
//
// HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
//}
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
// eeprom_cs = 1;
// eeprom.format(8,3);
// eeprom.frequency(5000000); //5M
// eeprom_cs = 0;
// make_delay();
//
// enc_cs = 1; //sw add
// enc.format(8,0);
// enc.frequency(5000000); //10M
// enc_cs = 0; //sw add
//
// make_delay();
//
// // spi _ enc
// spi_enc_set_init();
// make_delay();
//
// ////// bno rom
// spi_eeprom_write(RID_BNO, (int16_t) 0);
// make_delay();
////////
// rom
ROM_CALL_DATA();
make_delay();
// ADC init
Init_ADC();
make_delay();
// Pwm init
Init_PWM();
TIM4->CR1 ^= TIM_CR1_UDIS;
make_delay();
// // CAN
// can.attach(&CAN_RX_HANDLER);
CAN_ID_INIT();
make_delay();
//can.reset();
// can.filter(msg.id, 0xFFFFF000, CANStandard);
// // TMR2 init
// Init_TMR2();
// TIM2->CR1 ^= TIM_CR1_UDIS;
// make_delay();
// TMR3 init
Init_TMR3();
TIM3->CR1 ^= TIM_CR1_UDIS;
make_delay();
// //Timer priority
NVIC_SetPriority(TIM3_IRQn, 3);
NVIC_SetPriority(TIM4_IRQn, 2);
// NVIC_SetPriority(TIM2_IRQn, 2);
//
//
// //DAC init
// if (SENSING_MODE == 0) {
// dac_1 = FORCE_VREF / 3.3f;
// dac_2 = 0.0f;
// } else if (SENSING_MODE == 1) {
// // if (DIR_VALVE_ENC > 0) {
// dac_1 = PRES_A_VREF / 3.3f;
// dac_2 = PRES_B_VREF / 3.3f;
// // } else {
// // dac_1 = PRES_B_VREF / 3.3f;
// // dac_2 = PRES_A_VREF / 3.3f;
// // }
// }
// make_delay();
TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
/************************************
*** Program is operating!
*************************************/
while(1) {
// if (LED > 0) LED = 0;
// else LED = 1;
// ADC1->CR2 |= 0x40000000;
// LVDT_new = ((float)ADC1->DR) - 2047.5f;
// TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
// TIM4->CCR1 = (PWM_ARR)*(1.0f-0.3f);
}
}
//------------------------------------------------
// TMR4 : Sensor Read & Data Handling
//-----------------------------------------------
float FREQ_TMR4 = (float)FREQ_20k;
float DT_TMR4 = (float)DT_20k;
long CNT_TMR4 = 0;
int TMR4_FREQ_20k = (int)FREQ_20k;
//int toggle = 0;
int PWM_Flag = 0;
int TMR4_timer = 0;
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;
extern "C" void TIM4_IRQHandler(void)
{
if (TIM4->SR & TIM_SR_UIF ) {
// if (LED > 0) LED = 0;
// else LED = 1;
PWM_Flag++;
if(PWM_Flag <= 15) {
if (PWM_Flag >= 4 && PWM_Flag <=13) {
ADC1->CR2 |= 0x40000000;
LVDT_new = ((float)ADC1->DR) - 2047.5f;
// if (ADC1->SR &= ~(ADC_SR_EOC)) {
// if (LED > 0) LED = 0;
// else LED = 1;
// LED = 1;
// }
LVDT_sum = LVDT_sum + LVDT_new;
if (LED > 0) LED = 0;
else LED = 1;
}
//pwm
TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
TIM4->CCR1 = (PWM_ARR)*(1.0f-1.0f);
} else if(PWM_Flag <= 500) {
TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
} else {
PWM_Flag = 0;
LVDT_LPF = 0.0f;
force.sen = LVDT_sum * 0.1f;
LVDT_sum = 0.0f;
}
CNT_TMR4++;
}
TIM4->SR = 0x0; // reset the status register
}
int j =0;
float FREQ_TMR3 = (float)FREQ_5k;
float DT_TMR3 = (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;
extern "C" void TIM3_IRQHandler(void)
{
if (TIM3->SR & TIM_SR_UIF ) {
// // =====================================================================
// // CONTROL LOOP --------------------------------------------------------
// // =====================================================================
// int UTILITY_MODE = 0;
// int CONTROL_MODE = 0;
//
// if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) {
// UTILITY_MODE = CONTROL_UTILITY_MODE;
// CONTROL_MODE = MODE_NO_ACT;
// } else {
// CONTROL_MODE = CONTROL_UTILITY_MODE;
// UTILITY_MODE = MODE_NO_ACT;
// }
// // UTILITY MODE ------------------------------------------------------------
// switch (UTILITY_MODE) {
// case MODE_NO_ACT: {
// break;
// }
//
// case MODE_TORQUE_SENSOR_NULLING: {
// static float FORCE_pulse_sum = 0.0;
// static float PresA_pulse_sum = 0.0;
// static float PresB_pulse_sum = 0.0;
//
// // DAC Voltage reference set
// float VREF_TuningGain = -0.00000003f;
// if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
// if(SENSING_MODE == 0) { // Force Sensor (Loadcell)
// FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE;
// if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
// float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f;
// FORCE_pulse_sum = 0.0f;
//
// FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean);
// if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f;
// if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
// dac_1 = FORCE_VREF / 3.3f;
// }
// } else if (SENSING_MODE == 1) { // Pressure Sensor
// PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR;
// PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR;
// if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
// float PresA_pluse_mean = PresA_pulse_sum / 10.0f;
// float PresB_pluse_mean = PresB_pulse_sum / 10.0f;
// PresA_pulse_sum = 0.0f;
// PresB_pulse_sum = 0.0f;
//
// PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean);
// if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
// if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
// dac_1 = PRES_A_VREF / 3.3f;
// PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean);
// if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
// if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
// dac_2 = PRES_B_VREF / 3.3f;
// }
// }
// TMR3_COUNT_TORQUE_NULL++;
// } else {
// if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell)
// FORCE_pulse_sum = 0.0f;
// dac_1 = FORCE_VREF / 3.3f;
// spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f));
// } else if (SENSING_MODE == 1) {
// PresA_pulse_sum = 0.0f;
// PresB_pulse_sum = 0.0f;
// dac_1 = PRES_A_VREF / 3.3f;
// dac_2 = PRES_B_VREF / 3.3f;
// spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f));
// spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f));
// }
// CONTROL_UTILITY_MODE = MODE_NO_ACT;
// TMR3_COUNT_TORQUE_NULL = 0;
// }
// break;
// }
//
//
// default:
// break;
// }
//
// // CONTROL MODE ------------------------------------------------------------
// switch (CONTROL_MODE) {
// case MODE_NO_ACT: {
// V_out = 0.0f;
// break;
// }
//
// case MODE_VALVE_OPEN_LOOP: {
// V_out = (float) Vout.ref;
// break;
// }
//
// default:
// break;
// }
//
// if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
// else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f;
// else V_out = 0.0f;
//
//
// ////////////////////////////////////////////////////////////////////
// /////////////////// PWM Command ///////////////////////////////////
// ////////////////////////////////////////////////////////////////////
//
// 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) {
// V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
// }
// PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f);
//
// // Saturation of output voltage
// if(PWM_out > 1.0f) PWM_out=1.0f;
// else if (PWM_out < -1.0f) PWM_out=-1.0f;
//
// if (PWM_out>0.0f) {
// dtc_v=0.0f;
// dtc_w=PWM_out;
// } else {
// dtc_v=-PWM_out;
// 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) {
// Position, Velocity, and Torque (ID:1200)
if (flag_data_request[0] == LOW) {
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));
} 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));
// CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (valve_pos_can*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));
}
// Others : Pressure A, B, Supply Pressure, etc. (for Debugging) (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);
}
TMR2_COUNT_CAN_TX = 0;
// }
TMR2_COUNT_CAN_TX++;
}
TIM3->SR = 0x0; // reset the status register
}
//
//float FREQ_TMR2 = (float)FREQ_400k;
//float DT_TMR2 = (float)DT_400k;
//int TMR2_timer = 0;
//int toggle_old = 0;
//
//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;
//
//extern "C" void TIM2_IRQHandler(void)
//{
// if (TIM2->SR & TIM_SR_UIF ) {
//// if (LED > 0) LED = 0;
//// else LED = 1;
//// LED = 1;
//
// if (toggle != toggle_old) {
// TMR2_timer = 0;
// LVDT_LPF = 0.0f;
// LVDT_sum = 0.0f;
//// if (LED > 0) LED = 0;
//// else LED = 1;
// }
//
// if (TMR2_timer >= 6 && TMR2_timer <=45) {
//// if (TMR2_timer >= 6 && TMR2_timer <=25) {
//// if (LED > 0) LED = 0;
//// else LED = 1;
//// LED = 0;
// ADC1->CR2 |= 0x40000000;
// LVDT_new = ((float)ADC1->DR) - 2047.5f;
// if (ADC1->SR &= ~(ADC_SR_EOC)) {
//// if (LED > 0) LED = 0;
//// else LED = 1;
//// LED = 1;
// }
//// float alpha_update = 1.0f / (1.0f + FREQ_TMR2 / (2.0f * 3.14f * LVDT_f_cut)); // f_cutoff : 100Hz
//// LVDT_LPF = (1.0f - alpha_update) * LVDT_LPF + alpha_update * LVDT_new;
//// LVDT_sum = LVDT_sum + LVDT_LPF;
// LVDT_sum = LVDT_sum + LVDT_new;
//
//// force.UpdateSen((((float)ADC1->DR) - 2047.5f), FREQ_TMR2, 100.0f); // unit : N
//// this->sen_diff = (sen_new-this->sen)*Freq_update;
//// float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz
//// this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new;
//// force.sen = LVDT_new;
// }
//
// else if (TMR2_timer == 46) { //46
//// if (TMR2_timer == 50) {
//// LED = 1;
//// ADC1->CR2 |= 0x40000000;
//// LVDT_new = ((float)ADC1->DR) - 2047.5f;
//// force.sen = LVDT_new;
// force.sen = LVDT_sum * 0.025f;
//// force.sen = LVDT_sum;
//
//// ADC1->CR2 |= 0x40000000;
//// LVDT_new = ((float)ADC1->DR) - 2047.5f;
//// force.sen = LVDT_new;
//// force.UpdateSen(LVDT_sum * 0.025f, 1000.0f, 10.0f);
//// LED = 0;
// }
//// LED = 0;
// toggle_old = toggle;
// TMR2_timer++;
// }
// TIM2->SR = 0x0; // reset the status register
//
//}