HydraulicControlBoard_Start - 20210305

Users » hyhoon » Code » HydraulicControlBoard_Start

Seunghoon Shin / Mbed 2 deprecated HydraulicControlBoard_Start

20210305

main.cpp@113:4535bc97a1db, 2020-09-12 (annotated)

Committer:: Lightvalve
Date:: Sat Sep 12 08:58:57 2020 +0000
Revision:: 113:4535bc97a1db
Parent:: 112:8dcb1600cb90
Child:: 114:09a6bccaf17d

200912_2

Who changed what in which revision?

User	Revision	Line number	New contents of line
Lightvalve	113:4535bc97a1db	1	//200912-2
GiJeongKim	0:51c43836c1d7	2	#include "mbed.h"
GiJeongKim	0:51c43836c1d7	3	#include "FastPWM.h"
GiJeongKim	0:51c43836c1d7	4	#include "INIT_HW.h"
jobuuu	2:a1c0a37df760	5	#include "function_CAN.h"
GiJeongKim	0:51c43836c1d7	6	#include "SPI_EEP_ENC.h"
GiJeongKim	0:51c43836c1d7	7	#include "I2C_AS5510.h"
GiJeongKim	0:51c43836c1d7	8	#include "setting.h"
Lightvalve	11:82d8768d7351	9	#include "function_utilities.h"
Lightvalve	16:903b5a4433b4	10	#include "stm32f4xx_flash.h"
Lightvalve	65:a2d7c63419c2	11	#include "FlashWriter.h"
Lightvalve	61:bc8c8270f0ab	12
Lightvalve	61:bc8c8270f0ab	13	using namespace std;
Lightvalve	61:bc8c8270f0ab	14	Timer t;
GiJeongKim	0:51c43836c1d7	15
Lightvalve	31:66738bfecec5	16	///191008////
Lightvalve	31:66738bfecec5	17
jobuuu	7:e9086c72bb22	18	// dac & check ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	19	DigitalOut check(PC_2);
GiJeongKim	0:51c43836c1d7	20	DigitalOut check_2(PC_3);
GiJeongKim	0:51c43836c1d7	21	AnalogOut dac_1(PA_4);
GiJeongKim	0:51c43836c1d7	22	AnalogOut dac_2(PA_5);
Lightvalve	14:8e7590227d22	23	AnalogIn adc1(PC_4); //pressure_1
Lightvalve	14:8e7590227d22	24	AnalogIn adc2(PB_0); //pressure_2
Lightvalve	14:8e7590227d22	25	AnalogIn adc3(PC_1); //current
GiJeongKim	0:51c43836c1d7	26
Lightvalve	24:ef6e1092e9e6	27
jobuuu	7:e9086c72bb22	28	// PWM ///////////////////////////////////////////
Lightvalve	30:8d561f16383b	29	float dtc_v=0.0f;
Lightvalve	30:8d561f16383b	30	float dtc_w=0.0f;
GiJeongKim	0:51c43836c1d7	31
jobuuu	7:e9086c72bb22	32	// I2C ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	33	I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
GiJeongKim	0:51c43836c1d7	34	const int i2c_slave_addr1 = 0x56;
GiJeongKim	0:51c43836c1d7	35	unsigned int value; // 10bit output of reading sensor AS5510
GiJeongKim	0:51c43836c1d7	36
jobuuu	7:e9086c72bb22	37	// SPI ///////////////////////////////////////////
Lightvalve	16:903b5a4433b4	38	//SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
Lightvalve	16:903b5a4433b4	39	//DigitalOut eeprom_cs(PB_12);
Lightvalve	16:903b5a4433b4	40	//FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
GiJeongKim	0:51c43836c1d7	41	SPI enc(PC_12,PC_11,PC_10);
GiJeongKim	0:51c43836c1d7	42	DigitalOut enc_cs(PD_2);
Lightvalve	11:82d8768d7351	43	DigitalOut LED(PA_15);
GiJeongKim	0:51c43836c1d7	44
jobuuu	7:e9086c72bb22	45	// UART ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	46	Serial pc(PA_9,PA_10); // _ UART
GiJeongKim	0:51c43836c1d7	47
jobuuu	7:e9086c72bb22	48	// CAN ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	49	CAN can(PB_8, PB_9, 1000000);
GiJeongKim	0:51c43836c1d7	50	CANMessage msg;
Lightvalve	11:82d8768d7351	51	void onMsgReceived()
Lightvalve	11:82d8768d7351	52	{
Lightvalve	11:82d8768d7351	53	CAN_RX_HANDLER();
Lightvalve	11:82d8768d7351	54	}
jobuuu	2:a1c0a37df760	55
jobuuu	7:e9086c72bb22	56	// Variables ///////////////////////////////////////////
jobuuu	7:e9086c72bb22	57	State pos;
jobuuu	7:e9086c72bb22	58	State vel;
jobuuu	7:e9086c72bb22	59	State Vout;
jobuuu	7:e9086c72bb22	60	State torq;
jobuuu	7:e9086c72bb22	61	State pres_A;
jobuuu	7:e9086c72bb22	62	State pres_B;
jobuuu	7:e9086c72bb22	63	State cur;
Lightvalve	14:8e7590227d22	64	State valve_pos;
Lightvalve	14:8e7590227d22	65
Lightvalve	14:8e7590227d22	66	State INIT_Vout;
Lightvalve	14:8e7590227d22	67	State INIT_Valve_Pos;
Lightvalve	14:8e7590227d22	68	State INIT_Pos;
Lightvalve	14:8e7590227d22	69	State INIT_torq;
jobuuu	5:a4319f79457b	70
Lightvalve	19:23b7c1ad8683	71	extern int CID_RX_CMD;
Lightvalve	19:23b7c1ad8683	72	extern int CID_RX_REF_POSITION;
Lightvalve	49:e7bcfc244d40	73	extern int CID_RX_REF_VALVE_POS;
Lightvalve	45:35fa6884d0c6	74	extern int CID_RX_REF_PWM;
Lightvalve	19:23b7c1ad8683	75
Lightvalve	19:23b7c1ad8683	76	extern int CID_TX_INFO;
Lightvalve	19:23b7c1ad8683	77	extern int CID_TX_POSITION;
Lightvalve	19:23b7c1ad8683	78	extern int CID_TX_TORQUE;
Lightvalve	19:23b7c1ad8683	79	extern int CID_TX_PRES;
Lightvalve	19:23b7c1ad8683	80	extern int CID_TX_VOUT;
Lightvalve	19:23b7c1ad8683	81	extern int CID_TX_VALVE_POSITION;
Lightvalve	19:23b7c1ad8683	82
Lightvalve	61:bc8c8270f0ab	83
Lightvalve	61:bc8c8270f0ab	84
Lightvalve	61:bc8c8270f0ab	85
jobuuu	7:e9086c72bb22	86	// =============================================================================
jobuuu	7:e9086c72bb22	87	// =============================================================================
jobuuu	7:e9086c72bb22	88	// =============================================================================
jobuuu	7:e9086c72bb22	89
Lightvalve	12:6f2531038ea4	90	/*******************************************************************************
Lightvalve	12:6f2531038ea4	91	* REFERENCE MODE
Lightvalve	12:6f2531038ea4	92	******************************************************************************/
Lightvalve	13:747daba9cf59	93	enum _REFERENCE_MODE {
Lightvalve	13:747daba9cf59	94	MODE_REF_NO_ACT = 0, //0
Lightvalve	12:6f2531038ea4	95	MODE_REF_DIRECT, //1
Lightvalve	12:6f2531038ea4	96	MODE_REF_COS_INC, //2
Lightvalve	12:6f2531038ea4	97	MODE_REF_LINE_INC, //3
Lightvalve	12:6f2531038ea4	98	MODE_REF_SIN_WAVE, //4
Lightvalve	12:6f2531038ea4	99	MODE_REF_SQUARE_WAVE, //5
Lightvalve	12:6f2531038ea4	100	};
Lightvalve	12:6f2531038ea4	101
Lightvalve	12:6f2531038ea4	102	/*******************************************************************************
Lightvalve	12:6f2531038ea4	103	* CONTROL MODE
Lightvalve	12:6f2531038ea4	104	******************************************************************************/
Lightvalve	13:747daba9cf59	105	enum _CONTROL_MODE {
Lightvalve	12:6f2531038ea4	106	//control mode
Lightvalve	12:6f2531038ea4	107	MODE_NO_ACT = 0, //0
Lightvalve	47:fdcb8bd86fd6	108	MODE_VALVE_POSITION_CONTROL, //1
Lightvalve	47:fdcb8bd86fd6	109	MODE_JOINT_CONTROL, //2
Lightvalve	50:3c630b5eba9f	110
Lightvalve	47:fdcb8bd86fd6	111	MODE_VALVE_OPEN_LOOP, //3
Lightvalve	12:6f2531038ea4	112	MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
Lightvalve	12:6f2531038ea4	113	MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
Lightvalve	13:747daba9cf59	114
Lightvalve	12:6f2531038ea4	115	MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
Lightvalve	12:6f2531038ea4	116	MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
Lightvalve	12:6f2531038ea4	117	MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
Lightvalve	13:747daba9cf59	118
Lightvalve	12:6f2531038ea4	119	MODE_TEST_CURRENT_CONTROL, //9
Lightvalve	12:6f2531038ea4	120	MODE_TEST_PWM_CONTROL, //10
Lightvalve	21:e5f1a43ea6f9	121
Lightvalve	14:8e7590227d22	122	MODE_CURRENT_CONTROL, //11
Lightvalve	14:8e7590227d22	123	MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12
Lightvalve	14:8e7590227d22	124	MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13
Lightvalve	13:747daba9cf59	125
Lightvalve	12:6f2531038ea4	126	//utility
Lightvalve	12:6f2531038ea4	127	MODE_TORQUE_SENSOR_NULLING = 20, //20
Lightvalve	12:6f2531038ea4	128	MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
Lightvalve	12:6f2531038ea4	129	MODE_FIND_HOME, //22
Lightvalve	57:f4819de54e7a	130	MODE_VALVE_GAIN_SETTING, //23
Lightvalve	12:6f2531038ea4	131	MODE_PRESSURE_SENSOR_NULLING, //24
Lightvalve	12:6f2531038ea4	132	MODE_PRESSURE_SENSOR_CALIB, //25
Lightvalve	12:6f2531038ea4	133	MODE_ROTARY_FRICTION_TUNING, //26
Lightvalve	13:747daba9cf59	134
Lightvalve	12:6f2531038ea4	135	MODE_DDV_POS_VS_PWM_ID = 30, //30
Lightvalve	12:6f2531038ea4	136	MODE_DDV_DEADZONE_AND_CENTER, //31
Lightvalve	12:6f2531038ea4	137	MODE_DDV_POS_VS_FLOWRATE, //32
Lightvalve	57:f4819de54e7a	138	MODE_SYSTEM_ID, //33
Lightvalve	12:6f2531038ea4	139	};
Lightvalve	12:6f2531038ea4	140
Lightvalve	65:a2d7c63419c2	141	void SystemClock_Config(void)
Lightvalve	65:a2d7c63419c2	142	{
Lightvalve	65:a2d7c63419c2	143	RCC_OscInitTypeDef RCC_OscInitStruct = {0};
Lightvalve	65:a2d7c63419c2	144	RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
Lightvalve	65:a2d7c63419c2	145
Lightvalve	65:a2d7c63419c2	146	/** Configure the main internal regulator output voltage
Lightvalve	65:a2d7c63419c2	147	*/
Lightvalve	65:a2d7c63419c2	148	__HAL_RCC_PWR_CLK_ENABLE();
Lightvalve	65:a2d7c63419c2	149	__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
Lightvalve	65:a2d7c63419c2	150	/** Initializes the CPU, AHB and APB busses clocks
Lightvalve	65:a2d7c63419c2	151	*/
Lightvalve	65:a2d7c63419c2	152	RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
Lightvalve	65:a2d7c63419c2	153	RCC_OscInitStruct.HSIState = RCC_HSI_ON;
Lightvalve	65:a2d7c63419c2	154	RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
Lightvalve	65:a2d7c63419c2	155	RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
Lightvalve	65:a2d7c63419c2	156	RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
Lightvalve	65:a2d7c63419c2	157	RCC_OscInitStruct.PLL.PLLM = 8;//8
Lightvalve	65:a2d7c63419c2	158	RCC_OscInitStruct.PLL.PLLN = 180; //180
Lightvalve	65:a2d7c63419c2	159	RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
Lightvalve	65:a2d7c63419c2	160	RCC_OscInitStruct.PLL.PLLQ = 2;
Lightvalve	65:a2d7c63419c2	161	RCC_OscInitStruct.PLL.PLLR = 2;
Lightvalve	65:a2d7c63419c2	162	if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
Lightvalve	65:a2d7c63419c2	163	{
Lightvalve	65:a2d7c63419c2	164	//Error_Handler();
Lightvalve	65:a2d7c63419c2	165	}
Lightvalve	65:a2d7c63419c2	166	/** Activate the Over-Drive mode
Lightvalve	65:a2d7c63419c2	167	*/
Lightvalve	65:a2d7c63419c2	168	if (HAL_PWREx_EnableOverDrive() != HAL_OK)
Lightvalve	65:a2d7c63419c2	169	{
Lightvalve	65:a2d7c63419c2	170	//Error_Handler();
Lightvalve	65:a2d7c63419c2	171	}
Lightvalve	65:a2d7c63419c2	172	/** Initializes the CPU, AHB and APB busses clocks
Lightvalve	65:a2d7c63419c2	173	*/
Lightvalve	65:a2d7c63419c2	174	RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK\|RCC_CLOCKTYPE_SYSCLK
Lightvalve	65:a2d7c63419c2	175	\|RCC_CLOCKTYPE_PCLK1\|RCC_CLOCKTYPE_PCLK2;
Lightvalve	65:a2d7c63419c2	176	RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
Lightvalve	65:a2d7c63419c2	177	RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
Lightvalve	65:a2d7c63419c2	178	RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
Lightvalve	65:a2d7c63419c2	179	RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
Lightvalve	65:a2d7c63419c2	180
Lightvalve	65:a2d7c63419c2	181	if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
Lightvalve	65:a2d7c63419c2	182	{
Lightvalve	65:a2d7c63419c2	183	//Error_Handler();
Lightvalve	65:a2d7c63419c2	184	}
Lightvalve	65:a2d7c63419c2	185	}
Lightvalve	65:a2d7c63419c2	186
Lightvalve	99:7bbcb3c0fb06	187	float u_past[num_array_u_past] = {0.0f};
Lightvalve	68:328e1be06f5d	188	float x_past[num_array_x_past] = {0.0f};
Lightvalve	85:a3b46118b5cd	189	float x_future[num_array_x_future] = {0.0f};
Lightvalve	68:328e1be06f5d	190	float f_past[num_array_f_past] = {0.0f};
Lightvalve	68:328e1be06f5d	191	float f_future[num_array_f_future] = {0.0f};
Lightvalve	66:a8e6799dbce3	192
Lightvalve	73:f80dc3970c99	193	float input_NN[num_input] = { 0.0f };
Lightvalve	65:a2d7c63419c2	194
Lightvalve	112:8dcb1600cb90	195	const float h1[num_input][16] = {
Lightvalve	113:4535bc97a1db	196	{0.08238110691308975f,-0.08961541950702667f,0.11304140090942383f,0.2760394215583801f,-0.7498500943183899f,0.3081931173801422f,-0.15306922793388367f,-0.09221038222312927f,-0.2735815942287445f,-0.07937835901975632f,-0.4525371491909027f,-0.39731699228286743f,-0.308616578578949f,0.18082286417484283f,-0.22528137266635895f,-0.6054583787918091f},
Lightvalve	113:4535bc97a1db	197	{-0.12830595672130585f,0.006713761016726494f,-0.3440950810909271f,-0.11241301149129868f,-0.21983236074447632f,0.1156276986002922f,-0.3551466166973114f,-0.11156616359949112f,-0.09336893260478973f,0.1392476111650467f,-0.6042727828025818f,-0.05485542118549347f,0.2608185112476349f,-0.1971801370382309f,-0.1836855560541153f,-0.46543365716934204f},
Lightvalve	113:4535bc97a1db	198	{-0.2113606184720993f,-0.10917621105909348f,0.3188004493713379f,-0.3602854311466217f,-0.3418009281158447f,0.2324504256248474f,0.2516096830368042f,0.025377541780471802f,-0.16486278176307678f,0.14110800623893738f,-0.9271578788757324f,-0.5703809261322021f,-0.20862722396850586f,0.1354394555091858f,0.04897245764732361f,-0.9888147115707397f},
Lightvalve	113:4535bc97a1db	199	{-0.18535427749156952f,-0.14856520295143127f,0.34815579652786255f,0.20181334018707275f,-0.5048770904541016f,-0.05141099914908409f,-0.10148510336875916f,0.3254654109477997f,-0.003915203269571066f,0.25671783089637756f,-0.984145998954773f,-0.732689619064331f,-0.5098223090171814f,0.046679943799972534f,-0.2594349980354309f,-1.2771731615066528f},
Lightvalve	113:4535bc97a1db	200	{-0.7982119917869568f,1.8691412210464478f,-0.004507303237915039f,-0.41337528824806213f,-0.5850437879562378f,-0.7259997129440308f,0.11336228251457214f,0.8583602905273438f,0.0017320261104032397f,0.0926690325140953f,-0.5004006624221802f,-2.227193832397461f,-0.08651071786880493f,1.1023963689804077f,-0.33859342336654663f,-0.8644697070121765f},
Lightvalve	113:4535bc97a1db	201	{0.3163878619670868f,0.1946386694908142f,0.19074231386184692f,0.22623515129089355f,0.060158733278512955f,-0.2312968522310257f,0.1446705460548401f,-0.46448758244514465f,0.004422776401042938f,0.09692279994487762f,-0.11860773712396622f,-0.30901530385017395f,-0.19687047600746155f,0.2739292085170746f,-0.36205175518989563f,-0.5423892140388489f},
Lightvalve	113:4535bc97a1db	202	{0.04715665429830551f,0.4120287597179413f,-0.21918344497680664f,-0.44296982884407043f,-0.46212509274482727f,0.1812339723110199f,0.05268651247024536f,-0.5160120129585266f,0.295302152633667f,0.052635882049798965f,0.18726375699043274f,-0.17878800630569458f,-0.11949975788593292f,0.28833886981010437f,-0.13682329654693604f,-0.34507280588150024f},
Lightvalve	113:4535bc97a1db	203	{0.04375242441892624f,0.16456671059131622f,-0.23319895565509796f,0.0566110834479332f,-0.4158187210559845f,-0.0969480350613594f,-0.15703755617141724f,-0.5499584078788757f,0.00013322447193786502f,0.0929379090666771f,0.08850504457950592f,-0.5020623803138733f,0.35569849610328674f,0.050450775772333145f,0.03769642114639282f,-0.16214480996131897f},
Lightvalve	113:4535bc97a1db	204	{0.240993469953537f,0.6182478666305542f,0.08977723121643066f,-0.3202260732650757f,-0.2757028639316559f,0.188012033700943f,-0.06348633766174316f,-0.6417542099952698f,-0.2595629096031189f,0.2955130636692047f,-0.33913350105285645f,-0.10932902246713638f,0.2107759565114975f,0.06947705894708633f,-0.2676756680011749f,-0.17070630192756653f},
Lightvalve	113:4535bc97a1db	205	{0.14472809433937073f,0.04991544410586357f,0.16173982620239258f,-0.03383071348071098f,-0.07667389512062073f,0.4410749673843384f,-0.26198261976242065f,-0.40114665031433105f,-0.029340164735913277f,-0.4238303601741791f,-0.20219571888446808f,-0.8573110699653625f,0.2923631966114044f,-0.42867499589920044f,0.3052974343299866f,-0.4081484377384186f},
Lightvalve	113:4535bc97a1db	206	{0.6457957029342651f,0.3333607614040375f,0.32573211193084717f,0.3406955301761627f,-0.1765735000371933f,0.2948165237903595f,-0.17817988991737366f,-0.31869208812713623f,-0.3962319493293762f,0.012360160239040852f,-0.011035910807549953f,-0.6471021175384521f,-0.10371317714452744f,-0.26326075196266174f,0.23379981517791748f,-0.7066872119903564f},
Lightvalve	113:4535bc97a1db	207	{-0.10245873034000397f,-0.6810756921768188f,-0.3532183766365051f,0.023810051381587982f,0.1919257938861847f,0.23078951239585876f,0.13436567783355713f,0.20962554216384888f,-0.15341709554195404f,0.16348986327648163f,-0.43814653158187866f,0.3474365770816803f,0.019524723291397095f,-0.1138937771320343f,0.09193113446235657f,-0.10852809995412827f},
Lightvalve	113:4535bc97a1db	208	{0.08872741460800171f,-0.5603640675544739f,0.173875629901886f,0.09513795375823975f,0.17256668210029602f,0.24432475864887238f,-0.21062730252742767f,0.10210034996271133f,-0.17251047492027283f,-0.1929878145456314f,-0.14158886671066284f,-0.23105527460575104f,-0.01567455567419529f,-0.016878396272659302f,-0.19507692754268646f,-0.29847875237464905f},
Lightvalve	113:4535bc97a1db	209	{-0.04670553281903267f,-0.482174813747406f,0.36161231994628906f,-0.24248476326465607f,-0.3098406493663788f,-0.2584144175052643f,-0.2803434431552887f,-0.15470920503139496f,-0.12586930394172668f,-0.22432377934455872f,-0.14531145989894867f,-0.39429396390914917f,-0.2555939555168152f,-0.0592232421040535f,0.04202204942703247f,0.22219786047935486f},
Lightvalve	113:4535bc97a1db	210	{-0.28927093744277954f,0.26264137029647827f,0.1776806116104126f,-0.31751537322998047f,0.7402405738830566f,-0.7990986108779907f,0.036818891763687134f,0.4778040051460266f,0.25883719325065613f,0.0417952723801136f,-0.13543257117271423f,-0.7137618064880371f,0.29373836517333984f,0.4710771143436432f,0.058527231216430664f,0.4053569734096527f},
Lightvalve	113:4535bc97a1db	211	{-1.20416259765625f,0.7237461805343628f,-0.3542669713497162f,-0.44570106267929077f,1.0011931657791138f,-1.3723636865615845f,-0.13638360798358917f,0.12881898880004883f,0.16959118843078613f,-0.03936115652322769f,-0.4757843613624573f,-0.3493559658527374f,-0.08204221725463867f,1.340761423110962f,-0.07223698496818542f,0.32134485244750977f},
Lightvalve	113:4535bc97a1db	212	{1.587872862815857f,-1.451370358467102f,0.14635330438613892f,0.7568324208259583f,0.37735533714294434f,1.494177222251892f,-0.03628063201904297f,-0.7949840426445007f,-0.062402594834566116f,0.03867268189787865f,-1.5538501739501953f,1.2851613759994507f,-0.2553684413433075f,-0.982872486114502f,0.25779521465301514f,-0.28556445240974426f},
Lightvalve	113:4535bc97a1db	213	{-0.12825320661067963f,0.12288405001163483f,0.18322283029556274f,-0.19842621684074402f,0.8198286890983582f,0.041014984250068665f,-0.3163079023361206f,-0.05294492468237877f,-0.36373722553253174f,-0.14535510540008545f,-0.32957836985588074f,2.6591529846191406f,0.5904089212417603f,0.22923114895820618f,-0.2407861351966858f,0.25907382369041443f},
Lightvalve	113:4535bc97a1db	214	{-0.7170370817184448f,0.19116301834583282f,-0.2609631419181824f,-0.13221608102321625f,2.194356679916382f,-0.31213393807411194f,0.2401489019393921f,-0.20553678274154663f,-0.35975316166877747f,-0.01309242844581604f,-0.4241333603858948f,3.6729111671447754f,0.7621604800224304f,0.8725821375846863f,-0.28592410683631897f,0.3371560573577881f},
Lightvalve	113:4535bc97a1db	215	{0.03899780288338661f,0.5801780819892883f,0.1335710883140564f,-0.4388673007488251f,0.43298983573913574f,-0.082271046936512f,-0.1246849000453949f,-0.29775792360305786f,-0.15970076620578766f,0.002899272134527564f,0.37377816438674927f,1.6107573509216309f,0.6399732232093811f,0.5948517918586731f,0.1255095899105072f,0.6594066619873047f},
Lightvalve	113:4535bc97a1db	216	{-0.13053759932518005f,-0.37264150381088257f,-0.07108169794082642f,0.23301273584365845f,-1.1603906154632568f,-0.18824516236782074f,0.13563674688339233f,-0.1116926446557045f,-0.34506022930145264f,-0.30885881185531616f,0.029483351856470108f,-0.3498949706554413f,-0.24269425868988037f,0.1451246738433838f,0.01829466223716736f,0.37815138697624207f},
Lightvalve	113:4535bc97a1db	217	{-0.14311794936656952f,-0.9311226010322571f,0.24740570783615112f,-0.273596853017807f,-1.1528133153915405f,-0.3346102237701416f,0.25593245029449463f,-0.1785348802804947f,-0.07106246054172516f,0.052634645253419876f,0.27635839581489563f,-0.5498412847518921f,-0.10320106148719788f,-0.1412816196680069f,0.056514471769332886f,0.07012096047401428f},
Lightvalve	113:4535bc97a1db	218	{0.14739277958869934f,-0.19856983423233032f,-0.3288555443286896f,-0.10809275507926941f,-1.2299606800079346f,-0.45326781272888184f,-0.11800059676170349f,-0.12314795702695847f,-0.27156248688697815f,0.28150904178619385f,0.4421814978122711f,0.0713525339961052f,-0.2596205472946167f,-0.07402632385492325f,-0.3194865584373474f,0.20242354273796082f},
Lightvalve	113:4535bc97a1db	219	{-0.08163876086473465f,-0.41752099990844727f,-0.29844698309898376f,-0.4437902271747589f,-0.6436755061149597f,0.09186983853578568f,-0.249517560005188f,0.2938408851623535f,-0.41435784101486206f,0.16828367114067078f,0.4007464051246643f,-0.011783730238676071f,0.04081519693136215f,0.2355841100215912f,0.23831695318222046f,-0.11805100739002228f},
Lightvalve	113:4535bc97a1db	220	{-0.03227778151631355f,-0.4336826503276825f,0.20589900016784668f,0.05139650031924248f,-1.028411626815796f,-0.25429999828338623f,-0.2483310103416443f,0.22440573573112488f,-0.14384999871253967f,0.28843462467193604f,-0.04356471449136734f,0.041426245123147964f,-0.4966038465499878f,-0.16486214101314545f,0.06927120685577393f,-0.03157913684844971f},
Lightvalve	113:4535bc97a1db	221	{-0.33219003677368164f,-0.07518492639064789f,0.008588135242462158f,-0.45205068588256836f,-1.3535797595977783f,-0.03416458144783974f,0.013287186622619629f,-0.06247078999876976f,0.18176743388175964f,-0.02428596466779709f,0.3507067561149597f,-0.009459034539759159f,-0.2504180073738098f,-0.2928818166255951f,-0.1702093929052353f,0.08025657385587692f},
Lightvalve	113:4535bc97a1db	222	{-0.18047378957271576f,-0.3990851044654846f,0.06708025932312012f,0.15620632469654083f,-0.9475982189178467f,0.077489472925663f,0.27470332384109497f,-0.1436568647623062f,-0.09286155551671982f,0.23921003937721252f,0.3713104724884033f,-0.04637960344552994f,-0.2589351236820221f,0.1323644369840622f,0.11447501182556152f,-0.07560984045267105f},
Lightvalve	65:a2d7c63419c2	223	};
Lightvalve	65:a2d7c63419c2	224
Lightvalve	112:8dcb1600cb90	225	const float h2[16][16] = {
Lightvalve	113:4535bc97a1db	226	{-0.4416000545024872f,-0.08174923807382584f,-0.06966331601142883f,1.4168658256530762f,-0.21907491981983185f,0.34244245290756226f,0.12155939638614655f,0.0904630720615387f,-0.43008196353912354f,-0.1415480375289917f,-1.1304616928100586f,0.38759899139404297f,0.12968102097511292f,1.3622114658355713f,0.4533298909664154f,-0.6724657416343689f},
Lightvalve	113:4535bc97a1db	227	{-0.2245597094297409f,-0.19340869784355164f,0.057057321071624756f,0.000475427572382614f,-0.35503754019737244f,-0.6832019090652466f,-0.7009918689727783f,-0.2990124225616455f,-0.33757925033569336f,0.2895788848400116f,-0.23149366676807404f,-3.508873701095581f,0.21295784413814545f,0.012084810994565487f,-1.8932721614837646f,0.5581379532814026f},
Lightvalve	112:8dcb1600cb90	228	{-0.22745239734649658f,0.003037691116333008f,-0.061119019985198975f,0.35696902871131897f,0.05568113923072815f,0.011741191148757935f,-0.20225946605205536f,-0.08465918898582458f,0.3489862382411957f,0.0687277615070343f,0.31964078545570374f,0.3004753887653351f,0.36063823103904724f,-0.42892736196517944f,0.08652284741401672f,0.027493387460708618f},
Lightvalve	113:4535bc97a1db	229	{-0.01778075285255909f,-0.23104725778102875f,-0.2894435524940491f,0.10867004096508026f,0.08946844935417175f,0.07808632403612137f,-0.4573591351509094f,0.4122363030910492f,0.26211628317832947f,0.2953031361103058f,0.20370014011859894f,0.010252464562654495f,-0.43868115544319153f,-0.03280303627252579f,0.28507310152053833f,0.07095210254192352f},
Lightvalve	113:4535bc97a1db	230	{-0.13348069787025452f,0.3367266058921814f,-0.25313520431518555f,-0.03403173387050629f,0.036378175020217896f,0.1093469187617302f,-0.5513286590576172f,-0.08570799231529236f,-0.024399548768997192f,-0.39465832710266113f,0.03879040107131004f,-0.8223557472229004f,0.09156011044979095f,0.2768576741218567f,-0.17694859206676483f,0.11134585738182068f},
Lightvalve	113:4535bc97a1db	231	{0.16555435955524445f,0.4176541566848755f,-0.3519742488861084f,1.3333228826522827f,-0.3015052080154419f,0.03819012641906738f,-0.5399168729782104f,-0.019414573907852173f,-0.23047015070915222f,0.3579089343547821f,-1.0312609672546387f,-0.6689232587814331f,-0.272077351808548f,1.1587179899215698f,1.0721826553344727f,-1.3411850929260254f},
Lightvalve	112:8dcb1600cb90	232	{-0.1828227937221527f,-0.02554568648338318f,-0.3260969817638397f,0.08422836661338806f,-0.38453540205955505f,-0.25432005524635315f,0.285016268491745f,0.12387624382972717f,-0.0982072651386261f,0.13111665844917297f,-0.03692615032196045f,-0.32796353101730347f,-0.21546880900859833f,0.049302369356155396f,-0.27088475227355957f,-0.4124959409236908f},
Lightvalve	113:4535bc97a1db	233	{0.01398484781384468f,0.15759709477424622f,0.33083590865135193f,-0.26944801211357117f,-0.14358049631118774f,-0.33699458837509155f,-0.38920095562934875f,-0.3003333508968353f,0.2600560486316681f,-0.37898191809654236f,0.2948208749294281f,-0.9432287812232971f,-0.42878255248069763f,0.5726852416992188f,0.02221696637570858f,0.05635474622249603f},
Lightvalve	113:4535bc97a1db	234	{-0.25820738077163696f,0.3117407262325287f,-0.11379697918891907f,0.12228615581989288f,0.041274964809417725f,-0.34911200404167175f,-0.10284432768821716f,0.0019084513187408447f,0.06103590130805969f,-0.38046833872795105f,0.0233750119805336f,-0.3197441101074219f,-0.37024784088134766f,-0.25981605052948f,-0.35837259888648987f,-0.24591223895549774f},
Lightvalve	113:4535bc97a1db	235	{-0.17263101041316986f,0.23423008620738983f,-0.13502129912376404f,0.1286323517560959f,0.12987366318702698f,0.1080603152513504f,0.2711336314678192f,-0.3632148504257202f,-0.13619378209114075f,0.16938945651054382f,0.24636374413967133f,0.36848944425582886f,0.3811538517475128f,-0.2520766258239746f,0.0013287918409332633f,-0.2210550457239151f},
Lightvalve	113:4535bc97a1db	236	{-0.3058125972747803f,0.08181517571210861f,0.0457797646522522f,-2.636444330215454f,-0.03321319818496704f,-0.541618824005127f,0.17873415350914001f,-0.20421427488327026f,-0.050184011459350586f,0.12480869889259338f,-0.15830643475055695f,-0.5519965291023254f,-0.4537477195262909f,0.050337303429841995f,-0.08985583484172821f,-0.2977418303489685f},
Lightvalve	113:4535bc97a1db	237	{0.04818347468972206f,0.8838717937469482f,0.3954955041408539f,0.5095310211181641f,0.0033026933670043945f,0.7002139687538147f,-0.36933785676956177f,-0.11795541644096375f,0.3487861454486847f,-0.32520344853401184f,0.3224901556968689f,-0.9617040753364563f,-0.05594838038086891f,-1.3294044733047485f,0.772002100944519f,-0.21592769026756287f},
Lightvalve	113:4535bc97a1db	238	{-0.3529583215713501f,0.08254975080490112f,-0.15798500180244446f,0.05475207790732384f,-0.37393757700920105f,0.24033845961093903f,0.18650469183921814f,-0.2947862446308136f,-0.3764709532260895f,0.2424570620059967f,-0.09720540791749954f,-0.09403084218502045f,0.3314500153064728f,-0.39308783411979675f,-0.010754602961242199f,0.12959282100200653f},
Lightvalve	113:4535bc97a1db	239	{-0.6177845597267151f,1.206647515296936f,-0.2018718123435974f,-0.6062206625938416f,0.07545611262321472f,-0.5194984078407288f,-0.32654547691345215f,-0.21416273713111877f,-0.24509364366531372f,0.19522181153297424f,0.7575910091400146f,0.5801213383674622f,-0.2729317247867584f,-0.8992892503738403f,-0.9347679018974304f,0.29456380009651184f},
Lightvalve	112:8dcb1600cb90	240	{0.4138670265674591f,0.1604653298854828f,0.056746453046798706f,0.036025404930114746f,0.3228367865085602f,-0.07083973288536072f,0.018455177545547485f,0.0059362053871154785f,0.40515169501304626f,0.014240056276321411f,-0.07738298177719116f,0.1407785713672638f,-0.13024571537971497f,-0.29546058177948f,-0.11976784467697144f,-0.35825538635253906f},
Lightvalve	113:4535bc97a1db	241	{-0.46657031774520874f,0.3603224754333496f,0.12081471085548401f,-0.6525219678878784f,0.29976895451545715f,-0.3780849874019623f,0.04173128679394722f,0.31089308857917786f,-0.3893685042858124f,-0.02222958207130432f,0.6254821419715881f,-0.06403668224811554f,-0.2615106403827667f,-0.2841106355190277f,-0.020944762974977493f,0.24394959211349487f},
Lightvalve	65:a2d7c63419c2	242	};
Lightvalve	65:a2d7c63419c2	243
Lightvalve	112:8dcb1600cb90	244	const float h3[16][16] = {
Lightvalve	113:4535bc97a1db	245	{-0.36079341173171997f,-0.030723435804247856f,-0.10280819237232208f,-0.3743261694908142f,-0.1324009746313095f,0.10063944011926651f,0.3332441747188568f,0.3036012351512909f,-0.2405819296836853f,0.04891335964202881f,0.06792311370372772f,0.33556240797042847f,0.03479746729135513f,0.19397124648094177f,-0.3191870450973511f,0.1584681123495102f},
Lightvalve	113:4535bc97a1db	246	{0.047732532024383545f,0.5462093353271484f,-0.19018949568271637f,-1.725974202156067f,-0.6673309206962585f,0.1470886617898941f,-0.3198729455471039f,0.009827982634305954f,0.003414712380617857f,-0.464531809091568f,1.3382433652877808f,-2.142193078994751f,-0.061363279819488525f,0.5286969542503357f,-0.4145585298538208f,-0.3840222656726837f},
Lightvalve	112:8dcb1600cb90	247	{0.07903262972831726f,0.2790505588054657f,-0.07798504829406738f,0.04248586297035217f,-0.1963958442211151f,-0.19260792434215546f,-0.4038352966308594f,0.015906542539596558f,0.15353140234947205f,0.030178606510162354f,0.2488909661769867f,0.13805970549583435f,-0.0816211998462677f,-0.20733052492141724f,-0.3036302626132965f,0.054825395345687866f},
Lightvalve	113:4535bc97a1db	248	{-0.30922991037368774f,0.006792995613068342f,0.9229696989059448f,-1.7489866018295288f,0.4089786410331726f,-0.6779340505599976f,0.20021501183509827f,-0.176965594291687f,-0.2636560797691345f,-0.5356540679931641f,0.5737893581390381f,-0.3973005414009094f,-0.13222742080688477f,1.3011301755905151f,-0.1792915016412735f,-0.21804869174957275f},
Lightvalve	112:8dcb1600cb90	249	{-0.39607733488082886f,-0.05481579899787903f,0.1976260244846344f,0.022423356771469116f,0.16892847418785095f,-0.27518749237060547f,0.16012099385261536f,0.3626593053340912f,-0.08640444278717041f,-0.11053556203842163f,-0.10529157519340515f,-0.31317979097366333f,-0.1530032455921173f,-0.1336749792098999f,0.22959044575691223f,0.19986507296562195f},
Lightvalve	113:4535bc97a1db	250	{-0.37449589371681213f,-0.3864476978778839f,0.19304296374320984f,-0.49359002709388733f,0.25771966576576233f,-0.9093904495239258f,-0.18732719123363495f,0.12788733839988708f,-0.46900054812431335f,-0.16388018429279327f,0.19361239671707153f,1.002976417541504f,-0.035307805985212326f,-0.6030420064926147f,0.0823487937450409f,-0.4255358576774597f},
Lightvalve	113:4535bc97a1db	251	{0.4110594093799591f,0.1925012469291687f,-0.1622374951839447f,0.4338245093822479f,0.270425945520401f,-0.22044363617897034f,-0.08670487999916077f,-0.25336313247680664f,-0.030661463737487793f,-0.06259563565254211f,-0.1344406008720398f,0.35313835740089417f,0.335384726524353f,0.03966398909687996f,0.07285787910223007f,-0.3391006588935852f},
Lightvalve	113:4535bc97a1db	252	{-0.40892091393470764f,0.043769627809524536f,-0.3867315948009491f,0.25968697667121887f,0.3424709737300873f,-0.051169753074645996f,-0.23312048614025116f,-0.390264093875885f,0.28059282898902893f,-0.1559126079082489f,-0.14134526252746582f,-0.0003446042537689209f,-0.2742875814437866f,-0.36560842394828796f,0.07994696497917175f,0.005298197269439697f},
Lightvalve	112:8dcb1600cb90	253	{0.10697010159492493f,-0.12228584289550781f,-0.37870171666145325f,0.21184906363487244f,-0.37222859263420105f,-0.17138728499412537f,-0.1382003128528595f,0.3493293821811676f,-0.360889196395874f,-0.3875247836112976f,0.42142823338508606f,-0.3482915461063385f,-0.3289247751235962f,-0.2186824083328247f,0.09620395302772522f,-0.06898030638694763f},
Lightvalve	112:8dcb1600cb90	254	{0.2847062647342682f,0.018552124500274658f,0.11435768008232117f,0.36562982201576233f,-0.047046810388565063f,0.30447837710380554f,0.2430230677127838f,0.2909286320209503f,-0.2802048921585083f,0.18043199181556702f,0.41849127411842346f,-0.287167489528656f,0.24394884705543518f,-0.14084559679031372f,-0.10168051719665527f,0.010465055704116821f},
Lightvalve	113:4535bc97a1db	255	{0.15459725260734558f,0.5850727558135986f,0.32904475927352905f,-0.5207881927490234f,-0.23494544625282288f,0.220808744430542f,-0.08742031455039978f,-0.01188978273421526f,-0.3207188844680786f,-0.5697242617607117f,0.059458356350660324f,-1.7261109352111816f,-0.12069235742092133f,0.2702104449272156f,0.4019995927810669f,0.16767722368240356f},
Lightvalve	113:4535bc97a1db	256	{-0.408692330121994f,1.1359740495681763f,2.1213362216949463f,0.7298450469970703f,1.7460517883300781f,0.05990634113550186f,0.03285527229309082f,1.0026781558990479f,-0.20705322921276093f,-0.25883403420448303f,0.6337341070175171f,2.4105594158172607f,0.6547544598579407f,-2.30305814743042f,-0.8899465799331665f,0.3150562644004822f},
Lightvalve	113:4535bc97a1db	257	{-0.2991822361946106f,0.3664271831512451f,-0.07084015011787415f,-0.05932474136352539f,0.09361109882593155f,0.23075295984745026f,-0.11253207921981812f,0.34576353430747986f,0.04814547300338745f,-0.35770976543426514f,-0.044228196144104004f,-0.36229726672172546f,0.023185068741440773f,-0.13895061612129211f,0.36124154925346375f,-0.16869547963142395f},
Lightvalve	113:4535bc97a1db	258	{-0.2675279378890991f,0.04231102392077446f,-0.0044008963741362095f,0.05267181620001793f,0.3642056882381439f,-0.022781625390052795f,-0.32875844836235046f,-2.4273524284362793f,-0.6806216239929199f,-0.6331622004508972f,-2.9961183071136475f,0.4442698657512665f,-0.17551426589488983f,0.012292241677641869f,0.38773462176322937f,-0.006947796791791916f},
Lightvalve	113:4535bc97a1db	259	{-0.015470266342163086f,-0.7161818742752075f,1.1149377822875977f,0.1646902710199356f,-0.0402379110455513f,0.7326511144638062f,0.05196094512939453f,0.03451240807771683f,-0.38354402780532837f,-0.5163152813911438f,0.9525309801101685f,1.1795172691345215f,0.21576564013957977f,-1.4809402227401733f,-0.019000373780727386f,-0.7551562190055847f},
Lightvalve	113:4535bc97a1db	260	{0.1863725483417511f,0.4091108739376068f,-0.08009207993745804f,-0.7416639924049377f,-0.646676778793335f,0.10882991552352905f,0.04860696196556091f,-0.4419192373752594f,-0.30845537781715393f,-0.012700200080871582f,-0.5827067494392395f,2.0203957557678223f,0.1488654762506485f,-1.7528129816055298f,-0.06102534011006355f,-0.25199466943740845f},
Lightvalve	66:a8e6799dbce3	261	};
Lightvalve	65:a2d7c63419c2	262
Lightvalve	113:4535bc97a1db	263	const float hout[16] = { 0.45773375034332275f,0.29249057173728943f,-0.1589747667312622f,-0.11081868410110474f,-0.19395703077316284f,0.25246232748031616f,0.0030125975608825684f,0.6155519485473633f,-0.1411089152097702f,-0.06577444821596146f,0.3741997182369232f,-0.3702811300754547f,-0.075167715549469f,-0.1739802360534668f,0.10441919416189194f,-0.12528467178344727f };
Lightvalve	66:a8e6799dbce3	264
Lightvalve	113:4535bc97a1db	265	const float b1[16] = { 0.8555348515510559f,1.3975776433944702f,-1.7145336866378784f,0.4208109676837921f,0.8277526497840881f,0.948124885559082f,-0.058932315558195114f,0.6894977688789368f,0.5092201232910156f,-0.9256697297096252f,0.9643260836601257f,0.9915532469749451f,-0.3523768186569214f,-1.0846443176269531f,-1.087764859199524f,0.7544463276863098f };
Lightvalve	87:471334725012	266
Lightvalve	113:4535bc97a1db	267	const float b2[16] = { -0.3902733623981476f,-0.5920320749282837f,-1.4564176797866821f,-0.2647169232368469f,-0.6939148902893066f,0.3262087404727936f,0.2816527187824249f,-0.30969977378845215f,-1.20063054561615f,-1.912178635597229f,0.08835988491773605f,1.761587142944336f,-0.20341315865516663f,1.883382797241211f,0.3987988531589508f,0.9743563532829285f };
Lightvalve	65:a2d7c63419c2	268
Lightvalve	113:4535bc97a1db	269	const float b3[16] = { -1.963319182395935f,-0.18621507287025452f,0.0443459190428257f,-0.5485855340957642f,-0.2735750079154968f,0.7779544591903687f,-0.45353031158447266f,-1.1538832187652588f,0.22698374092578888f,0.18241459131240845f,-1.5108184814453125f,0.144838348031044f,-0.8488171696662903f,-0.19366617500782013f,-1.19119393825531f,-0.6076282262802124f };
Lightvalve	87:471334725012	270
Lightvalve	113:4535bc97a1db	271	const float bout[1] = { -0.2704888880252838f };
Lightvalve	65:a2d7c63419c2	272
Lightvalve	66:a8e6799dbce3	273
Lightvalve	87:471334725012	274	float VALVE_POS_RAW_NN = 0.0f;
Lightvalve	87:471334725012	275	float DDV_JOINT_POS_FF(float REF_JOINT_VEL);
Lightvalve	61:bc8c8270f0ab	276
GiJeongKim	0:51c43836c1d7	277	int main()
GiJeongKim	0:51c43836c1d7	278	{
Lightvalve	66:a8e6799dbce3	279
Lightvalve	65:a2d7c63419c2	280	HAL_Init();
Lightvalve	65:a2d7c63419c2	281	SystemClock_Config();
Lightvalve	65:a2d7c63419c2	282
jobuuu	6:df07d3491e3a	283	/*********************************
jobuuu	1:e04e563be5ce	284	*** Initialization
jobuuu	6:df07d3491e3a	285	*********************************/
Lightvalve	69:3995ffeaa786	286	LED = 0;
Lightvalve	61:bc8c8270f0ab	287	pc.baud(9600);
Lightvalve	21:e5f1a43ea6f9	288
GiJeongKim	0:51c43836c1d7	289	// i2c init
Lightvalve	8:5d2eebdad025	290	i2c.frequency(400 * 1000); // 0.4 mHz
Lightvalve	8:5d2eebdad025	291	wait_ms(2); // Power Up wait
Lightvalve	8:5d2eebdad025	292	look_for_hardware_i2c(); // Hardware present
Lightvalve	8:5d2eebdad025	293	init_as5510(i2c_slave_addr1);
Lightvalve	11:82d8768d7351	294	make_delay();
jobuuu	2:a1c0a37df760	295
GiJeongKim	0:51c43836c1d7	296	// // spi init
Lightvalve	16:903b5a4433b4	297	//eeprom.format(8,3);
Lightvalve	16:903b5a4433b4	298	//eeprom.frequency(5000000); //5M
GiJeongKim	0:51c43836c1d7	299	enc.format(8,0);
GiJeongKim	0:51c43836c1d7	300	enc.frequency(5000000); //5M
Lightvalve	11:82d8768d7351	301	make_delay();
Lightvalve	21:e5f1a43ea6f9	302
Lightvalve	16:903b5a4433b4	303	//rom
Lightvalve	19:23b7c1ad8683	304	ROM_CALL_DATA();
Lightvalve	16:903b5a4433b4	305	make_delay();
Lightvalve	13:747daba9cf59	306
GiJeongKim	0:51c43836c1d7	307	// ADC init
jobuuu	5:a4319f79457b	308	Init_ADC();
Lightvalve	11:82d8768d7351	309	make_delay();
jobuuu	2:a1c0a37df760	310
GiJeongKim	0:51c43836c1d7	311	// Pwm init
GiJeongKim	0:51c43836c1d7	312	Init_PWM();
GiJeongKim	0:51c43836c1d7	313	TIM4->CR1 ^= TIM_CR1_UDIS;
Lightvalve	11:82d8768d7351	314	make_delay();
Lightvalve	13:747daba9cf59	315
Lightvalve	11:82d8768d7351	316	// TMR3 init
Lightvalve	11:82d8768d7351	317	Init_TMR3();
Lightvalve	11:82d8768d7351	318	TIM3->CR1 ^= TIM_CR1_UDIS;
Lightvalve	11:82d8768d7351	319	make_delay();
Lightvalve	21:e5f1a43ea6f9	320
Lightvalve	50:3c630b5eba9f	321	// TMR2 init
Lightvalve	56:6f50d9d3bfee	322	// Init_TMR2();
Lightvalve	56:6f50d9d3bfee	323	// TIM2->CR1 ^= TIM_CR1_UDIS;
Lightvalve	56:6f50d9d3bfee	324	// make_delay();
Lightvalve	21:e5f1a43ea6f9	325
GiJeongKim	0:51c43836c1d7	326	// CAN
jobuuu	2:a1c0a37df760	327	can.attach(&CAN_RX_HANDLER);
Lightvalve	11:82d8768d7351	328	CAN_ID_INIT();
Lightvalve	11:82d8768d7351	329	make_delay();
Lightvalve	34:bb2ca2fc2a8e	330
Lightvalve	23:59218d4a256d	331	//Timer priority
Lightvalve	23:59218d4a256d	332	NVIC_SetPriority(TIM3_IRQn, 2);
Lightvalve	57:f4819de54e7a	333	//NVIC_SetPriority(TIM2_IRQn, 3);
Lightvalve	54:647072f5307a	334	NVIC_SetPriority(TIM4_IRQn, 3);
Lightvalve	34:bb2ca2fc2a8e	335
Lightvalve	23:59218d4a256d	336	//can.reset();
Lightvalve	19:23b7c1ad8683	337	can.filter(msg.id, 0xFFFFF000, CANStandard);
Lightvalve	34:bb2ca2fc2a8e	338
GiJeongKim	0:51c43836c1d7	339	// spi _ enc
GiJeongKim	0:51c43836c1d7	340	spi_enc_set_init();
Lightvalve	11:82d8768d7351	341	make_delay();
Lightvalve	13:747daba9cf59	342
Lightvalve	11:82d8768d7351	343	//DAC init
Lightvalve	58:2eade98630e2	344	if (SENSING_MODE == 0) {
Lightvalve	58:2eade98630e2	345	dac_1 = TORQUE_VREF / 3.3f;
Lightvalve	58:2eade98630e2	346	dac_2 = 0.0f;
Lightvalve	58:2eade98630e2	347	} else if (SENSING_MODE == 1) {
Lightvalve	58:2eade98630e2	348	dac_1 = PRES_A_VREF / 3.3f;
Lightvalve	58:2eade98630e2	349	dac_2 = PRES_B_VREF / 3.3f;
Lightvalve	58:2eade98630e2	350	}
Lightvalve	11:82d8768d7351	351	make_delay();
Lightvalve	13:747daba9cf59	352
Lightvalve	19:23b7c1ad8683	353	for (int i=0; i<50; i++) {
Lightvalve	11:82d8768d7351	354	if(i%2==0)
Lightvalve	38:118df027d851	355	ID_index_array[i] = - i * 0.5f;
Lightvalve	11:82d8768d7351	356	else
Lightvalve	38:118df027d851	357	ID_index_array[i] = (i+1) * 0.5f;
Lightvalve	11:82d8768d7351	358	}
Lightvalve	61:bc8c8270f0ab	359
Lightvalve	61:bc8c8270f0ab	360
jobuuu	6:df07d3491e3a	361	/************************************
jobuuu	1:e04e563be5ce	362	*** Program is operating!
jobuuu	6:df07d3491e3a	363	*************************************/
GiJeongKim	0:51c43836c1d7	364	while(1) {
Lightvalve	66:a8e6799dbce3	365	// if(timer_while==1000 && OPERATING_MODE==5) {
Lightvalve	66:a8e6799dbce3	366	//if(timer_while==1000) {
Lightvalve	65:a2d7c63419c2	367	//i2c
Lightvalve	66:a8e6799dbce3	368
Lightvalve	65:a2d7c63419c2	369	read_field(i2c_slave_addr1);
Lightvalve	65:a2d7c63419c2	370	if(DIR_VALVE_ENC < 0) value = 1023 - value;
Lightvalve	65:a2d7c63419c2	371	// if(LED==1) {
Lightvalve	65:a2d7c63419c2	372	// LED=0;
Lightvalve	65:a2d7c63419c2	373	// } else
Lightvalve	65:a2d7c63419c2	374	// LED = 1;
Lightvalve	65:a2d7c63419c2	375	timer_while = 0;
Lightvalve	66:a8e6799dbce3	376	//}
Lightvalve	66:a8e6799dbce3	377
Lightvalve	66:a8e6799dbce3	378	timer_while ++;
Lightvalve	66:a8e6799dbce3	379
Lightvalve	87:471334725012	380	//LED = 0;
Lightvalve	87:471334725012	381
Lightvalve	73:f80dc3970c99	382	if(NN_Control_Flag == 0) {
Lightvalve	73:f80dc3970c99	383	LED = 0;
Lightvalve	73:f80dc3970c99	384	}
Lightvalve	66:a8e6799dbce3	385
Lightvalve	73:f80dc3970c99	386	else if(NN_Control_Flag == 1) {
Lightvalve	112:8dcb1600cb90	387	float output1[16] = { 0.0f };
Lightvalve	112:8dcb1600cb90	388	float output2[16] = { 0.0f };
Lightvalve	112:8dcb1600cb90	389	float output3[16] = { 0.0f };
Lightvalve	66:a8e6799dbce3	390	float output = 0.0f;
Lightvalve	65:a2d7c63419c2	391
Lightvalve	112:8dcb1600cb90	392	for (int index2 = 0; index2 < 16; index2++) {
Lightvalve	68:328e1be06f5d	393	for (int index1 = 0; index1 < num_input; index1++) {
Lightvalve	66:a8e6799dbce3	394	output1[index2] = output1[index2]
Lightvalve	73:f80dc3970c99	395	+ h1[index1][index2] * input_NN[index1];
Lightvalve	66:a8e6799dbce3	396	}
Lightvalve	66:a8e6799dbce3	397	output1[index2] = output1[index2] + b1[index2];
Lightvalve	66:a8e6799dbce3	398	if (output1[index2] < 0) {
Lightvalve	66:a8e6799dbce3	399	output1[index2] = 0;
Lightvalve	66:a8e6799dbce3	400	}
Lightvalve	66:a8e6799dbce3	401	}
Lightvalve	65:a2d7c63419c2	402
Lightvalve	112:8dcb1600cb90	403	for (int index2 = 0; index2 < 16; index2++) {
Lightvalve	112:8dcb1600cb90	404	for (int index1 = 0; index1 < 16; index1++) {
Lightvalve	66:a8e6799dbce3	405	output2[index2] = output2[index2]
Lightvalve	66:a8e6799dbce3	406	+ h2[index1][index2] * output1[index1];
Lightvalve	66:a8e6799dbce3	407	}
Lightvalve	66:a8e6799dbce3	408	output2[index2] = output2[index2] + b2[index2];
Lightvalve	66:a8e6799dbce3	409	if (output2[index2] < 0) {
Lightvalve	66:a8e6799dbce3	410	output2[index2] = 0;
Lightvalve	66:a8e6799dbce3	411	}
Lightvalve	66:a8e6799dbce3	412	}
Lightvalve	65:a2d7c63419c2	413
Lightvalve	112:8dcb1600cb90	414	for (int index2 = 0; index2 < 16; index2++) {
Lightvalve	112:8dcb1600cb90	415	for (int index1 = 0; index1 < 16; index1++) {
Lightvalve	66:a8e6799dbce3	416	output3[index2] = output3[index2]
Lightvalve	66:a8e6799dbce3	417	+ h3[index1][index2] * output2[index1];
Lightvalve	66:a8e6799dbce3	418	}
Lightvalve	66:a8e6799dbce3	419	output3[index2] = output3[index2] + b3[index2];
Lightvalve	66:a8e6799dbce3	420	if (output3[index2] < 0) {
Lightvalve	66:a8e6799dbce3	421	output3[index2] = 0;
Lightvalve	66:a8e6799dbce3	422	}
Lightvalve	65:a2d7c63419c2	423	}
Lightvalve	66:a8e6799dbce3	424
Lightvalve	66:a8e6799dbce3	425	for (int index2 = 0; index2 < 1; index2++) {
Lightvalve	112:8dcb1600cb90	426	for (int index1 = 0; index1 < 16; index1++) {
Lightvalve	73:f80dc3970c99	427	output = output + hout[index1] * output3[index1];
Lightvalve	66:a8e6799dbce3	428	}
Lightvalve	66:a8e6799dbce3	429	output = output + bout[index2];
Lightvalve	73:f80dc3970c99	430
Lightvalve	66:a8e6799dbce3	431	}
Lightvalve	73:f80dc3970c99	432	output = 1.0f/(1.0f+exp(-output));
Lightvalve	101:50159049a518	433	output_normalized = output;
Lightvalve	68:328e1be06f5d	434	output = output * 20000.0f - 10000.0f;
Lightvalve	101:50159049a518	435
Lightvalve	66:a8e6799dbce3	436	if(output>=0) {
Lightvalve	66:a8e6799dbce3	437	valve_pos.ref = output0.0001f((double)VALVE_MAX_POS - (double) VALVE_CENTER) + (double) VALVE_CENTER;
Lightvalve	66:a8e6799dbce3	438	} else {
Lightvalve	66:a8e6799dbce3	439	valve_pos.ref = -output0.0001f((double)VALVE_MIN_POS - (double) VALVE_CENTER) + (double) VALVE_CENTER;
Lightvalve	65:a2d7c63419c2	440	}
Lightvalve	87:471334725012	441
Lightvalve	88:d6e591bece22	442	// // torque feedback
Lightvalve	88:d6e591bece22	443	// torq.err = f_past[0] - torq.sen; //[N]
Lightvalve	88:d6e591bece22	444	//// torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
Lightvalve	88:d6e591bece22	445	// torq.err_sum += torq.err/(float) 1500.0f; //[N]
Lightvalve	88:d6e591bece22	446	//
Lightvalve	88:d6e591bece22	447	//
Lightvalve	88:d6e591bece22	448	// valve_pos.ref = ((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f + DDV_JOINT_POS_FF(vel.sen) + valve_pos.ref * 0.01f;
Lightvalve	88:d6e591bece22	449	//
Lightvalve	88:d6e591bece22	450	// if(I_GAIN_JOINT_TORQUE != 0) {
Lightvalve	88:d6e591bece22	451	// double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f;
Lightvalve	88:d6e591bece22	452	// if(valve_pos.ref>VALVE_MAX_POS) {
Lightvalve	88:d6e591bece22	453	// double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS;
Lightvalve	88:d6e591bece22	454	// valve_pos_rem = valve_pos_rem * Ka;
Lightvalve	88:d6e591bece22	455	// valve_pos.ref = VALVE_MAX_POS;
Lightvalve	88:d6e591bece22	456	// torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f;
Lightvalve	88:d6e591bece22	457	// } else if(valve_pos.ref < VALVE_MIN_POS) {
Lightvalve	88:d6e591bece22	458	// double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS;
Lightvalve	88:d6e591bece22	459	// valve_pos_rem = valve_pos_rem * Ka;
Lightvalve	88:d6e591bece22	460	// valve_pos.ref = VALVE_MIN_POS;
Lightvalve	88:d6e591bece22	461	// torq.err_sum = torq.err_sum - valve_pos_rem/(float) 1500.0f;
Lightvalve	88:d6e591bece22	462	// }
Lightvalve	88:d6e591bece22	463	// }
Lightvalve	87:471334725012	464
Lightvalve	69:3995ffeaa786	465
Lightvalve	69:3995ffeaa786	466	if(LED==1) {
Lightvalve	69:3995ffeaa786	467	LED=0;
Lightvalve	69:3995ffeaa786	468	} else
Lightvalve	69:3995ffeaa786	469	LED = 1;
Lightvalve	69:3995ffeaa786	470
Lightvalve	65:a2d7c63419c2	471	}
Lightvalve	62:b5452adfb2cd	472
Lightvalve	87:471334725012	473	//LED = 1;
Lightvalve	69:3995ffeaa786	474
Lightvalve	66:a8e6799dbce3	475
GiJeongKim	0:51c43836c1d7	476	}
jobuuu	1:e04e563be5ce	477	}
jobuuu	1:e04e563be5ce	478
Lightvalve	33:91b17819ec30	479	float DDV_JOINT_POS_FF(float REF_JOINT_VEL)
Lightvalve	14:8e7590227d22	480	{
Lightvalve	14:8e7590227d22	481
Lightvalve	13:747daba9cf59	482	int i = 0;
Lightvalve	48:889798ff9329	483	float Ref_Valve_Pos_FF = 0.0f;
Lightvalve	14:8e7590227d22	484	for(i=0; i<VALVE_POS_NUM; i++) {
Lightvalve	14:8e7590227d22	485	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	486	if(i==0) {
Lightvalve	50:3c630b5eba9f	487	if(JOINT_VEL[i+1] == JOINT_VEL[i]) {
Lightvalve	57:f4819de54e7a	488	Ref_Valve_Pos_FF = (float) VALVE_CENTER;
Lightvalve	50:3c630b5eba9f	489	} else {
Lightvalve	57:f4819de54e7a	490	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	491	}
Lightvalve	14:8e7590227d22	492	} else {
Lightvalve	50:3c630b5eba9f	493	if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) {
Lightvalve	57:f4819de54e7a	494	Ref_Valve_Pos_FF = (float) VALVE_CENTER;
Lightvalve	50:3c630b5eba9f	495	} else {
Lightvalve	57:f4819de54e7a	496	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	497	}
Lightvalve	13:747daba9cf59	498	}
Lightvalve	13:747daba9cf59	499	break;
Lightvalve	13:747daba9cf59	500	}
Lightvalve	13:747daba9cf59	501	}
Lightvalve	14:8e7590227d22	502	if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
Lightvalve	33:91b17819ec30	503	Ref_Valve_Pos_FF = (float) VALVE_MAX_POS;
Lightvalve	14:8e7590227d22	504	} else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
Lightvalve	33:91b17819ec30	505	Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
Lightvalve	13:747daba9cf59	506	}
Lightvalve	36:a46e63505ed8	507
Lightvalve	57:f4819de54e7a	508	Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER);
Lightvalve	13:747daba9cf59	509	return Ref_Valve_Pos_FF;
Lightvalve	50:3c630b5eba9f	510
Lightvalve	13:747daba9cf59	511	}
jobuuu	6:df07d3491e3a	512
jobuuu	6:df07d3491e3a	513
Lightvalve	30:8d561f16383b	514	void VALVE_POS_CONTROL(float REF_VALVE_POS)
Lightvalve	14:8e7590227d22	515	{
Lightvalve	13:747daba9cf59	516	int i = 0;
Lightvalve	13:747daba9cf59	517
Lightvalve	38:118df027d851	518	if(REF_VALVE_POS > VALVE_MAX_POS) {
Lightvalve	38:118df027d851	519	REF_VALVE_POS = VALVE_MAX_POS;
Lightvalve	38:118df027d851	520	} else if(REF_VALVE_POS < VALVE_MIN_POS) {
Lightvalve	38:118df027d851	521	REF_VALVE_POS = VALVE_MIN_POS;
Lightvalve	38:118df027d851	522	}
Lightvalve	38:118df027d851	523
Lightvalve	89:a7b45368ea0f	524	valve_pos_err = (float) (REF_VALVE_POS - value);
Lightvalve	13:747daba9cf59	525	valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
Lightvalve	13:747daba9cf59	526	valve_pos_err_old = valve_pos_err;
Lightvalve	13:747daba9cf59	527	valve_pos_err_sum += valve_pos_err;
Lightvalve	89:a7b45368ea0f	528	if (valve_pos_err_sum > 1000.0f) valve_pos_err_sum = 1000.0f;
Lightvalve	89:a7b45368ea0f	529	if (valve_pos_err_sum<-1000.0f) valve_pos_err_sum = -1000.0f;
Lightvalve	13:747daba9cf59	530
Lightvalve	13:747daba9cf59	531	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	532
Lightvalve	18:b8adf1582ea3	533	for(i=0; i<24; i++) {
Lightvalve	89:a7b45368ea0f	534	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	535	if(i==0) {
Lightvalve	48:889798ff9329	536	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	537	} else {
Lightvalve	48:889798ff9329	538	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	539	}
Lightvalve	13:747daba9cf59	540	break;
Lightvalve	13:747daba9cf59	541	}
Lightvalve	13:747daba9cf59	542	}
Lightvalve	59:f308b1656d9c	543	Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
Lightvalve	13:747daba9cf59	544	}
Lightvalve	13:747daba9cf59	545
Lightvalve	14:8e7590227d22	546	#define LT_MAX_IDX 57
Lightvalve	30:8d561f16383b	547	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	548	-19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f,
Lightvalve	34:bb2ca2fc2a8e	549	-9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f,
Lightvalve	34:bb2ca2fc2a8e	550	1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
Lightvalve	34:bb2ca2fc2a8e	551	11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f,
Lightvalve	34:bb2ca2fc2a8e	552	25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f
Lightvalve	34:bb2ca2fc2a8e	553	}; // duty
Lightvalve	67:c2812cf26c38	554	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	67:c2812cf26c38	555	-150.0f, -145.0f, -145.0f, -145.0f, -135.0f, -135.0f, -135.0f, -127.5f, -127.5f, -115.0f,
Lightvalve	67:c2812cf26c38	556	-115.0f, -115.0F, -100.0f, -100.0f, -100.0f, -60.0f, -60.0f, -10.0f, -5.0f, 0.0f,
Lightvalve	67:c2812cf26c38	557	7.5f, 14.0f, 14.0f, 14.0f, 42.5f, 42.5f, 42.5f, 80.0f, 80.0f, 105.0f,
Lightvalve	67:c2812cf26c38	558	105.0f, 105.0f, 120.0f, 120.0f, 120.0f, 131.0f, 131.0f, 140.0f, 140.0f, 140.0f,
Lightvalve	67:c2812cf26c38	559	155.0f, 160.0f, 170.0f, 174.0f, 182.0f, 191.0f, 212.0f, 230.0f
Lightvalve	34:bb2ca2fc2a8e	560	}; // mV
Lightvalve	13:747daba9cf59	561
Lightvalve	30:8d561f16383b	562	float PWM_duty_byLT(float Ref_V)
Lightvalve	14:8e7590227d22	563	{
Lightvalve	30:8d561f16383b	564	float PWM_duty = 0.0f;
Lightvalve	13:747daba9cf59	565	if(Ref_V<LT_Voltage_Output[0]) {
Lightvalve	30:8d561f16383b	566	PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5f+LT_PWM_duty[0];
Lightvalve	13:747daba9cf59	567	} else if (Ref_V>=LT_Voltage_Output[LT_MAX_IDX-1]) {
Lightvalve	30:8d561f16383b	568	PWM_duty = (Ref_V-LT_Voltage_Output[LT_MAX_IDX-1])/1.5f+LT_PWM_duty[LT_MAX_IDX-1];
Lightvalve	13:747daba9cf59	569	} else {
Lightvalve	13:747daba9cf59	570	int idx = 0;
Lightvalve	13:747daba9cf59	571	for(idx=0; idx<LT_MAX_IDX-1; idx++) {
Lightvalve	30:8d561f16383b	572	float ini_x = LT_Voltage_Output[idx];
Lightvalve	30:8d561f16383b	573	float fin_x = LT_Voltage_Output[idx+1];
Lightvalve	30:8d561f16383b	574	float ini_y = LT_PWM_duty[idx];
Lightvalve	30:8d561f16383b	575	float fin_y = LT_PWM_duty[idx+1];
Lightvalve	13:747daba9cf59	576	if(Ref_V>=ini_x && Ref_V<fin_x) {
Lightvalve	13:747daba9cf59	577	PWM_duty = (fin_y-ini_y)/(fin_x-ini_x)*(Ref_V-ini_x) + ini_y;
Lightvalve	13:747daba9cf59	578	break;
Lightvalve	13:747daba9cf59	579	}
Lightvalve	13:747daba9cf59	580	}
Lightvalve	13:747daba9cf59	581	}
Lightvalve	14:8e7590227d22	582
Lightvalve	13:747daba9cf59	583	return PWM_duty;
Lightvalve	13:747daba9cf59	584	}
jobuuu	6:df07d3491e3a	585
Lightvalve	57:f4819de54e7a	586
Lightvalve	57:f4819de54e7a	587
Lightvalve	57:f4819de54e7a	588
Lightvalve	57:f4819de54e7a	589
jobuuu	2:a1c0a37df760	590	/*******************************************************************************
jobuuu	2:a1c0a37df760	591	TIMER INTERRUPT
jobuuu	2:a1c0a37df760	592	*******************************************************************************/
jobuuu	2:a1c0a37df760	593
Lightvalve	51:b46bed7fec80	594	float FREQ_TMR4 = (float)FREQ_20k;
Lightvalve	51:b46bed7fec80	595	float DT_TMR4 = (float)DT_20k;
Lightvalve	57:f4819de54e7a	596	long CNT_TMR4 = 0;
Lightvalve	57:f4819de54e7a	597	int TMR4_FREQ_10k = (int)FREQ_10k;
jobuuu	1:e04e563be5ce	598	extern "C" void TIM4_IRQHandler(void)
jobuuu	1:e04e563be5ce	599	{
Lightvalve	19:23b7c1ad8683	600	if (TIM4->SR & TIM_SR_UIF ) {
Lightvalve	21:e5f1a43ea6f9	601
Lightvalve	21:e5f1a43ea6f9	602	/*******************************************************
Lightvalve	21:e5f1a43ea6f9	603	*** Sensor Read & Data Handling
Lightvalve	21:e5f1a43ea6f9	604	********************************************************/
Lightvalve	13:747daba9cf59	605
Lightvalve	57:f4819de54e7a	606	//Encoder
Lightvalve	57:f4819de54e7a	607	if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
Lightvalve	57:f4819de54e7a	608	ENC_UPDATE();
Lightvalve	57:f4819de54e7a	609	}
Lightvalve	61:bc8c8270f0ab	610
Lightvalve	61:bc8c8270f0ab	611	ADC1->CR2 \|= 0x40000000;
Lightvalve	58:2eade98630e2	612	if (SENSING_MODE == 0) {
Lightvalve	58:2eade98630e2	613	// Torque Sensing (0~210)bar =============================================
Lightvalve	58:2eade98630e2	614	float pres_A_new = (((float) ADC1->DR) - 2047.5f);
Lightvalve	58:2eade98630e2	615	double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 200Hz
Lightvalve	58:2eade98630e2	616	pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
Lightvalve	67:c2812cf26c38	617	torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
Lightvalve	67:c2812cf26c38	618
Lightvalve	67:c2812cf26c38	619
Lightvalve	67:c2812cf26c38	620	// float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f3.14f100.0f));
Lightvalve	67:c2812cf26c38	621	//// float pres_A_new = ((float)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR;
Lightvalve	67:c2812cf26c38	622	// float pres_A_new = ((float)ADC1->DR);
Lightvalve	67:c2812cf26c38	623	// pres_A.sen = pres_A.sen(1.0f-alpha_update_pres_A)+pres_A_new(alpha_update_pres_A);
Lightvalve	67:c2812cf26c38	624	// torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047
Lightvalve	67:c2812cf26c38	625
Lightvalve	17:1865016ca2e7	626
Lightvalve	58:2eade98630e2	627	} else if (SENSING_MODE == 1) {
Lightvalve	58:2eade98630e2	628	// Pressure Sensing (0~210)bar =============================================
Lightvalve	58:2eade98630e2	629	float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
Lightvalve	58:2eade98630e2	630	float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
Lightvalve	58:2eade98630e2	631	double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 500Hz
Lightvalve	58:2eade98630e2	632	pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new;
Lightvalve	58:2eade98630e2	633	pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new;
Lightvalve	58:2eade98630e2	634	CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR;
Lightvalve	58:2eade98630e2	635	CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR;
Lightvalve	50:3c630b5eba9f	636
Lightvalve	58:2eade98630e2	637	if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
Lightvalve	58:2eade98630e2	638	torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.0001f; // mm^3*bar >> Nm
Lightvalve	58:2eade98630e2	639	} else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
Lightvalve	58:2eade98630e2	640	torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N
Lightvalve	58:2eade98630e2	641	}
Lightvalve	58:2eade98630e2	642	}
Lightvalve	61:bc8c8270f0ab	643
Lightvalve	58:2eade98630e2	644	// //Pressure sensor A
Lightvalve	58:2eade98630e2	645	// ADC1->CR2 \|= 0x40000000; // adc _ 12bit
Lightvalve	58:2eade98630e2	646	// //while((ADC1->SR & 0b10));
Lightvalve	58:2eade98630e2	647	// float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f3.14f100.0f));
Lightvalve	58:2eade98630e2	648	// float pres_A_new = ((float)ADC1->DR);
Lightvalve	58:2eade98630e2	649	// pres_A.sen = pres_A.sen(1.0f-alpha_update_pres_A)+pres_A_new(alpha_update_pres_A);
Lightvalve	58:2eade98630e2	650	// torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047 //SW just changed the sign to correct the direction of loadcell on LIGHT. Correct later.
Lightvalve	58:2eade98630e2	651	//
Lightvalve	58:2eade98630e2	652	//
Lightvalve	58:2eade98630e2	653	// //Pressure sensor B
Lightvalve	58:2eade98630e2	654	// float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f3.14f100.0f));
Lightvalve	58:2eade98630e2	655	// float pres_B_new = ((float)ADC2->DR);
Lightvalve	58:2eade98630e2	656	// pres_B.sen = pres_B.sen(1.0f-alpha_update_pres_B)+pres_B_new(alpha_update_pres_B);
Lightvalve	58:2eade98630e2	657	// //torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
Lightvalve	17:1865016ca2e7	658
Lightvalve	17:1865016ca2e7	659
Lightvalve	21:e5f1a43ea6f9	660	//Current
Lightvalve	21:e5f1a43ea6f9	661	//ADC3->CR2 \|= 0x40000000; // adc _ 12bit
Lightvalve	25:3e6b574cab5c	662	//int raw_cur = ADC3->DR;
Lightvalve	21:e5f1a43ea6f9	663	//while((ADC3->SR & 0b10));
Lightvalve	51:b46bed7fec80	664	float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f3.14f500.0f)); // f_cutoff : 500Hz
Lightvalve	30:8d561f16383b	665	float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA
Lightvalve	30:8d561f16383b	666	cur.sen=cur.sen(1.0f-alpha_update_cur)+cur_new(alpha_update_cur);
Lightvalve	25:3e6b574cab5c	667	//cur.sen = raw_cur;
Lightvalve	58:2eade98630e2	668
Lightvalve	57:f4819de54e7a	669	CNT_TMR4++;
Lightvalve	21:e5f1a43ea6f9	670	}
Lightvalve	11:82d8768d7351	671	TIM4->SR = 0x0; // reset the status register
Lightvalve	11:82d8768d7351	672	}
Lightvalve	19:23b7c1ad8683	673
Lightvalve	19:23b7c1ad8683	674
Lightvalve	18:b8adf1582ea3	675	int j =0;
Lightvalve	54:647072f5307a	676	float FREQ_TMR3 = (float)FREQ_5k;
Lightvalve	48:889798ff9329	677	float DT_TMR3 = (float)DT_5k;
Lightvalve	45:35fa6884d0c6	678	int cnt_trans = 0;
Lightvalve	48:889798ff9329	679	double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
Lightvalve	57:f4819de54e7a	680	int can_rest =0;
Lightvalve	48:889798ff9329	681
Lightvalve	11:82d8768d7351	682	extern "C" void TIM3_IRQHandler(void)
Lightvalve	21:e5f1a43ea6f9	683	{
Lightvalve	19:23b7c1ad8683	684	if (TIM3->SR & TIM_SR_UIF ) {
Lightvalve	57:f4819de54e7a	685
Lightvalve	57:f4819de54e7a	686	if (((OPERATING_MODE&0b110)>>1) == 0) {
Lightvalve	57:f4819de54e7a	687	K_v = 0.4f; // Moog (LPM >> mA) , 100bar
Lightvalve	57:f4819de54e7a	688	mV_PER_mA = 500.0f; // 5000mV/10mA
Lightvalve	57:f4819de54e7a	689	mV_PER_pulse = 0.5f; // 5000mV/10000pulse
Lightvalve	57:f4819de54e7a	690	mA_PER_pulse = 0.001f; // 10mA/10000pulse
Lightvalve	57:f4819de54e7a	691	} else if (((OPERATING_MODE&0b110)>>1) == 1) {
Lightvalve	57:f4819de54e7a	692	K_v = 0.5f; // KNR (LPM >> mA) , 100bar
Lightvalve	57:f4819de54e7a	693	mV_PER_mA = 166.6666f; // 5000mV/30mA
Lightvalve	57:f4819de54e7a	694	mV_PER_pulse = 0.5f; // 5000mV/10000pulse
Lightvalve	57:f4819de54e7a	695	mA_PER_pulse = 0.003f; // 30mA/10000pulse
Lightvalve	57:f4819de54e7a	696	}
Lightvalve	50:3c630b5eba9f	697
Lightvalve	50:3c630b5eba9f	698	if(MODE_POS_FT_TRANS == 1) {
Lightvalve	48:889798ff9329	699	alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
Lightvalve	45:35fa6884d0c6	700	cnt_trans++;
Lightvalve	46:2694daea349b	701	torq.err_sum = 0;
Lightvalve	48:889798ff9329	702	if((float)cnt_trans * DT_TMR3 > 3.0f)
Lightvalve	45:35fa6884d0c6	703	MODE_POS_FT_TRANS = 2;
Lightvalve	50:3c630b5eba9f	704	} else if(MODE_POS_FT_TRANS == 3) {
Lightvalve	48:889798ff9329	705	alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
Lightvalve	45:35fa6884d0c6	706	cnt_trans++;
Lightvalve	46:2694daea349b	707	torq.err_sum = 0;
Lightvalve	48:889798ff9329	708	if((float) cnt_trans * DT_TMR3 > 3.0f )
Lightvalve	45:35fa6884d0c6	709	MODE_POS_FT_TRANS = 0;
Lightvalve	50:3c630b5eba9f	710	} else if(MODE_POS_FT_TRANS == 2) {
Lightvalve	58:2eade98630e2	711	alpha_trans = 1.0f;
Lightvalve	45:35fa6884d0c6	712	cnt_trans = 0;
Lightvalve	50:3c630b5eba9f	713	} else {
Lightvalve	58:2eade98630e2	714	alpha_trans = 0.0f;
Lightvalve	45:35fa6884d0c6	715	cnt_trans = 0;
Lightvalve	45:35fa6884d0c6	716	}
Lightvalve	45:35fa6884d0c6	717
Lightvalve	50:3c630b5eba9f	718
Lightvalve	57:f4819de54e7a	719	int UTILITY_MODE = 0;
Lightvalve	57:f4819de54e7a	720	int CONTROL_MODE = 0;
Lightvalve	58:2eade98630e2	721
Lightvalve	57:f4819de54e7a	722	if (CONTROL_UTILITY_MODE >= 20 \|\| CONTROL_UTILITY_MODE == 0) {
Lightvalve	57:f4819de54e7a	723	UTILITY_MODE = CONTROL_UTILITY_MODE;
Lightvalve	57:f4819de54e7a	724	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	57:f4819de54e7a	725	} else {
Lightvalve	57:f4819de54e7a	726	CONTROL_MODE = CONTROL_UTILITY_MODE;
Lightvalve	57:f4819de54e7a	727	UTILITY_MODE = MODE_NO_ACT;
Lightvalve	57:f4819de54e7a	728	}
Lightvalve	56:6f50d9d3bfee	729
Lightvalve	56:6f50d9d3bfee	730
Lightvalve	56:6f50d9d3bfee	731
Lightvalve	57:f4819de54e7a	732	// UTILITY MODE ------------------------------------------------------------
Lightvalve	56:6f50d9d3bfee	733
Lightvalve	57:f4819de54e7a	734	switch (UTILITY_MODE) {
Lightvalve	57:f4819de54e7a	735	case MODE_NO_ACT: {
Lightvalve	13:747daba9cf59	736	break;
Lightvalve	13:747daba9cf59	737	}
Lightvalve	14:8e7590227d22	738
Lightvalve	14:8e7590227d22	739	case MODE_TORQUE_SENSOR_NULLING: {
Lightvalve	13:747daba9cf59	740	// DAC Voltage reference set
Lightvalve	14:8e7590227d22	741	if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
Lightvalve	15:bd0d12728506	742	CUR_TORQUE_sum += torq.sen;
Lightvalve	19:23b7c1ad8683	743
Lightvalve	14:8e7590227d22	744	if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
Lightvalve	30:8d561f16383b	745	CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f;
Lightvalve	13:747daba9cf59	746	CUR_TORQUE_sum = 0;
Lightvalve	14:8e7590227d22	747
Lightvalve	84:c355d3e52bf1	748	TORQUE_VREF += 0.000003f * (0.0f - CUR_TORQUE_mean);
Lightvalve	19:23b7c1ad8683	749
Lightvalve	30:8d561f16383b	750	if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
Lightvalve	59:f308b1656d9c	751	if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f;
Lightvalve	50:3c630b5eba9f	752
Lightvalve	16:903b5a4433b4	753	//spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
Lightvalve	30:8d561f16383b	754	dac_1 = TORQUE_VREF / 3.3f;
Lightvalve	13:747daba9cf59	755	}
Lightvalve	13:747daba9cf59	756	} else {
Lightvalve	58:2eade98630e2	757	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	14:8e7590227d22	758	TMR3_COUNT_TORQUE_NULL = 0;
Lightvalve	13:747daba9cf59	759	CUR_TORQUE_sum = 0;
Lightvalve	13:747daba9cf59	760	CUR_TORQUE_mean = 0;
Lightvalve	19:23b7c1ad8683	761
Lightvalve	16:903b5a4433b4	762	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	763
Lightvalve	30:8d561f16383b	764	dac_1 = TORQUE_VREF / 3.3f;
Lightvalve	14:8e7590227d22	765
Lightvalve	13:747daba9cf59	766	}
Lightvalve	14:8e7590227d22	767	TMR3_COUNT_TORQUE_NULL++;
Lightvalve	13:747daba9cf59	768	break;
Lightvalve	19:23b7c1ad8683	769	}
Lightvalve	14:8e7590227d22	770
Lightvalve	50:3c630b5eba9f	771	// case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
Lightvalve	50:3c630b5eba9f	772	// if (TMR3_COUNT_DEADZONE == 0) {
Lightvalve	50:3c630b5eba9f	773	// if (pos_plus_end == pos_minus_end) need_enc_init = true;
Lightvalve	50:3c630b5eba9f	774	// else temp_time = 0;
Lightvalve	50:3c630b5eba9f	775	// }
Lightvalve	50:3c630b5eba9f	776	// if (need_enc_init) {
Lightvalve	50:3c630b5eba9f	777	// if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
Lightvalve	50:3c630b5eba9f	778	// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	779	// pos_plus_end = pos.sen;
Lightvalve	50:3c630b5eba9f	780	// } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
Lightvalve	50:3c630b5eba9f	781	// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	782	// pos_minus_end = pos.sen;
Lightvalve	50:3c630b5eba9f	783	// } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
Lightvalve	50:3c630b5eba9f	784	// temp_time = TMR_FREQ_5k;
Lightvalve	50:3c630b5eba9f	785	// }
Lightvalve	50:3c630b5eba9f	786	//
Lightvalve	50:3c630b5eba9f	787	// if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
Lightvalve	50:3c630b5eba9f	788	// V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
Lightvalve	50:3c630b5eba9f	789	// VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
Lightvalve	50:3c630b5eba9f	790	//
Lightvalve	50:3c630b5eba9f	791	// } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) {
Lightvalve	50:3c630b5eba9f	792	// V_out = 0;
Lightvalve	50:3c630b5eba9f	793	// CUR_VELOCITY_sum += CUR_VELOCITY;
Lightvalve	50:3c630b5eba9f	794	// } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
Lightvalve	50:3c630b5eba9f	795	// if (CUR_VELOCITY_sum == 0) DZ_dir = 1;
Lightvalve	50:3c630b5eba9f	796	// else if (CUR_VELOCITY_sum > 0) DZ_dir = 1;
Lightvalve	50:3c630b5eba9f	797	// else if (CUR_VELOCITY_sum < 0) DZ_dir = -1;
Lightvalve	50:3c630b5eba9f	798	// else DZ_temp_cnt2 = DZ_end;
Lightvalve	50:3c630b5eba9f	799	// CUR_VELOCITY_sum = 0;
Lightvalve	50:3c630b5eba9f	800	// } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
Lightvalve	50:3c630b5eba9f	801	// if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
Lightvalve	50:3c630b5eba9f	802	//
Lightvalve	50:3c630b5eba9f	803	// // Position of Dead Zone
Lightvalve	50:3c630b5eba9f	804	// // (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0)
Lightvalve	50:3c630b5eba9f	805	// // \| / \| / \|/
Lightvalve	50:3c630b5eba9f	806	// // \| ______/ ___\|___/ ______/\|
Lightvalve	50:3c630b5eba9f	807	// // \|/ / \| / \|
Lightvalve	50:3c630b5eba9f	808	// // /\| / \| / \|
Lightvalve	50:3c630b5eba9f	809	// // 0V 0V 0V
Lightvalve	50:3c630b5eba9f	810	//
Lightvalve	50:3c630b5eba9f	811	// if (DZ_temp_cnt2 < DZ_end) {
Lightvalve	50:3c630b5eba9f	812	// if (TMR3_COUNT_DEADZONE % 20 != 0) {
Lightvalve	50:3c630b5eba9f	813	// CUR_VELOCITY_sum += CUR_VELOCITY;
Lightvalve	50:3c630b5eba9f	814	// } else {
Lightvalve	50:3c630b5eba9f	815	// V_out -= DZ_dir;
Lightvalve	50:3c630b5eba9f	816	// if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
Lightvalve	50:3c630b5eba9f	817	// CUR_VELOCITY_sum = 0;
Lightvalve	50:3c630b5eba9f	818	// }
Lightvalve	50:3c630b5eba9f	819	// if (DZ_temp_cnt == 5) {
Lightvalve	50:3c630b5eba9f	820	// if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out;
Lightvalve	50:3c630b5eba9f	821	// else VALVE_DEADZONE_PLUS = (int16_t) V_out;
Lightvalve	50:3c630b5eba9f	822	// DZ_dir = -DZ_dir;
Lightvalve	50:3c630b5eba9f	823	// DZ_temp_cnt = 0;
Lightvalve	50:3c630b5eba9f	824	// DZ_temp_cnt2++;
Lightvalve	50:3c630b5eba9f	825	// }
Lightvalve	50:3c630b5eba9f	826	// } else {
Lightvalve	50:3c630b5eba9f	827	// TMR3_COUNT_DEADZONE = -1;
Lightvalve	50:3c630b5eba9f	828	// VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2;
Lightvalve	50:3c630b5eba9f	829	// if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) {
Lightvalve	50:3c630b5eba9f	830	// VALVE_DEADZONE_PLUS = VALVE_CENTER;
Lightvalve	50:3c630b5eba9f	831	// VALVE_DEADZONE_MINUS = VALVE_CENTER;
Lightvalve	50:3c630b5eba9f	832	// }
Lightvalve	50:3c630b5eba9f	833	// V_out = 0;
Lightvalve	50:3c630b5eba9f	834	//
Lightvalve	50:3c630b5eba9f	835	// ROM_RESET_DATA();
Lightvalve	50:3c630b5eba9f	836	//
Lightvalve	50:3c630b5eba9f	837	// //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
Lightvalve	50:3c630b5eba9f	838	// //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
Lightvalve	50:3c630b5eba9f	839	//
Lightvalve	50:3c630b5eba9f	840	// CONTROL_MODE = MODE_NO_ACT;
Lightvalve	50:3c630b5eba9f	841	// DZ_temp_cnt2 = 0;
Lightvalve	50:3c630b5eba9f	842	// }
Lightvalve	50:3c630b5eba9f	843	// }
Lightvalve	50:3c630b5eba9f	844	// TMR3_COUNT_DEADZONE++;
Lightvalve	50:3c630b5eba9f	845	// break;
Lightvalve	50:3c630b5eba9f	846	// }
Lightvalve	14:8e7590227d22	847
Lightvalve	14:8e7590227d22	848	case MODE_FIND_HOME: {
Lightvalve	29:69f3f5445d6d	849	if (FINDHOME_STAGE == FINDHOME_INIT) {
Lightvalve	14:8e7590227d22	850	cnt_findhome = 0;
Lightvalve	14:8e7590227d22	851	cnt_vel_findhome = 0;
Lightvalve	14:8e7590227d22	852	//REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
Lightvalve	59:f308b1656d9c	853	pos.ref = pos.sen;
Lightvalve	59:f308b1656d9c	854	vel.ref = 0.0f;
Lightvalve	29:69f3f5445d6d	855	FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
Lightvalve	29:69f3f5445d6d	856	} else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
Lightvalve	67:c2812cf26c38	857	int cnt_check_enc = (TMR_FREQ_5k/20);
Lightvalve	29:69f3f5445d6d	858	if(cnt_findhome%cnt_check_enc == 0) {
Lightvalve	29:69f3f5445d6d	859	FINDHOME_POSITION = pos.sen;
Lightvalve	29:69f3f5445d6d	860	FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
Lightvalve	29:69f3f5445d6d	861	FINDHOME_POSITION_OLD = FINDHOME_POSITION;
Lightvalve	29:69f3f5445d6d	862	}
Lightvalve	29:69f3f5445d6d	863	cnt_findhome++;
Lightvalve	14:8e7590227d22	864
Lightvalve	29:69f3f5445d6d	865	if (abs(FINDHOME_VELOCITY) <= 1) {
Lightvalve	29:69f3f5445d6d	866	cnt_vel_findhome = cnt_vel_findhome + 1;
Lightvalve	29:69f3f5445d6d	867	} else {
Lightvalve	29:69f3f5445d6d	868	cnt_vel_findhome = 0;
Lightvalve	29:69f3f5445d6d	869	}
Lightvalve	19:23b7c1ad8683	870
Lightvalve	57:f4819de54e7a	871	if ((cnt_vel_findhome < 3TMR_FREQ_5k) && cnt_findhome < 10TMR_FREQ_5k) { // wait for 3sec
Lightvalve	29:69f3f5445d6d	872	//REFERENCE_MODE = MODE_REF_NO_ACT;
Lightvalve	67:c2812cf26c38	873	if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f;
Lightvalve	67:c2812cf26c38	874	else pos.ref = pos.ref - 12.0f;
Lightvalve	61:bc8c8270f0ab	875
Lightvalve	59:f308b1656d9c	876	// pos.err = pos.ref_home_pos - pos.sen;
Lightvalve	59:f308b1656d9c	877	// float VALVE_POS_RAW_POS_FB = 0.0f;
Lightvalve	59:f308b1656d9c	878	// VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
Lightvalve	59:f308b1656d9c	879	// valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
Lightvalve	59:f308b1656d9c	880	// VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	61:bc8c8270f0ab	881
Lightvalve	59:f308b1656d9c	882	CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	59:f308b1656d9c	883	alpha_trans = 0.0f;
Lightvalve	61:bc8c8270f0ab	884
Lightvalve	34:bb2ca2fc2a8e	885
Lightvalve	29:69f3f5445d6d	886	} else {
Lightvalve	29:69f3f5445d6d	887	ENC_SET(HOMEPOS_OFFSET);
Lightvalve	67:c2812cf26c38	888	// ENC_SET_ZERO();
Lightvalve	29:69f3f5445d6d	889	INIT_REF_POS = HOMEPOS_OFFSET;
Lightvalve	29:69f3f5445d6d	890	REF_POSITION = 0;
Lightvalve	29:69f3f5445d6d	891	REF_VELOCITY = 0;
Lightvalve	29:69f3f5445d6d	892	FINDHOME_POSITION = 0;
Lightvalve	29:69f3f5445d6d	893	FINDHOME_POSITION_OLD = 0;
Lightvalve	29:69f3f5445d6d	894	FINDHOME_VELOCITY = 0;
Lightvalve	29:69f3f5445d6d	895	cnt_findhome = 0;
Lightvalve	29:69f3f5445d6d	896	cnt_vel_findhome = 0;
Lightvalve	29:69f3f5445d6d	897	FINDHOME_STAGE = FINDHOME_ZEROPOSE;
Lightvalve	67:c2812cf26c38	898
Lightvalve	67:c2812cf26c38	899
Lightvalve	67:c2812cf26c38	900	cnt_findhome = 0;
Lightvalve	67:c2812cf26c38	901	pos.ref = 0.0f;
Lightvalve	67:c2812cf26c38	902	vel.ref = 0.0f;
Lightvalve	67:c2812cf26c38	903	pos.ref_home_pos = 0.0f;
Lightvalve	67:c2812cf26c38	904	vel.ref_home_pos = 0.0f;
Lightvalve	67:c2812cf26c38	905	//FINDHOME_STAGE = FINDHOME_INIT;
Lightvalve	67:c2812cf26c38	906	//CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
Lightvalve	67:c2812cf26c38	907
Lightvalve	67:c2812cf26c38	908
Lightvalve	29:69f3f5445d6d	909	}
Lightvalve	29:69f3f5445d6d	910	} else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
Lightvalve	29:69f3f5445d6d	911	int T_move = 2*TMR_FREQ_5k;
Lightvalve	59:f308b1656d9c	912	pos.ref = (0.0f - (float)INIT_REF_POS)0.5f(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS;
Lightvalve	67:c2812cf26c38	913	//pos.ref = 0.0f;
Lightvalve	59:f308b1656d9c	914	vel.ref = 0.0f;
Lightvalve	67:c2812cf26c38	915
Lightvalve	29:69f3f5445d6d	916	// input for position control
Lightvalve	67:c2812cf26c38	917
Lightvalve	67:c2812cf26c38	918	// CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	67:c2812cf26c38	919	alpha_trans = 0.0f;
Lightvalve	61:bc8c8270f0ab	920
Lightvalve	67:c2812cf26c38	921	double torq_ref = 0.0f;
Lightvalve	67:c2812cf26c38	922	pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
Lightvalve	67:c2812cf26c38	923	vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
Lightvalve	67:c2812cf26c38	924	pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
Lightvalve	67:c2812cf26c38	925
Lightvalve	67:c2812cf26c38	926	if (((OPERATING_MODE&0b110)>>1) == 0 \|\| ((OPERATING_MODE&0b110)>>1) == 1) {
Lightvalve	67:c2812cf26c38	927
Lightvalve	67:c2812cf26c38	928	double I_REF_POS = 0.0f;
Lightvalve	67:c2812cf26c38	929	double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
Lightvalve	67:c2812cf26c38	930	double I_REF_VC = 0.0f; // I_REF for velocity compensation
Lightvalve	67:c2812cf26c38	931
Lightvalve	67:c2812cf26c38	932	double temp_vel_pos = 0.0f;
Lightvalve	67:c2812cf26c38	933	double temp_vel_torq = 0.0f;
Lightvalve	67:c2812cf26c38	934	double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
Lightvalve	67:c2812cf26c38	935
Lightvalve	69:3995ffeaa786	936	if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
Lightvalve	67:c2812cf26c38	937	temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * 3.14159f / 180.0f; // rad/s
Lightvalve	67:c2812cf26c38	938	// L when P-gain = 100, f_cut = 10Hz L feedforward velocity
Lightvalve	69:3995ffeaa786	939	} else if ((OPERATING_MODE & 0x01) == 1) {
Lightvalve	67:c2812cf26c38	940	temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
Lightvalve	67:c2812cf26c38	941	// L when P-gain = 100, f_cut = 10Hz L feedforward velocity
Lightvalve	67:c2812cf26c38	942	}
Lightvalve	67:c2812cf26c38	943	if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
Lightvalve	67:c2812cf26c38	944	else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
Lightvalve	67:c2812cf26c38	945
Lightvalve	67:c2812cf26c38	946	I_REF = I_REF_POS;
Lightvalve	67:c2812cf26c38	947
Lightvalve	67:c2812cf26c38	948
Lightvalve	67:c2812cf26c38	949
Lightvalve	67:c2812cf26c38	950	} else {
Lightvalve	67:c2812cf26c38	951	float VALVE_POS_RAW_FORCE_FB = 0.0f;
Lightvalve	67:c2812cf26c38	952	VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
Lightvalve	67:c2812cf26c38	953
Lightvalve	67:c2812cf26c38	954	if (VALVE_POS_RAW_FORCE_FB >= 0) {
Lightvalve	67:c2812cf26c38	955	valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
Lightvalve	67:c2812cf26c38	956	} else {
Lightvalve	67:c2812cf26c38	957	valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
Lightvalve	67:c2812cf26c38	958	}
Lightvalve	67:c2812cf26c38	959
Lightvalve	67:c2812cf26c38	960	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	67:c2812cf26c38	961
Lightvalve	67:c2812cf26c38	962	V_out = (float) Vout.ref;
Lightvalve	67:c2812cf26c38	963
Lightvalve	67:c2812cf26c38	964	}
Lightvalve	67:c2812cf26c38	965
Lightvalve	67:c2812cf26c38	966
Lightvalve	67:c2812cf26c38	967
Lightvalve	67:c2812cf26c38	968
Lightvalve	59:f308b1656d9c	969	// pos.err = pos.ref - (float)pos.sen;
Lightvalve	59:f308b1656d9c	970	// float VALVE_POS_RAW_POS_FB = 0.0f;
Lightvalve	59:f308b1656d9c	971	// VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
Lightvalve	59:f308b1656d9c	972	// valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
Lightvalve	59:f308b1656d9c	973	// VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	29:69f3f5445d6d	974
Lightvalve	29:69f3f5445d6d	975	cnt_findhome++;
Lightvalve	29:69f3f5445d6d	976	if (cnt_findhome >= T_move) {
Lightvalve	29:69f3f5445d6d	977	//REFERENCE_MODE = MODE_REF_DIRECT;
Lightvalve	29:69f3f5445d6d	978	cnt_findhome = 0;
Lightvalve	30:8d561f16383b	979	pos.ref = 0.0f;
Lightvalve	30:8d561f16383b	980	vel.ref = 0.0f;
Lightvalve	30:8d561f16383b	981	pos.ref_home_pos = 0.0f;
Lightvalve	30:8d561f16383b	982	vel.ref_home_pos = 0.0f;
Lightvalve	29:69f3f5445d6d	983	FINDHOME_STAGE = FINDHOME_INIT;
Lightvalve	57:f4819de54e7a	984	CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
Lightvalve	29:69f3f5445d6d	985	}
Lightvalve	13:747daba9cf59	986	}
Lightvalve	19:23b7c1ad8683	987
Lightvalve	13:747daba9cf59	988	break;
Lightvalve	13:747daba9cf59	989	}
Lightvalve	14:8e7590227d22	990
Lightvalve	50:3c630b5eba9f	991	// case MODE_VALVE_GAIN_SETTING: {
Lightvalve	50:3c630b5eba9f	992	// if (TMR3_COUNT_FLOWRATE == 0) {
Lightvalve	50:3c630b5eba9f	993	// if (pos_plus_end == pos_minus_end) need_enc_init = true;
Lightvalve	50:3c630b5eba9f	994	// else {
Lightvalve	50:3c630b5eba9f	995	// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	996	// temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
Lightvalve	50:3c630b5eba9f	997	// }
Lightvalve	50:3c630b5eba9f	998	// }
Lightvalve	50:3c630b5eba9f	999	// if (need_enc_init) {
Lightvalve	50:3c630b5eba9f	1000	// if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
Lightvalve	50:3c630b5eba9f	1001	// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	1002	// pos_plus_end = pos.sen;
Lightvalve	50:3c630b5eba9f	1003	// } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
Lightvalve	50:3c630b5eba9f	1004	// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	1005	// pos_minus_end = pos.sen;
Lightvalve	50:3c630b5eba9f	1006	// } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
Lightvalve	50:3c630b5eba9f	1007	// need_enc_init = false;
Lightvalve	50:3c630b5eba9f	1008	// check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end));
Lightvalve	50:3c630b5eba9f	1009	// check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end));
Lightvalve	50:3c630b5eba9f	1010	// check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
Lightvalve	50:3c630b5eba9f	1011	// }
Lightvalve	50:3c630b5eba9f	1012	// temp_time = TMR_FREQ_5k;
Lightvalve	50:3c630b5eba9f	1013	// }
Lightvalve	50:3c630b5eba9f	1014	// TMR3_COUNT_FLOWRATE++;
Lightvalve	50:3c630b5eba9f	1015	// if (TMR3_COUNT_FLOWRATE > temp_time) {
Lightvalve	50:3c630b5eba9f	1016	// if (flag_flowrate % 2 == 0) { // (+)
Lightvalve	50:3c630b5eba9f	1017	// VALVE_VOLTAGE = 1000.0f * (float) (flag_flowrate / 2 + 1);
Lightvalve	50:3c630b5eba9f	1018	// V_out = VALVE_VOLTAGE;
Lightvalve	50:3c630b5eba9f	1019	// if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
Lightvalve	50:3c630b5eba9f	1020	// fl_temp_cnt++;
Lightvalve	50:3c630b5eba9f	1021	// } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
Lightvalve	50:3c630b5eba9f	1022	// VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6pi/6553610000 0.5757=0.025250.020.0095260*1000
Lightvalve	50:3c630b5eba9f	1023	// // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
Lightvalve	50:3c630b5eba9f	1024	// fl_temp_cnt2++;
Lightvalve	50:3c630b5eba9f	1025	// }
Lightvalve	50:3c630b5eba9f	1026	// } else if (flag_flowrate % 2 == 1) { // (-)
Lightvalve	50:3c630b5eba9f	1027	// VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1);
Lightvalve	50:3c630b5eba9f	1028	// V_out = VALVE_VOLTAGE;
Lightvalve	50:3c630b5eba9f	1029	// if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
Lightvalve	50:3c630b5eba9f	1030	// fl_temp_cnt++;
Lightvalve	50:3c630b5eba9f	1031	// } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
Lightvalve	50:3c630b5eba9f	1032	// VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE);
Lightvalve	50:3c630b5eba9f	1033	// // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
Lightvalve	50:3c630b5eba9f	1034	// fl_temp_cnt2++;
Lightvalve	50:3c630b5eba9f	1035	// }
Lightvalve	50:3c630b5eba9f	1036	// }
Lightvalve	50:3c630b5eba9f	1037	// if (fl_temp_cnt2 == 100) {
Lightvalve	50:3c630b5eba9f	1038	//
Lightvalve	50:3c630b5eba9f	1039	// ROM_RESET_DATA();
Lightvalve	50:3c630b5eba9f	1040	//
Lightvalve	50:3c630b5eba9f	1041	// //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
Lightvalve	50:3c630b5eba9f	1042	// cur_vel_sum = 0;
Lightvalve	50:3c630b5eba9f	1043	// fl_temp_cnt = 0;
Lightvalve	50:3c630b5eba9f	1044	// fl_temp_cnt2 = 0;
Lightvalve	50:3c630b5eba9f	1045	// flag_flowrate++;
Lightvalve	50:3c630b5eba9f	1046	// }
Lightvalve	50:3c630b5eba9f	1047	// if (flag_flowrate == 10) {
Lightvalve	50:3c630b5eba9f	1048	// V_out = 0;
Lightvalve	50:3c630b5eba9f	1049	// flag_flowrate = 0;
Lightvalve	50:3c630b5eba9f	1050	// TMR3_COUNT_FLOWRATE = 0;
Lightvalve	50:3c630b5eba9f	1051	// valve_gain_repeat_cnt++;
Lightvalve	50:3c630b5eba9f	1052	// if (valve_gain_repeat_cnt >= 1) {
Lightvalve	50:3c630b5eba9f	1053	// CONTROL_MODE = MODE_NO_ACT;
Lightvalve	50:3c630b5eba9f	1054	// valve_gain_repeat_cnt = 0;
Lightvalve	50:3c630b5eba9f	1055	// }
Lightvalve	50:3c630b5eba9f	1056	//
Lightvalve	50:3c630b5eba9f	1057	// }
Lightvalve	50:3c630b5eba9f	1058	// break;
Lightvalve	50:3c630b5eba9f	1059	// }
Lightvalve	50:3c630b5eba9f	1060	//
Lightvalve	50:3c630b5eba9f	1061	// }
Lightvalve	14:8e7590227d22	1062	case MODE_PRESSURE_SENSOR_NULLING: {
Lightvalve	13:747daba9cf59	1063	// DAC Voltage reference set
Lightvalve	14:8e7590227d22	1064	if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
Lightvalve	14:8e7590227d22	1065	CUR_PRES_A_sum += pres_A.sen;
Lightvalve	14:8e7590227d22	1066	CUR_PRES_B_sum += pres_B.sen;
Lightvalve	14:8e7590227d22	1067
Lightvalve	14:8e7590227d22	1068	if (TMR3_COUNT_PRES_NULL % 10 == 0) {
Lightvalve	30:8d561f16383b	1069	CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f;
Lightvalve	30:8d561f16383b	1070	CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f;
Lightvalve	13:747daba9cf59	1071	CUR_PRES_A_sum = 0;
Lightvalve	13:747daba9cf59	1072	CUR_PRES_B_sum = 0;
Lightvalve	14:8e7590227d22	1073
Lightvalve	38:118df027d851	1074	float VREF_NullingGain = 0.0003f;
Lightvalve	58:2eade98630e2	1075	PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean;
Lightvalve	58:2eade98630e2	1076	PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean;
Lightvalve	14:8e7590227d22	1077
Lightvalve	30:8d561f16383b	1078	if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
Lightvalve	30:8d561f16383b	1079	if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
Lightvalve	30:8d561f16383b	1080	if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
Lightvalve	30:8d561f16383b	1081	if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
Lightvalve	19:23b7c1ad8683	1082
Lightvalve	30:8d561f16383b	1083	dac_1 = PRES_A_VREF / 3.3f;
Lightvalve	30:8d561f16383b	1084	dac_2 = PRES_B_VREF / 3.3f;
Lightvalve	13:747daba9cf59	1085	}
Lightvalve	13:747daba9cf59	1086	} else {
Lightvalve	57:f4819de54e7a	1087	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	14:8e7590227d22	1088	TMR3_COUNT_PRES_NULL = 0;
Lightvalve	13:747daba9cf59	1089	CUR_PRES_A_sum = 0;
Lightvalve	13:747daba9cf59	1090	CUR_PRES_B_sum = 0;
Lightvalve	13:747daba9cf59	1091	CUR_PRES_A_mean = 0;
Lightvalve	13:747daba9cf59	1092	CUR_PRES_B_mean = 0;
Lightvalve	14:8e7590227d22	1093
Lightvalve	16:903b5a4433b4	1094	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	1095
Lightvalve	30:8d561f16383b	1096	dac_1 = PRES_A_VREF / 3.3f;
Lightvalve	30:8d561f16383b	1097	dac_2 = PRES_B_VREF / 3.3f;
Lightvalve	17:1865016ca2e7	1098	//pc.printf("nulling end");
Lightvalve	13:747daba9cf59	1099	}
Lightvalve	14:8e7590227d22	1100	TMR3_COUNT_PRES_NULL++;
Lightvalve	13:747daba9cf59	1101	break;
Lightvalve	13:747daba9cf59	1102	}
Lightvalve	14:8e7590227d22	1103
Lightvalve	50:3c630b5eba9f	1104	// case MODE_PRESSURE_SENSOR_CALIB: {
Lightvalve	50:3c630b5eba9f	1105	// if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
Lightvalve	50:3c630b5eba9f	1106	// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	1107	// if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
Lightvalve	50:3c630b5eba9f	1108	// CUR_PRES_A_sum += CUR_PRES_A;
Lightvalve	50:3c630b5eba9f	1109	// }
Lightvalve	50:3c630b5eba9f	1110	// } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
Lightvalve	50:3c630b5eba9f	1111	// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	1112	// if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
Lightvalve	50:3c630b5eba9f	1113	// CUR_PRES_B_sum += CUR_PRES_B;
Lightvalve	50:3c630b5eba9f	1114	// }
Lightvalve	50:3c630b5eba9f	1115	// } else {
Lightvalve	50:3c630b5eba9f	1116	// CONTROL_MODE = MODE_NO_ACT;
Lightvalve	50:3c630b5eba9f	1117	// TMR3_COUNT_PRES_CALIB = 0;
Lightvalve	50:3c630b5eba9f	1118	// V_out = 0;
Lightvalve	50:3c630b5eba9f	1119	// PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL;
Lightvalve	50:3c630b5eba9f	1120	// PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
Lightvalve	50:3c630b5eba9f	1121	// PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL;
Lightvalve	50:3c630b5eba9f	1122	// PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
Lightvalve	50:3c630b5eba9f	1123	// CUR_PRES_A_sum = 0;
Lightvalve	50:3c630b5eba9f	1124	// CUR_PRES_B_sum = 0;
Lightvalve	50:3c630b5eba9f	1125	// CUR_PRES_A_mean = 0;
Lightvalve	50:3c630b5eba9f	1126	// CUR_PRES_B_mean = 0;
Lightvalve	50:3c630b5eba9f	1127	//
Lightvalve	50:3c630b5eba9f	1128	// ROM_RESET_DATA();
Lightvalve	50:3c630b5eba9f	1129	//
Lightvalve	50:3c630b5eba9f	1130	// //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
Lightvalve	50:3c630b5eba9f	1131	// //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
Lightvalve	50:3c630b5eba9f	1132	// }
Lightvalve	50:3c630b5eba9f	1133	// TMR3_COUNT_PRES_CALIB++;
Lightvalve	50:3c630b5eba9f	1134	// break;
Lightvalve	50:3c630b5eba9f	1135	// }
Lightvalve	19:23b7c1ad8683	1136
Lightvalve	50:3c630b5eba9f	1137	// case MODE_ROTARY_FRICTION_TUNING: {
Lightvalve	50:3c630b5eba9f	1138	// if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
Lightvalve	50:3c630b5eba9f	1139	// V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
Lightvalve	50:3c630b5eba9f	1140	// if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	1141	// else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	50:3c630b5eba9f	1142	// TMR3_COUNT_ROTARY_FRIC_TUNE++;
Lightvalve	50:3c630b5eba9f	1143	// if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
Lightvalve	50:3c630b5eba9f	1144	// TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
Lightvalve	50:3c630b5eba9f	1145	// V_out = 0.0f;
Lightvalve	50:3c630b5eba9f	1146	// CONTROL_MODE = MODE_NO_ACT;
Lightvalve	50:3c630b5eba9f	1147	// }
Lightvalve	50:3c630b5eba9f	1148	// break;
Lightvalve	50:3c630b5eba9f	1149	// }
Lightvalve	14:8e7590227d22	1150
Lightvalve	14:8e7590227d22	1151	case MODE_DDV_POS_VS_PWM_ID: {
Lightvalve	59:f308b1656d9c	1152	CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
Lightvalve	13:747daba9cf59	1153	VALVE_ID_timer = VALVE_ID_timer + 1;
Lightvalve	14:8e7590227d22	1154
Lightvalve	14:8e7590227d22	1155	if(VALVE_ID_timer < TMR_FREQ_5k*1) {
Lightvalve	60:64181f1d3e60	1156	Vout.ref = 3000.0f * sin(2.0f3.14fVALVE_ID_timer/TMR_FREQ_5k * 100.0f);
Lightvalve	14:8e7590227d22	1157	} else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
Lightvalve	59:f308b1656d9c	1158	Vout.ref = 1000.0f*(ID_index_array[ID_index]);
Lightvalve	14:8e7590227d22	1159	} else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
Lightvalve	13:747daba9cf59	1160	VALVE_POS_TMP = 0;
Lightvalve	13:747daba9cf59	1161	data_num = 0;
Lightvalve	14:8e7590227d22	1162	} else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
Lightvalve	13:747daba9cf59	1163	data_num = data_num + 1;
Lightvalve	13:747daba9cf59	1164	VALVE_POS_TMP = VALVE_POS_TMP + value;
Lightvalve	14:8e7590227d22	1165	} else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
Lightvalve	59:f308b1656d9c	1166	Vout.ref = 0.0f;
Lightvalve	59:f308b1656d9c	1167	} else {
Lightvalve	13:747daba9cf59	1168	VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
Lightvalve	13:747daba9cf59	1169	VALVE_ID_timer = 0;
Lightvalve	13:747daba9cf59	1170	ID_index= ID_index +1;
Lightvalve	13:747daba9cf59	1171	}
Lightvalve	14:8e7590227d22	1172
Lightvalve	17:1865016ca2e7	1173	if(ID_index>=25) {
Lightvalve	13:747daba9cf59	1174	int i;
Lightvalve	13:747daba9cf59	1175	VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
Lightvalve	17:1865016ca2e7	1176	for(i=0; i<25; i++) {
Lightvalve	13:747daba9cf59	1177	VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
Lightvalve	14:8e7590227d22	1178	if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
Lightvalve	13:747daba9cf59	1179	VALVE_MAX_POS = VALVE_POS_AVG[i];
Lightvalve	13:747daba9cf59	1180	VALVE_POS_AVG_OLD = VALVE_MAX_POS;
Lightvalve	14:8e7590227d22	1181	} else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) {
Lightvalve	13:747daba9cf59	1182	VALVE_MIN_POS = VALVE_POS_AVG[i];
Lightvalve	13:747daba9cf59	1183	VALVE_POS_AVG_OLD = VALVE_MIN_POS;
Lightvalve	13:747daba9cf59	1184	}
Lightvalve	13:747daba9cf59	1185	}
Lightvalve	59:f308b1656d9c	1186	ROM_RESET_DATA();
Lightvalve	13:747daba9cf59	1187	ID_index = 0;
Lightvalve	57:f4819de54e7a	1188	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1189	}
Lightvalve	14:8e7590227d22	1190
Lightvalve	14:8e7590227d22	1191
Lightvalve	13:747daba9cf59	1192	break;
Lightvalve	13:747daba9cf59	1193	}
Lightvalve	14:8e7590227d22	1194
Lightvalve	14:8e7590227d22	1195	case MODE_DDV_DEADZONE_AND_CENTER: {
Lightvalve	59:f308b1656d9c	1196	CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
Lightvalve	13:747daba9cf59	1197	VALVE_DZ_timer = VALVE_DZ_timer + 1;
Lightvalve	14:8e7590227d22	1198	if(first_check == 0) {
Lightvalve	30:8d561f16383b	1199	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1200	Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	30:8d561f16383b	1201	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1202	Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	14:8e7590227d22	1203	pos_plus_end = pos.sen;
Lightvalve	30:8d561f16383b	1204	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1205	Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	30:8d561f16383b	1206	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1207	Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	14:8e7590227d22	1208	pos_minus_end = pos.sen;
Lightvalve	30:8d561f16383b	1209	} else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1210	Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
Lightvalve	30:8d561f16383b	1211	} else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1212	Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
Lightvalve	13:747daba9cf59	1213	data_num = data_num + 1;
Lightvalve	13:747daba9cf59	1214	VALVE_POS_TMP = VALVE_POS_TMP + value;
Lightvalve	30:8d561f16383b	1215	} else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1216	Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
Lightvalve	13:747daba9cf59	1217	DDV_POS_AVG = VALVE_POS_TMP / data_num;
Lightvalve	14:8e7590227d22	1218	START_POS = pos.sen;
Lightvalve	13:747daba9cf59	1219	VALVE_POS_TMP = 0;
Lightvalve	13:747daba9cf59	1220	data_num = 0;
Lightvalve	14:8e7590227d22	1221
Lightvalve	30:8d561f16383b	1222	} else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1223	valve_pos.ref = DDV_POS_AVG;
Lightvalve	14:8e7590227d22	1224	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	14:8e7590227d22	1225
Lightvalve	30:8d561f16383b	1226	} else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1227	valve_pos.ref = DDV_POS_AVG;
Lightvalve	14:8e7590227d22	1228	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	60:64181f1d3e60	1229
Lightvalve	30:8d561f16383b	1230	} else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1231	valve_pos.ref = DDV_POS_AVG;
Lightvalve	14:8e7590227d22	1232	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	14:8e7590227d22	1233	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1234
Lightvalve	60:64181f1d3e60	1235	if((FINAL_POS - START_POS)>200) {
Lightvalve	13:747daba9cf59	1236	DZ_case = 1;
Lightvalve	60:64181f1d3e60	1237	} else if((FINAL_POS - START_POS)<-200) {
Lightvalve	13:747daba9cf59	1238	DZ_case = -1;
Lightvalve	14:8e7590227d22	1239	} else {
Lightvalve	13:747daba9cf59	1240	DZ_case = 0;
Lightvalve	13:747daba9cf59	1241	}
Lightvalve	61:bc8c8270f0ab	1242
Lightvalve	60:64181f1d3e60	1243	CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
Lightvalve	50:3c630b5eba9f	1244
Lightvalve	13:747daba9cf59	1245	first_check = 1;
Lightvalve	13:747daba9cf59	1246	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1247	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1248	Ref_Valve_Pos_Old = DDV_POS_AVG;
Lightvalve	13:747daba9cf59	1249	DZ_NUM = 1;
Lightvalve	13:747daba9cf59	1250	DZ_index = 1;
Lightvalve	14:8e7590227d22	1251
Lightvalve	13:747daba9cf59	1252	}
Lightvalve	19:23b7c1ad8683	1253	} else {
Lightvalve	14:8e7590227d22	1254	if((DZ_case == -1 && DZ_NUM == 1) \| (DZ_case == 1 && DZ_NUM == 1)) {
Lightvalve	30:8d561f16383b	1255	if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) {
Lightvalve	60:64181f1d3e60	1256	Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
Lightvalve	60:64181f1d3e60	1257	//pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
Lightvalve	60:64181f1d3e60	1258	//CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	30:8d561f16383b	1259	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1260	START_POS = pos.sen;
Lightvalve	30:8d561f16383b	1261	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1262	valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
Lightvalve	14:8e7590227d22	1263	if(valve_pos.ref <= VALVE_MIN_POS) {
Lightvalve	14:8e7590227d22	1264	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	14:8e7590227d22	1265	} else if(valve_pos.ref >= VALVE_MAX_POS) {
Lightvalve	14:8e7590227d22	1266	valve_pos.ref = VALVE_MAX_POS;
Lightvalve	13:747daba9cf59	1267	}
Lightvalve	14:8e7590227d22	1268	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	60:64181f1d3e60	1269
Lightvalve	30:8d561f16383b	1270	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1271	Ref_Valve_Pos_Old = valve_pos.ref;
Lightvalve	14:8e7590227d22	1272	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1273
Lightvalve	14:8e7590227d22	1274	if((FINAL_POS - START_POS)>100) {
Lightvalve	14:8e7590227d22	1275	DZ_DIRECTION = 1 * DZ_case;
Lightvalve	14:8e7590227d22	1276	} else if((FINAL_POS - START_POS)<-100) {
Lightvalve	14:8e7590227d22	1277	DZ_DIRECTION = -1 * DZ_case;
Lightvalve	14:8e7590227d22	1278	} else {
Lightvalve	13:747daba9cf59	1279	DZ_DIRECTION = 1 * DZ_case;
Lightvalve	13:747daba9cf59	1280	}
Lightvalve	14:8e7590227d22	1281
Lightvalve	13:747daba9cf59	1282	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1283	DZ_index= DZ_index *2;
Lightvalve	14:8e7590227d22	1284	if(DZ_index >= 128) {
Lightvalve	14:8e7590227d22	1285	FIRST_DZ = valve_pos.ref;
Lightvalve	13:747daba9cf59	1286	DZ_NUM = 2;
Lightvalve	13:747daba9cf59	1287	Ref_Valve_Pos_Old = FIRST_DZ;
Lightvalve	13:747daba9cf59	1288	DZ_index = 1;
Lightvalve	13:747daba9cf59	1289	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1290	}
Lightvalve	13:747daba9cf59	1291	}
Lightvalve	14:8e7590227d22	1292	} else if((DZ_case == -1 && DZ_NUM == 2) \| (DZ_case == 1 && DZ_NUM == 2)) {
Lightvalve	30:8d561f16383b	1293	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	60:64181f1d3e60	1294	Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
Lightvalve	60:64181f1d3e60	1295	//pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
Lightvalve	60:64181f1d3e60	1296	//CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	30:8d561f16383b	1297	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1298	START_POS = pos.sen;
Lightvalve	30:8d561f16383b	1299	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	60:64181f1d3e60	1300	valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
Lightvalve	14:8e7590227d22	1301	if(valve_pos.ref <= VALVE_MIN_POS) {
Lightvalve	14:8e7590227d22	1302	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	14:8e7590227d22	1303	} else if(valve_pos.ref >= VALVE_MAX_POS) {
Lightvalve	14:8e7590227d22	1304	valve_pos.ref = VALVE_MAX_POS;
Lightvalve	13:747daba9cf59	1305	}
Lightvalve	14:8e7590227d22	1306	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	61:bc8c8270f0ab	1307
Lightvalve	30:8d561f16383b	1308	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1309	Vout.ref = 0.0f;
Lightvalve	59:f308b1656d9c	1310	} else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1311	Ref_Valve_Pos_Old = valve_pos.ref;
Lightvalve	14:8e7590227d22	1312	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1313
Lightvalve	14:8e7590227d22	1314	if((FINAL_POS - START_POS)>100) {
Lightvalve	60:64181f1d3e60	1315	DZ_DIRECTION = 1 * DZ_case;
Lightvalve	14:8e7590227d22	1316	} else if((FINAL_POS - START_POS)<-100) {
Lightvalve	60:64181f1d3e60	1317	DZ_DIRECTION = -1 * DZ_case;
Lightvalve	14:8e7590227d22	1318	} else {
Lightvalve	60:64181f1d3e60	1319	DZ_DIRECTION = -1 * DZ_case;
Lightvalve	13:747daba9cf59	1320	}
Lightvalve	14:8e7590227d22	1321
Lightvalve	13:747daba9cf59	1322	VALVE_DZ_timer = 0;
Lightvalve	60:64181f1d3e60	1323	DZ_index= DZ_index * 2;
Lightvalve	14:8e7590227d22	1324	if(DZ_index >= 128) {
Lightvalve	14:8e7590227d22	1325	SECOND_DZ = valve_pos.ref;
Lightvalve	57:f4819de54e7a	1326	VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
Lightvalve	13:747daba9cf59	1327	first_check = 0;
Lightvalve	33:91b17819ec30	1328	VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
Lightvalve	33:91b17819ec30	1329	VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
Lightvalve	19:23b7c1ad8683	1330
Lightvalve	16:903b5a4433b4	1331	ROM_RESET_DATA();
Lightvalve	34:bb2ca2fc2a8e	1332
Lightvalve	60:64181f1d3e60	1333	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1334	DZ_index = 1;
Lightvalve	13:747daba9cf59	1335	}
Lightvalve	13:747daba9cf59	1336	}
Lightvalve	14:8e7590227d22	1337	} else if(DZ_case == 0 && DZ_NUM ==1) {
Lightvalve	30:8d561f16383b	1338	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	60:64181f1d3e60	1339	Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
Lightvalve	60:64181f1d3e60	1340	//pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
Lightvalve	60:64181f1d3e60	1341	//CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	30:8d561f16383b	1342	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1343	START_POS = pos.sen;
Lightvalve	30:8d561f16383b	1344	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1345	valve_pos.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index;
Lightvalve	14:8e7590227d22	1346	if(valve_pos.ref <= VALVE_MIN_POS) {
Lightvalve	14:8e7590227d22	1347	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	14:8e7590227d22	1348	} else if(valve_pos.ref >= VALVE_MAX_POS) {
Lightvalve	14:8e7590227d22	1349	valve_pos.ref = VALVE_MAX_POS;
Lightvalve	13:747daba9cf59	1350	}
Lightvalve	14:8e7590227d22	1351	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	61:bc8c8270f0ab	1352
Lightvalve	30:8d561f16383b	1353	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1354	Ref_Valve_Pos_Old = valve_pos.ref;
Lightvalve	14:8e7590227d22	1355	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1356
Lightvalve	14:8e7590227d22	1357	if((FINAL_POS - START_POS)>100) {
Lightvalve	14:8e7590227d22	1358	DZ_DIRECTION = 1;
Lightvalve	14:8e7590227d22	1359	} else if((FINAL_POS - START_POS)<-100) {
Lightvalve	14:8e7590227d22	1360	DZ_DIRECTION = -1;
Lightvalve	14:8e7590227d22	1361	} else {
Lightvalve	13:747daba9cf59	1362	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1363	}
Lightvalve	13:747daba9cf59	1364	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1365	DZ_index= DZ_index *2;
Lightvalve	14:8e7590227d22	1366	if(DZ_index >= 128) {
Lightvalve	14:8e7590227d22	1367	FIRST_DZ = valve_pos.ref;
Lightvalve	13:747daba9cf59	1368	DZ_NUM = 2;
Lightvalve	13:747daba9cf59	1369	Ref_Valve_Pos_Old = FIRST_DZ;
Lightvalve	13:747daba9cf59	1370	DZ_index = 1;
Lightvalve	13:747daba9cf59	1371	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1372	}
Lightvalve	13:747daba9cf59	1373	}
Lightvalve	14:8e7590227d22	1374	} else {
Lightvalve	30:8d561f16383b	1375	if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	60:64181f1d3e60	1376	Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
Lightvalve	60:64181f1d3e60	1377	//pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
Lightvalve	60:64181f1d3e60	1378	//CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	30:8d561f16383b	1379	} else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1380	START_POS = pos.sen;
Lightvalve	30:8d561f16383b	1381	} else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1382	valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
Lightvalve	14:8e7590227d22	1383	if(valve_pos.ref <= VALVE_MIN_POS) {
Lightvalve	14:8e7590227d22	1384	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	14:8e7590227d22	1385	} else if(valve_pos.ref > VALVE_MAX_POS) {
Lightvalve	14:8e7590227d22	1386	valve_pos.ref = VALVE_MAX_POS - 1;
Lightvalve	13:747daba9cf59	1387	}
Lightvalve	14:8e7590227d22	1388	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	60:64181f1d3e60	1389
Lightvalve	30:8d561f16383b	1390	} else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1391	Vout.ref = 0.0f;
Lightvalve	59:f308b1656d9c	1392	} else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1393	Ref_Valve_Pos_Old = valve_pos.ref;
Lightvalve	14:8e7590227d22	1394	FINAL_POS = pos.sen;
Lightvalve	61:bc8c8270f0ab	1395
Lightvalve	60:64181f1d3e60	1396	if((FINAL_POS - START_POS)>100) {
Lightvalve	13:747daba9cf59	1397	DZ_DIRECTION = 1;
Lightvalve	60:64181f1d3e60	1398	} else if((FINAL_POS - START_POS)<-100) {
Lightvalve	60:64181f1d3e60	1399	DZ_DIRECTION = -1;
Lightvalve	60:64181f1d3e60	1400	} else {
Lightvalve	13:747daba9cf59	1401	DZ_DIRECTION = 1;
Lightvalve	60:64181f1d3e60	1402	}
Lightvalve	14:8e7590227d22	1403
Lightvalve	13:747daba9cf59	1404	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1405	DZ_index= DZ_index *2;
Lightvalve	14:8e7590227d22	1406	if(DZ_index >= 128) {
Lightvalve	14:8e7590227d22	1407	SECOND_DZ = valve_pos.ref;
Lightvalve	57:f4819de54e7a	1408	VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
Lightvalve	13:747daba9cf59	1409	first_check = 0;
Lightvalve	33:91b17819ec30	1410	VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
Lightvalve	33:91b17819ec30	1411	VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
Lightvalve	61:bc8c8270f0ab	1412
Lightvalve	59:f308b1656d9c	1413	ROM_RESET_DATA();
Lightvalve	34:bb2ca2fc2a8e	1414
Lightvalve	57:f4819de54e7a	1415	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1416	DZ_index = 1;
Lightvalve	13:747daba9cf59	1417	}
Lightvalve	13:747daba9cf59	1418	}
Lightvalve	13:747daba9cf59	1419	}
Lightvalve	14:8e7590227d22	1420	}
Lightvalve	13:747daba9cf59	1421	break;
Lightvalve	13:747daba9cf59	1422	}
Lightvalve	14:8e7590227d22	1423
Lightvalve	14:8e7590227d22	1424	case MODE_DDV_POS_VS_FLOWRATE: {
Lightvalve	59:f308b1656d9c	1425	CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
Lightvalve	13:747daba9cf59	1426	VALVE_FR_timer = VALVE_FR_timer + 1;
Lightvalve	14:8e7590227d22	1427	if(first_check == 0) {
Lightvalve	30:8d561f16383b	1428	if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1429	Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	23:59218d4a256d	1430	//CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
Lightvalve	30:8d561f16383b	1431	} else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1432	Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	14:8e7590227d22	1433	pos_plus_end = pos.sen;
Lightvalve	14:8e7590227d22	1434	// CAN_TX_PRES((int16_t) (V_out), (int16_t) (7));
Lightvalve	30:8d561f16383b	1435	} else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1436	Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	30:8d561f16383b	1437	} else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1438	// CAN_TX_PRES((int16_t) (V_out), (int16_t) (8));
Lightvalve	59:f308b1656d9c	1439	Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
Lightvalve	14:8e7590227d22	1440	pos_minus_end = pos.sen;
Lightvalve	13:747daba9cf59	1441	first_check = 1;
Lightvalve	13:747daba9cf59	1442	VALVE_FR_timer = 0;
Lightvalve	57:f4819de54e7a	1443	valve_pos.ref = (float) VALVE_CENTER;
Lightvalve	13:747daba9cf59	1444	ID_index = 0;
Lightvalve	13:747daba9cf59	1445	max_check = 0;
Lightvalve	13:747daba9cf59	1446	min_check = 0;
Lightvalve	13:747daba9cf59	1447	}
Lightvalve	14:8e7590227d22	1448	} else {
Lightvalve	30:8d561f16383b	1449	if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	59:f308b1656d9c	1450	//V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
Lightvalve	59:f308b1656d9c	1451	pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
Lightvalve	59:f308b1656d9c	1452	CONTROL_MODE = MODE_JOINT_CONTROL;
Lightvalve	30:8d561f16383b	1453	} else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
Lightvalve	13:747daba9cf59	1454	data_num = 0;
Lightvalve	57:f4819de54e7a	1455	valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
Lightvalve	14:8e7590227d22	1456
Lightvalve	14:8e7590227d22	1457	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	14:8e7590227d22	1458	START_POS = pos.sen;
Lightvalve	30:8d561f16383b	1459	} else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
Lightvalve	57:f4819de54e7a	1460	valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
Lightvalve	14:8e7590227d22	1461	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	13:747daba9cf59	1462	data_num = data_num + 1;
Lightvalve	30:8d561f16383b	1463	if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) {
Lightvalve	14:8e7590227d22	1464	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1465	one_period_end = 1;
Lightvalve	13:747daba9cf59	1466	}
Lightvalve	30:8d561f16383b	1467	} else if(VALVE_FR_timer == (int) (5.0f * (float) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1468	FINAL_POS = pos.sen;
Lightvalve	13:747daba9cf59	1469	one_period_end = 1;
Lightvalve	59:f308b1656d9c	1470	V_out = 0.0f;
Lightvalve	13:747daba9cf59	1471	}
Lightvalve	14:8e7590227d22	1472
Lightvalve	14:8e7590227d22	1473	if(one_period_end == 1) {
Lightvalve	14:8e7590227d22	1474	if(valve_pos.ref > VALVE_MAX_POS) {
Lightvalve	13:747daba9cf59	1475	max_check = 1;
Lightvalve	14:8e7590227d22	1476	} else if(valve_pos.ref < VALVE_MIN_POS) {
Lightvalve	13:747daba9cf59	1477	min_check = 1;
Lightvalve	13:747daba9cf59	1478	}
Lightvalve	13:747daba9cf59	1479	JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec
Lightvalve	19:23b7c1ad8683	1480
Lightvalve	13:747daba9cf59	1481	VALVE_FR_timer = 0;
Lightvalve	13:747daba9cf59	1482	one_period_end = 0;
Lightvalve	13:747daba9cf59	1483	ID_index= ID_index +1;
Lightvalve	30:8d561f16383b	1484	V_out = 0.0f;
Lightvalve	13:747daba9cf59	1485	}
Lightvalve	14:8e7590227d22	1486
Lightvalve	14:8e7590227d22	1487	if(max_check == 1 && min_check == 1) {
Lightvalve	19:23b7c1ad8683	1488
Lightvalve	13:747daba9cf59	1489	VALVE_POS_NUM = ID_index;
Lightvalve	59:f308b1656d9c	1490	ROM_RESET_DATA();
Lightvalve	13:747daba9cf59	1491	ID_index = 0;
Lightvalve	13:747daba9cf59	1492	first_check = 0;
Lightvalve	13:747daba9cf59	1493	VALVE_FR_timer = 0;
Lightvalve	57:f4819de54e7a	1494	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	49:e7bcfc244d40	1495	// CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
Lightvalve	13:747daba9cf59	1496	}
Lightvalve	13:747daba9cf59	1497	}
Lightvalve	13:747daba9cf59	1498	break;
Lightvalve	13:747daba9cf59	1499	}
Lightvalve	58:2eade98630e2	1500
Lightvalve	57:f4819de54e7a	1501	case MODE_SYSTEM_ID: {
Lightvalve	57:f4819de54e7a	1502	freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.;
Lightvalve	57:f4819de54e7a	1503	valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3);
Lightvalve	57:f4819de54e7a	1504	CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
Lightvalve	57:f4819de54e7a	1505	cnt_sysid++;
Lightvalve	57:f4819de54e7a	1506	if (freq_sysid_Iref >= 300) {
Lightvalve	57:f4819de54e7a	1507	cnt_sysid = 0;
Lightvalve	57:f4819de54e7a	1508	CONTROL_UTILITY_MODE = MODE_NO_ACT;
Lightvalve	57:f4819de54e7a	1509	}
Lightvalve	57:f4819de54e7a	1510	break;
Lightvalve	57:f4819de54e7a	1511	}
Lightvalve	58:2eade98630e2	1512
Lightvalve	58:2eade98630e2	1513
Lightvalve	57:f4819de54e7a	1514
Lightvalve	57:f4819de54e7a	1515	default:
Lightvalve	57:f4819de54e7a	1516	break;
Lightvalve	57:f4819de54e7a	1517	}
Lightvalve	57:f4819de54e7a	1518
Lightvalve	57:f4819de54e7a	1519	// CONTROL MODE ------------------------------------------------------------
Lightvalve	57:f4819de54e7a	1520
Lightvalve	57:f4819de54e7a	1521	switch (CONTROL_MODE) {
Lightvalve	57:f4819de54e7a	1522	case MODE_NO_ACT: {
Lightvalve	57:f4819de54e7a	1523	V_out = 0.0f;
Lightvalve	57:f4819de54e7a	1524	break;
Lightvalve	57:f4819de54e7a	1525	}
Lightvalve	57:f4819de54e7a	1526
Lightvalve	57:f4819de54e7a	1527	case MODE_VALVE_POSITION_CONTROL: {
Lightvalve	67:c2812cf26c38	1528	if (OPERATING_MODE == 5) { //SW Valve
Lightvalve	57:f4819de54e7a	1529	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	59:f308b1656d9c	1530	V_out = Vout.ref;
Lightvalve	67:c2812cf26c38	1531	} else if (CURRENT_CONTROL_MODE == 0) { //PWM
Lightvalve	67:c2812cf26c38	1532	V_out = valve_pos.ref;
Lightvalve	67:c2812cf26c38	1533	} else {
Lightvalve	67:c2812cf26c38	1534	I_REF = valve_pos.ref * 0.001f;
Lightvalve	57:f4819de54e7a	1535	}
Lightvalve	58:2eade98630e2	1536
Lightvalve	57:f4819de54e7a	1537	break;
Lightvalve	57:f4819de54e7a	1538	}
Lightvalve	57:f4819de54e7a	1539
Lightvalve	57:f4819de54e7a	1540	case MODE_JOINT_CONTROL: {
Lightvalve	57:f4819de54e7a	1541	double torq_ref = 0.0f;
Lightvalve	57:f4819de54e7a	1542	pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
Lightvalve	67:c2812cf26c38	1543	vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
Lightvalve	57:f4819de54e7a	1544	pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
Lightvalve	67:c2812cf26c38	1545
Lightvalve	67:c2812cf26c38	1546	//K & D Low Pass Filter
Lightvalve	67:c2812cf26c38	1547	float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f3.14f30.0f)); // f_cutoff : 30Hz
Lightvalve	67:c2812cf26c38	1548	K_LPF = K_LPF(1.0f-alpha_K_D)+K_SPRING(alpha_K_D);
Lightvalve	67:c2812cf26c38	1549	D_LPF = D_LPF(1.0f-alpha_K_D)+D_DAMPER(alpha_K_D);
Lightvalve	67:c2812cf26c38	1550
Lightvalve	67:c2812cf26c38	1551	torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N]
Lightvalve	57:f4819de54e7a	1552
Lightvalve	57:f4819de54e7a	1553	// torque feedback
Lightvalve	67:c2812cf26c38	1554	torq.err = torq_ref - torq.sen; //[N]
Lightvalve	57:f4819de54e7a	1555	torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
Lightvalve	57:f4819de54e7a	1556
Lightvalve	57:f4819de54e7a	1557	if (((OPERATING_MODE&0b110)>>1) == 0 \|\| ((OPERATING_MODE&0b110)>>1) == 1) {
Lightvalve	58:2eade98630e2	1558
Lightvalve	57:f4819de54e7a	1559	double I_REF_POS = 0.0f;
Lightvalve	57:f4819de54e7a	1560	double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
Lightvalve	57:f4819de54e7a	1561	double I_REF_VC = 0.0f; // I_REF for velocity compensation
Lightvalve	57:f4819de54e7a	1562
Lightvalve	57:f4819de54e7a	1563	double temp_vel_pos = 0.0f;
Lightvalve	57:f4819de54e7a	1564	double temp_vel_torq = 0.0f;
Lightvalve	57:f4819de54e7a	1565	double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
Lightvalve	58:2eade98630e2	1566
Lightvalve	69:3995ffeaa786	1567	if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
Lightvalve	57:f4819de54e7a	1568	temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
Lightvalve	57:f4819de54e7a	1569	// L when P-gain = 100, f_cut = 10Hz L feedforward velocity
Lightvalve	69:3995ffeaa786	1570	} else if ((OPERATING_MODE & 0x01) == 1) {
Lightvalve	57:f4819de54e7a	1571	temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
Lightvalve	57:f4819de54e7a	1572	// L when P-gain = 100, f_cut = 10Hz L feedforward velocity
Lightvalve	57:f4819de54e7a	1573	}
Lightvalve	57:f4819de54e7a	1574	if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
Lightvalve	57:f4819de54e7a	1575	else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
Lightvalve	57:f4819de54e7a	1576
Lightvalve	57:f4819de54e7a	1577	// velocity compensation for torque control
Lightvalve	69:3995ffeaa786	1578	if ((OPERATING_MODE & 0x01) == 0) { // Rotary Mode
Lightvalve	57:f4819de54e7a	1579	I_REF_FORCE_FB = 0.001f * ((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum);
Lightvalve	57:f4819de54e7a	1580	// temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION) * PI / 180.0; // rad/s
Lightvalve	57:f4819de54e7a	1581	temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
Lightvalve	57:f4819de54e7a	1582	// L feedforward velocity
Lightvalve	69:3995ffeaa786	1583	} else if ((OPERATING_MODE & 0x01) == 1) {
Lightvalve	57:f4819de54e7a	1584	I_REF_FORCE_FB = 0.001f * 0.01f((double) P_GAIN_JOINT_TORQUE torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); // Linear Actuators are more sensitive.
Lightvalve	57:f4819de54e7a	1585	// temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION); // mm/s
Lightvalve	57:f4819de54e7a	1586	temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION); // mm/s
Lightvalve	57:f4819de54e7a	1587	// L feedforward velocity
Lightvalve	57:f4819de54e7a	1588	}
Lightvalve	57:f4819de54e7a	1589	if (temp_vel_torq > 0.0f) I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
Lightvalve	57:f4819de54e7a	1590	else I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
Lightvalve	57:f4819de54e7a	1591	// L velocity(rad/s or mm/s) >> I_ref(mA)
Lightvalve	57:f4819de54e7a	1592	// Ref_Joint_FT_dot = (Ref_Joint_FT_Nm - Ref_Joint_FT_Nm_old) / TMR_DT_5k;
Lightvalve	57:f4819de54e7a	1593	// Ref_Joint_FT_Nm_old = Ref_Joint_FT_Nm;
Lightvalve	57:f4819de54e7a	1594
Lightvalve	57:f4819de54e7a	1595	I_REF = (1.0f - alpha_trans) * I_REF_POS + alpha_trans * (I_REF_VC + I_REF_FORCE_FB);
Lightvalve	67:c2812cf26c38	1596
Lightvalve	67:c2812cf26c38	1597	// Anti-windup for FT
Lightvalve	67:c2812cf26c38	1598	if (I_GAIN_JOINT_TORQUE != 0) {
Lightvalve	67:c2812cf26c38	1599	double I_MAX = 10.0f; // Maximum Current : 10mV
Lightvalve	67:c2812cf26c38	1600	double Ka = 2.0f / ((double) I_GAIN_JOINT_TORQUE * 0.001f);
Lightvalve	67:c2812cf26c38	1601	if (I_REF > I_MAX) {
Lightvalve	67:c2812cf26c38	1602	double I_rem = I_REF - I_MAX;
Lightvalve	67:c2812cf26c38	1603	I_rem = Ka*I_rem;
Lightvalve	67:c2812cf26c38	1604	I_REF = I_MAX;
Lightvalve	67:c2812cf26c38	1605	torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
Lightvalve	67:c2812cf26c38	1606	} else if (I_REF < -I_MAX) {
Lightvalve	67:c2812cf26c38	1607	double I_rem = I_REF - (-I_MAX);
Lightvalve	67:c2812cf26c38	1608	I_rem = Ka*I_rem;
Lightvalve	67:c2812cf26c38	1609	I_REF = -I_MAX;
Lightvalve	67:c2812cf26c38	1610	torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
Lightvalve	67:c2812cf26c38	1611	}
Lightvalve	67:c2812cf26c38	1612	}
Lightvalve	57:f4819de54e7a	1613
Lightvalve	57:f4819de54e7a	1614	} else {
Lightvalve	57:f4819de54e7a	1615	float VALVE_POS_RAW_FORCE_FB = 0.0f;
Lightvalve	72:3436ce769b1e	1616	float VALVE_POS_RAW_FORCE_FF = 0.0f;
Lightvalve	72:3436ce769b1e	1617	float VALVE_POS_RAW = 0.0f;
Lightvalve	72:3436ce769b1e	1618
Lightvalve	57:f4819de54e7a	1619	VALVE_POS_RAW_FORCE_FB = alpha_trans(((float) P_GAIN_JOINT_TORQUE torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f
Lightvalve	57:f4819de54e7a	1620	+ DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
Lightvalve	57:f4819de54e7a	1621
Lightvalve	72:3436ce769b1e	1622	VALVE_POS_RAW_FORCE_FF = P_GAIN_JOINT_TORQUE_FF * torq_ref * 0.001f + D_GAIN_JOINT_TORQUE_FF * (torq_ref - torq_ref_past) * 0.0001f;
Lightvalve	72:3436ce769b1e	1623
Lightvalve	72:3436ce769b1e	1624	VALVE_POS_RAW = VALVE_POS_RAW_FORCE_FB + VALVE_POS_RAW_FORCE_FF;
Lightvalve	72:3436ce769b1e	1625
Lightvalve	72:3436ce769b1e	1626
Lightvalve	72:3436ce769b1e	1627	if (VALVE_POS_RAW >= 0) {
Lightvalve	72:3436ce769b1e	1628	valve_pos.ref = VALVE_POS_RAW + VALVE_DEADZONE_PLUS;
Lightvalve	57:f4819de54e7a	1629	} else {
Lightvalve	72:3436ce769b1e	1630	valve_pos.ref = VALVE_POS_RAW + VALVE_DEADZONE_MINUS;
Lightvalve	57:f4819de54e7a	1631	}
Lightvalve	57:f4819de54e7a	1632
Lightvalve	57:f4819de54e7a	1633	if(I_GAIN_JOINT_TORQUE != 0) {
Lightvalve	57:f4819de54e7a	1634	double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f;
Lightvalve	57:f4819de54e7a	1635	if(valve_pos.ref>VALVE_MAX_POS) {
Lightvalve	57:f4819de54e7a	1636	double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS;
Lightvalve	57:f4819de54e7a	1637	valve_pos_rem = valve_pos_rem * Ka;
Lightvalve	57:f4819de54e7a	1638	valve_pos.ref = VALVE_MAX_POS;
Lightvalve	57:f4819de54e7a	1639	torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
Lightvalve	57:f4819de54e7a	1640	} else if(valve_pos.ref < VALVE_MIN_POS) {
Lightvalve	57:f4819de54e7a	1641	double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS;
Lightvalve	57:f4819de54e7a	1642	valve_pos_rem = valve_pos_rem * Ka;
Lightvalve	57:f4819de54e7a	1643	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	57:f4819de54e7a	1644	torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
Lightvalve	57:f4819de54e7a	1645	}
Lightvalve	57:f4819de54e7a	1646	}
Lightvalve	61:bc8c8270f0ab	1647
Lightvalve	57:f4819de54e7a	1648	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	61:bc8c8270f0ab	1649
Lightvalve	67:c2812cf26c38	1650	// Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err);
Lightvalve	60:64181f1d3e60	1651	V_out = (float) Vout.ref;
Lightvalve	57:f4819de54e7a	1652
Lightvalve	57:f4819de54e7a	1653	}
Lightvalve	72:3436ce769b1e	1654
Lightvalve	72:3436ce769b1e	1655	torq_ref_past = torq_ref;
Lightvalve	58:2eade98630e2	1656
Lightvalve	57:f4819de54e7a	1657	break;
Lightvalve	57:f4819de54e7a	1658	}
Lightvalve	58:2eade98630e2	1659
Lightvalve	57:f4819de54e7a	1660	case MODE_VALVE_OPEN_LOOP: {
Lightvalve	57:f4819de54e7a	1661	V_out = (float) Vout.ref;
Lightvalve	57:f4819de54e7a	1662	break;
Lightvalve	57:f4819de54e7a	1663	}
Lightvalve	14:8e7590227d22	1664
Lightvalve	12:6f2531038ea4	1665	default:
Lightvalve	12:6f2531038ea4	1666	break;
Lightvalve	12:6f2531038ea4	1667	}
Lightvalve	14:8e7590227d22	1668
Lightvalve	57:f4819de54e7a	1669
Lightvalve	57:f4819de54e7a	1670	if (((OPERATING_MODE&0b110)>>1) == 0 \|\| ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
Lightvalve	58:2eade98630e2	1671
Lightvalve	57:f4819de54e7a	1672	////////////////////////////////////////////////////////////////////////////
Lightvalve	57:f4819de54e7a	1673	//////////////////////////// CURRENT CONTROL //////////////////////////////
Lightvalve	57:f4819de54e7a	1674	////////////////////////////////////////////////////////////////////////////
Lightvalve	57:f4819de54e7a	1675	if (CURRENT_CONTROL_MODE) {
Lightvalve	67:c2812cf26c38	1676	double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
Lightvalve	57:f4819de54e7a	1677	I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
Lightvalve	57:f4819de54e7a	1678
Lightvalve	57:f4819de54e7a	1679	I_ERR = I_REF_fil - cur.sen;
Lightvalve	67:c2812cf26c38	1680	I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
Lightvalve	57:f4819de54e7a	1681
Lightvalve	57:f4819de54e7a	1682
Lightvalve	57:f4819de54e7a	1683	// Moog Valve Current Control Gain
Lightvalve	67:c2812cf26c38	1684	double R_model = 500.0f; // ohm
Lightvalve	57:f4819de54e7a	1685	double L_model = 1.2f;
Lightvalve	57:f4819de54e7a	1686	double w0 = 2.0f * 3.14f * 150.0f;
Lightvalve	57:f4819de54e7a	1687	double KP_I = 0.1f * L_model*w0;
Lightvalve	57:f4819de54e7a	1688	double KI_I = 0.1f * R_model*w0;
Lightvalve	57:f4819de54e7a	1689
Lightvalve	57:f4819de54e7a	1690	// KNR Valve Current Control Gain
Lightvalve	57:f4819de54e7a	1691	if (((OPERATING_MODE & 0b110)>>1) == 1) { // KNR Valve
Lightvalve	57:f4819de54e7a	1692	R_model = 163.0f; // ohm
Lightvalve	57:f4819de54e7a	1693	L_model = 1.0f;
Lightvalve	57:f4819de54e7a	1694	w0 = 2.0f * 3.14f * 80.0f;
Lightvalve	57:f4819de54e7a	1695	KP_I = 1.0f * L_model*w0;
Lightvalve	57:f4819de54e7a	1696	KI_I = 0.08f * R_model*w0;
Lightvalve	57:f4819de54e7a	1697	}
Lightvalve	57:f4819de54e7a	1698
Lightvalve	57:f4819de54e7a	1699	double FF_gain = 1.0f;
Lightvalve	57:f4819de54e7a	1700
Lightvalve	67:c2812cf26c38	1701	VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
Lightvalve	57:f4819de54e7a	1702	// VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model*I_REF); // Unit : mV
Lightvalve	57:f4819de54e7a	1703	I_REF_fil_diff = I_REF_fil - I_REF_fil_old;
Lightvalve	57:f4819de54e7a	1704	I_REF_fil_old = I_REF_fil;
Lightvalve	67:c2812cf26c38	1705	// VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV
Lightvalve	67:c2812cf26c38	1706	VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil); // Unit : mV
Lightvalve	67:c2812cf26c38	1707	double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
Lightvalve	57:f4819de54e7a	1708
Lightvalve	57:f4819de54e7a	1709	double Ka = 3.0f / KP_I;
Lightvalve	57:f4819de54e7a	1710	if (VALVE_PWM_RAW > V_MAX) {
Lightvalve	57:f4819de54e7a	1711	V_rem = VALVE_PWM_RAW - V_MAX;
Lightvalve	57:f4819de54e7a	1712	V_rem = Ka*V_rem;
Lightvalve	57:f4819de54e7a	1713	VALVE_PWM_RAW = V_MAX;
Lightvalve	67:c2812cf26c38	1714	I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
Lightvalve	57:f4819de54e7a	1715	} else if (VALVE_PWM_RAW < -V_MAX) {
Lightvalve	57:f4819de54e7a	1716	V_rem = VALVE_PWM_RAW - (-V_MAX);
Lightvalve	57:f4819de54e7a	1717	V_rem = Ka*V_rem;
Lightvalve	57:f4819de54e7a	1718	VALVE_PWM_RAW = -V_MAX;
Lightvalve	67:c2812cf26c38	1719	I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
Lightvalve	57:f4819de54e7a	1720	}
Lightvalve	57:f4819de54e7a	1721	Cur_Valve_Open_pulse = cur.sen / mA_PER_pulse;
Lightvalve	57:f4819de54e7a	1722	} else {
Lightvalve	57:f4819de54e7a	1723	VALVE_PWM_RAW = I_REF * mV_PER_mA;
Lightvalve	57:f4819de54e7a	1724	Cur_Valve_Open_pulse = I_REF / mA_PER_pulse;
Lightvalve	57:f4819de54e7a	1725	}
Lightvalve	57:f4819de54e7a	1726
Lightvalve	57:f4819de54e7a	1727	////////////////////////////////////////////////////////////////////////////
Lightvalve	57:f4819de54e7a	1728	///////////////// Dead Zone Cancellation & Linearization //////////////////
Lightvalve	57:f4819de54e7a	1729	////////////////////////////////////////////////////////////////////////////
Lightvalve	57:f4819de54e7a	1730	// Dead Zone Cancellation (Mechanical Valve dead-zone)
Lightvalve	57:f4819de54e7a	1731	if (FLAG_VALVE_DEADZONE) {
Lightvalve	57:f4819de54e7a	1732	if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
Lightvalve	57:f4819de54e7a	1733	else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
Lightvalve	57:f4819de54e7a	1734
Lightvalve	57:f4819de54e7a	1735	VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
Lightvalve	57:f4819de54e7a	1736
Lightvalve	57:f4819de54e7a	1737	} else {
Lightvalve	57:f4819de54e7a	1738	VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
Lightvalve	57:f4819de54e7a	1739	}
Lightvalve	57:f4819de54e7a	1740
Lightvalve	57:f4819de54e7a	1741	// Output Voltage Linearization
Lightvalve	57:f4819de54e7a	1742	double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV
Lightvalve	67:c2812cf26c38	1743	double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000
Lightvalve	57:f4819de54e7a	1744
Lightvalve	57:f4819de54e7a	1745	// Dead Zone Cancellation (Electrical dead-zone)
Lightvalve	67:c2812cf26c38	1746	if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f);
Lightvalve	67:c2812cf26c38	1747	else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
Lightvalve	67:c2812cf26c38	1748	else V_out = (float) (CUR_PWM_lin);
Lightvalve	57:f4819de54e7a	1749	}
Lightvalve	89:a7b45368ea0f	1750	else { //////////////////////////sw valve
Lightvalve	89:a7b45368ea0f	1751	// Output Voltage Linearization
Lightvalve	89:a7b45368ea0f	1752	double CUR_PWM_nonlin = V_out; // Unit : mV
Lightvalve	89:a7b45368ea0f	1753	double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000
Lightvalve	89:a7b45368ea0f	1754
Lightvalve	89:a7b45368ea0f	1755	// Dead Zone Cancellation (Electrical dead-zone)
Lightvalve	89:a7b45368ea0f	1756	if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f);
Lightvalve	89:a7b45368ea0f	1757	else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
Lightvalve	89:a7b45368ea0f	1758	else V_out = (float) (CUR_PWM_lin);
Lightvalve	89:a7b45368ea0f	1759	}
Lightvalve	67:c2812cf26c38	1760
Lightvalve	67:c2812cf26c38	1761	// if(V_out > 0.0f) V_out = (float) (V_out + 169.0f);
Lightvalve	67:c2812cf26c38	1762	// else if(V_out < 0.0f) V_out = (float) (V_out - 174.0f);
Lightvalve	67:c2812cf26c38	1763	// else V_out = V_out;
Lightvalve	67:c2812cf26c38	1764
jobuuu	7:e9086c72bb22	1765	/*******************************************************
jobuuu	7:e9086c72bb22	1766	*** PWM
jobuuu	7:e9086c72bb22	1767	********************************************************/
Lightvalve	67:c2812cf26c38	1768	if(DIR_VALVE<0){
Lightvalve	67:c2812cf26c38	1769	V_out = -V_out;
Lightvalve	67:c2812cf26c38	1770	}
Lightvalve	67:c2812cf26c38	1771
Lightvalve	49:e7bcfc244d40	1772	if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) {
Lightvalve	49:e7bcfc244d40	1773	V_out = VALVE_VOLTAGE_LIMIT*1000.0f;
Lightvalve	49:e7bcfc244d40	1774	} else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
Lightvalve	49:e7bcfc244d40	1775	V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
Lightvalve	27:a2254a485f23	1776	}
Lightvalve	49:e7bcfc244d40	1777	PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // Full duty : 12000.0mV
Lightvalve	13:747daba9cf59	1778
Lightvalve	19:23b7c1ad8683	1779	// Saturation of output voltage to 12.0V
Lightvalve	30:8d561f16383b	1780	if(PWM_out > 1.0f) PWM_out=1.0f;
Lightvalve	30:8d561f16383b	1781	else if (PWM_out < -1.0f) PWM_out=-1.0f;
Lightvalve	13:747daba9cf59	1782
Lightvalve	30:8d561f16383b	1783	if (PWM_out>0.0f) {
Lightvalve	30:8d561f16383b	1784	dtc_v=0.0f;
jobuuu	1:e04e563be5ce	1785	dtc_w=PWM_out;
jobuuu	1:e04e563be5ce	1786	} else {
jobuuu	2:a1c0a37df760	1787	dtc_v=-PWM_out;
Lightvalve	30:8d561f16383b	1788	dtc_w=0.0f;
jobuuu	1:e04e563be5ce	1789	}
Lightvalve	13:747daba9cf59	1790
jobuuu	1:e04e563be5ce	1791	//pwm
Lightvalve	30:8d561f16383b	1792	TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
Lightvalve	30:8d561f16383b	1793	TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
Lightvalve	67:c2812cf26c38	1794
Lightvalve	61:bc8c8270f0ab	1795
Lightvalve	57:f4819de54e7a	1796	if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
Lightvalve	20:806196fda269	1797
Lightvalve	54:647072f5307a	1798	// Position, Velocity, and Torque (ID:1200)
Lightvalve	56:6f50d9d3bfee	1799	if (flag_data_request[0] == HIGH) {
Lightvalve	57:f4819de54e7a	1800	if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
Lightvalve	54:647072f5307a	1801	if (SENSING_MODE == 0) {
Lightvalve	67:c2812cf26c38	1802	CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
Lightvalve	54:647072f5307a	1803	} else if (SENSING_MODE == 1) {
Lightvalve	58:2eade98630e2	1804	CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)5.0f), (int16_t) ((pres_B.sen)5.0f));
Lightvalve	54:647072f5307a	1805	}
Lightvalve	57:f4819de54e7a	1806	} else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
Lightvalve	54:647072f5307a	1807	if (SENSING_MODE == 0) {
Lightvalve	67:c2812cf26c38	1808	CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
Lightvalve	54:647072f5307a	1809	} else if (SENSING_MODE == 1) {
Lightvalve	67:c2812cf26c38	1810	CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)5.0f), (int16_t) ((pres_B.sen)5.0f));
Lightvalve	54:647072f5307a	1811	}
Lightvalve	52:8ea76864368a	1812	}
Lightvalve	52:8ea76864368a	1813	}
Lightvalve	56:6f50d9d3bfee	1814	if (flag_data_request[1] == HIGH) {
Lightvalve	54:647072f5307a	1815	//valve position
Lightvalve	54:647072f5307a	1816	double t_value = 0;
Lightvalve	97:d71c57e3515e	1817	if(valve_pos.ref>=(float) VALVE_CENTER) {
Lightvalve	97:d71c57e3515e	1818	t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
Lightvalve	97:d71c57e3515e	1819	} else {
Lightvalve	97:d71c57e3515e	1820	t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
Lightvalve	97:d71c57e3515e	1821	}
Lightvalve	97:d71c57e3515e	1822	// if(OPERATING_MODE==5) {
Lightvalve	97:d71c57e3515e	1823	// if(valve_pos.ref>=(float) VALVE_CENTER) {
Lightvalve	97:d71c57e3515e	1824	// t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
Lightvalve	97:d71c57e3515e	1825	// } else {
Lightvalve	97:d71c57e3515e	1826	// t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
Lightvalve	97:d71c57e3515e	1827	// }
Lightvalve	97:d71c57e3515e	1828	//t_value = (double) value;
Lightvalve	97:d71c57e3515e	1829	// } else if(CURRENT_CONTROL_MODE==1) {
Lightvalve	97:d71c57e3515e	1830	// t_value = cur.sen * 1000.0f;
Lightvalve	89:a7b45368ea0f	1831	// } else {
Lightvalve	97:d71c57e3515e	1832	// t_value = V_out;
Lightvalve	89:a7b45368ea0f	1833	// }
Lightvalve	67:c2812cf26c38	1834	CAN_TX_TORQUE((int16_t) (t_value)); //1300
Lightvalve	67:c2812cf26c38	1835	//CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300
Lightvalve	67:c2812cf26c38	1836	//CAN_TX_TORQUE((int16_t) (I_REF * 1000.0f)); //1300
Lightvalve	54:647072f5307a	1837	}
Lightvalve	58:2eade98630e2	1838
Lightvalve	58:2eade98630e2	1839
Lightvalve	98:cd1b2da4704f	1840	if (flag_data_request[2] == HIGH) {
Lightvalve	89:a7b45368ea0f	1841	//CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // 1400
Lightvalve	89:a7b45368ea0f	1842	double t_value = 0;
Lightvalve	89:a7b45368ea0f	1843	if(valve_pos.ref>=(float) VALVE_CENTER) {
Lightvalve	89:a7b45368ea0f	1844	t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
Lightvalve	89:a7b45368ea0f	1845	} else {
Lightvalve	89:a7b45368ea0f	1846	t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
Lightvalve	89:a7b45368ea0f	1847	}
Lightvalve	89:a7b45368ea0f	1848	CAN_TX_PRES((int16_t) (t_value), (int16_t) (V_out)); // 1400
Lightvalve	55:b25725257569	1849	}
Lightvalve	58:2eade98630e2	1850
Lightvalve	57:f4819de54e7a	1851	//If it doesn't rest, below can can not work.
Lightvalve	57:f4819de54e7a	1852	for (can_rest = 0; can_rest < 10000; can_rest++) {
Lightvalve	57:f4819de54e7a	1853	;
Lightvalve	57:f4819de54e7a	1854	}
Lightvalve	58:2eade98630e2	1855
Lightvalve	56:6f50d9d3bfee	1856	if (flag_data_request[3] == HIGH) {
Lightvalve	54:647072f5307a	1857	//PWM
Lightvalve	73:f80dc3970c99	1858	//CAN_TX_PWM((int16_t) value); //1500
Lightvalve	73:f80dc3970c99	1859	CAN_TX_PWM((int16_t) input_NN[0] * 100.0f); //1500
Lightvalve	54:647072f5307a	1860	}
Lightvalve	57:f4819de54e7a	1861	//for (i = 0; i < 10000; i++) {
Lightvalve	57:f4819de54e7a	1862	// ;
Lightvalve	57:f4819de54e7a	1863	// }
Lightvalve	56:6f50d9d3bfee	1864	if (flag_data_request[4] == HIGH) {
Lightvalve	54:647072f5307a	1865	//valve position
Lightvalve	73:f80dc3970c99	1866	CAN_TX_VALVE_POSITION((int16_t) (input_NN[1] * 100.0f), (int16_t) (input_NN[2]* 100.0f), (int16_t) (input_NN[3]* 100.0f), (int16_t) (input_NN[4]* 100.0f)); //1600
Lightvalve	54:647072f5307a	1867	}
Lightvalve	20:806196fda269	1868
Lightvalve	54:647072f5307a	1869	// Others : Reference position, Reference FT, PWM, Current (ID:1300)
Lightvalve	52:8ea76864368a	1870	// if (flag_data_request[1] == HIGH) {
Lightvalve	52:8ea76864368a	1871	// CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3));
Lightvalve	52:8ea76864368a	1872	// }
Lightvalve	54:647072f5307a	1873	//if (flag_delay_test == 1){
Lightvalve	58:2eade98630e2	1874	//CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
Lightvalve	54:647072f5307a	1875	//}
Lightvalve	52:8ea76864368a	1876
Lightvalve	54:647072f5307a	1877	TMR2_COUNT_CAN_TX = 0;
Lightvalve	54:647072f5307a	1878	}
Lightvalve	54:647072f5307a	1879	TMR2_COUNT_CAN_TX++;
Lightvalve	52:8ea76864368a	1880
Lightvalve	20:806196fda269	1881	}
Lightvalve	52:8ea76864368a	1882	TIM3->SR = 0x0; // reset the status register
Lightvalve	52:8ea76864368a	1883
Lightvalve	58:2eade98630e2	1884	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 Mar 2021
Imports:	3
Forks:	0
Commits:	228
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@113:4535bc97a1db, 2020-09-12 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning