HydraulicControlBoard_Rainbow_v1_2_copy - 2011

Users » Lightvalve » Code » HydraulicControlBoard_Rainbow_v1_2_copy

Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Rainbow_v1_2_copy

2011

main.cpp@236:544011ad9c85, 2022-06-24 (annotated)

Committer:: Lightvalve
Date:: Fri Jun 24 05:55:29 2022 +0000
Revision:: 236:544011ad9c85
Parent:: 235:bfdf0f479a38
Child:: 237:f69cd0553be0

220624_2

Who changed what in which revision?

User	Revision	Line number	New contents of line
Lightvalve	231:30896263bd8b	1	//Hydraulic Control Board Rainbow
Lightvalve	212:985dba42f261	2	//distributed by Sungwoo Kim
Lightvalve	231:30896263bd8b	3	// 2022/05/31
Lightvalve	212:985dba42f261	4
GiJeongKim	0:51c43836c1d7	5	#include "mbed.h"
GiJeongKim	0:51c43836c1d7	6	#include "FastPWM.h"
GiJeongKim	0:51c43836c1d7	7	#include "INIT_HW.h"
jobuuu	2:a1c0a37df760	8	#include "function_CAN.h"
GiJeongKim	0:51c43836c1d7	9	#include "SPI_EEP_ENC.h"
GiJeongKim	0:51c43836c1d7	10	#include "setting.h"
Lightvalve	11:82d8768d7351	11	#include "function_utilities.h"
Lightvalve	16:903b5a4433b4	12	#include "stm32f4xx_flash.h"
Lightvalve	164:645207e160ca	13	#include "FlashWriter.h"
Lightvalve	165:42c938a40313	14	#include <string>
Lightvalve	165:42c938a40313	15	#include <iostream>
Lightvalve	174:d5377766d7ea	16	#include <cmath>
Lightvalve	59:bc8c8270f0ab	17
GiJeongKim	0:51c43836c1d7	18
Lightvalve	231:30896263bd8b	19	// DAC ///////////////////////////////////////////
jobuuu	218:2c3e5ecbe7e1	20	AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f
jobuuu	218:2c3e5ecbe7e1	21	AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
Lightvalve	231:30896263bd8b	22
Lightvalve	231:30896263bd8b	23	// ADC ///////////////////////////////////////////
Lightvalve	232:e9c5ec04e378	24	ADC_HandleTypeDef hadc1;
Lightvalve	232:e9c5ec04e378	25	ADC_HandleTypeDef hadc2;
Lightvalve	232:e9c5ec04e378	26	ADC_HandleTypeDef hadc3;
Lightvalve	232:e9c5ec04e378	27
Lightvalve	232:e9c5ec04e378	28	AnalogIn adc1(PC_4); //pressure_1
Lightvalve	232:e9c5ec04e378	29	AnalogIn adc2(PC_5); //pressure_2
Lightvalve	232:e9c5ec04e378	30	AnalogIn adc3(PC_1); //current
Lightvalve	232:e9c5ec04e378	31	AnalogIn adc4(PB_1); //V_EXI
Lightvalve	232:e9c5ec04e378	32	AnalogIn adc5(PA_1); //LVDT
GiJeongKim	0:51c43836c1d7	33
jobuuu	7:e9086c72bb22	34	// PWM ///////////////////////////////////////////
Lightvalve	232:e9c5ec04e378	35	float PWM_duty = 0.0f;
GiJeongKim	0:51c43836c1d7	36
jobuuu	7:e9086c72bb22	37	// SPI ///////////////////////////////////////////
Lightvalve	165:42c938a40313	38	SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
Lightvalve	165:42c938a40313	39	DigitalOut eeprom_cs(PB_12);
GiJeongKim	0:51c43836c1d7	40	SPI enc(PC_12,PC_11,PC_10);
GiJeongKim	0:51c43836c1d7	41	DigitalOut enc_cs(PD_2);
Lightvalve	231:30896263bd8b	42
Lightvalve	231:30896263bd8b	43	// LED ///////////////////////////////////////////
Lightvalve	11:82d8768d7351	44	DigitalOut LED(PA_15);
GiJeongKim	0:51c43836c1d7	45
Lightvalve	232:e9c5ec04e378	46	// LVDT ///////////////////////////////////////////
Lightvalve	232:e9c5ec04e378	47	DigitalOut LVDT_H(PB_6);
Lightvalve	232:e9c5ec04e378	48	DigitalOut LVDT_L(PB_7);
Lightvalve	232:e9c5ec04e378	49
Lightvalve	232:e9c5ec04e378	50	// MOTOR_ENA ///////////////////////////////////////////
Lightvalve	232:e9c5ec04e378	51	DigitalOut M_ENABLE(PA_2);
Lightvalve	232:e9c5ec04e378	52
jobuuu	7:e9086c72bb22	53	// CAN ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	54	CAN can(PB_8, PB_9, 1000000);
GiJeongKim	0:51c43836c1d7	55	CANMessage msg;
Lightvalve	230:b235d67d25ba	56	//void onMsgReceived()
Lightvalve	230:b235d67d25ba	57	//{
Lightvalve	230:b235d67d25ba	58	// CAN_RX_HANDLER();
Lightvalve	230:b235d67d25ba	59	//}
jobuuu	2:a1c0a37df760	60
jobuuu	7:e9086c72bb22	61	// Variables ///////////////////////////////////////////
jobuuu	7:e9086c72bb22	62	State pos;
jobuuu	7:e9086c72bb22	63	State vel;
jobuuu	7:e9086c72bb22	64	State Vout;
jobuuu	218:2c3e5ecbe7e1	65	State force;
jobuuu	218:2c3e5ecbe7e1	66	State torq; // unit : N
Lightvalve	198:c70c5a9f17dd	67	State torq_dot;
jobuuu	218:2c3e5ecbe7e1	68	State pres_A; // unit : bar
jobuuu	7:e9086c72bb22	69	State pres_B;
jobuuu	218:2c3e5ecbe7e1	70	State cur; // unit : mA
Lightvalve	14:8e7590227d22	71	State valve_pos;
Lightvalve	226:4e660aa77eb7	72	State valve_pos_raw;
Lightvalve	14:8e7590227d22	73
Lightvalve	19:23b7c1ad8683	74	extern int CID_RX_CMD;
Lightvalve	19:23b7c1ad8683	75	extern int CID_RX_REF_POSITION;
hyhoon	215:699c3e572283	76	extern int CID_RX_REF_OPENLOOP;
Lightvalve	44:35fa6884d0c6	77	extern int CID_RX_REF_PWM;
Lightvalve	19:23b7c1ad8683	78
Lightvalve	19:23b7c1ad8683	79	extern int CID_TX_INFO;
hyhoon	215:699c3e572283	80	extern int CID_TX_POS_VEL_TORQ;
hyhoon	215:699c3e572283	81	extern int CID_TX_PWM;
hyhoon	215:699c3e572283	82	extern int CID_TX_CURRENT;
Lightvalve	19:23b7c1ad8683	83	extern int CID_TX_VOUT;
Lightvalve	19:23b7c1ad8683	84	extern int CID_TX_VALVE_POSITION;
jeongyeseong	213:9c0becc196ba	85	extern int CID_TX_SOMETHING;
Lightvalve	19:23b7c1ad8683	86
Lightvalve	223:c831718303c9	87	inline float tanh_inv(float y)
Lightvalve	223:c831718303c9	88	{
Lightvalve	223:c831718303c9	89	if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
Lightvalve	223:c831718303c9	90	if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f;
jobuuu	218:2c3e5ecbe7e1	91	return log(sqrt((1.0f+y)/(1.0f-y)));
jobuuu	218:2c3e5ecbe7e1	92	}
jobuuu	218:2c3e5ecbe7e1	93
hyhoon	215:699c3e572283	94
Lightvalve	12:6f2531038ea4	95	/*******************************************************************************
Lightvalve	12:6f2531038ea4	96	* REFERENCE MODE
Lightvalve	12:6f2531038ea4	97	******************************************************************************/
Lightvalve	13:747daba9cf59	98	enum _REFERENCE_MODE {
Lightvalve	223:c831718303c9	99	MODE_REF_NO_ACT = 0,
Lightvalve	223:c831718303c9	100	MODE_REF_DIRECT,
Lightvalve	223:c831718303c9	101	MODE_REF_FINDHOME
Lightvalve	12:6f2531038ea4	102	};
Lightvalve	12:6f2531038ea4	103
Lightvalve	12:6f2531038ea4	104	/*******************************************************************************
Lightvalve	12:6f2531038ea4	105	* CONTROL MODE
Lightvalve	12:6f2531038ea4	106	******************************************************************************/
Lightvalve	13:747daba9cf59	107	enum _CONTROL_MODE {
Lightvalve	12:6f2531038ea4	108	//control mode
Lightvalve	12:6f2531038ea4	109	MODE_NO_ACT = 0, //0
Lightvalve	46:fdcb8bd86fd6	110	MODE_VALVE_POSITION_CONTROL, //1
Lightvalve	46:fdcb8bd86fd6	111	MODE_JOINT_CONTROL, //2
Lightvalve	49:3c630b5eba9f	112
Lightvalve	46:fdcb8bd86fd6	113	MODE_VALVE_OPEN_LOOP, //3
Lightvalve	136:a843f32ced33	114	MODE_JOINT_ADAPTIVE_BACKSTEPPING, //4
Lightvalve	166:bfc1fd2629d8	115	MODE_RL, //5
Lightvalve	13:747daba9cf59	116
Lightvalve	12:6f2531038ea4	117	MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
Lightvalve	12:6f2531038ea4	118	MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
Lightvalve	12:6f2531038ea4	119	MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
Lightvalve	13:747daba9cf59	120
Lightvalve	12:6f2531038ea4	121	MODE_TEST_CURRENT_CONTROL, //9
Lightvalve	12:6f2531038ea4	122	MODE_TEST_PWM_CONTROL, //10
Lightvalve	21:e5f1a43ea6f9	123
Lightvalve	14:8e7590227d22	124	MODE_CURRENT_CONTROL, //11
Lightvalve	14:8e7590227d22	125	MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12
Lightvalve	14:8e7590227d22	126	MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13
Lightvalve	212:985dba42f261	127	MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //14
Lightvalve	13:747daba9cf59	128
Lightvalve	12:6f2531038ea4	129	//utility
Lightvalve	12:6f2531038ea4	130	MODE_TORQUE_SENSOR_NULLING = 20, //20
Lightvalve	12:6f2531038ea4	131	MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
Lightvalve	12:6f2531038ea4	132	MODE_FIND_HOME, //22
Lightvalve	55:f4819de54e7a	133	MODE_VALVE_GAIN_SETTING, //23
Lightvalve	12:6f2531038ea4	134	MODE_PRESSURE_SENSOR_NULLING, //24
Lightvalve	12:6f2531038ea4	135	MODE_PRESSURE_SENSOR_CALIB, //25
Lightvalve	12:6f2531038ea4	136	MODE_ROTARY_FRICTION_TUNING, //26
Lightvalve	13:747daba9cf59	137
Lightvalve	212:985dba42f261	138	MODE_DDV_POS_VS_PWM_ID = 30, //30
Lightvalve	12:6f2531038ea4	139	MODE_DDV_DEADZONE_AND_CENTER, //31
Lightvalve	12:6f2531038ea4	140	MODE_DDV_POS_VS_FLOWRATE, //32
Lightvalve	55:f4819de54e7a	141	MODE_SYSTEM_ID, //33
Lightvalve	164:645207e160ca	142	MODE_FREQ_TEST, //34
Lightvalve	164:645207e160ca	143	MODE_SEND_BUFFER, //35
Lightvalve	164:645207e160ca	144	MODE_SEND_OVER, //36
Lightvalve	164:645207e160ca	145	MODE_STEP_TEST, //37
Lightvalve	12:6f2531038ea4	146	};
Lightvalve	12:6f2531038ea4	147
Lightvalve	63:a2d7c63419c2	148	void SystemClock_Config(void)
Lightvalve	63:a2d7c63419c2	149	{
Lightvalve	164:645207e160ca	150	RCC_OscInitTypeDef RCC_OscInitStruct = {0};
Lightvalve	164:645207e160ca	151	RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
Lightvalve	63:a2d7c63419c2	152
Lightvalve	164:645207e160ca	153	__HAL_RCC_PWR_CLK_ENABLE();
Lightvalve	164:645207e160ca	154	__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
Lightvalve	231:30896263bd8b	155
Lightvalve	228:fb5b57e3f157	156	RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
Lightvalve	228:fb5b57e3f157	157	RCC_OscInitStruct.HSEState = RCC_HSE_ON;
Lightvalve	228:fb5b57e3f157	158	// RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
Lightvalve	164:645207e160ca	159	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
Lightvalve	228:fb5b57e3f157	160	RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
Lightvalve	228:fb5b57e3f157	161	RCC_OscInitStruct.PLL.PLLM = 4;//4
Lightvalve	228:fb5b57e3f157	162	RCC_OscInitStruct.PLL.PLLN = 96; //96
Lightvalve	164:645207e160ca	163	RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
Lightvalve	164:645207e160ca	164	RCC_OscInitStruct.PLL.PLLQ = 2;
Lightvalve	164:645207e160ca	165	RCC_OscInitStruct.PLL.PLLR = 2;
Lightvalve	164:645207e160ca	166	if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
Lightvalve	164:645207e160ca	167	//Error_Handler();
Lightvalve	164:645207e160ca	168	}
Lightvalve	164:645207e160ca	169	if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
Lightvalve	164:645207e160ca	170	//Error_Handler();
Lightvalve	164:645207e160ca	171	}
Lightvalve	164:645207e160ca	172	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK\|RCC_CLOCKTYPE_SYSCLK
Lightvalve	164:645207e160ca	173	\|RCC_CLOCKTYPE_PCLK1\|RCC_CLOCKTYPE_PCLK2;
Lightvalve	164:645207e160ca	174	RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
Lightvalve	164:645207e160ca	175	RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
Lightvalve	164:645207e160ca	176	RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
Lightvalve	164:645207e160ca	177	RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
Lightvalve	63:a2d7c63419c2	178	}
Lightvalve	63:a2d7c63419c2	179
Lightvalve	165:42c938a40313	180
Lightvalve	231:30896263bd8b	181
GiJeongKim	0:51c43836c1d7	182	int main()
GiJeongKim	0:51c43836c1d7	183	{
Lightvalve	232:e9c5ec04e378	184
Lightvalve	212:985dba42f261	185	HAL_Init();
Lightvalve	212:985dba42f261	186	SystemClock_Config();
hyhoon	214:82a3ca333004	187
Lightvalve	67:3995ffeaa786	188	LED = 0;
Lightvalve	21:e5f1a43ea6f9	189
Lightvalve	231:30896263bd8b	190	// SPI INIT
hyhoon	215:699c3e572283	191	eeprom_cs = 1;
Lightvalve	165:42c938a40313	192	eeprom.format(8,3);
Lightvalve	165:42c938a40313	193	eeprom.frequency(5000000); //5M
hyhoon	215:699c3e572283	194	eeprom_cs = 0;
hyhoon	215:699c3e572283	195	make_delay();
hyhoon	215:699c3e572283	196
Lightvalve	232:e9c5ec04e378	197	enc_cs = 1;
GiJeongKim	0:51c43836c1d7	198	enc.format(8,0);
hyhoon	215:699c3e572283	199	enc.frequency(5000000); //10M
Lightvalve	231:30896263bd8b	200	enc_cs = 0;
Lightvalve	11:82d8768d7351	201	make_delay();
Lightvalve	21:e5f1a43ea6f9	202
hyhoon	214:82a3ca333004	203	// spi _ enc
hyhoon	214:82a3ca333004	204	spi_enc_set_init();
hyhoon	214:82a3ca333004	205	make_delay();
hyhoon	214:82a3ca333004	206
Lightvalve	231:30896263bd8b	207	// bno rom
Lightvalve	228:fb5b57e3f157	208	spi_eeprom_write(RID_BNO, (int16_t) 0);
Lightvalve	226:4e660aa77eb7	209	make_delay();
hyhoon	214:82a3ca333004	210	////////
hyhoon	214:82a3ca333004	211
Lightvalve	212:985dba42f261	212	// rom
Lightvalve	19:23b7c1ad8683	213	ROM_CALL_DATA();
Lightvalve	16:903b5a4433b4	214	make_delay();
Lightvalve	13:747daba9cf59	215
GiJeongKim	0:51c43836c1d7	216	// ADC init
Lightvalve	232:e9c5ec04e378	217	RCC->APB2ENR \|= RCC_APB2ENR_ADC1EN; // clock for ADC3
Lightvalve	232:e9c5ec04e378	218	RCC->APB2ENR \|= RCC_APB2ENR_ADC2EN; // clock for ADC2
Lightvalve	232:e9c5ec04e378	219	RCC->APB2ENR \|= RCC_APB2ENR_ADC3EN; // clock for ADC1
jobuuu	2:a1c0a37df760	220
Lightvalve	232:e9c5ec04e378	221	RCC->AHB1ENR \|= RCC_AHB1ENR_GPIOAEN; // Enable clock for GPIOC
Lightvalve	232:e9c5ec04e378	222	RCC->AHB1ENR \|= RCC_AHB1ENR_GPIOBEN; // Enable clock for GPIOB
Lightvalve	232:e9c5ec04e378	223	RCC->AHB1ENR \|= RCC_AHB1ENR_GPIOCEN; // Enable clock for GPIOB
Lightvalve	232:e9c5ec04e378	224
Lightvalve	232:e9c5ec04e378	225	Init_ADC1();
Lightvalve	232:e9c5ec04e378	226	Init_ADC2();
Lightvalve	232:e9c5ec04e378	227	Init_ADC3();
Lightvalve	11:82d8768d7351	228	make_delay();
Lightvalve	13:747daba9cf59	229
GiJeongKim	0:51c43836c1d7	230	// CAN
jobuuu	2:a1c0a37df760	231	can.attach(&CAN_RX_HANDLER);
Lightvalve	11:82d8768d7351	232	CAN_ID_INIT();
Lightvalve	11:82d8768d7351	233	make_delay();
Lightvalve	34:bb2ca2fc2a8e	234
hyhoon	214:82a3ca333004	235	//can.reset();
Lightvalve	231:30896263bd8b	236	can.filter(msg.id, 0xFFFFF000, CANStandard);
Lightvalve	231:30896263bd8b	237	// can.filter(0b100000000, 0b100000010, CANStandard); //CAN ID 100~400번대 통과하게
hyhoon	214:82a3ca333004	238
Lightvalve	232:e9c5ec04e378	239	// TMR1 init (PWM)
Lightvalve	231:30896263bd8b	240	Init_TMR1();
Lightvalve	235:bfdf0f479a38	241	TIM1->CR1 &= ~(TIM_CR1_UDIS);
Lightvalve	231:30896263bd8b	242	make_delay();
Lightvalve	231:30896263bd8b	243
Lightvalve	232:e9c5ec04e378	244	// TMR2 init (Control)
Lightvalve	232:e9c5ec04e378	245	Init_TMR2();
Lightvalve	235:bfdf0f479a38	246	TIM2->CR1 &= ~(TIM_CR1_UDIS);
Lightvalve	232:e9c5ec04e378	247	make_delay();
Lightvalve	232:e9c5ec04e378	248
Lightvalve	232:e9c5ec04e378	249	// TMR3 init (Sensors)
Lightvalve	232:e9c5ec04e378	250	Init_TMR3();
Lightvalve	235:bfdf0f479a38	251	TIM3->CR1 &= ~(TIM_CR1_UDIS);
Lightvalve	232:e9c5ec04e378	252	make_delay();
Lightvalve	34:bb2ca2fc2a8e	253
Lightvalve	232:e9c5ec04e378	254	// TIM4 init (LVDT)
Lightvalve	232:e9c5ec04e378	255	Init_TIM4();
Lightvalve	235:bfdf0f479a38	256	TIM4->CR1 &= ~(TIM_CR1_UDIS);
Lightvalve	232:e9c5ec04e378	257	make_delay();
Lightvalve	231:30896263bd8b	258
Lightvalve	232:e9c5ec04e378	259	//Timer priority
Lightvalve	232:e9c5ec04e378	260	NVIC_SetPriority(TIM3_IRQn, 3);
Lightvalve	232:e9c5ec04e378	261	NVIC_SetPriority(TIM2_IRQn, 4);
Lightvalve	232:e9c5ec04e378	262	NVIC_SetPriority(TIM4_IRQn, 2);
Lightvalve	232:e9c5ec04e378	263	HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 14, 0);
hyhoon	214:82a3ca333004	264
Lightvalve	11:82d8768d7351	265	//DAC init
Lightvalve	56:2eade98630e2	266	if (SENSING_MODE == 0) {
jobuuu	218:2c3e5ecbe7e1	267	dac_1 = FORCE_VREF / 3.3f;
Lightvalve	56:2eade98630e2	268	dac_2 = 0.0f;
Lightvalve	56:2eade98630e2	269	} else if (SENSING_MODE == 1) {
Lightvalve	228:fb5b57e3f157	270	dac_1 = PRES_A_VREF / 3.3f;
Lightvalve	228:fb5b57e3f157	271	dac_2 = PRES_B_VREF / 3.3f;
Lightvalve	56:2eade98630e2	272	}
Lightvalve	232:e9c5ec04e378	273
Lightvalve	11:82d8768d7351	274	make_delay();
Lightvalve	13:747daba9cf59	275
Lightvalve	19:23b7c1ad8683	276	for (int i=0; i<50; i++) {
Lightvalve	11:82d8768d7351	277	if(i%2==0)
Lightvalve	38:118df027d851	278	ID_index_array[i] = - i * 0.5f;
Lightvalve	11:82d8768d7351	279	else
Lightvalve	38:118df027d851	280	ID_index_array[i] = (i+1) * 0.5f;
Lightvalve	11:82d8768d7351	281	}
Lightvalve	164:645207e160ca	282
Lightvalve	232:e9c5ec04e378	283	M_ENABLE = 1;
Lightvalve	232:e9c5ec04e378	284
jobuuu	6:df07d3491e3a	285	/************************************
jobuuu	1:e04e563be5ce	286	*** Program is operating!
jobuuu	6:df07d3491e3a	287	*************************************/
GiJeongKim	0:51c43836c1d7	288	while(1) {
Lightvalve	164:645207e160ca	289
Lightvalve	232:e9c5ec04e378	290	// if (LED > 0) LED = 0;
Lightvalve	232:e9c5ec04e378	291	// else LED = 1;
GiJeongKim	0:51c43836c1d7	292	}
jobuuu	1:e04e563be5ce	293	}
jobuuu	1:e04e563be5ce	294
Lightvalve	212:985dba42f261	295
Lightvalve	212:985dba42f261	296	// Velocity feedforward for SW valve
Lightvalve	33:91b17819ec30	297	float DDV_JOINT_POS_FF(float REF_JOINT_VEL)
Lightvalve	14:8e7590227d22	298	{
Lightvalve	13:747daba9cf59	299	int i = 0;
Lightvalve	47:889798ff9329	300	float Ref_Valve_Pos_FF = 0.0f;
Lightvalve	14:8e7590227d22	301	for(i=0; i<VALVE_POS_NUM; i++) {
Lightvalve	14:8e7590227d22	302	if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) {
Lightvalve	14:8e7590227d22	303	if(i==0) {
Lightvalve	49:3c630b5eba9f	304	if(JOINT_VEL[i+1] == JOINT_VEL[i]) {
Lightvalve	55:f4819de54e7a	305	Ref_Valve_Pos_FF = (float) VALVE_CENTER;
Lightvalve	49:3c630b5eba9f	306	} else {
Lightvalve	55:f4819de54e7a	307	Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + (float) VALVE_CENTER;
Lightvalve	38:118df027d851	308	}
Lightvalve	14:8e7590227d22	309	} else {
Lightvalve	49:3c630b5eba9f	310	if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) {
Lightvalve	55:f4819de54e7a	311	Ref_Valve_Pos_FF = (float) VALVE_CENTER;
Lightvalve	49:3c630b5eba9f	312	} else {
Lightvalve	55:f4819de54e7a	313	Ref_Valve_Pos_FF = ((float) 10(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) (REF_JOINT_VEL - JOINT_VEL[i-1])) + (float) VALVE_CENTER + (float) (10*ID_index_array[i-1]);
Lightvalve	38:118df027d851	314	}
Lightvalve	13:747daba9cf59	315	}
Lightvalve	13:747daba9cf59	316	break;
Lightvalve	13:747daba9cf59	317	}
Lightvalve	13:747daba9cf59	318	}
Lightvalve	14:8e7590227d22	319	if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
Lightvalve	33:91b17819ec30	320	Ref_Valve_Pos_FF = (float) VALVE_MAX_POS;
Lightvalve	14:8e7590227d22	321	} else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
Lightvalve	33:91b17819ec30	322	Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
Lightvalve	13:747daba9cf59	323	}
Lightvalve	36:a46e63505ed8	324
Lightvalve	212:985dba42f261	325	Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER); //VELOCITY_COMP_GAIN : 0~100
Lightvalve	13:747daba9cf59	326	return Ref_Valve_Pos_FF;
Lightvalve	13:747daba9cf59	327	}
jobuuu	6:df07d3491e3a	328
Lightvalve	212:985dba42f261	329	// Valve feedforward for SW valve
Lightvalve	30:8d561f16383b	330	void VALVE_POS_CONTROL(float REF_VALVE_POS)
Lightvalve	14:8e7590227d22	331	{
Lightvalve	13:747daba9cf59	332	int i = 0;
Lightvalve	13:747daba9cf59	333
Lightvalve	38:118df027d851	334	if(REF_VALVE_POS > VALVE_MAX_POS) {
Lightvalve	38:118df027d851	335	REF_VALVE_POS = VALVE_MAX_POS;
Lightvalve	38:118df027d851	336	} else if(REF_VALVE_POS < VALVE_MIN_POS) {
Lightvalve	38:118df027d851	337	REF_VALVE_POS = VALVE_MIN_POS;
Lightvalve	38:118df027d851	338	}
Lightvalve	231:30896263bd8b	339	valve_pos_err = (float) (REF_VALVE_POS - valve_pos.sen);
Lightvalve	13:747daba9cf59	340	valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
Lightvalve	13:747daba9cf59	341	valve_pos_err_old = valve_pos_err;
Lightvalve	13:747daba9cf59	342	valve_pos_err_sum += valve_pos_err;
Lightvalve	87:a7b45368ea0f	343	if (valve_pos_err_sum > 1000.0f) valve_pos_err_sum = 1000.0f;
Lightvalve	87:a7b45368ea0f	344	if (valve_pos_err_sum<-1000.0f) valve_pos_err_sum = -1000.0f;
Lightvalve	13:747daba9cf59	345
Lightvalve	13:747daba9cf59	346	VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff;
Lightvalve	14:8e7590227d22	347
Lightvalve	18:b8adf1582ea3	348	for(i=0; i<24; i++) {
Lightvalve	87:a7b45368ea0f	349	if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && (float) REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) {
Lightvalve	14:8e7590227d22	350	if(i==0) {
Lightvalve	47:889798ff9329	351	VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i]);
Lightvalve	14:8e7590227d22	352	} else {
Lightvalve	47:889798ff9329	353	VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1];
Lightvalve	13:747daba9cf59	354	}
Lightvalve	13:747daba9cf59	355	break;
Lightvalve	13:747daba9cf59	356	}
Lightvalve	13:747daba9cf59	357	}
Lightvalve	57:f308b1656d9c	358	Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
Lightvalve	13:747daba9cf59	359	}
Lightvalve	13:747daba9cf59	360
Lightvalve	226:4e660aa77eb7	361	// Valve feedforward for SW valve
Lightvalve	236:544011ad9c85	362	void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS)
Lightvalve	226:4e660aa77eb7	363	{
Lightvalve	226:4e660aa77eb7	364	int i = 0;
Lightvalve	226:4e660aa77eb7	365	float valve_pos_ref_dz = 0.0f;
Lightvalve	236:544011ad9c85	366	if(REF_VALVE_POS > VALVE_MAX_POS) {
Lightvalve	236:544011ad9c85	367	REF_VALVE_POS = VALVE_MAX_POS;
Lightvalve	236:544011ad9c85	368	} else if(REF_VALVE_POS < VALVE_MIN_POS) {
Lightvalve	236:544011ad9c85	369	REF_VALVE_POS = VALVE_MIN_POS;
Lightvalve	226:4e660aa77eb7	370	}
Lightvalve	236:544011ad9c85	371	if (REF_VALVE_POS >= 0) {
Lightvalve	236:544011ad9c85	372	valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_PLUS - VALVE_ELECTRIC_CENTER;
Lightvalve	226:4e660aa77eb7	373	} else {
Lightvalve	236:544011ad9c85	374	valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_MINUS - VALVE_ELECTRIC_CENTER;
Lightvalve	226:4e660aa77eb7	375	}
Lightvalve	230:b235d67d25ba	376	valve_pos_err = (float) (valve_pos_ref_dz - valve_pos.sen);
Lightvalve	226:4e660aa77eb7	377	valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
Lightvalve	226:4e660aa77eb7	378	valve_pos_err_old = valve_pos_err;
Lightvalve	226:4e660aa77eb7	379	valve_pos_err_sum += valve_pos_err;
Lightvalve	226:4e660aa77eb7	380	if (valve_pos_err_sum > 1000.0f) valve_pos_err_sum = 1000.0f;
Lightvalve	226:4e660aa77eb7	381	if (valve_pos_err_sum<-1000.0f) valve_pos_err_sum = -1000.0f;
Lightvalve	226:4e660aa77eb7	382
Lightvalve	226:4e660aa77eb7	383	VALVE_PWM_RAW_FB = P_GAIN_VALVE_POSITION * valve_pos_err + I_GAIN_VALVE_POSITION * valve_pos_err_sum + D_GAIN_VALVE_POSITION * valve_pos_err_diff;
Lightvalve	226:4e660aa77eb7	384
Lightvalve	226:4e660aa77eb7	385	for(i=0; i<24; i++) {
Lightvalve	226:4e660aa77eb7	386	if(valve_pos_ref_dz >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && (float) valve_pos_ref_dz <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) {
Lightvalve	226:4e660aa77eb7	387	if(i==0) {
Lightvalve	226:4e660aa77eb7	388	VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) valve_pos_ref_dz - VALVE_POS_VS_PWM[i]);
Lightvalve	226:4e660aa77eb7	389	} else {
Lightvalve	226:4e660aa77eb7	390	VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) valve_pos_ref_dz - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1];
Lightvalve	226:4e660aa77eb7	391	}
Lightvalve	226:4e660aa77eb7	392	break;
Lightvalve	226:4e660aa77eb7	393	}
Lightvalve	226:4e660aa77eb7	394	}
Lightvalve	226:4e660aa77eb7	395	Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
Lightvalve	226:4e660aa77eb7	396	}
Lightvalve	226:4e660aa77eb7	397
Lightvalve	212:985dba42f261	398	// PWM duty vs. voltage output of L6205 in STM board
Lightvalve	14:8e7590227d22	399	#define LT_MAX_IDX 57
Lightvalve	30:8d561f16383b	400	float LT_PWM_duty[LT_MAX_IDX] = {-100.0f, -80.0f, -60.0f, -50.0f, -40.0f, -35.0f, -30.0f, -25.0f, -20.0f,
Lightvalve	34:bb2ca2fc2a8e	401	-19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f,
Lightvalve	34:bb2ca2fc2a8e	402	-9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f,
Lightvalve	34:bb2ca2fc2a8e	403	1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
Lightvalve	34:bb2ca2fc2a8e	404	11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f,
Lightvalve	34:bb2ca2fc2a8e	405	25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f
Lightvalve	34:bb2ca2fc2a8e	406	}; // duty
Lightvalve	65:c2812cf26c38	407	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,
Lightvalve	65:c2812cf26c38	408	-150.0f, -145.0f, -145.0f, -145.0f, -135.0f, -135.0f, -135.0f, -127.5f, -127.5f, -115.0f,
Lightvalve	65:c2812cf26c38	409	-115.0f, -115.0F, -100.0f, -100.0f, -100.0f, -60.0f, -60.0f, -10.0f, -5.0f, 0.0f,
Lightvalve	65:c2812cf26c38	410	7.5f, 14.0f, 14.0f, 14.0f, 42.5f, 42.5f, 42.5f, 80.0f, 80.0f, 105.0f,
Lightvalve	65:c2812cf26c38	411	105.0f, 105.0f, 120.0f, 120.0f, 120.0f, 131.0f, 131.0f, 140.0f, 140.0f, 140.0f,
Lightvalve	65:c2812cf26c38	412	155.0f, 160.0f, 170.0f, 174.0f, 182.0f, 191.0f, 212.0f, 230.0f
Lightvalve	34:bb2ca2fc2a8e	413	}; // mV
Lightvalve	13:747daba9cf59	414
Lightvalve	30:8d561f16383b	415	float PWM_duty_byLT(float Ref_V)
Lightvalve	14:8e7590227d22	416	{
Lightvalve	30:8d561f16383b	417	float PWM_duty = 0.0f;
Lightvalve	13:747daba9cf59	418	if(Ref_V<LT_Voltage_Output[0]) {
Lightvalve	30:8d561f16383b	419	PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5f+LT_PWM_duty[0];
Lightvalve	13:747daba9cf59	420	} else if (Ref_V>=LT_Voltage_Output[LT_MAX_IDX-1]) {
Lightvalve	30:8d561f16383b	421	PWM_duty = (Ref_V-LT_Voltage_Output[LT_MAX_IDX-1])/1.5f+LT_PWM_duty[LT_MAX_IDX-1];
Lightvalve	13:747daba9cf59	422	} else {
Lightvalve	13:747daba9cf59	423	int idx = 0;
Lightvalve	13:747daba9cf59	424	for(idx=0; idx<LT_MAX_IDX-1; idx++) {
Lightvalve	30:8d561f16383b	425	float ini_x = LT_Voltage_Output[idx];
Lightvalve	30:8d561f16383b	426	float fin_x = LT_Voltage_Output[idx+1];
Lightvalve	30:8d561f16383b	427	float ini_y = LT_PWM_duty[idx];
Lightvalve	30:8d561f16383b	428	float fin_y = LT_PWM_duty[idx+1];
Lightvalve	13:747daba9cf59	429	if(Ref_V>=ini_x && Ref_V<fin_x) {
Lightvalve	13:747daba9cf59	430	PWM_duty = (fin_y-ini_y)/(fin_x-ini_x)*(Ref_V-ini_x) + ini_y;
Lightvalve	13:747daba9cf59	431	break;
Lightvalve	13:747daba9cf59	432	}
Lightvalve	13:747daba9cf59	433	}
Lightvalve	13:747daba9cf59	434	}
Lightvalve	13:747daba9cf59	435	return PWM_duty;
Lightvalve	13:747daba9cf59	436	}
jobuuu	6:df07d3491e3a	437
Lightvalve	55:f4819de54e7a	438
Lightvalve	55:f4819de54e7a	439
jobuuu	2:a1c0a37df760	440	/*******************************************************************************
jobuuu	2:a1c0a37df760	441	TIMER INTERRUPT
jobuuu	2:a1c0a37df760	442	*******************************************************************************/
jobuuu	2:a1c0a37df760	443
Lightvalve	232:e9c5ec04e378	444	//------------------------------------------------
Lightvalve	235:bfdf0f479a38	445	// TMR3 : Sensor 20kHz
Lightvalve	235:bfdf0f479a38	446	//------------------------------------------------
Lightvalve	232:e9c5ec04e378	447	float FREQ_TMR3 = (float)FREQ_20k;
Lightvalve	232:e9c5ec04e378	448	long CNT_TMR3 = 0;
Lightvalve	232:e9c5ec04e378	449	float DT_TMR3 = (float)DT_20k;
Lightvalve	228:fb5b57e3f157	450	extern "C" void TIM3_IRQHandler(void)
Lightvalve	228:fb5b57e3f157	451	{
Lightvalve	228:fb5b57e3f157	452	if (TIM3->SR & TIM_SR_UIF ) {
Lightvalve	228:fb5b57e3f157	453
Lightvalve	233:071785d74ad0	454	// if (LED > 0) LED = 0;
Lightvalve	233:071785d74ad0	455	// else LED = 1;
Lightvalve	232:e9c5ec04e378	456
Lightvalve	232:e9c5ec04e378	457	float PSEN1 = 0.0f;
Lightvalve	232:e9c5ec04e378	458	float PSEN2 = 0.0f;
Lightvalve	232:e9c5ec04e378	459	float CURRENT_SEN = 0.0f;
Lightvalve	232:e9c5ec04e378	460
Lightvalve	232:e9c5ec04e378	461	/////////////////////////Current////////////////////////////////////////////////////////////////////////////
Lightvalve	232:e9c5ec04e378	462	HAL_ADC_Start(&hadc2);
Lightvalve	232:e9c5ec04e378	463	HAL_ADC_PollForConversion(&hadc2, 1);
Lightvalve	232:e9c5ec04e378	464	CURRENT_SEN = (float) HAL_ADC_GetValue(&hadc2);
Lightvalve	232:e9c5ec04e378	465	cur.UpdateSen(((float)CURRENT_SEN-2047.5f)/2047.5f*10.0f, FREQ_TMR3, 500.0f); // unit : mA
Lightvalve	232:e9c5ec04e378	466
Lightvalve	232:e9c5ec04e378	467	/////////////////////////V_EXI////////////////////////////////////////////////////////////////////////////
Lightvalve	232:e9c5ec04e378	468	HAL_ADC_Start(&hadc2);
Lightvalve	232:e9c5ec04e378	469	HAL_ADC_PollForConversion(&hadc2, 1);
Lightvalve	232:e9c5ec04e378	470	V_EXI = (float) HAL_ADC_GetValue(&hadc2);
Lightvalve	232:e9c5ec04e378	471
Lightvalve	232:e9c5ec04e378	472	/////////////////////////Encoder////////////////////////////////////////////////////////////////////////////
Lightvalve	232:e9c5ec04e378	473	// if (CNT_TMR1 % 2) == 0) {
Lightvalve	232:e9c5ec04e378	474	ENC_UPDATE();
Lightvalve	231:30896263bd8b	475	// }
Lightvalve	228:fb5b57e3f157	476
Lightvalve	232:e9c5ec04e378	477	/////////////////////////Force or Pressure//////////////////////////////////////////////////////////////////
Lightvalve	232:e9c5ec04e378	478	if (SENSING_MODE == 0) { // Force sensing
Lightvalve	233:071785d74ad0	479
Lightvalve	232:e9c5ec04e378	480	HAL_ADC_Start(&hadc1);
Lightvalve	232:e9c5ec04e378	481	HAL_ADC_PollForConversion(&hadc1, 1);
Lightvalve	232:e9c5ec04e378	482	PSEN1 = (float) HAL_ADC_GetValue(&hadc1);
Lightvalve	233:071785d74ad0	483
Lightvalve	233:071785d74ad0	484	HAL_ADC_Start(&hadc1);
Lightvalve	233:071785d74ad0	485	HAL_ADC_PollForConversion(&hadc1, 1);
Lightvalve	233:071785d74ad0	486	PSEN2 = (float) HAL_ADC_GetValue(&hadc1);
Lightvalve	233:071785d74ad0	487
Lightvalve	232:e9c5ec04e378	488	force.UpdateSen((((float)PSEN1) - 2047.5f)/TORQUE_SENSOR_PULSE_PER_TORQUE, FREQ_TMR3, 100.0f); // unit : N
Lightvalve	231:30896263bd8b	489
Lightvalve	232:e9c5ec04e378	490	} else if (SENSING_MODE == 1) { // Pressure sensing
Lightvalve	228:fb5b57e3f157	491
Lightvalve	232:e9c5ec04e378	492	HAL_ADC_Start(&hadc1);
Lightvalve	232:e9c5ec04e378	493	HAL_ADC_PollForConversion(&hadc1, 1);
Lightvalve	232:e9c5ec04e378	494	PSEN1 = (float) HAL_ADC_GetValue(&hadc1);
Lightvalve	231:30896263bd8b	495
Lightvalve	232:e9c5ec04e378	496	HAL_ADC_Start(&hadc1);
Lightvalve	232:e9c5ec04e378	497	HAL_ADC_PollForConversion(&hadc1, 1);
Lightvalve	232:e9c5ec04e378	498	PSEN2 = (float) HAL_ADC_GetValue(&hadc1);
Lightvalve	231:30896263bd8b	499
Lightvalve	232:e9c5ec04e378	500	float pres_A_new, pres_B_new;
Lightvalve	232:e9c5ec04e378	501	pres_A_new = (((float)PSEN1) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
Lightvalve	232:e9c5ec04e378	502	pres_B_new = (((float)PSEN2) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
Lightvalve	228:fb5b57e3f157	503
Lightvalve	232:e9c5ec04e378	504	pres_A.UpdateSen(pres_A_new,FREQ_TMR3,200.0f);
Lightvalve	232:e9c5ec04e378	505	pres_B.UpdateSen(pres_B_new,FREQ_TMR3,200.0f);
Lightvalve	228:fb5b57e3f157	506
Lightvalve	232:e9c5ec04e378	507	if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
Lightvalve	232:e9c5ec04e378	508	float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm
Lightvalve	232:e9c5ec04e378	509	torq.UpdateSen(torq_new,FREQ_TMR3,1000.0f); // unit : Nm
Lightvalve	232:e9c5ec04e378	510	} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
Lightvalve	232:e9c5ec04e378	511	float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
Lightvalve	232:e9c5ec04e378	512	force.UpdateSen(force_new,FREQ_TMR3,1000.0f); // unit : N
Lightvalve	232:e9c5ec04e378	513	}
Lightvalve	232:e9c5ec04e378	514	}
Lightvalve	232:e9c5ec04e378	515	CNT_TMR3++;
Lightvalve	228:fb5b57e3f157	516	}
Lightvalve	228:fb5b57e3f157	517	TIM3->SR = 0x0; // reset the status register
Lightvalve	228:fb5b57e3f157	518	}
Lightvalve	228:fb5b57e3f157	519
Lightvalve	228:fb5b57e3f157	520
Lightvalve	228:fb5b57e3f157	521
jobuuu	218:2c3e5ecbe7e1	522	//------------------------------------------------
Lightvalve	232:e9c5ec04e378	523	// TMR4 : LVDT 1kHz
Lightvalve	232:e9c5ec04e378	524	//------------------------------------------------
Lightvalve	235:bfdf0f479a38	525	float LVDT_new = 0.0f;
Lightvalve	235:bfdf0f479a38	526	float LVDT_old = 0.0f;
Lightvalve	235:bfdf0f479a38	527	float LVDT_f_cut = 1000.0f;
Lightvalve	235:bfdf0f479a38	528	float LVDT_LPF = 0.0f;
Lightvalve	235:bfdf0f479a38	529	float LVDT_sum = 0.0f;
Lightvalve	235:bfdf0f479a38	530	float FREQ_TMR4 = (float)FREQ_1k;
Lightvalve	235:bfdf0f479a38	531	float DT_TMR4 = (float)DT_1k;
Lightvalve	55:f4819de54e7a	532	long CNT_TMR4 = 0;
Lightvalve	55:f4819de54e7a	533	int TMR4_FREQ_10k = (int)FREQ_10k;
jobuuu	1:e04e563be5ce	534	extern "C" void TIM4_IRQHandler(void)
jobuuu	1:e04e563be5ce	535	{
Lightvalve	19:23b7c1ad8683	536	if (TIM4->SR & TIM_SR_UIF ) {
Lightvalve	13:747daba9cf59	537
Lightvalve	232:e9c5ec04e378	538	float LVDT_OUT = 0.0f;
Lightvalve	232:e9c5ec04e378	539	LVDT_sum = 0.0f;
Lightvalve	232:e9c5ec04e378	540
Lightvalve	232:e9c5ec04e378	541	LVDT_L = 0;
Lightvalve	232:e9c5ec04e378	542	LVDT_H = 1;
Lightvalve	223:c831718303c9	543
Lightvalve	232:e9c5ec04e378	544	for (int ij = 0; ij<150; ij++) {
jobuuu	218:2c3e5ecbe7e1	545
Lightvalve	232:e9c5ec04e378	546	ADC3->CR2 \|= 0x40000000;
Lightvalve	236:544011ad9c85	547	LVDT_new = ((float)ADC3->DR);
Lightvalve	232:e9c5ec04e378	548	LVDT_sum = LVDT_sum + LVDT_new;
Lightvalve	55:f4819de54e7a	549	}
Lightvalve	59:bc8c8270f0ab	550
Lightvalve	232:e9c5ec04e378	551	LVDT_H = 0;
Lightvalve	232:e9c5ec04e378	552	LVDT_L = 1;
Lightvalve	49:3c630b5eba9f	553
Lightvalve	232:e9c5ec04e378	554	LVDT_new = LVDT_sum * 0.01f*2.0f;
Lightvalve	59:bc8c8270f0ab	555
Lightvalve	232:e9c5ec04e378	556	float alpha_LVDT = 1.0f/(1.0f+TMR_FREQ_1k/(2.0fPI300.0f));
Lightvalve	232:e9c5ec04e378	557	LVDT_LPF = (1.0f-alpha_LVDT) * LVDT_LPF + alpha_LVDT * LVDT_new;
Lightvalve	232:e9c5ec04e378	558	valve_pos.sen = LVDT_LPF;
Lightvalve	232:e9c5ec04e378	559	if(DIR_VALVE_ENC < 0) valve_pos.sen = 0.0f - valve_pos.sen;
Lightvalve	231:30896263bd8b	560
Lightvalve	55:f4819de54e7a	561	CNT_TMR4++;
Lightvalve	21:e5f1a43ea6f9	562	}
Lightvalve	11:82d8768d7351	563	TIM4->SR = 0x0; // reset the status register
Lightvalve	11:82d8768d7351	564	}
Lightvalve	19:23b7c1ad8683	565
Lightvalve	232:e9c5ec04e378	566	//------------------------------------------------
Lightvalve	232:e9c5ec04e378	567	// TMR2 : Control 5kHz
Lightvalve	232:e9c5ec04e378	568	//------------------------------------------------
Lightvalve	232:e9c5ec04e378	569	float FREQ_TMR2 = (float)FREQ_5k;
Lightvalve	232:e9c5ec04e378	570	float DT_TMR2 = (float)DT_5k;
Lightvalve	44:35fa6884d0c6	571	int cnt_trans = 0;
Lightvalve	228:fb5b57e3f157	572	extern "C" void TIM2_IRQHandler(void)
Lightvalve	21:e5f1a43ea6f9	573	{
Lightvalve	228:fb5b57e3f157	574	if (TIM2->SR & TIM_SR_UIF ) {
Lightvalve	228:fb5b57e3f157	575
Lightvalve	49:3c630b5eba9f	576	if(MODE_POS_FT_TRANS == 1) {
jobuuu	218:2c3e5ecbe7e1	577	if (alpha_trans == 1.0f) MODE_POS_FT_TRANS = 2;
Lightvalve	232:e9c5ec04e378	578	alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f;
Lightvalve	44:35fa6884d0c6	579	cnt_trans++;
Lightvalve	221:2ec0de8590d3	580	torq.err_int = 0.0f;
Lightvalve	221:2ec0de8590d3	581	force.err_int = 0.0f;
Lightvalve	232:e9c5ec04e378	582	if((float)cnt_trans * DT_TMR2 > 3.0f)
Lightvalve	44:35fa6884d0c6	583	MODE_POS_FT_TRANS = 2;
Lightvalve	49:3c630b5eba9f	584	} else if(MODE_POS_FT_TRANS == 3) {
jobuuu	218:2c3e5ecbe7e1	585	if (alpha_trans == 0.0f) MODE_POS_FT_TRANS = 0;
Lightvalve	232:e9c5ec04e378	586	alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR2 /3.0f))/2.0f;
Lightvalve	44:35fa6884d0c6	587	cnt_trans++;
Lightvalve	221:2ec0de8590d3	588	torq.err_int = 0.0f;
Lightvalve	221:2ec0de8590d3	589	force.err_int = 0.0f;
Lightvalve	232:e9c5ec04e378	590	if((float) cnt_trans * DT_TMR2 > 3.0f )
Lightvalve	44:35fa6884d0c6	591	MODE_POS_FT_TRANS = 0;
Lightvalve	49:3c630b5eba9f	592	} else if(MODE_POS_FT_TRANS == 2) {
Lightvalve	56:2eade98630e2	593	alpha_trans = 1.0f;
Lightvalve	44:35fa6884d0c6	594	cnt_trans = 0;
Lightvalve	49:3c630b5eba9f	595	} else {
Lightvalve	56:2eade98630e2	596	alpha_trans = 0.0f;
Lightvalve	44:35fa6884d0c6	597	cnt_trans = 0;
Lightvalve	44:35fa6884d0c6	598	}
Lightvalve	44:35fa6884d0c6	599
Lightvalve	49:3c630b5eba9f	600
jobuuu	218:2c3e5ecbe7e1	601	// Reference Update ==========================================================
hyhoon	215:699c3e572283	602	switch (REFERENCE_MODE) {
hyhoon	215:699c3e572283	603	case MODE_REF_NO_ACT: {
hyhoon	215:699c3e572283	604	break;
hyhoon	215:699c3e572283	605	}
hyhoon	215:699c3e572283	606	case MODE_REF_DIRECT: {
hyhoon	215:699c3e572283	607	pos.ref = REF_POSITION;
hyhoon	215:699c3e572283	608	vel.ref = REF_VELOCITY;
hyhoon	215:699c3e572283	609	torq.ref = REF_TORQUE;
jobuuu	218:2c3e5ecbe7e1	610	force.ref = REF_FORCE;
jobuuu	218:2c3e5ecbe7e1	611	break;
jobuuu	218:2c3e5ecbe7e1	612	}
jobuuu	218:2c3e5ecbe7e1	613	case MODE_REF_FINDHOME: {
jobuuu	218:2c3e5ecbe7e1	614	pos.ref = REF_POSITION_FINDHOME;
jobuuu	218:2c3e5ecbe7e1	615	vel.ref = 0.0f;
jobuuu	218:2c3e5ecbe7e1	616	torq.ref = 0.0f;
jobuuu	218:2c3e5ecbe7e1	617	force.ref = 0.0f;
hyhoon	215:699c3e572283	618	break;
hyhoon	215:699c3e572283	619	}
hyhoon	215:699c3e572283	620	default:
hyhoon	215:699c3e572283	621	break;
hyhoon	215:699c3e572283	622	}
hyhoon	215:699c3e572283	623
Lightvalve	225:c2cfe349f5c1	624	if (((OPERATING_MODE&0b110)>>1) == 0) {
jobuuu	218:2c3e5ecbe7e1	625	K_v = 1.03f; // Q = K_vsqrt(deltaP)tanh(C_d*Xv);
Lightvalve	223:c831718303c9	626	C_d = 0.16f;
jobuuu	218:2c3e5ecbe7e1	627	mV_PER_mA = 500.0f; // 5000mV/10mA
jobuuu	218:2c3e5ecbe7e1	628	mV_PER_pulse = 0.5f; // 5000mV/10000pulse
jobuuu	218:2c3e5ecbe7e1	629	mA_PER_pulse = 0.001f; // 10mA/10000pulse
Lightvalve	225:c2cfe349f5c1	630	} else if (((OPERATING_MODE&0b110)>>1) == 1) {
jobuuu	218:2c3e5ecbe7e1	631	K_v = 0.5f; // KNR (LPM >> mA) , 100bar
jobuuu	218:2c3e5ecbe7e1	632	mV_PER_mA = 166.6666f; // 5000mV/30mA
jobuuu	218:2c3e5ecbe7e1	633	mV_PER_pulse = 0.5f; // 5000mV/10000pulse
jobuuu	218:2c3e5ecbe7e1	634	mA_PER_pulse = 0.003f; // 30mA/10000pulse
Lightvalve	225:c2cfe349f5c1	635	} else if (((OPERATING_MODE&0b110)>>1) == 2) {
Lightvalve	225:c2cfe349f5c1	636	C_d = 0.0000845f; // Q = C_d * Valve_pos * sqrt(deltaP*alpha/(1+alpha)) : Valve_pos = 10000, deltaP = 70, alpha = 1 -> Q = 5
jobuuu	218:2c3e5ecbe7e1	637	}
Lightvalve	223:c831718303c9	638
jobuuu	218:2c3e5ecbe7e1	639	// =====================================================================
jobuuu	218:2c3e5ecbe7e1	640	// CONTROL LOOP --------------------------------------------------------
jobuuu	218:2c3e5ecbe7e1	641	// =====================================================================
Lightvalve	55:f4819de54e7a	642	int UTILITY_MODE = 0;
Lightvalve	55:f4819de54e7a	643	int CONTROL_MODE = 0;
Lightvalve	56:2eade98630e2	644
Lightvalve	55:f4819de54e7a	645	if (CONTROL_UTILITY_MODE >= 20 \|\| CONTROL_UTILITY_MODE == 0) {
Lightvalve	55:f4819de54e7a	646	UTILITY_MODE = CONTROL_UTILITY_MODE;
Lightvalve	55:f4819de54e7a	647	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	55:f4819de54e7a	648	} else {
Lightvalve	55:f4819de54e7a	649	CONTROL_MODE = CONTROL_UTILITY_MODE;
Lightvalve	55:f4819de54e7a	650	UTILITY_MODE = MODE_NO_ACT;
Lightvalve	55:f4819de54e7a	651	}
Lightvalve	55:f4819de54e7a	652	// UTILITY MODE ------------------------------------------------------------
Lightvalve	55:f4819de54e7a	653	switch (UTILITY_MODE) {
Lightvalve	55:f4819de54e7a	654	case MODE_NO_ACT: {
Lightvalve	13:747daba9cf59	655	break;
Lightvalve	13:747daba9cf59	656	}
Lightvalve	14:8e7590227d22	657
Lightvalve	223:c831718303c9	658	case MODE_TORQUE_SENSOR_NULLING: {
Lightvalve	233:071785d74ad0	659	static float FORCE_pulse_sum = 0.0f;
Lightvalve	233:071785d74ad0	660	static float PresA_pulse_sum = 0.0f;
Lightvalve	233:071785d74ad0	661	static float PresB_pulse_sum = 0.0f;
Lightvalve	223:c831718303c9	662
Lightvalve	13:747daba9cf59	663	// DAC Voltage reference set
Lightvalve	220:2976cf1b4252	664	float VREF_TuningGain = -0.000003f;
jobuuu	218:2c3e5ecbe7e1	665	if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
jobuuu	218:2c3e5ecbe7e1	666	LED = 1;
jobuuu	218:2c3e5ecbe7e1	667	if(SENSING_MODE == 0) { // Force Sensor (Loadcell)
jobuuu	218:2c3e5ecbe7e1	668	FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE;
jobuuu	218:2c3e5ecbe7e1	669	if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
jobuuu	218:2c3e5ecbe7e1	670	float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f;
jobuuu	218:2c3e5ecbe7e1	671	FORCE_pulse_sum = 0.0f;
jobuuu	218:2c3e5ecbe7e1	672	FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean);
jobuuu	218:2c3e5ecbe7e1	673	if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f;
jobuuu	218:2c3e5ecbe7e1	674	if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
jobuuu	218:2c3e5ecbe7e1	675	dac_1 = FORCE_VREF / 3.3f;
jobuuu	218:2c3e5ecbe7e1	676	}
Lightvalve	223:c831718303c9	677	} else if (SENSING_MODE == 1) { // Pressure Sensor
jobuuu	218:2c3e5ecbe7e1	678	PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR;
jobuuu	218:2c3e5ecbe7e1	679	PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR;
jobuuu	218:2c3e5ecbe7e1	680	if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
jobuuu	218:2c3e5ecbe7e1	681	float PresA_pluse_mean = PresA_pulse_sum / 10.0f;
jobuuu	218:2c3e5ecbe7e1	682	float PresB_pluse_mean = PresB_pulse_sum / 10.0f;
jobuuu	218:2c3e5ecbe7e1	683	PresA_pulse_sum = 0.0f;
jobuuu	218:2c3e5ecbe7e1	684	PresB_pulse_sum = 0.0f;
Lightvalve	19:23b7c1ad8683	685
jobuuu	218:2c3e5ecbe7e1	686	PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean);
jobuuu	218:2c3e5ecbe7e1	687	if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
jobuuu	218:2c3e5ecbe7e1	688	if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
jobuuu	218:2c3e5ecbe7e1	689	dac_1 = PRES_A_VREF / 3.3f;
jobuuu	218:2c3e5ecbe7e1	690	PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean);
jobuuu	218:2c3e5ecbe7e1	691	if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
Lightvalve	223:c831718303c9	692	if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
jobuuu	218:2c3e5ecbe7e1	693	dac_2 = PRES_B_VREF / 3.3f;
jobuuu	218:2c3e5ecbe7e1	694	}
Lightvalve	13:747daba9cf59	695	}
jobuuu	218:2c3e5ecbe7e1	696	TMR3_COUNT_TORQUE_NULL++;
Lightvalve	13:747daba9cf59	697	} else {
jobuuu	218:2c3e5ecbe7e1	698	if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell)
jobuuu	218:2c3e5ecbe7e1	699	FORCE_pulse_sum = 0.0f;
jobuuu	218:2c3e5ecbe7e1	700	dac_1 = FORCE_VREF / 3.3f;
jobuuu	218:2c3e5ecbe7e1	701	spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f));
jobuuu	218:2c3e5ecbe7e1	702	} else if (SENSING_MODE == 1) {
jobuuu	218:2c3e5ecbe7e1	703	PresA_pulse_sum = 0.0f;
jobuuu	218:2c3e5ecbe7e1	704	PresB_pulse_sum = 0.0f;
jobuuu	218:2c3e5ecbe7e1	705	dac_1 = PRES_A_VREF / 3.3f;
jobuuu	218:2c3e5ecbe7e1	706	dac_2 = PRES_B_VREF / 3.3f;
jobuuu	218:2c3e5ecbe7e1	707	spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f));
jobuuu	218:2c3e5ecbe7e1	708	spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f));
jobuuu	218:2c3e5ecbe7e1	709	}
Lightvalve	56:2eade98630e2	710	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	14:8e7590227d22	711	TMR3_COUNT_TORQUE_NULL = 0;
Lightvalve	13:747daba9cf59	712	}
Lightvalve	13:747daba9cf59	713	break;
Lightvalve	19:23b7c1ad8683	714	}
Lightvalve	14:8e7590227d22	715
Lightvalve	223:c831718303c9	716	case MODE_FIND_HOME: {
jobuuu	218:2c3e5ecbe7e1	717	static int cnt_findhome = 0;
jobuuu	218:2c3e5ecbe7e1	718	static int cnt_terminate_findhome = 0;
jobuuu	218:2c3e5ecbe7e1	719	static float FINDHOME_POSITION_pulse = 0.0f;
jobuuu	218:2c3e5ecbe7e1	720	static float FINDHOME_POSITION_pulse_OLD = 0.0f;
jobuuu	218:2c3e5ecbe7e1	721	static float FINDHOME_VELOCITY_pulse = 0.0f;
Lightvalve	223:c831718303c9	722	static float REF_POSITION_FINDHOME_INIT = 0.0f;
jobuuu	218:2c3e5ecbe7e1	723
Lightvalve	29:69f3f5445d6d	724	if (FINDHOME_STAGE == FINDHOME_INIT) {
jobuuu	218:2c3e5ecbe7e1	725	REFERENCE_MODE = MODE_REF_FINDHOME;
Lightvalve	14:8e7590227d22	726	cnt_findhome = 0;
jobuuu	218:2c3e5ecbe7e1	727	cnt_terminate_findhome = 0;
Lightvalve	57:f308b1656d9c	728	pos.ref = pos.sen;
Lightvalve	57:f308b1656d9c	729	vel.ref = 0.0f;
jobuuu	218:2c3e5ecbe7e1	730	REF_POSITION_FINDHOME = pos.ref;
Lightvalve	29:69f3f5445d6d	731	FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
Lightvalve	29:69f3f5445d6d	732	} else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
jobuuu	218:2c3e5ecbe7e1	733	int cnt_check_enc = (TMR_FREQ_5k/20); // 5000/20 = 250tic = 50msec
Lightvalve	29:69f3f5445d6d	734	if(cnt_findhome%cnt_check_enc == 0) {
jobuuu	218:2c3e5ecbe7e1	735	FINDHOME_POSITION_pulse = pos.sen*ENC_PULSE_PER_POSITION;
jobuuu	218:2c3e5ecbe7e1	736	FINDHOME_VELOCITY_pulse = FINDHOME_POSITION_pulse - FINDHOME_POSITION_pulse_OLD;
jobuuu	218:2c3e5ecbe7e1	737	FINDHOME_POSITION_pulse_OLD = FINDHOME_POSITION_pulse;
Lightvalve	29:69f3f5445d6d	738	}
Lightvalve	29:69f3f5445d6d	739	cnt_findhome++;
Lightvalve	14:8e7590227d22	740
jobuuu	218:2c3e5ecbe7e1	741	if (fabs(FINDHOME_VELOCITY_pulse) <= 1) {
jobuuu	218:2c3e5ecbe7e1	742	cnt_terminate_findhome = cnt_terminate_findhome + 1;
Lightvalve	29:69f3f5445d6d	743	} else {
jobuuu	218:2c3e5ecbe7e1	744	cnt_terminate_findhome = 0;
Lightvalve	29:69f3f5445d6d	745	}
Lightvalve	19:23b7c1ad8683	746
jobuuu	218:2c3e5ecbe7e1	747	if ((cnt_terminate_findhome < 3TMR_FREQ_5k) && cnt_findhome < 10TMR_FREQ_5k) { // wait for 3sec
jobuuu	218:2c3e5ecbe7e1	748	double GOTOHOME_SPEED = 10.0f; // 20mm/s or 20deg/s
hyhoon	214:82a3ca333004	749	if (HOMEPOS_OFFSET > 0) {
Lightvalve	223:c831718303c9	750	REF_POSITION_FINDHOME = REF_POSITION_FINDHOME + GOTOHOME_SPEED*DT_5k;
hyhoon	214:82a3ca333004	751	} else {
jobuuu	218:2c3e5ecbe7e1	752	REF_POSITION_FINDHOME = REF_POSITION_FINDHOME - GOTOHOME_SPEED*DT_5k;
hyhoon	214:82a3ca333004	753	}
Lightvalve	57:f308b1656d9c	754	CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	57:f308b1656d9c	755	alpha_trans = 0.0f;
Lightvalve	29:69f3f5445d6d	756	} else {
jobuuu	218:2c3e5ecbe7e1	757	ENC_SET((long)((long)HOMEPOS_OFFSET*10));
jobuuu	218:2c3e5ecbe7e1	758	REF_POSITION_FINDHOME_INIT = (float)((long)HOMEPOS_OFFSET*10);
jobuuu	218:2c3e5ecbe7e1	759	FINDHOME_POSITION_pulse = 0;
jobuuu	218:2c3e5ecbe7e1	760	FINDHOME_POSITION_pulse_OLD = 0;
jobuuu	218:2c3e5ecbe7e1	761	FINDHOME_VELOCITY_pulse = 0;
Lightvalve	223:c831718303c9	762
Lightvalve	29:69f3f5445d6d	763	cnt_findhome = 0;
jobuuu	218:2c3e5ecbe7e1	764	cnt_terminate_findhome = 0;
jobuuu	218:2c3e5ecbe7e1	765	pos.ref = 0.0f;
Lightvalve	29:69f3f5445d6d	766	FINDHOME_STAGE = FINDHOME_ZEROPOSE;
Lightvalve	29:69f3f5445d6d	767	}
Lightvalve	29:69f3f5445d6d	768	} else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
Lightvalve	223:c831718303c9	769
Lightvalve	220:2976cf1b4252	770	// int T_move = 2*TMR_FREQ_5k;
Lightvalve	220:2976cf1b4252	771	int T_move = 10000;
Lightvalve	220:2976cf1b4252	772	REF_POSITION_FINDHOME = ((0.0f - REF_POSITION_FINDHOME_INIT)0.5f(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT)/ENC_PULSE_PER_POSITION;
Lightvalve	223:c831718303c9	773
Lightvalve	29:69f3f5445d6d	774	cnt_findhome++;
Lightvalve	223:c831718303c9	775
Lightvalve	220:2976cf1b4252	776	REFERENCE_MODE = MODE_REF_FINDHOME;
Lightvalve	220:2976cf1b4252	777	CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	220:2976cf1b4252	778	alpha_trans = 0.0f;
Lightvalve	223:c831718303c9	779
Lightvalve	29:69f3f5445d6d	780	if (cnt_findhome >= T_move) {
Lightvalve	29:69f3f5445d6d	781	cnt_findhome = 0;
Lightvalve	30:8d561f16383b	782	pos.ref = 0.0f;
Lightvalve	29:69f3f5445d6d	783	FINDHOME_STAGE = FINDHOME_INIT;
Lightvalve	55:f4819de54e7a	784	CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
jobuuu	218:2c3e5ecbe7e1	785	REFERENCE_MODE = MODE_REF_DIRECT;
hyhoon	214:82a3ca333004	786	}
Lightvalve	13:747daba9cf59	787	}
Lightvalve	13:747daba9cf59	788	break;
Lightvalve	13:747daba9cf59	789	}
Lightvalve	225:c2cfe349f5c1	790
Lightvalve	225:c2cfe349f5c1	791	case MODE_DDV_POS_VS_PWM_ID: {
Lightvalve	225:c2cfe349f5c1	792	CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
Lightvalve	225:c2cfe349f5c1	793	VALVE_ID_timer = VALVE_ID_timer + 1;
Lightvalve	225:c2cfe349f5c1	794
Lightvalve	225:c2cfe349f5c1	795	if(VALVE_ID_timer < TMR_FREQ_5k*1) {
Lightvalve	225:c2cfe349f5c1	796	Vout.ref = 3000.0f * sin(2.0f3.14159fVALVE_ID_timer/TMR_FREQ_5k * 100.0f);
Lightvalve	225:c2cfe349f5c1	797	} else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
Lightvalve	225:c2cfe349f5c1	798	Vout.ref = 1000.0f*(ID_index_array[ID_index]);
Lightvalve	225:c2cfe349f5c1	799	} else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
Lightvalve	225:c2cfe349f5c1	800	VALVE_POS_TMP = 0;
Lightvalve	225:c2cfe349f5c1	801	data_num = 0;
Lightvalve	225:c2cfe349f5c1	802	} else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
Lightvalve	225:c2cfe349f5c1	803	data_num = data_num + 1;
Lightvalve	230:b235d67d25ba	804	VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
Lightvalve	225:c2cfe349f5c1	805	} else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
Lightvalve	225:c2cfe349f5c1	806	Vout.ref = 0.0f;
Lightvalve	225:c2cfe349f5c1	807	} else {
Lightvalve	225:c2cfe349f5c1	808	VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
Lightvalve	225:c2cfe349f5c1	809	VALVE_ID_timer = 0;
Lightvalve	225:c2cfe349f5c1	810	ID_index= ID_index +1;
Lightvalve	225:c2cfe349f5c1	811	}
Lightvalve	225:c2cfe349f5c1	812
Lightvalve	225:c2cfe349f5c1	813	if(ID_index>=25) {
Lightvalve	225:c2cfe349f5c1	814	int i;
Lightvalve	225:c2cfe349f5c1	815	VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
Lightvalve	225:c2cfe349f5c1	816	for(i=0; i<25; i++) {
Lightvalve	225:c2cfe349f5c1	817	VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
Lightvalve	225:c2cfe349f5c1	818	if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
Lightvalve	225:c2cfe349f5c1	819	VALVE_MAX_POS = VALVE_POS_AVG[i];
Lightvalve	225:c2cfe349f5c1	820	VALVE_POS_AVG_OLD = VALVE_MAX_POS;
Lightvalve	225:c2cfe349f5c1	821	} else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) {
Lightvalve	225:c2cfe349f5c1	822	VALVE_MIN_POS = VALVE_POS_AVG[i];
Lightvalve	225:c2cfe349f5c1	823	VALVE_POS_AVG_OLD = VALVE_MIN_POS;
Lightvalve	225:c2cfe349f5c1	824	}
Lightvalve	225:c2cfe349f5c1	825	}
Lightvalve	225:c2cfe349f5c1	826	VALVE_ELECTRIC_CENTER = VALVE_POS_VS_PWM[0];
Lightvalve	225:c2cfe349f5c1	827	spi_eeprom_write(RID_VALVE_ELECTRIC_CENTER, (int16_t) VALVE_ELECTRIC_CENTER);
Lightvalve	225:c2cfe349f5c1	828	spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS);
Lightvalve	225:c2cfe349f5c1	829	spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS);
Lightvalve	225:c2cfe349f5c1	830	for(int i=0; i<25; i++) {
Lightvalve	225:c2cfe349f5c1	831	spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) VALVE_POS_VS_PWM[i]);
Lightvalve	225:c2cfe349f5c1	832	}
Lightvalve	225:c2cfe349f5c1	833	ID_index = 0;
Lightvalve	225:c2cfe349f5c1	834	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	835	}
Lightvalve	225:c2cfe349f5c1	836	break;
Lightvalve	225:c2cfe349f5c1	837	}
Lightvalve	225:c2cfe349f5c1	838	case MODE_DDV_DEADZONE_AND_CENTER: {
Lightvalve	225:c2cfe349f5c1	839	CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
Lightvalve	225:c2cfe349f5c1	840	VALVE_DZ_timer = VALVE_DZ_timer + 1;
Lightvalve	225:c2cfe349f5c1	841	if(first_check == 0) {
Lightvalve	225:c2cfe349f5c1	842	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	843	Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	225:c2cfe349f5c1	844	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	845	Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	225:c2cfe349f5c1	846	pos_plus_end = pos.sen;
Lightvalve	225:c2cfe349f5c1	847	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	848	Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	225:c2cfe349f5c1	849	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	850	Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	225:c2cfe349f5c1	851	pos_minus_end = pos.sen;
Lightvalve	225:c2cfe349f5c1	852	} else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	853	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;
Lightvalve	225:c2cfe349f5c1	854	} else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	855	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;
Lightvalve	225:c2cfe349f5c1	856	data_num = data_num + 1;
Lightvalve	231:30896263bd8b	857	VALVE_POS_TMP = VALVE_POS_TMP + valve_pos.sen;
Lightvalve	225:c2cfe349f5c1	858	} else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	859	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;
Lightvalve	225:c2cfe349f5c1	860	DDV_POS_AVG = VALVE_POS_TMP / data_num;
Lightvalve	225:c2cfe349f5c1	861	START_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	862	VALVE_POS_TMP = 0;
Lightvalve	225:c2cfe349f5c1	863	data_num = 0;
Lightvalve	225:c2cfe349f5c1	864
Lightvalve	225:c2cfe349f5c1	865	} else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	866	valve_pos_raw.ref = DDV_POS_AVG;
Lightvalve	226:4e660aa77eb7	867	VALVE_POS_CONTROL(valve_pos_raw.ref);
Lightvalve	225:c2cfe349f5c1	868
Lightvalve	225:c2cfe349f5c1	869	} else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	870	valve_pos_raw.ref = DDV_POS_AVG;
Lightvalve	226:4e660aa77eb7	871	VALVE_POS_CONTROL(valve_pos_raw.ref);
Lightvalve	225:c2cfe349f5c1	872
Lightvalve	225:c2cfe349f5c1	873	} else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	874	valve_pos_raw.ref = DDV_POS_AVG;
Lightvalve	226:4e660aa77eb7	875	VALVE_POS_CONTROL(valve_pos_raw.ref);
Lightvalve	225:c2cfe349f5c1	876	FINAL_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	877
Lightvalve	226:4e660aa77eb7	878	if((FINAL_POS - START_POS)>1) {
Lightvalve	225:c2cfe349f5c1	879	DZ_case = 1;
Lightvalve	226:4e660aa77eb7	880	} else if((FINAL_POS - START_POS)<-1) {
Lightvalve	225:c2cfe349f5c1	881	DZ_case = -1;
Lightvalve	225:c2cfe349f5c1	882	} else {
Lightvalve	225:c2cfe349f5c1	883	DZ_case = 0;
Lightvalve	225:c2cfe349f5c1	884	}
Lightvalve	225:c2cfe349f5c1	885
Lightvalve	225:c2cfe349f5c1	886	first_check = 1;
Lightvalve	225:c2cfe349f5c1	887	DZ_DIRECTION = 1;
Lightvalve	225:c2cfe349f5c1	888	VALVE_DZ_timer = 0;
Lightvalve	225:c2cfe349f5c1	889	Ref_Valve_Pos_Old = DDV_POS_AVG;
Lightvalve	225:c2cfe349f5c1	890	DZ_NUM = 1;
Lightvalve	225:c2cfe349f5c1	891	DZ_index = 1;
Lightvalve	225:c2cfe349f5c1	892
Lightvalve	225:c2cfe349f5c1	893	}
Lightvalve	225:c2cfe349f5c1	894	} else {
Lightvalve	225:c2cfe349f5c1	895	if((DZ_case == -1 && DZ_NUM == 1) \| (DZ_case == 1 && DZ_NUM == 1)) {
Lightvalve	231:30896263bd8b	896	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	897	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;
Lightvalve	225:c2cfe349f5c1	898	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	899	START_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	900	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	901	valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
Lightvalve	226:4e660aa77eb7	902	if(valve_pos_raw.ref <= VALVE_MIN_POS) {
Lightvalve	226:4e660aa77eb7	903	valve_pos_raw.ref = VALVE_MIN_POS;
Lightvalve	226:4e660aa77eb7	904	} else if(valve_pos_raw.ref >= VALVE_MAX_POS) {
Lightvalve	226:4e660aa77eb7	905	valve_pos_raw.ref = VALVE_MAX_POS;
Lightvalve	225:c2cfe349f5c1	906	}
Lightvalve	226:4e660aa77eb7	907	VALVE_POS_CONTROL(valve_pos_raw.ref);
Lightvalve	225:c2cfe349f5c1	908
Lightvalve	225:c2cfe349f5c1	909	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	910	Ref_Valve_Pos_Old = valve_pos_raw.ref;
Lightvalve	225:c2cfe349f5c1	911	FINAL_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	912
Lightvalve	226:4e660aa77eb7	913	if((FINAL_POS - START_POS)>1) {
Lightvalve	225:c2cfe349f5c1	914	DZ_DIRECTION = 1 * DZ_case;
Lightvalve	226:4e660aa77eb7	915	} else if((FINAL_POS - START_POS)<-1) {
Lightvalve	225:c2cfe349f5c1	916	DZ_DIRECTION = -1 * DZ_case;
Lightvalve	225:c2cfe349f5c1	917	} else {
Lightvalve	225:c2cfe349f5c1	918	DZ_DIRECTION = 1 * DZ_case;
Lightvalve	225:c2cfe349f5c1	919	}
Lightvalve	225:c2cfe349f5c1	920
Lightvalve	225:c2cfe349f5c1	921	VALVE_DZ_timer = 0;
Lightvalve	225:c2cfe349f5c1	922	DZ_index= DZ_index *2;
Lightvalve	225:c2cfe349f5c1	923	if(DZ_index >= 128) {
Lightvalve	226:4e660aa77eb7	924	FIRST_DZ = valve_pos_raw.ref;
Lightvalve	225:c2cfe349f5c1	925	DZ_NUM = 2;
Lightvalve	225:c2cfe349f5c1	926	Ref_Valve_Pos_Old = FIRST_DZ;
Lightvalve	225:c2cfe349f5c1	927	DZ_index = 1;
Lightvalve	225:c2cfe349f5c1	928	DZ_DIRECTION = 1;
Lightvalve	225:c2cfe349f5c1	929	}
Lightvalve	225:c2cfe349f5c1	930	}
Lightvalve	225:c2cfe349f5c1	931	} else if((DZ_case == -1 && DZ_NUM == 2) \| (DZ_case == 1 && DZ_NUM == 2)) {
Lightvalve	225:c2cfe349f5c1	932	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	933	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;
Lightvalve	225:c2cfe349f5c1	934	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	935	START_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	936	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	937	valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
Lightvalve	226:4e660aa77eb7	938	if(valve_pos_raw.ref <= VALVE_MIN_POS) {
Lightvalve	226:4e660aa77eb7	939	valve_pos_raw.ref = VALVE_MIN_POS;
Lightvalve	226:4e660aa77eb7	940	} else if(valve_pos_raw.ref >= VALVE_MAX_POS) {
Lightvalve	226:4e660aa77eb7	941	valve_pos_raw.ref = VALVE_MAX_POS;
Lightvalve	225:c2cfe349f5c1	942	}
Lightvalve	226:4e660aa77eb7	943	VALVE_POS_CONTROL(valve_pos_raw.ref);
Lightvalve	225:c2cfe349f5c1	944
Lightvalve	225:c2cfe349f5c1	945	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	946	Vout.ref = 0.0f;
Lightvalve	225:c2cfe349f5c1	947	} else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	948	Ref_Valve_Pos_Old = valve_pos_raw.ref;
Lightvalve	225:c2cfe349f5c1	949	FINAL_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	950
Lightvalve	226:4e660aa77eb7	951	if((FINAL_POS - START_POS)>1) {
Lightvalve	225:c2cfe349f5c1	952	DZ_DIRECTION = 1 * DZ_case;
Lightvalve	226:4e660aa77eb7	953	} else if((FINAL_POS - START_POS)<-1) {
Lightvalve	225:c2cfe349f5c1	954	DZ_DIRECTION = -1 * DZ_case;
Lightvalve	225:c2cfe349f5c1	955	} else {
Lightvalve	225:c2cfe349f5c1	956	DZ_DIRECTION = -1 * DZ_case;
Lightvalve	225:c2cfe349f5c1	957	}
Lightvalve	225:c2cfe349f5c1	958
Lightvalve	225:c2cfe349f5c1	959	VALVE_DZ_timer = 0;
Lightvalve	225:c2cfe349f5c1	960	DZ_index= DZ_index * 2;
Lightvalve	225:c2cfe349f5c1	961	if(DZ_index >= 128) {
Lightvalve	226:4e660aa77eb7	962	SECOND_DZ = valve_pos_raw.ref;
Lightvalve	225:c2cfe349f5c1	963	VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
Lightvalve	225:c2cfe349f5c1	964	first_check = 0;
Lightvalve	225:c2cfe349f5c1	965	VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
Lightvalve	225:c2cfe349f5c1	966	VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
Lightvalve	225:c2cfe349f5c1	967
Lightvalve	225:c2cfe349f5c1	968	spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
Lightvalve	225:c2cfe349f5c1	969	spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) VALVE_DEADZONE_PLUS);
Lightvalve	225:c2cfe349f5c1	970	spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) VALVE_DEADZONE_MINUS);
Lightvalve	225:c2cfe349f5c1	971
Lightvalve	225:c2cfe349f5c1	972	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	973	DZ_index = 1;
Lightvalve	225:c2cfe349f5c1	974	}
Lightvalve	225:c2cfe349f5c1	975	}
Lightvalve	225:c2cfe349f5c1	976	} else if(DZ_case == 0 && DZ_NUM ==1) {
Lightvalve	225:c2cfe349f5c1	977	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	978	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;
Lightvalve	225:c2cfe349f5c1	979	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	980	START_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	981	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	982	valve_pos_raw.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index;
Lightvalve	226:4e660aa77eb7	983	if(valve_pos_raw.ref <= VALVE_MIN_POS) {
Lightvalve	226:4e660aa77eb7	984	valve_pos_raw.ref = VALVE_MIN_POS;
Lightvalve	226:4e660aa77eb7	985	} else if(valve_pos_raw.ref >= VALVE_MAX_POS) {
Lightvalve	226:4e660aa77eb7	986	valve_pos_raw.ref = VALVE_MAX_POS;
Lightvalve	225:c2cfe349f5c1	987	}
Lightvalve	226:4e660aa77eb7	988	VALVE_POS_CONTROL(valve_pos_raw.ref);
Lightvalve	225:c2cfe349f5c1	989
Lightvalve	225:c2cfe349f5c1	990	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	991	Ref_Valve_Pos_Old = valve_pos_raw.ref;
Lightvalve	225:c2cfe349f5c1	992	FINAL_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	993
Lightvalve	226:4e660aa77eb7	994	if((FINAL_POS - START_POS)>1) {
Lightvalve	225:c2cfe349f5c1	995	DZ_DIRECTION = 1;
Lightvalve	226:4e660aa77eb7	996	} else if((FINAL_POS - START_POS)<-1) {
Lightvalve	225:c2cfe349f5c1	997	DZ_DIRECTION = -1;
Lightvalve	225:c2cfe349f5c1	998	} else {
Lightvalve	225:c2cfe349f5c1	999	DZ_DIRECTION = 1;
Lightvalve	225:c2cfe349f5c1	1000	}
Lightvalve	225:c2cfe349f5c1	1001	VALVE_DZ_timer = 0;
Lightvalve	225:c2cfe349f5c1	1002	DZ_index= DZ_index *2;
Lightvalve	225:c2cfe349f5c1	1003	if(DZ_index >= 128) {
Lightvalve	226:4e660aa77eb7	1004	FIRST_DZ = valve_pos_raw.ref;
Lightvalve	225:c2cfe349f5c1	1005	DZ_NUM = 2;
Lightvalve	225:c2cfe349f5c1	1006	Ref_Valve_Pos_Old = FIRST_DZ;
Lightvalve	225:c2cfe349f5c1	1007	DZ_index = 1;
Lightvalve	225:c2cfe349f5c1	1008	DZ_DIRECTION = 1;
Lightvalve	225:c2cfe349f5c1	1009	}
Lightvalve	225:c2cfe349f5c1	1010	}
Lightvalve	225:c2cfe349f5c1	1011	} else {
Lightvalve	225:c2cfe349f5c1	1012	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	1013	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;
Lightvalve	225:c2cfe349f5c1	1014	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	1015	START_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	1016	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	1017	valve_pos_raw.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
Lightvalve	226:4e660aa77eb7	1018	if(valve_pos_raw.ref <= VALVE_MIN_POS) {
Lightvalve	226:4e660aa77eb7	1019	valve_pos_raw.ref = VALVE_MIN_POS;
Lightvalve	226:4e660aa77eb7	1020	} else if(valve_pos_raw.ref > VALVE_MAX_POS) {
Lightvalve	226:4e660aa77eb7	1021	valve_pos_raw.ref = VALVE_MAX_POS;
Lightvalve	225:c2cfe349f5c1	1022	}
Lightvalve	226:4e660aa77eb7	1023	VALVE_POS_CONTROL(valve_pos_raw.ref);
Lightvalve	225:c2cfe349f5c1	1024
Lightvalve	225:c2cfe349f5c1	1025	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	1026	Vout.ref = 0.0f;
Lightvalve	225:c2cfe349f5c1	1027	} else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	226:4e660aa77eb7	1028	Ref_Valve_Pos_Old = valve_pos_raw.ref;
Lightvalve	225:c2cfe349f5c1	1029	FINAL_POS = pos.sen;
Lightvalve	225:c2cfe349f5c1	1030
Lightvalve	226:4e660aa77eb7	1031	if((FINAL_POS - START_POS)>1) {
Lightvalve	225:c2cfe349f5c1	1032	DZ_DIRECTION = -1;
Lightvalve	226:4e660aa77eb7	1033	} else if((FINAL_POS - START_POS)<-1) {
Lightvalve	225:c2cfe349f5c1	1034	DZ_DIRECTION = 1;
Lightvalve	225:c2cfe349f5c1	1035	} else {
Lightvalve	225:c2cfe349f5c1	1036	DZ_DIRECTION = 1;
Lightvalve	225:c2cfe349f5c1	1037	}
Lightvalve	225:c2cfe349f5c1	1038
Lightvalve	225:c2cfe349f5c1	1039	VALVE_DZ_timer = 0;
Lightvalve	225:c2cfe349f5c1	1040	DZ_index= DZ_index *2;
Lightvalve	225:c2cfe349f5c1	1041	if(DZ_index >= 128) {
Lightvalve	226:4e660aa77eb7	1042	SECOND_DZ = valve_pos_raw.ref;
Lightvalve	225:c2cfe349f5c1	1043	VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
Lightvalve	225:c2cfe349f5c1	1044	first_check = 0;
Lightvalve	225:c2cfe349f5c1	1045	VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
Lightvalve	225:c2cfe349f5c1	1046	VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
Lightvalve	225:c2cfe349f5c1	1047
Lightvalve	225:c2cfe349f5c1	1048	spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
Lightvalve	225:c2cfe349f5c1	1049	spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) VALVE_DEADZONE_PLUS);
Lightvalve	225:c2cfe349f5c1	1050	spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) VALVE_DEADZONE_MINUS);
Lightvalve	225:c2cfe349f5c1	1051
Lightvalve	225:c2cfe349f5c1	1052	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	1053	DZ_index = 1;
Lightvalve	225:c2cfe349f5c1	1054	}
Lightvalve	225:c2cfe349f5c1	1055	}
Lightvalve	225:c2cfe349f5c1	1056	}
Lightvalve	225:c2cfe349f5c1	1057	}
Lightvalve	225:c2cfe349f5c1	1058	break;
Lightvalve	225:c2cfe349f5c1	1059	}
Lightvalve	226:4e660aa77eb7	1060
Lightvalve	225:c2cfe349f5c1	1061	case MODE_STEP_TEST: {
Lightvalve	225:c2cfe349f5c1	1062	float valve_pos_ref = 0.0f;
Lightvalve	225:c2cfe349f5c1	1063	if (cnt_step_test < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	1064	valve_pos_ref = 0.0f;
Lightvalve	225:c2cfe349f5c1	1065	} else {
Lightvalve	225:c2cfe349f5c1	1066	valve_pos_ref = 10000.0f;
Lightvalve	225:c2cfe349f5c1	1067	}
Lightvalve	225:c2cfe349f5c1	1068	if(valve_pos_ref >= 0) {
Lightvalve	231:30896263bd8b	1069	valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
Lightvalve	225:c2cfe349f5c1	1070	} else {
Lightvalve	231:30896263bd8b	1071	valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
Lightvalve	225:c2cfe349f5c1	1072	}
Lightvalve	228:fb5b57e3f157	1073
Lightvalve	236:544011ad9c85	1074	VALVE_POS_CONTROL_DZ(valve_pos_raw.ref);
Lightvalve	228:fb5b57e3f157	1075
Lightvalve	225:c2cfe349f5c1	1076	ref_array[cnt_step_test] = valve_pos_ref;
Lightvalve	232:e9c5ec04e378	1077	if(valve_pos.sen >= (float) VALVE_ELECTRIC_CENTER) {
Lightvalve	231:30896263bd8b	1078	pos_array[cnt_step_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
Lightvalve	225:c2cfe349f5c1	1079	} else {
Lightvalve	231:30896263bd8b	1080	pos_array[cnt_step_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
Lightvalve	225:c2cfe349f5c1	1081	}
Lightvalve	225:c2cfe349f5c1	1082
Lightvalve	226:4e660aa77eb7	1083	CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
Lightvalve	225:c2cfe349f5c1	1084	cnt_step_test++;
Lightvalve	225:c2cfe349f5c1	1085	if (cnt_step_test > (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	225:c2cfe349f5c1	1086	buffer_data_size = cnt_step_test;
Lightvalve	225:c2cfe349f5c1	1087	cnt_step_test = 0;
Lightvalve	225:c2cfe349f5c1	1088	cnt_send_buffer = 0;
Lightvalve	225:c2cfe349f5c1	1089	CONTROL_UTILITY_MODE = MODE_SEND_OVER;
Lightvalve	225:c2cfe349f5c1	1090	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	1091	}
Lightvalve	225:c2cfe349f5c1	1092
Lightvalve	225:c2cfe349f5c1	1093	break;
Lightvalve	225:c2cfe349f5c1	1094	}
Lightvalve	225:c2cfe349f5c1	1095	case MODE_SEND_OVER: {
Lightvalve	225:c2cfe349f5c1	1096	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	1097	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	1098	break;
Lightvalve	225:c2cfe349f5c1	1099	}
Lightvalve	226:4e660aa77eb7	1100
Lightvalve	225:c2cfe349f5c1	1101	case MODE_FREQ_TEST: {
Lightvalve	232:e9c5ec04e378	1102	float valve_pos_ref = 2500.0f * sin(2.0f * 3.141592f * freq_test_valve_ref * (float) cnt_freq_test * DT_TMR2);
Lightvalve	225:c2cfe349f5c1	1103	if(valve_pos_ref >= 0) {
Lightvalve	231:30896263bd8b	1104	valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER + (float)valve_pos_ref * ((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
Lightvalve	225:c2cfe349f5c1	1105	} else {
Lightvalve	231:30896263bd8b	1106	valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f;
Lightvalve	225:c2cfe349f5c1	1107	}
Lightvalve	228:fb5b57e3f157	1108
Lightvalve	236:544011ad9c85	1109	VALVE_POS_CONTROL_DZ(valve_pos_raw.ref);
Lightvalve	228:fb5b57e3f157	1110
Lightvalve	225:c2cfe349f5c1	1111	ref_array[cnt_freq_test] = valve_pos_ref;
Lightvalve	231:30896263bd8b	1112	// if(value>=(float) VALVE_ELECTRIC_CENTER) {
Lightvalve	232:e9c5ec04e378	1113	if(valve_pos.sen>=(float) VALVE_ELECTRIC_CENTER) {
Lightvalve	231:30896263bd8b	1114	// pos_array[cnt_freq_test] = 10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
Lightvalve	231:30896263bd8b	1115	pos_array[cnt_freq_test] = 10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS - (float)VALVE_ELECTRIC_CENTER);
Lightvalve	225:c2cfe349f5c1	1116	} else {
Lightvalve	231:30896263bd8b	1117	// pos_array[cnt_freq_test] = -10000.0f*((float)value - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
Lightvalve	231:30896263bd8b	1118	pos_array[cnt_freq_test] = -10000.0f*((float)valve_pos.sen - (float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS - (float)VALVE_ELECTRIC_CENTER);
Lightvalve	225:c2cfe349f5c1	1119	}
Lightvalve	225:c2cfe349f5c1	1120
Lightvalve	226:4e660aa77eb7	1121	CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
Lightvalve	225:c2cfe349f5c1	1122	cnt_freq_test++;
Lightvalve	232:e9c5ec04e378	1123	if (freq_test_valve_ref * (float) cnt_freq_test * DT_TMR2 > 2) {
Lightvalve	225:c2cfe349f5c1	1124	buffer_data_size = cnt_freq_test;
Lightvalve	225:c2cfe349f5c1	1125	cnt_freq_test = 0;
Lightvalve	225:c2cfe349f5c1	1126	cnt_send_buffer = 0;
Lightvalve	225:c2cfe349f5c1	1127	freq_test_valve_ref = freq_test_valve_ref * 1.05f;
Lightvalve	225:c2cfe349f5c1	1128	if (freq_test_valve_ref >= 400) {
Lightvalve	225:c2cfe349f5c1	1129	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	1130	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	1131	}
Lightvalve	225:c2cfe349f5c1	1132	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	225:c2cfe349f5c1	1133	CONTROL_UTILITY_MODE = MODE_SEND_OVER;
Lightvalve	225:c2cfe349f5c1	1134
Lightvalve	225:c2cfe349f5c1	1135	}
Lightvalve	225:c2cfe349f5c1	1136	break;
Lightvalve	225:c2cfe349f5c1	1137	}
Lightvalve	225:c2cfe349f5c1	1138
Lightvalve	55:f4819de54e7a	1139	default:
Lightvalve	55:f4819de54e7a	1140	break;
Lightvalve	55:f4819de54e7a	1141	}
Lightvalve	55:f4819de54e7a	1142
Lightvalve	55:f4819de54e7a	1143	// CONTROL MODE ------------------------------------------------------------
Lightvalve	55:f4819de54e7a	1144	switch (CONTROL_MODE) {
Lightvalve	55:f4819de54e7a	1145	case MODE_NO_ACT: {
Lightvalve	55:f4819de54e7a	1146	V_out = 0.0f;
Lightvalve	55:f4819de54e7a	1147	break;
Lightvalve	55:f4819de54e7a	1148	}
Lightvalve	55:f4819de54e7a	1149
Lightvalve	55:f4819de54e7a	1150	case MODE_VALVE_POSITION_CONTROL: {
Lightvalve	65:c2812cf26c38	1151	if (OPERATING_MODE == 5) { //SW Valve
Lightvalve	226:4e660aa77eb7	1152	VALVE_POS_CONTROL_DZ(valve_pos.ref);
Lightvalve	57:f308b1656d9c	1153	V_out = Vout.ref;
Lightvalve	65:c2812cf26c38	1154	} else if (CURRENT_CONTROL_MODE == 0) { //PWM
Lightvalve	227:8ac5c6162bc1	1155	I_REF = valve_pos.ref;
Lightvalve	164:645207e160ca	1156	} else {
jobuuu	218:2c3e5ecbe7e1	1157	I_REF = valve_pos.ref * 0.001f; // Unit : pulse >> mA
jobuuu	218:2c3e5ecbe7e1	1158	float I_MAX = 10.0f; // Max : 10mA
jobuuu	218:2c3e5ecbe7e1	1159	if (I_REF > I_MAX) {
jobuuu	218:2c3e5ecbe7e1	1160	I_REF = I_MAX;
jobuuu	218:2c3e5ecbe7e1	1161	} else if (I_REF < -I_MAX) {
jobuuu	218:2c3e5ecbe7e1	1162	I_REF = -I_MAX;
jobuuu	218:2c3e5ecbe7e1	1163	}
Lightvalve	55:f4819de54e7a	1164	}
Lightvalve	55:f4819de54e7a	1165	break;
Lightvalve	55:f4819de54e7a	1166	}
Lightvalve	55:f4819de54e7a	1167
Lightvalve	223:c831718303c9	1168	case MODE_JOINT_CONTROL: {
Lightvalve	55:f4819de54e7a	1169
jobuuu	218:2c3e5ecbe7e1	1170	float temp_vel_pos = 0.0f; // desired velocity for position control
Lightvalve	223:c831718303c9	1171	float temp_vel_FT = 0.0f; // desired velocity for force/torque control
Lightvalve	223:c831718303c9	1172	float temp_vel_ff = 0.0f; // desired velocity for feedforward control
jobuuu	218:2c3e5ecbe7e1	1173	float temp_vel = 0.0f;
Lightvalve	56:2eade98630e2	1174
jobuuu	218:2c3e5ecbe7e1	1175	float wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
Lightvalve	223:c831718303c9	1176
jobuuu	218:2c3e5ecbe7e1	1177	pos.err = pos.ref - pos.sen; // Unit : mm or deg
jobuuu	218:2c3e5ecbe7e1	1178	vel.err = vel.ref - vel.sen; // Unit : mm/s or deg/s
Lightvalve	223:c831718303c9	1179
jobuuu	218:2c3e5ecbe7e1	1180	// position control command ===============================================================================================================================================
Lightvalve	226:4e660aa77eb7	1181	if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode
Lightvalve	223:c831718303c9	1182	temp_vel_pos = 0.1f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err) * PI / 180.0f; // rad/s
Lightvalve	223:c831718303c9	1183	// L when P-gain = 100, f_cut = 10Hz
jobuuu	218:2c3e5ecbe7e1	1184	} else {
Lightvalve	223:c831718303c9	1185	temp_vel_pos = 0.1f * (P_GAIN_JOINT_POSITION * wn_Pos * pos.err); // mm/s
Lightvalve	223:c831718303c9	1186	// L when P-gain = 100, f_cut = 10Hz
jobuuu	218:2c3e5ecbe7e1	1187	}
Lightvalve	55:f4819de54e7a	1188
jobuuu	218:2c3e5ecbe7e1	1189	// torque control command ===============================================================================================================================================
jobuuu	218:2c3e5ecbe7e1	1190	float alpha_SpringDamper = 1.0f/(1.0f+TMR_FREQ_5k/(2.0fPI30.0f));
Lightvalve	226:4e660aa77eb7	1191	K_LPF = (1.0f-alpha_SpringDamper) * K_LPF + alpha_SpringDamper * K_SPRING;
Lightvalve	226:4e660aa77eb7	1192	D_LPF = (1.0f-alpha_SpringDamper) * D_LPF + alpha_SpringDamper * D_DAMPER;
Lightvalve	223:c831718303c9	1193
Lightvalve	226:4e660aa77eb7	1194	if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode
Lightvalve	226:4e660aa77eb7	1195	float torq_ref_act = torq.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : Nm
jobuuu	218:2c3e5ecbe7e1	1196	torq.err = torq_ref_act - torq.sen;
jobuuu	218:2c3e5ecbe7e1	1197	torq.err_int += torq.err/((float)TMR_FREQ_5k);
Lightvalve	223:c831718303c9	1198	temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * torq.err + I_GAIN_JOINT_TORQUE * torq.err_int); // Nm >> rad/s
Lightvalve	55:f4819de54e7a	1199	} else {
Lightvalve	226:4e660aa77eb7	1200	float force_ref_act = force.ref + K_LPF * pos.err + D_LPF * vel.err; // unit : N
Lightvalve	227:8ac5c6162bc1	1201	//////////////////////////////////////////////////force_reference_filter////////////////////////////////////////////////////////////////////
Lightvalve	228:fb5b57e3f157	1202	// float alpha_torque_ref = 1.0f/(1.0f+TMR_FREQ_5k/(2.0fPI1.0f));
Lightvalve	228:fb5b57e3f157	1203	// force_ref_filter = (1.0f-alpha_torque_ref) * force_ref_filter + alpha_torque_ref * force_ref_act;
Lightvalve	227:8ac5c6162bc1	1204	////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Lightvalve	228:fb5b57e3f157	1205	force.err = force_ref_act - force.sen;
jobuuu	218:2c3e5ecbe7e1	1206	force.err_int += force.err/((float)TMR_FREQ_5k);
Lightvalve	223:c831718303c9	1207	temp_vel_FT = 0.001f * (P_GAIN_JOINT_TORQUE * force.err + I_GAIN_JOINT_TORQUE * force.err_int); // N >> mm/s
jobuuu	218:2c3e5ecbe7e1	1208	}
Lightvalve	223:c831718303c9	1209
Lightvalve	223:c831718303c9	1210
Lightvalve	223:c831718303c9	1211	// velocity feedforward command ========================================================================================================================================
Lightvalve	226:4e660aa77eb7	1212	if ((OPERATING_MODE & 0b01) == 0) { // Rotary Mode
jobuuu	218:2c3e5ecbe7e1	1213	temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref * PI / 180.0f; // rad/s
jobuuu	218:2c3e5ecbe7e1	1214	} else {
jobuuu	218:2c3e5ecbe7e1	1215	temp_vel_ff = 0.01f * (float)VELOCITY_COMP_GAIN * vel.ref; // mm/s
jobuuu	218:2c3e5ecbe7e1	1216	}
Lightvalve	164:645207e160ca	1217
Lightvalve	223:c831718303c9	1218	// command integration =================================================================================================================================================
jobuuu	218:2c3e5ecbe7e1	1219	temp_vel = (1.0f - alpha_trans) * temp_vel_pos + alpha_trans * temp_vel_FT + temp_vel_ff; // Position Control + Torque Control + Velocity Feedforward
Lightvalve	225:c2cfe349f5c1	1220
Lightvalve	223:c831718303c9	1221	float Qact = 0.0f; // required flow rate
Lightvalve	225:c2cfe349f5c1	1222	float valve_pos_pulse = 0.0f;
jobuuu	218:2c3e5ecbe7e1	1223	if( temp_vel > 0.0f ) {
jobuuu	218:2c3e5ecbe7e1	1224	Qact = temp_vel * ((float)PISTON_AREA_A * 0.00006f); // mm^3/sec >> LPM
Lightvalve	225:c2cfe349f5c1	1225	if (((OPERATING_MODE&0b110)>>1) == 0 \|\| ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
Lightvalve	225:c2cfe349f5c1	1226	I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f))))/C_d;
Lightvalve	226:4e660aa77eb7	1227	// I_REF = Qact*2.0f;
Lightvalve	225:c2cfe349f5c1	1228	} else { // SW valve
Lightvalve	225:c2cfe349f5c1	1229	valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)));
Lightvalve	225:c2cfe349f5c1	1230	}
jobuuu	218:2c3e5ecbe7e1	1231	} else {
jobuuu	218:2c3e5ecbe7e1	1232	Qact = temp_vel * ((float)PISTON_AREA_B * 0.00006f); // mm^3/sec >> LPM
Lightvalve	225:c2cfe349f5c1	1233	if (((OPERATING_MODE&0b110)>>1) == 0 \|\| ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
Lightvalve	225:c2cfe349f5c1	1234	I_REF = tanh_inv(Qact/(K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f))))/C_d;
Lightvalve	226:4e660aa77eb7	1235	// I_REF = Qact*2.0f;
Lightvalve	225:c2cfe349f5c1	1236	} else { // SW valve
Lightvalve	225:c2cfe349f5c1	1237	valve_pos_pulse = Qact / (C_d * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)));
Lightvalve	225:c2cfe349f5c1	1238	}
jobuuu	218:2c3e5ecbe7e1	1239	}
Lightvalve	225:c2cfe349f5c1	1240
Lightvalve	225:c2cfe349f5c1	1241	if (((OPERATING_MODE&0b110)>>1) == 0 \|\| ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
Lightvalve	225:c2cfe349f5c1	1242
Lightvalve	225:c2cfe349f5c1	1243	float I_MAX = 10.0f; // Maximum Current : 10mA
Lightvalve	225:c2cfe349f5c1	1244	// Anti-windup for FT
Lightvalve	225:c2cfe349f5c1	1245	if (I_GAIN_JOINT_TORQUE > 0.001f) {
Lightvalve	225:c2cfe349f5c1	1246	float Ka = 2.0f;
Lightvalve	225:c2cfe349f5c1	1247	if (I_REF > I_MAX) {
Lightvalve	225:c2cfe349f5c1	1248	float I_rem = I_REF - I_MAX;
Lightvalve	225:c2cfe349f5c1	1249	I_REF = I_MAX;
Lightvalve	225:c2cfe349f5c1	1250	float temp_vel_rem = K_v * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) * tanh(C_dI_rem) / ((double) PISTON_AREA_A 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
Lightvalve	225:c2cfe349f5c1	1251	torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
Lightvalve	225:c2cfe349f5c1	1252	} else if (I_REF < -I_MAX) {
Lightvalve	225:c2cfe349f5c1	1253	double I_rem = I_REF - (-I_MAX);
Lightvalve	225:c2cfe349f5c1	1254	I_REF = -I_MAX;
Lightvalve	225:c2cfe349f5c1	1255	float temp_vel_rem = K_v * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) * tanh(C_dI_rem) / ((double) PISTON_AREA_B 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
Lightvalve	225:c2cfe349f5c1	1256	torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
Lightvalve	225:c2cfe349f5c1	1257	}
Lightvalve	225:c2cfe349f5c1	1258	} else {
Lightvalve	225:c2cfe349f5c1	1259	if(I_REF > I_MAX) {
Lightvalve	225:c2cfe349f5c1	1260	I_REF = I_MAX;
Lightvalve	225:c2cfe349f5c1	1261	} else if (I_REF < -I_MAX) {
Lightvalve	225:c2cfe349f5c1	1262	I_REF = -I_MAX;
Lightvalve	225:c2cfe349f5c1	1263	}
Lightvalve	55:f4819de54e7a	1264	}
Lightvalve	225:c2cfe349f5c1	1265	} else { //SW valve
Lightvalve	225:c2cfe349f5c1	1266	float Valve_pos_MAX = 10000.0f; // Maximum Valve Pos : 10000
Lightvalve	225:c2cfe349f5c1	1267	// Anti-windup for FT
Lightvalve	225:c2cfe349f5c1	1268	if (I_GAIN_JOINT_TORQUE > 0.001f) {
Lightvalve	225:c2cfe349f5c1	1269	float Ka = 2.0f;
Lightvalve	225:c2cfe349f5c1	1270	if (valve_pos_pulse > Valve_pos_MAX) {
Lightvalve	225:c2cfe349f5c1	1271	float valve_pos_rem = valve_pos_pulse - Valve_pos_MAX;
Lightvalve	225:c2cfe349f5c1	1272	valve_pos_pulse = Valve_pos_MAX;
Lightvalve	225:c2cfe349f5c1	1273	float temp_vel_rem = C_d * valve_pos_rem * sqrt(PRES_SUPPLY * alpha3 / (alpha3 + 1.0f)) / ((double) PISTON_AREA_A * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
Lightvalve	225:c2cfe349f5c1	1274	torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
Lightvalve	225:c2cfe349f5c1	1275	} else if (valve_pos_pulse < -Valve_pos_MAX) {
Lightvalve	225:c2cfe349f5c1	1276	double valve_pos_rem = valve_pos_pulse - (-Valve_pos_MAX);
Lightvalve	225:c2cfe349f5c1	1277	valve_pos_pulse = -Valve_pos_MAX;
Lightvalve	225:c2cfe349f5c1	1278	float temp_vel_rem = C_d * valve_pos_rem * sqrt(PRES_SUPPLY / (alpha3 + 1.0f)) / ((double) PISTON_AREA_B * 0.00006f); // Unit : mm/s [linear] / rad/s [rotary]
Lightvalve	225:c2cfe349f5c1	1279	torq.err_int = torq.err_int - Ka * temp_vel_rem * (10000.0f/I_GAIN_JOINT_TORQUE);
Lightvalve	225:c2cfe349f5c1	1280	}
Lightvalve	225:c2cfe349f5c1	1281	} else {
Lightvalve	225:c2cfe349f5c1	1282	if(valve_pos_pulse > 10000.0f) {
Lightvalve	225:c2cfe349f5c1	1283	valve_pos_pulse = 10000.0f;
Lightvalve	225:c2cfe349f5c1	1284	} else if(valve_pos_pulse < -10000.0f) {
Lightvalve	225:c2cfe349f5c1	1285	valve_pos_pulse = -10000.0f;
Lightvalve	225:c2cfe349f5c1	1286	}
Lightvalve	223:c831718303c9	1287	}
Lightvalve	226:4e660aa77eb7	1288	// if (valve_pos_pulse >= 0) {
Lightvalve	226:4e660aa77eb7	1289	// valve_pos.ref = valve_pos_pulse/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS;
Lightvalve	226:4e660aa77eb7	1290	// } else {
Lightvalve	226:4e660aa77eb7	1291	// valve_pos.ref = -valve_pos_pulse/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS;
Lightvalve	226:4e660aa77eb7	1292	// }
Lightvalve	226:4e660aa77eb7	1293	VALVE_POS_CONTROL_DZ(valve_pos_pulse);
Lightvalve	225:c2cfe349f5c1	1294	V_out = Vout.ref;
Lightvalve	55:f4819de54e7a	1295	}
Lightvalve	55:f4819de54e7a	1296	break;
Lightvalve	55:f4819de54e7a	1297	}
Lightvalve	56:2eade98630e2	1298
Lightvalve	55:f4819de54e7a	1299	case MODE_VALVE_OPEN_LOOP: {
Lightvalve	55:f4819de54e7a	1300	V_out = (float) Vout.ref;
Lightvalve	55:f4819de54e7a	1301	break;
Lightvalve	55:f4819de54e7a	1302	}
Lightvalve	164:645207e160ca	1303
Lightvalve	12:6f2531038ea4	1304	default:
Lightvalve	12:6f2531038ea4	1305	break;
Lightvalve	12:6f2531038ea4	1306	}
Lightvalve	14:8e7590227d22	1307
Lightvalve	55:f4819de54e7a	1308
Lightvalve	55:f4819de54e7a	1309	if (((OPERATING_MODE&0b110)>>1) == 0 \|\| ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
Lightvalve	56:2eade98630e2	1310
Lightvalve	55:f4819de54e7a	1311	////////////////////////////////////////////////////////////////////////////
Lightvalve	55:f4819de54e7a	1312	//////////////////////////// CURRENT CONTROL //////////////////////////////
Lightvalve	55:f4819de54e7a	1313	////////////////////////////////////////////////////////////////////////////
Lightvalve	55:f4819de54e7a	1314	if (CURRENT_CONTROL_MODE) {
Lightvalve	65:c2812cf26c38	1315	double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
Lightvalve	55:f4819de54e7a	1316	I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
Lightvalve	55:f4819de54e7a	1317
jobuuu	218:2c3e5ecbe7e1	1318	if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA
jobuuu	218:2c3e5ecbe7e1	1319	else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA
jobuuu	218:2c3e5ecbe7e1	1320	else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA
Lightvalve	223:c831718303c9	1321
hyhoon	214:82a3ca333004	1322	I_ERR = I_REF_fil_DZ - (double)cur.sen;
Lightvalve	65:c2812cf26c38	1323	I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
Lightvalve	55:f4819de54e7a	1324
Lightvalve	55:f4819de54e7a	1325	// Moog Valve Current Control Gain
Lightvalve	65:c2812cf26c38	1326	double R_model = 500.0f; // ohm
Lightvalve	55:f4819de54e7a	1327	double L_model = 1.2f;
Lightvalve	227:8ac5c6162bc1	1328	double w0 = 2.0f * 3.14f * 100.0f;
Lightvalve	55:f4819de54e7a	1329	double KP_I = 0.1f * L_model*w0;
Lightvalve	55:f4819de54e7a	1330	double KI_I = 0.1f * R_model*w0;
Lightvalve	55:f4819de54e7a	1331
Lightvalve	55:f4819de54e7a	1332	// KNR Valve Current Control Gain
Lightvalve	55:f4819de54e7a	1333	if (((OPERATING_MODE & 0b110)>>1) == 1) { // KNR Valve
Lightvalve	55:f4819de54e7a	1334	R_model = 163.0f; // ohm
Lightvalve	55:f4819de54e7a	1335	L_model = 1.0f;
Lightvalve	55:f4819de54e7a	1336	w0 = 2.0f * 3.14f * 80.0f;
Lightvalve	55:f4819de54e7a	1337	KP_I = 1.0f * L_model*w0;
Lightvalve	55:f4819de54e7a	1338	KI_I = 0.08f * R_model*w0;
Lightvalve	55:f4819de54e7a	1339	}
Lightvalve	55:f4819de54e7a	1340
Lightvalve	55:f4819de54e7a	1341	double FF_gain = 1.0f;
Lightvalve	65:c2812cf26c38	1342	VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
hyhoon	214:82a3ca333004	1343	I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old;
hyhoon	214:82a3ca333004	1344	I_REF_fil_old = I_REF_fil_DZ;
Lightvalve	65:c2812cf26c38	1345	// VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV
hyhoon	214:82a3ca333004	1346	VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV
Lightvalve	226:4e660aa77eb7	1347	double V_MAX = VALVE_VOLTAGE_LIMIT*1000.0f; // Maximum Voltage : 12V = 12000mV
Lightvalve	55:f4819de54e7a	1348
Lightvalve	55:f4819de54e7a	1349	double Ka = 3.0f / KP_I;
Lightvalve	55:f4819de54e7a	1350	if (VALVE_PWM_RAW > V_MAX) {
Lightvalve	55:f4819de54e7a	1351	V_rem = VALVE_PWM_RAW - V_MAX;
Lightvalve	55:f4819de54e7a	1352	V_rem = Ka*V_rem;
Lightvalve	55:f4819de54e7a	1353	VALVE_PWM_RAW = V_MAX;
Lightvalve	222:bd00f763acb1	1354	// I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
Lightvalve	222:bd00f763acb1	1355	I_ERR_INT = I_ERR_INT - V_rem;
Lightvalve	55:f4819de54e7a	1356	} else if (VALVE_PWM_RAW < -V_MAX) {
Lightvalve	55:f4819de54e7a	1357	V_rem = VALVE_PWM_RAW - (-V_MAX);
Lightvalve	55:f4819de54e7a	1358	V_rem = Ka*V_rem;
Lightvalve	55:f4819de54e7a	1359	VALVE_PWM_RAW = -V_MAX;
Lightvalve	222:bd00f763acb1	1360	// I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
Lightvalve	222:bd00f763acb1	1361	I_ERR_INT = I_ERR_INT - V_rem;
Lightvalve	55:f4819de54e7a	1362	}
Lightvalve	55:f4819de54e7a	1363	} else {
Lightvalve	55:f4819de54e7a	1364	VALVE_PWM_RAW = I_REF * mV_PER_mA;
Lightvalve	55:f4819de54e7a	1365	}
Lightvalve	55:f4819de54e7a	1366
Lightvalve	55:f4819de54e7a	1367	////////////////////////////////////////////////////////////////////////////
Lightvalve	55:f4819de54e7a	1368	///////////////// Dead Zone Cancellation & Linearization //////////////////
Lightvalve	55:f4819de54e7a	1369	////////////////////////////////////////////////////////////////////////////
Lightvalve	55:f4819de54e7a	1370
Lightvalve	55:f4819de54e7a	1371	// Output Voltage Linearization
hyhoon	214:82a3ca333004	1372	double CUR_PWM_nonlin = (double)VALVE_PWM_RAW; // Unit : mV
Lightvalve	65:c2812cf26c38	1373	double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000
Lightvalve	55:f4819de54e7a	1374
Lightvalve	55:f4819de54e7a	1375	// Dead Zone Cancellation (Electrical dead-zone)
Lightvalve	65:c2812cf26c38	1376	if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f);
Lightvalve	65:c2812cf26c38	1377	else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
Lightvalve	65:c2812cf26c38	1378	else V_out = (float) (CUR_PWM_lin);
Lightvalve	223:c831718303c9	1379
Lightvalve	164:645207e160ca	1380	} else { //////////////////////////sw valve
Lightvalve	212:985dba42f261	1381	// Output Voltage Linearization & Dead Zone Cancellation (Electrical dead-zone) by SW
Lightvalve	133:79885a39c161	1382	if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
Lightvalve	133:79885a39c161	1383	else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f;
Lightvalve	133:79885a39c161	1384	else V_out = 0.0f;
Lightvalve	87:a7b45368ea0f	1385	}
Lightvalve	164:645207e160ca	1386
jobuuu	218:2c3e5ecbe7e1	1387	////////////////////////////////////////////////////////////////////
jobuuu	218:2c3e5ecbe7e1	1388	/////////////////// PWM Command ///////////////////////////////////
jobuuu	218:2c3e5ecbe7e1	1389	////////////////////////////////////////////////////////////////////
Lightvalve	164:645207e160ca	1390	if(DIR_VALVE<0) {
Lightvalve	65:c2812cf26c38	1391	V_out = -V_out;
Lightvalve	65:c2812cf26c38	1392	}
Lightvalve	164:645207e160ca	1393
Lightvalve	48:e7bcfc244d40	1394	if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) {
Lightvalve	48:e7bcfc244d40	1395	V_out = VALVE_VOLTAGE_LIMIT*1000.0f;
Lightvalve	48:e7bcfc244d40	1396	} else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
Lightvalve	48:e7bcfc244d40	1397	V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
Lightvalve	27:a2254a485f23	1398	}
Lightvalve	212:985dba42f261	1399	PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f);
Lightvalve	13:747daba9cf59	1400
Lightvalve	212:985dba42f261	1401	// Saturation of output voltage
Lightvalve	30:8d561f16383b	1402	if(PWM_out > 1.0f) PWM_out=1.0f;
Lightvalve	30:8d561f16383b	1403	else if (PWM_out < -1.0f) PWM_out=-1.0f;
Lightvalve	13:747daba9cf59	1404
Lightvalve	30:8d561f16383b	1405	if (PWM_out>0.0f) {
Lightvalve	232:e9c5ec04e378	1406	TIM1->CCR1 = (TMR1_COUNT)*(PWM_out);
Lightvalve	232:e9c5ec04e378	1407	TIM1->CCR2 = (TMR1_COUNT)*(0.0f);
jobuuu	1:e04e563be5ce	1408	} else {
Lightvalve	232:e9c5ec04e378	1409	TIM1->CCR1 = (TMR1_COUNT)*(0.0f);
Lightvalve	232:e9c5ec04e378	1410	TIM1->CCR2 = 0.0f -(TMR1_COUNT)*(PWM_out);
jobuuu	1:e04e563be5ce	1411	}
Lightvalve	13:747daba9cf59	1412
jobuuu	218:2c3e5ecbe7e1	1413	////////////////////////////////////////////////////////////////////////////
jobuuu	218:2c3e5ecbe7e1	1414	////////////////////// Data transmission through CAN //////////////////////
Lightvalve	223:c831718303c9	1415	////////////////////////////////////////////////////////////////////////////
Lightvalve	233:071785d74ad0	1416
Lightvalve	231:30896263bd8b	1417	// if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
Lightvalve	231:30896263bd8b	1418	if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/500) == 0) {
hyhoon	214:82a3ca333004	1419	// Position, Velocity, and Torque (ID:1200)
Lightvalve	225:c2cfe349f5c1	1420
Lightvalve	233:071785d74ad0	1421	if (flag_data_request[0] == HIGH) {
Lightvalve	233:071785d74ad0	1422	if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
Lightvalve	233:071785d74ad0	1423	CAN_TX_POSITION_FT((int16_t) (pos.sen200.0f), (int16_t) (vel.sen20.0f), (int16_t) (torq.senTORQUE_SENSOR_PULSE_PER_TORQUE10.0f));
Lightvalve	233:071785d74ad0	1424	} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
Lightvalve	233:071785d74ad0	1425	CAN_TX_POSITION_FT((int16_t) (pos.sen200.0f), (int16_t) (vel.sen20.0f), (int16_t) (force.senTORQUE_SENSOR_PULSE_PER_TORQUE10.0f));
Lightvalve	233:071785d74ad0	1426	}
Lightvalve	231:30896263bd8b	1427	}
hyhoon	214:82a3ca333004	1428
jobuuu	218:2c3e5ecbe7e1	1429	// Valve Position (ID:1300)
Lightvalve	54:6f50d9d3bfee	1430	if (flag_data_request[1] == HIGH) {
Lightvalve	231:30896263bd8b	1431	if (((OPERATING_MODE&0b110)>>1) == 0 \|\| ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
Lightvalve	231:30896263bd8b	1432	CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
Lightvalve	231:30896263bd8b	1433	} else {
Lightvalve	236:544011ad9c85	1434	float valve_pos_can = 0.0f;
Lightvalve	236:544011ad9c85	1435	if(valve_pos.sen >= VALVE_ELECTRIC_CENTER) {
Lightvalve	236:544011ad9c85	1436	valve_pos_can = 10000.0f*((float)valve_pos.sen-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
Lightvalve	236:544011ad9c85	1437	} else {
Lightvalve	236:544011ad9c85	1438	valve_pos_can = -10000.0f*((float)valve_pos.sen -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
Lightvalve	236:544011ad9c85	1439	}
Lightvalve	236:544011ad9c85	1440	CAN_TX_PWM((int16_t)(valve_pos_can));
Lightvalve	231:30896263bd8b	1441	}
Lightvalve	52:647072f5307a	1442	}
Lightvalve	56:2eade98630e2	1443
Lightvalve	231:30896263bd8b	1444	// Others : SW (ID:1400)
Lightvalve	166:bfc1fd2629d8	1445	if (flag_data_request[2] == HIGH) {
Lightvalve	231:30896263bd8b	1446	CAN_TX_CURRENT((int16_t) valve_pos.sen, (int16_t) valve_pos.ref);
Lightvalve	52:647072f5307a	1447	}
Lightvalve	20:806196fda269	1448
Lightvalve	52:647072f5307a	1449	TMR2_COUNT_CAN_TX = 0;
Lightvalve	52:647072f5307a	1450	}
Lightvalve	52:647072f5307a	1451	TMR2_COUNT_CAN_TX++;
Lightvalve	20:806196fda269	1452	}
Lightvalve	228:fb5b57e3f157	1453	TIM2->SR = 0x0; // reset the status register
Lightvalve	231:30896263bd8b	1454	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	01 Aug 2022
Imports:	0
Forks:	0
Commits:	245
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@236:544011ad9c85, 2022-06-24 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning