HydraulicControlBoard_PostLIGHT_210420 - LIGHT2

Users » Lightvalve » Code » HydraulicControlBoard_PostLIGHT_210420

Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_PostLIGHT_210420

LIGHT2

main.cpp@24:ef6e1092e9e6, 2019-09-26 (annotated)

Committer:: Lightvalve
Date:: Thu Sep 26 06:08:32 2019 +0000
Revision:: 24:ef6e1092e9e6
Parent:: 23:59218d4a256d
Child:: 25:3e6b574cab5c

Who changed what in which revision?

User	Revision	Line number	New contents of line
GiJeongKim	0:51c43836c1d7	1	#include "mbed.h"
GiJeongKim	0:51c43836c1d7	2	#include "FastPWM.h"
GiJeongKim	0:51c43836c1d7	3	#include "INIT_HW.h"
jobuuu	2:a1c0a37df760	4	#include "function_CAN.h"
GiJeongKim	0:51c43836c1d7	5	#include "SPI_EEP_ENC.h"
GiJeongKim	0:51c43836c1d7	6	#include "I2C_AS5510.h"
GiJeongKim	0:51c43836c1d7	7	#include "setting.h"
Lightvalve	11:82d8768d7351	8	#include "function_utilities.h"
Lightvalve	16:903b5a4433b4	9	#include "stm32f4xx_flash.h"
Lightvalve	16:903b5a4433b4	10	#include "FlashWriter.h"
GiJeongKim	0:51c43836c1d7	11
jobuuu	7:e9086c72bb22	12	// dac & check ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	13	DigitalOut check(PC_2);
GiJeongKim	0:51c43836c1d7	14	DigitalOut check_2(PC_3);
GiJeongKim	0:51c43836c1d7	15	AnalogOut dac_1(PA_4);
GiJeongKim	0:51c43836c1d7	16	AnalogOut dac_2(PA_5);
Lightvalve	14:8e7590227d22	17	AnalogIn adc1(PC_4); //pressure_1
Lightvalve	14:8e7590227d22	18	AnalogIn adc2(PB_0); //pressure_2
Lightvalve	14:8e7590227d22	19	AnalogIn adc3(PC_1); //current
GiJeongKim	0:51c43836c1d7	20
Lightvalve	24:ef6e1092e9e6	21
jobuuu	7:e9086c72bb22	22	// PWM ///////////////////////////////////////////
jobuuu	2:a1c0a37df760	23	double dtc_v=0.0;
jobuuu	2:a1c0a37df760	24	double dtc_w=0.0;
GiJeongKim	0:51c43836c1d7	25
jobuuu	7:e9086c72bb22	26	// I2C ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	27	I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
GiJeongKim	0:51c43836c1d7	28	const int i2c_slave_addr1 = 0x56;
GiJeongKim	0:51c43836c1d7	29	unsigned int value; // 10bit output of reading sensor AS5510
GiJeongKim	0:51c43836c1d7	30
jobuuu	7:e9086c72bb22	31	// SPI ///////////////////////////////////////////
Lightvalve	16:903b5a4433b4	32	//SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
Lightvalve	16:903b5a4433b4	33	//DigitalOut eeprom_cs(PB_12);
Lightvalve	16:903b5a4433b4	34	//FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
GiJeongKim	0:51c43836c1d7	35	SPI enc(PC_12,PC_11,PC_10);
GiJeongKim	0:51c43836c1d7	36	DigitalOut enc_cs(PD_2);
Lightvalve	11:82d8768d7351	37	DigitalOut LED(PA_15);
GiJeongKim	0:51c43836c1d7	38
jobuuu	7:e9086c72bb22	39	// UART ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	40	Serial pc(PA_9,PA_10); // _ UART
GiJeongKim	0:51c43836c1d7	41
jobuuu	7:e9086c72bb22	42	// CAN ///////////////////////////////////////////
GiJeongKim	0:51c43836c1d7	43	CAN can(PB_8, PB_9, 1000000);
GiJeongKim	0:51c43836c1d7	44	CANMessage msg;
Lightvalve	11:82d8768d7351	45	void onMsgReceived()
Lightvalve	11:82d8768d7351	46	{
Lightvalve	11:82d8768d7351	47	CAN_RX_HANDLER();
Lightvalve	11:82d8768d7351	48	}
jobuuu	2:a1c0a37df760	49
jobuuu	7:e9086c72bb22	50	// Variables ///////////////////////////////////////////
jobuuu	7:e9086c72bb22	51	State pos;
jobuuu	7:e9086c72bb22	52	State vel;
jobuuu	7:e9086c72bb22	53	State Vout;
jobuuu	7:e9086c72bb22	54	State torq;
jobuuu	7:e9086c72bb22	55	State pres_A;
jobuuu	7:e9086c72bb22	56	State pres_B;
jobuuu	7:e9086c72bb22	57	State cur;
Lightvalve	14:8e7590227d22	58	State valve_pos;
Lightvalve	14:8e7590227d22	59
Lightvalve	14:8e7590227d22	60	State INIT_Vout;
Lightvalve	14:8e7590227d22	61	State INIT_Valve_Pos;
Lightvalve	14:8e7590227d22	62	State INIT_Pos;
Lightvalve	14:8e7590227d22	63	State INIT_torq;
jobuuu	5:a4319f79457b	64
jobuuu	5:a4319f79457b	65	double V_out=0.0;
jobuuu	5:a4319f79457b	66	double V_rem=0.0; // for anti-windup
jobuuu	5:a4319f79457b	67	double V_MAX = 12000.0; // Maximum Voltage : 12V = 12000mV
jobuuu	5:a4319f79457b	68
jobuuu	5:a4319f79457b	69	double PWM_out=0.0;
Lightvalve	12:6f2531038ea4	70
Lightvalve	17:1865016ca2e7	71	int timer_while = 0;
Lightvalve	17:1865016ca2e7	72	int while_index = 0;
Lightvalve	17:1865016ca2e7	73
Lightvalve	19:23b7c1ad8683	74
Lightvalve	19:23b7c1ad8683	75	extern int CID_RX_CMD;
Lightvalve	19:23b7c1ad8683	76	extern int CID_RX_REF_POSITION;
Lightvalve	19:23b7c1ad8683	77	extern int CID_RX_REF_TORQUE;
Lightvalve	19:23b7c1ad8683	78	extern int CID_RX_REF_PRES_DIFF;
Lightvalve	19:23b7c1ad8683	79	extern int CID_RX_REF_VOUT;
Lightvalve	19:23b7c1ad8683	80	extern int CID_RX_REF_VALVE_POSITION;
Lightvalve	19:23b7c1ad8683	81	extern int CID_RX_REF_CURRENT;
Lightvalve	19:23b7c1ad8683	82
Lightvalve	19:23b7c1ad8683	83	extern int CID_TX_INFO;
Lightvalve	19:23b7c1ad8683	84	extern int CID_TX_POSITION;
Lightvalve	19:23b7c1ad8683	85	extern int CID_TX_TORQUE;
Lightvalve	19:23b7c1ad8683	86	extern int CID_TX_PRES;
Lightvalve	19:23b7c1ad8683	87	extern int CID_TX_VOUT;
Lightvalve	19:23b7c1ad8683	88	extern int CID_TX_VALVE_POSITION;
Lightvalve	19:23b7c1ad8683	89
jobuuu	7:e9086c72bb22	90	// =============================================================================
jobuuu	7:e9086c72bb22	91	// =============================================================================
jobuuu	7:e9086c72bb22	92	// =============================================================================
jobuuu	7:e9086c72bb22	93
Lightvalve	12:6f2531038ea4	94	/*******************************************************************************
Lightvalve	12:6f2531038ea4	95	* REFERENCE MODE
Lightvalve	12:6f2531038ea4	96	******************************************************************************/
Lightvalve	13:747daba9cf59	97	enum _REFERENCE_MODE {
Lightvalve	13:747daba9cf59	98	MODE_REF_NO_ACT = 0, //0
Lightvalve	12:6f2531038ea4	99	MODE_REF_DIRECT, //1
Lightvalve	12:6f2531038ea4	100	MODE_REF_COS_INC, //2
Lightvalve	12:6f2531038ea4	101	MODE_REF_LINE_INC, //3
Lightvalve	12:6f2531038ea4	102	MODE_REF_SIN_WAVE, //4
Lightvalve	12:6f2531038ea4	103	MODE_REF_SQUARE_WAVE, //5
Lightvalve	12:6f2531038ea4	104	};
Lightvalve	12:6f2531038ea4	105
Lightvalve	12:6f2531038ea4	106	/*******************************************************************************
Lightvalve	12:6f2531038ea4	107	* CONTROL MODE
Lightvalve	12:6f2531038ea4	108	******************************************************************************/
Lightvalve	13:747daba9cf59	109	enum _CONTROL_MODE {
Lightvalve	12:6f2531038ea4	110	//control mode
Lightvalve	12:6f2531038ea4	111	MODE_NO_ACT = 0, //0
Lightvalve	12:6f2531038ea4	112	MODE_VALVE_OPEN_LOOP, //1
Lightvalve	12:6f2531038ea4	113	MODE_VALVE_POSITION_CONTROL, //2
Lightvalve	13:747daba9cf59	114
Lightvalve	12:6f2531038ea4	115	MODE_JOINT_POSITION_TORQUE_CONTROL_PWM, //3
Lightvalve	12:6f2531038ea4	116	MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION, //4
Lightvalve	12:6f2531038ea4	117	MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING, //5
Lightvalve	13:747daba9cf59	118
Lightvalve	12:6f2531038ea4	119	MODE_JOINT_POSITION_PRES_CONTROL_PWM, //6
Lightvalve	12:6f2531038ea4	120	MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION, //7
Lightvalve	12:6f2531038ea4	121	MODE_VALVE_POSITION_PRES_CONTROL_LEARNING, //8
Lightvalve	13:747daba9cf59	122
Lightvalve	12:6f2531038ea4	123	MODE_TEST_CURRENT_CONTROL, //9
Lightvalve	12:6f2531038ea4	124	MODE_TEST_PWM_CONTROL, //10
Lightvalve	21:e5f1a43ea6f9	125
Lightvalve	14:8e7590227d22	126	MODE_CURRENT_CONTROL, //11
Lightvalve	14:8e7590227d22	127	MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12
Lightvalve	14:8e7590227d22	128	MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13
Lightvalve	13:747daba9cf59	129
Lightvalve	12:6f2531038ea4	130	//utility
Lightvalve	12:6f2531038ea4	131	MODE_TORQUE_SENSOR_NULLING = 20, //20
Lightvalve	12:6f2531038ea4	132	MODE_VALVE_NULLING_AND_DEADZONE_SETTING, //21
Lightvalve	12:6f2531038ea4	133	MODE_FIND_HOME, //22
Lightvalve	12:6f2531038ea4	134	MODE_VALVE_GAIN_SETTING, //23
Lightvalve	12:6f2531038ea4	135	MODE_PRESSURE_SENSOR_NULLING, //24
Lightvalve	12:6f2531038ea4	136	MODE_PRESSURE_SENSOR_CALIB, //25
Lightvalve	12:6f2531038ea4	137	MODE_ROTARY_FRICTION_TUNING, //26
Lightvalve	13:747daba9cf59	138
Lightvalve	12:6f2531038ea4	139	MODE_DDV_POS_VS_PWM_ID = 30, //30
Lightvalve	12:6f2531038ea4	140	MODE_DDV_DEADZONE_AND_CENTER, //31
Lightvalve	12:6f2531038ea4	141	MODE_DDV_POS_VS_FLOWRATE, //32
Lightvalve	12:6f2531038ea4	142	};
Lightvalve	12:6f2531038ea4	143
GiJeongKim	0:51c43836c1d7	144	int main()
GiJeongKim	0:51c43836c1d7	145	{
jobuuu	6:df07d3491e3a	146	/*********************************
jobuuu	1:e04e563be5ce	147	*** Initialization
jobuuu	6:df07d3491e3a	148	*********************************/
Lightvalve	19:23b7c1ad8683	149	//LED = 1;
Lightvalve	19:23b7c1ad8683	150	//pc.baud(9600);
Lightvalve	21:e5f1a43ea6f9	151
GiJeongKim	0:51c43836c1d7	152	// i2c init
Lightvalve	8:5d2eebdad025	153	i2c.frequency(400 * 1000); // 0.4 mHz
Lightvalve	8:5d2eebdad025	154	wait_ms(2); // Power Up wait
Lightvalve	8:5d2eebdad025	155	look_for_hardware_i2c(); // Hardware present
Lightvalve	8:5d2eebdad025	156	init_as5510(i2c_slave_addr1);
Lightvalve	11:82d8768d7351	157	make_delay();
jobuuu	2:a1c0a37df760	158
GiJeongKim	0:51c43836c1d7	159	// // spi init
Lightvalve	16:903b5a4433b4	160	//eeprom.format(8,3);
Lightvalve	16:903b5a4433b4	161	//eeprom.frequency(5000000); //5M
GiJeongKim	0:51c43836c1d7	162	enc.format(8,0);
GiJeongKim	0:51c43836c1d7	163	enc.frequency(5000000); //5M
Lightvalve	11:82d8768d7351	164	make_delay();
Lightvalve	21:e5f1a43ea6f9	165
Lightvalve	16:903b5a4433b4	166	//rom
Lightvalve	19:23b7c1ad8683	167	ROM_CALL_DATA();
Lightvalve	16:903b5a4433b4	168	make_delay();
Lightvalve	13:747daba9cf59	169
GiJeongKim	0:51c43836c1d7	170	// ADC init
jobuuu	5:a4319f79457b	171	Init_ADC();
Lightvalve	11:82d8768d7351	172	make_delay();
jobuuu	2:a1c0a37df760	173
GiJeongKim	0:51c43836c1d7	174	// Pwm init
GiJeongKim	0:51c43836c1d7	175	Init_PWM();
GiJeongKim	0:51c43836c1d7	176	TIM4->CR1 ^= TIM_CR1_UDIS;
Lightvalve	11:82d8768d7351	177	make_delay();
Lightvalve	13:747daba9cf59	178
Lightvalve	11:82d8768d7351	179	// TMR3 init
Lightvalve	11:82d8768d7351	180	Init_TMR3();
Lightvalve	11:82d8768d7351	181	TIM3->CR1 ^= TIM_CR1_UDIS;
Lightvalve	11:82d8768d7351	182	make_delay();
Lightvalve	21:e5f1a43ea6f9	183
Lightvalve	20:806196fda269	184	// TMR5 init
Lightvalve	23:59218d4a256d	185	Init_TMR2();
Lightvalve	23:59218d4a256d	186	TIM2->CR1 ^= TIM_CR1_UDIS;
Lightvalve	20:806196fda269	187	make_delay();
Lightvalve	21:e5f1a43ea6f9	188
GiJeongKim	0:51c43836c1d7	189	// CAN
jobuuu	2:a1c0a37df760	190	can.attach(&CAN_RX_HANDLER);
Lightvalve	11:82d8768d7351	191	CAN_ID_INIT();
Lightvalve	11:82d8768d7351	192	make_delay();
Lightvalve	23:59218d4a256d	193
Lightvalve	23:59218d4a256d	194	//Timer priority
Lightvalve	23:59218d4a256d	195	NVIC_SetPriority(TIM3_IRQn, 2);
Lightvalve	23:59218d4a256d	196	NVIC_SetPriority(TIM2_IRQn, 3);
Lightvalve	23:59218d4a256d	197	NVIC_SetPriority(TIM4_IRQn, 4);
Lightvalve	23:59218d4a256d	198
Lightvalve	23:59218d4a256d	199	//can.reset();
Lightvalve	19:23b7c1ad8683	200	can.filter(msg.id, 0xFFFFF000, CANStandard);
Lightvalve	23:59218d4a256d	201
GiJeongKim	0:51c43836c1d7	202	// spi _ enc
GiJeongKim	0:51c43836c1d7	203	spi_enc_set_init();
Lightvalve	11:82d8768d7351	204	make_delay();
Lightvalve	13:747daba9cf59	205
Lightvalve	11:82d8768d7351	206	//DAC init
Lightvalve	16:903b5a4433b4	207	dac_1 = PRES_A_VREF / 3.3;
Lightvalve	16:903b5a4433b4	208	dac_2 = PRES_B_VREF / 3.3;
Lightvalve	11:82d8768d7351	209	make_delay();
Lightvalve	13:747daba9cf59	210
Lightvalve	19:23b7c1ad8683	211	for (int i=0; i<50; i++) {
Lightvalve	11:82d8768d7351	212	if(i%2==0)
Lightvalve	11:82d8768d7351	213	ID_index_array[i] = - i * 0.5;
Lightvalve	11:82d8768d7351	214	else
Lightvalve	11:82d8768d7351	215	ID_index_array[i] = (i+1) * 0.5;
Lightvalve	11:82d8768d7351	216	}
Lightvalve	13:747daba9cf59	217
jobuuu	6:df07d3491e3a	218	/************************************
jobuuu	1:e04e563be5ce	219	*** Program is operating!
jobuuu	6:df07d3491e3a	220	*************************************/
GiJeongKim	0:51c43836c1d7	221	while(1) {
Lightvalve	21:e5f1a43ea6f9	222	if(timer_while==1000) {
Lightvalve	17:1865016ca2e7	223	//i2c
Lightvalve	17:1865016ca2e7	224	read_field(i2c_slave_addr1);
Lightvalve	17:1865016ca2e7	225	if(DIR_VALVE_ENC < 0) value = 1023 - value;
Lightvalve	21:e5f1a43ea6f9	226
Lightvalve	19:23b7c1ad8683	227	// if(LED==1) {
Lightvalve	19:23b7c1ad8683	228	// LED=0;
Lightvalve	19:23b7c1ad8683	229	// } else
Lightvalve	19:23b7c1ad8683	230	// LED = 1;
Lightvalve	17:1865016ca2e7	231	timer_while = 0;
Lightvalve	21:e5f1a43ea6f9	232
Lightvalve	17:1865016ca2e7	233	}
Lightvalve	17:1865016ca2e7	234	timer_while ++;
GiJeongKim	0:51c43836c1d7	235	}
jobuuu	1:e04e563be5ce	236	}
jobuuu	1:e04e563be5ce	237
Lightvalve	14:8e7590227d22	238	int DDV_JOINT_POS_FF(double REF_JOINT_VEL)
Lightvalve	14:8e7590227d22	239	{
Lightvalve	14:8e7590227d22	240
Lightvalve	13:747daba9cf59	241	int i = 0;
Lightvalve	13:747daba9cf59	242	int Ref_Valve_Pos_FF = DDV_CENTER;
Lightvalve	14:8e7590227d22	243	for(i=0; i<VALVE_POS_NUM; i++) {
Lightvalve	14:8e7590227d22	244	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	245	if(i==0) {
Lightvalve	19:23b7c1ad8683	246	Ref_Valve_Pos_FF = ((int) 10.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
Lightvalve	14:8e7590227d22	247	} else {
Lightvalve	19:23b7c1ad8683	248	Ref_Valve_Pos_FF = ((int) 10.0(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])) + DDV_CENTER + 10*ID_index_array[i-1];
Lightvalve	13:747daba9cf59	249	}
Lightvalve	13:747daba9cf59	250	break;
Lightvalve	13:747daba9cf59	251	}
Lightvalve	13:747daba9cf59	252	}
Lightvalve	14:8e7590227d22	253	if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
Lightvalve	13:747daba9cf59	254	Ref_Valve_Pos_FF = VALVE_MAX_POS;
Lightvalve	14:8e7590227d22	255	} else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
Lightvalve	13:747daba9cf59	256	Ref_Valve_Pos_FF = VALVE_MIN_POS;
Lightvalve	13:747daba9cf59	257	}
Lightvalve	19:23b7c1ad8683	258	Ref_Valve_Pos_FF_CAN = Ref_Valve_Pos_FF;
Lightvalve	13:747daba9cf59	259	return Ref_Valve_Pos_FF;
Lightvalve	13:747daba9cf59	260	}
jobuuu	6:df07d3491e3a	261
jobuuu	6:df07d3491e3a	262
Lightvalve	14:8e7590227d22	263	void VALVE_POS_CONTROL(double REF_VALVE_POS)
Lightvalve	14:8e7590227d22	264	{
Lightvalve	13:747daba9cf59	265	int i = 0;
Lightvalve	13:747daba9cf59	266
Lightvalve	13:747daba9cf59	267	valve_pos_err = REF_VALVE_POS - value;
Lightvalve	13:747daba9cf59	268	valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
Lightvalve	13:747daba9cf59	269	valve_pos_err_old = valve_pos_err;
Lightvalve	13:747daba9cf59	270	valve_pos_err_sum += valve_pos_err;
Lightvalve	13:747daba9cf59	271	if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000;
Lightvalve	13:747daba9cf59	272	if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000;
Lightvalve	13:747daba9cf59	273
Lightvalve	13:747daba9cf59	274	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	275
Lightvalve	14:8e7590227d22	276	if(REF_VALVE_POS > VALVE_MAX_POS) {
Lightvalve	13:747daba9cf59	277	REF_VALVE_POS = VALVE_MAX_POS;
Lightvalve	14:8e7590227d22	278	} else if(REF_VALVE_POS < VALVE_MIN_POS) {
Lightvalve	13:747daba9cf59	279	REF_VALVE_POS = VALVE_MIN_POS;
Lightvalve	13:747daba9cf59	280	}
Lightvalve	23:59218d4a256d	281
Lightvalve	23:59218d4a256d	282
Lightvalve	23:59218d4a256d	283	// if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0])
Lightvalve	23:59218d4a256d	284	// {
Lightvalve	23:59218d4a256d	285	// if(REF_VALVE_POS <= VALVE_POS_VS_PWM[1]) {
Lightvalve	23:59218d4a256d	286	// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[1] - VALVE_POS_VS_PWM[0]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[0]);
Lightvalve	23:59218d4a256d	287	// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[3]) {
Lightvalve	23:59218d4a256d	288	// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[3] - VALVE_POS_VS_PWM[1]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[1]) + 3000.0* (double) ID_index_array[1];
Lightvalve	23:59218d4a256d	289	// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[5]) {
Lightvalve	23:59218d4a256d	290	// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[5] - VALVE_POS_VS_PWM[3]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[3]) + 3000.0* (double) ID_index_array[3];
Lightvalve	23:59218d4a256d	291	// }else if(REF_VALVE_POS <= VALVE_POS_VS_PWM[7]) {
Lightvalve	23:59218d4a256d	292	// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[7] - VALVE_POS_VS_PWM[5]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[5]) + 3000.0* (double) ID_index_array[5];
Lightvalve	23:59218d4a256d	293	// }else
Lightvalve	23:59218d4a256d	294	// VALVE_PWM_RAW_FF = 12000.0;
Lightvalve	23:59218d4a256d	295	// }
Lightvalve	23:59218d4a256d	296	// else
Lightvalve	23:59218d4a256d	297	// {
Lightvalve	23:59218d4a256d	298	// if(REF_VALVE_POS >= VALVE_POS_VS_PWM[2]) {
Lightvalve	23:59218d4a256d	299	// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[0] - VALVE_POS_VS_PWM[2]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[2]) + 3000.0* (double) ID_index_array[2];
Lightvalve	23:59218d4a256d	300	// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[4]) {
Lightvalve	23:59218d4a256d	301	// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[2] - VALVE_POS_VS_PWM[4]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[4]) + 3000.0* (double) ID_index_array[4];
Lightvalve	23:59218d4a256d	302	// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[6]) {
Lightvalve	23:59218d4a256d	303	// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[4] - VALVE_POS_VS_PWM[6]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[6]) + 3000.0* (double) ID_index_array[6];
Lightvalve	23:59218d4a256d	304	// }else if(REF_VALVE_POS >= VALVE_POS_VS_PWM[8]) {
Lightvalve	23:59218d4a256d	305	// VALVE_PWM_RAW_FF = (double) 3000.0 / (double) (VALVE_POS_VS_PWM[6] - VALVE_POS_VS_PWM[8]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[8]) + 3000.0* (double) ID_index_array[8];
Lightvalve	23:59218d4a256d	306	// }else
Lightvalve	23:59218d4a256d	307	// VALVE_PWM_RAW_FF = -12000.0;
Lightvalve	23:59218d4a256d	308	// }
Lightvalve	14:8e7590227d22	309
Lightvalve	18:b8adf1582ea3	310	for(i=0; i<24; i++) {
Lightvalve	14:8e7590227d22	311	if(REF_VALVE_POS >= min(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1]) && REF_VALVE_POS <= max(VALVE_POS_VS_PWM[i],VALVE_POS_VS_PWM[i+1])) {
Lightvalve	14:8e7590227d22	312	if(i==0) {
Lightvalve	18:b8adf1582ea3	313	VALVE_PWM_RAW_FF = (double) 1000.0 / (double) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[i]);
Lightvalve	14:8e7590227d22	314	} else {
Lightvalve	18:b8adf1582ea3	315	VALVE_PWM_RAW_FF = (double) 1000.0* (double) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((double) REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000.0* (double) ID_index_array[i-1];
Lightvalve	13:747daba9cf59	316	}
Lightvalve	13:747daba9cf59	317	break;
Lightvalve	13:747daba9cf59	318	}
Lightvalve	13:747daba9cf59	319	}
Lightvalve	19:23b7c1ad8683	320
Lightvalve	14:8e7590227d22	321	V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
Lightvalve	13:747daba9cf59	322	}
Lightvalve	13:747daba9cf59	323
Lightvalve	14:8e7590227d22	324	#define LT_MAX_IDX 57
Lightvalve	14:8e7590227d22	325	double LT_PWM_duty[LT_MAX_IDX] = {-100.0, -80.0, -60.0, -50.0, -40.0, -35.0, -30.0, -25.0, -20.0,
Lightvalve	14:8e7590227d22	326	-19.0, -18.0, -17.0, -16.0, -15.0, -14.0, -13.0, -12.0, -11.0, -10.0,
Lightvalve	14:8e7590227d22	327	-9.0, -8.0, -7.0, -6.0, -5.0, -4.0, -3.0, -2.0, -1.0, 0.0,
Lightvalve	14:8e7590227d22	328	1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0,
Lightvalve	14:8e7590227d22	329	11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0,
Lightvalve	14:8e7590227d22	330	25.0, 30.0, 35.0, 40.0, 50.0, 60.0, 80.0, 100.0
Lightvalve	14:8e7590227d22	331	}; // duty
Lightvalve	14:8e7590227d22	332	double LT_Voltage_Output[LT_MAX_IDX] = {-321.4, -291.3, -261.5, -246.8, -231.7, -223.9, -216.1, -207.9, -198.8,
Lightvalve	14:8e7590227d22	333	-196.9, -195.0, -192.5, -188.8, -184.5, -180.2, -175.9, -171.5, -166.3, -161.0,
Lightvalve	14:8e7590227d22	334	-156.0, -149.5, -139.0, -126.0, -107.0, -87.5, -64.0, -38.5, -9.4, 0.0,
Lightvalve	14:8e7590227d22	335	12.0, 43.5, 69.0, 94.0, 114.0, 132.0, 146.0, 155.5, 162.3, 168.2,
Lightvalve	14:8e7590227d22	336	173.1, 178.2, 182.8, 187.4, 191.8, 196.0, 199.7, 201.9, 203.8, 205.6,
Lightvalve	14:8e7590227d22	337	214.6, 222.5, 230.4, 238.2, 253.3, 268.0, 297.6, 327.7
Lightvalve	14:8e7590227d22	338	}; // mV
Lightvalve	13:747daba9cf59	339
Lightvalve	14:8e7590227d22	340	double PWM_duty_byLT(double Ref_V)
Lightvalve	14:8e7590227d22	341	{
Lightvalve	13:747daba9cf59	342	double PWM_duty = 0.0;
Lightvalve	13:747daba9cf59	343	if(Ref_V<LT_Voltage_Output[0]) {
Lightvalve	13:747daba9cf59	344	PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5+LT_PWM_duty[0];
Lightvalve	13:747daba9cf59	345	} else if (Ref_V>=LT_Voltage_Output[LT_MAX_IDX-1]) {
Lightvalve	13:747daba9cf59	346	PWM_duty = (Ref_V-LT_Voltage_Output[LT_MAX_IDX-1])/1.5+LT_PWM_duty[LT_MAX_IDX-1];
Lightvalve	13:747daba9cf59	347	} else {
Lightvalve	13:747daba9cf59	348	int idx = 0;
Lightvalve	13:747daba9cf59	349	for(idx=0; idx<LT_MAX_IDX-1; idx++) {
Lightvalve	13:747daba9cf59	350	double ini_x = LT_Voltage_Output[idx];
Lightvalve	13:747daba9cf59	351	double fin_x = LT_Voltage_Output[idx+1];
Lightvalve	13:747daba9cf59	352	double ini_y = LT_PWM_duty[idx];
Lightvalve	13:747daba9cf59	353	double fin_y = LT_PWM_duty[idx+1];
Lightvalve	13:747daba9cf59	354	if(Ref_V>=ini_x && Ref_V<fin_x) {
Lightvalve	13:747daba9cf59	355	PWM_duty = (fin_y-ini_y)/(fin_x-ini_x)*(Ref_V-ini_x) + ini_y;
Lightvalve	13:747daba9cf59	356	break;
Lightvalve	13:747daba9cf59	357	}
Lightvalve	13:747daba9cf59	358	}
Lightvalve	13:747daba9cf59	359	}
Lightvalve	14:8e7590227d22	360
Lightvalve	13:747daba9cf59	361	return PWM_duty;
Lightvalve	13:747daba9cf59	362	}
jobuuu	6:df07d3491e3a	363
jobuuu	2:a1c0a37df760	364	/*******************************************************************************
jobuuu	2:a1c0a37df760	365	TIMER INTERRUPT
jobuuu	2:a1c0a37df760	366	*******************************************************************************/
jobuuu	2:a1c0a37df760	367
Lightvalve	19:23b7c1ad8683	368	//unsigned long CNT_TMR4 = 0;
Lightvalve	17:1865016ca2e7	369	double FREQ_TMR4 = (double)FREQ_10k;
Lightvalve	17:1865016ca2e7	370	double DT_TMR4 = (double)DT_10k;
jobuuu	1:e04e563be5ce	371	extern "C" void TIM4_IRQHandler(void)
jobuuu	1:e04e563be5ce	372	{
Lightvalve	23:59218d4a256d	373
Lightvalve	19:23b7c1ad8683	374	if (TIM4->SR & TIM_SR_UIF ) {
Lightvalve	21:e5f1a43ea6f9	375
Lightvalve	21:e5f1a43ea6f9	376	/*******************************************************
Lightvalve	21:e5f1a43ea6f9	377	*** Sensor Read & Data Handling
Lightvalve	21:e5f1a43ea6f9	378	********************************************************/
Lightvalve	13:747daba9cf59	379
Lightvalve	19:23b7c1ad8683	380
Lightvalve	21:e5f1a43ea6f9	381	//Using LoadCell
Lightvalve	15:bd0d12728506	382	// ADC1->CR2 \|= 0x40000000; // adc _ 12bit
Lightvalve	15:bd0d12728506	383	// //while((ADC1->SR & 0b10));
Lightvalve	15:bd0d12728506	384	// double alpha_update_torque = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.03.141000.0));
Lightvalve	15:bd0d12728506	385	// double torque_new = ((double)ADC1->DR - PRES_A_NULL) / TORQUE_SENSOR_PULSE_PER_TORQUE + 1.0;
Lightvalve	15:bd0d12728506	386	// torq.sen = torq.sen(1.0-alpha_update_torque)+torque_new(alpha_update_torque);
Lightvalve	17:1865016ca2e7	387
Lightvalve	19:23b7c1ad8683	388
Lightvalve	17:1865016ca2e7	389
Lightvalve	21:e5f1a43ea6f9	390	//Pressure sensor A
Lightvalve	21:e5f1a43ea6f9	391	ADC1->CR2 \|= 0x40000000; // adc _ 12bit
Lightvalve	21:e5f1a43ea6f9	392	//while((ADC1->SR & 0b10));
Lightvalve	21:e5f1a43ea6f9	393	double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.03.141000.0));
Lightvalve	21:e5f1a43ea6f9	394	double pres_A_new = ((double)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR;
Lightvalve	21:e5f1a43ea6f9	395	pres_A.sen = pres_A.sen(1.0-alpha_update_pres_A)+pres_A_new(alpha_update_pres_A);
Lightvalve	19:23b7c1ad8683	396
Lightvalve	21:e5f1a43ea6f9	397	//Pressure sensor 1B
Lightvalve	21:e5f1a43ea6f9	398	//ADC2->CR2 \|= 0x40000000; // adc _ 12bit
Lightvalve	21:e5f1a43ea6f9	399	//while((ADC2->SR & 0b10));
Lightvalve	21:e5f1a43ea6f9	400	double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.03.141000.0));
Lightvalve	21:e5f1a43ea6f9	401	double pres_B_new = ((double)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR;
Lightvalve	21:e5f1a43ea6f9	402	pres_B.sen = pres_B.sen(1.0-alpha_update_pres_B)+pres_B_new(alpha_update_pres_B);
Lightvalve	21:e5f1a43ea6f9	403	torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
Lightvalve	17:1865016ca2e7	404
Lightvalve	17:1865016ca2e7	405
Lightvalve	17:1865016ca2e7	406
Lightvalve	21:e5f1a43ea6f9	407	//Current
Lightvalve	21:e5f1a43ea6f9	408	//ADC3->CR2 \|= 0x40000000; // adc _ 12bit
Lightvalve	11:82d8768d7351	409	// a1=ADC2->DR;
Lightvalve	21:e5f1a43ea6f9	410	int raw_cur = ADC3->DR;
Lightvalve	21:e5f1a43ea6f9	411	//while((ADC3->SR & 0b10));
Lightvalve	21:e5f1a43ea6f9	412	double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.03.141000.0)); // f_cutoff : 500Hz
Lightvalve	21:e5f1a43ea6f9	413	double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
Lightvalve	21:e5f1a43ea6f9	414	cur.sen=cur.sen(1.0-alpha_update_cur)+cur_new(alpha_update_cur);
Lightvalve	21:e5f1a43ea6f9	415	cur.sen = raw_cur;
Lightvalve	17:1865016ca2e7	416
Lightvalve	21:e5f1a43ea6f9	417	/*******************************************************
Lightvalve	21:e5f1a43ea6f9	418	*** Timer Counting & etc.
Lightvalve	21:e5f1a43ea6f9	419	********************************************************/
Lightvalve	21:e5f1a43ea6f9	420	//CNT_TMR4++;
Lightvalve	21:e5f1a43ea6f9	421	}
Lightvalve	11:82d8768d7351	422	TIM4->SR = 0x0; // reset the status register
Lightvalve	11:82d8768d7351	423	}
Lightvalve	19:23b7c1ad8683	424
Lightvalve	19:23b7c1ad8683	425
Lightvalve	18:b8adf1582ea3	426	int j =0;
Lightvalve	19:23b7c1ad8683	427	//unsigned long CNT_TMR3 = 0;
Lightvalve	23:59218d4a256d	428	//double FREQ_TMR3 = (double)FREQ_5k;
Lightvalve	23:59218d4a256d	429	double FREQ_TMR3 = (double)FREQ_1k;
Lightvalve	23:59218d4a256d	430	//double DT_TMR3 = (double)DT_5k;
Lightvalve	23:59218d4a256d	431	double DT_TMR3 = (double)DT_1k;
Lightvalve	11:82d8768d7351	432	extern "C" void TIM3_IRQHandler(void)
Lightvalve	21:e5f1a43ea6f9	433	{
Lightvalve	19:23b7c1ad8683	434	if (TIM3->SR & TIM_SR_UIF ) {
Lightvalve	12:6f2531038ea4	435	ENC_UPDATE();
Lightvalve	13:747daba9cf59	436
Lightvalve	19:23b7c1ad8683	437	// CONTROL LOOP ------------------------------------------------------------
Lightvalve	13:747daba9cf59	438
Lightvalve	14:8e7590227d22	439	switch (CONTROL_MODE) {
Lightvalve	14:8e7590227d22	440	case MODE_NO_ACT: {
Lightvalve	14:8e7590227d22	441	V_out = 0;
Lightvalve	13:747daba9cf59	442	break;
Lightvalve	13:747daba9cf59	443	}
Lightvalve	14:8e7590227d22	444
Lightvalve	14:8e7590227d22	445	case MODE_VALVE_OPEN_LOOP: {
Lightvalve	18:b8adf1582ea3	446	V_out = (double) Vout.ref;
Lightvalve	14:8e7590227d22	447	break;
Lightvalve	14:8e7590227d22	448	}
Lightvalve	14:8e7590227d22	449
Lightvalve	14:8e7590227d22	450	case MODE_VALVE_POSITION_CONTROL: {
Lightvalve	14:8e7590227d22	451	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	14:8e7590227d22	452	break;
Lightvalve	14:8e7590227d22	453	}
Lightvalve	14:8e7590227d22	454
Lightvalve	14:8e7590227d22	455	case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: {
Lightvalve	13:747daba9cf59	456	double PWM_RAW_POS_FB = 0.0; // PWM by Position Feedback
Lightvalve	13:747daba9cf59	457	double PWM_RAW_POS_FF = 0.0; // PWM by Position Feedforward
Lightvalve	13:747daba9cf59	458	double PWM_RAW_FORCE_FB = 0.0; // PWM by Force Feedback
Lightvalve	14:8e7590227d22	459
Lightvalve	13:747daba9cf59	460	// feedback input for position control
Lightvalve	14:8e7590227d22	461	pos.err = pos.ref - (double) pos.sen;
Lightvalve	14:8e7590227d22	462	pos.err_diff = pos.err - pos.err_old;
Lightvalve	14:8e7590227d22	463	pos.err_old = pos.err;
Lightvalve	14:8e7590227d22	464	pos.err_sum += pos.err;
Lightvalve	14:8e7590227d22	465	if (pos.err_sum > 1000) pos.err_sum = 1000;
Lightvalve	14:8e7590227d22	466	if (pos.err_sum<-1000) pos.err_sum = -1000;
Lightvalve	14:8e7590227d22	467	// PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff;
Lightvalve	14:8e7590227d22	468	PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * pos.err;
Lightvalve	13:747daba9cf59	469	PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01;
Lightvalve	14:8e7590227d22	470
Lightvalve	13:747daba9cf59	471	// feedforward input for position control
Lightvalve	14:8e7590227d22	472	double Ref_Vel_Act = vel.ref/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
Lightvalve	13:747daba9cf59	473	double K_ff = 0.9;
Lightvalve	14:8e7590227d22	474	if(Ref_Vel_Act > 0) K_ff = 0.90; // open
Lightvalve	14:8e7590227d22	475	if(Ref_Vel_Act > 0) K_ff = 0.75; // close
Lightvalve	14:8e7590227d22	476	PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50;
Lightvalve	14:8e7590227d22	477
Lightvalve	13:747daba9cf59	478	// torque feedback
Lightvalve	15:bd0d12728506	479	// torq.err = torq.ref - torq.sen;
Lightvalve	14:8e7590227d22	480	// torq.err_diff = torq.err - torq.err_old;
Lightvalve	14:8e7590227d22	481	// torq.err_old = torq.err;
Lightvalve	14:8e7590227d22	482	// torq.err_sum += torq.err;
Lightvalve	14:8e7590227d22	483	// if (torq.err_sum > 1000) torq.err_sum = 1000;
Lightvalve	14:8e7590227d22	484	// if (torq.err_sum<-1000) torq.err_sum = -1000;
Lightvalve	14:8e7590227d22	485	// VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
Lightvalve	13:747daba9cf59	486	// VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
Lightvalve	19:23b7c1ad8683	487
Lightvalve	13:747daba9cf59	488	PWM_RAW_FORCE_FB = 0.0;
Lightvalve	14:8e7590227d22	489
Lightvalve	14:8e7590227d22	490	V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
Lightvalve	14:8e7590227d22	491
Lightvalve	13:747daba9cf59	492	break;
Lightvalve	13:747daba9cf59	493	}
Lightvalve	14:8e7590227d22	494
Lightvalve	14:8e7590227d22	495	case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: {
Lightvalve	23:59218d4a256d	496
Lightvalve	23:59218d4a256d	497
Lightvalve	13:747daba9cf59	498	double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback
Lightvalve	19:23b7c1ad8683	499	//double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward
Lightvalve	13:747daba9cf59	500	double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback
Lightvalve	19:23b7c1ad8683	501	//int DDV_JOINT_CAN = 0;
Lightvalve	13:747daba9cf59	502	// feedback input for position control
Lightvalve	14:8e7590227d22	503	pos.err = pos.ref - (double) pos.sen;
Lightvalve	14:8e7590227d22	504	pos.err_diff = pos.err - pos.err_old;
Lightvalve	14:8e7590227d22	505	pos.err_old = pos.err;
Lightvalve	14:8e7590227d22	506	pos.err_sum += pos.err;
Lightvalve	14:8e7590227d22	507	if (pos.err_sum > 1000) pos.err_sum = 1000;
Lightvalve	14:8e7590227d22	508	if (pos.err_sum<-1000) pos.err_sum = -1000;
Lightvalve	14:8e7590227d22	509	VALVE_POS_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * 0.01 * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff;
Lightvalve	13:747daba9cf59	510	VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01;
Lightvalve	14:8e7590227d22	511
Lightvalve	14:8e7590227d22	512
Lightvalve	13:747daba9cf59	513	//Ref_Joint_Vel = Ref_Vel_Test;
Lightvalve	13:747daba9cf59	514	// feedforward input for position control
Lightvalve	14:8e7590227d22	515	// double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
Lightvalve	14:8e7590227d22	516	// double K_ff = 0.9;
Lightvalve	14:8e7590227d22	517	// if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open
Lightvalve	14:8e7590227d22	518	// if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close
Lightvalve	14:8e7590227d22	519	// VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
Lightvalve	14:8e7590227d22	520
Lightvalve	13:747daba9cf59	521	// torque feedback
Lightvalve	15:bd0d12728506	522	// torq.err = torq.ref - torq.sen;
Lightvalve	14:8e7590227d22	523	// torq.err_diff = torq.err - torq.err_old;
Lightvalve	14:8e7590227d22	524	// torq.err_old = torq.err;
Lightvalve	14:8e7590227d22	525	// torq.err_sum += torq.err;
Lightvalve	14:8e7590227d22	526	// if (torq.err_sum > 1000) torq.err_sum = 1000;
Lightvalve	14:8e7590227d22	527	// if (torq.err_sum<-1000) torq.err_sum = -1000;
Lightvalve	14:8e7590227d22	528	// VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
Lightvalve	13:747daba9cf59	529	// VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
Lightvalve	13:747daba9cf59	530	VALVE_POS_RAW_FORCE_FB = 0.0;
Lightvalve	14:8e7590227d22	531
Lightvalve	23:59218d4a256d	532	valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
Lightvalve	23:59218d4a256d	533	//valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
Lightvalve	14:8e7590227d22	534	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	23:59218d4a256d	535
Lightvalve	23:59218d4a256d	536
Lightvalve	13:747daba9cf59	537	break;
Lightvalve	13:747daba9cf59	538	}
Lightvalve	14:8e7590227d22	539
Lightvalve	14:8e7590227d22	540	case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: {
Lightvalve	14:8e7590227d22	541
Lightvalve	13:747daba9cf59	542	break;
Lightvalve	13:747daba9cf59	543	}
Lightvalve	14:8e7590227d22	544
Lightvalve	14:8e7590227d22	545	case MODE_JOINT_POSITION_PRES_CONTROL_PWM: {
Lightvalve	14:8e7590227d22	546	pos.err = pos.ref - (double) pos.sen;
Lightvalve	14:8e7590227d22	547	pos.err_diff = pos.err - pos.err_old;
Lightvalve	14:8e7590227d22	548	pos.err_old = pos.err;
Lightvalve	14:8e7590227d22	549	pos.err_sum += pos.err;
Lightvalve	14:8e7590227d22	550	if (pos.err_sum > 1000) pos.err_sum = 1000;
Lightvalve	14:8e7590227d22	551	if (pos.err_sum<-1000) pos.err_sum = -1000;
Lightvalve	14:8e7590227d22	552	VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01;
Lightvalve	14:8e7590227d22	553
Lightvalve	14:8e7590227d22	554	torq.err = torq.ref - torq.sen;
Lightvalve	14:8e7590227d22	555	torq.err_diff = torq.err - torq.err_old;
Lightvalve	14:8e7590227d22	556	torq.err_old = torq.err;
Lightvalve	14:8e7590227d22	557	torq.err_sum += torq.err;
Lightvalve	14:8e7590227d22	558	if (torq.err_sum > 1000) torq.err_sum = 1000;
Lightvalve	14:8e7590227d22	559	if (torq.err_sum<-1000) torq.err_sum = -1000;
Lightvalve	14:8e7590227d22	560	VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
Lightvalve	14:8e7590227d22	561
Lightvalve	13:747daba9cf59	562	VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
Lightvalve	14:8e7590227d22	563
Lightvalve	14:8e7590227d22	564	V_out = VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
Lightvalve	14:8e7590227d22	565
Lightvalve	13:747daba9cf59	566	CUR_FLOWRATE = (double) CUR_VELOCITY * 0.00009587;
Lightvalve	13:747daba9cf59	567	CUR_FLOWRATE = CUR_FLOWRATE * 0.5757; // 0.4791=2pi/655365000(pulse/tic to rad/s) 0.5757=0.025250.020.00952601000 (radius area * 2 * 60(sec --> min) * 1000(m^3 --> L))
Lightvalve	13:747daba9cf59	568	if (DIR_VALVE > 0) {
Lightvalve	13:747daba9cf59	569	if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]1.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0) / (VALVE_GAIN_LPM_PER_V[0]1.0 - 0.0) + 0.0;
Lightvalve	13:747daba9cf59	570	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0]1.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]2.)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]1.0) / (VALVE_GAIN_LPM_PER_V[2]2.0 - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0;
Lightvalve	13:747daba9cf59	571	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2]2.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]3.)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]2.0) / (VALVE_GAIN_LPM_PER_V[4]3.0 - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0;
Lightvalve	13:747daba9cf59	572	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4]3.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]4.)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]3.0) / (VALVE_GAIN_LPM_PER_V[6]4.0 - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0;
Lightvalve	13:747daba9cf59	573	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6]4.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]5.)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]4.0) / (VALVE_GAIN_LPM_PER_V[8]5.0 - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0;
Lightvalve	13:747daba9cf59	574	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8]*5.0)) VALVE_FF_VOLTAGE = 5.0;
Lightvalve	13:747daba9cf59	575	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1](-1.0)) && CUR_FLOWRATE < 0.0) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1](-1.0)) / (0.0 - VALVE_GAIN_LPM_PER_V[1]*(-1.)) - 1.0;
Lightvalve	13:747daba9cf59	576	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3](-2.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1](-1.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3](-2.0)) / ((VALVE_GAIN_LPM_PER_V[1](-1.0)) - VALVE_GAIN_LPM_PER_V[3]*(-2.)) - 2.0;
Lightvalve	13:747daba9cf59	577	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5](-3.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3](-2.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5](-3.0)) / ((VALVE_GAIN_LPM_PER_V[3](-2.0)) - VALVE_GAIN_LPM_PER_V[5]*(-3.)) - 3.0;
Lightvalve	13:747daba9cf59	578	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7](-4.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5](-3.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7](-4.0)) / ((VALVE_GAIN_LPM_PER_V[5](-3.0)) - VALVE_GAIN_LPM_PER_V[7]*(-4.)) - 4.0;
Lightvalve	13:747daba9cf59	579	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9](-5.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7](-4.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9](-5.0)) / ((VALVE_GAIN_LPM_PER_V[7](-4.0)) - VALVE_GAIN_LPM_PER_V[9]*(-5.)) - 5.0;
Lightvalve	13:747daba9cf59	580	else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[9]*(-5.0))) VALVE_FF_VOLTAGE = -5;
Lightvalve	13:747daba9cf59	581	else VALVE_FF_VOLTAGE = 0;
Lightvalve	13:747daba9cf59	582	} else {
Lightvalve	13:747daba9cf59	583	if (CUR_FLOWRATE >= 0 && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[0]1.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - 0.0) / (VALVE_GAIN_LPM_PER_V[0]1.0 - 0.0) + 0.0;
Lightvalve	13:747daba9cf59	584	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[1]1.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[2]2.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[0]1.0) / (VALVE_GAIN_LPM_PER_V[2]2.0 - VALVE_GAIN_LPM_PER_V[0]*1.) + 1.0;
Lightvalve	13:747daba9cf59	585	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[3]2.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[4]3.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[2]2.0) / (VALVE_GAIN_LPM_PER_V[4]3.0 - VALVE_GAIN_LPM_PER_V[2]*2.) + 2.0;
Lightvalve	13:747daba9cf59	586	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[5]3.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[6]4.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[4]3.0) / (VALVE_GAIN_LPM_PER_V[6]4.0 - VALVE_GAIN_LPM_PER_V[4]*3.) + 3.0;
Lightvalve	13:747daba9cf59	587	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[7]4.0) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]5.0)) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[6]4.0) / (VALVE_GAIN_LPM_PER_V[8]5.0 - VALVE_GAIN_LPM_PER_V[6]*4.) + 4.0;
Lightvalve	13:747daba9cf59	588	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[9]*5.0)) VALVE_FF_VOLTAGE = 5.0;
Lightvalve	13:747daba9cf59	589	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[0](-1.0)) && CUR_FLOWRATE < 0.0) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[1](-1.0)) / (0.0 - VALVE_GAIN_LPM_PER_V[1]*(-1.0)) - 1.0;
Lightvalve	13:747daba9cf59	590	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[2](-2.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[1](-1.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[3](-2.0)) / ((VALVE_GAIN_LPM_PER_V[1](-1.0)) - VALVE_GAIN_LPM_PER_V[3]*(-2.0)) - 2.0;
Lightvalve	13:747daba9cf59	591	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[4](-3.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[3](-2.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[5](-3.0)) / ((VALVE_GAIN_LPM_PER_V[3](-2.0)) - VALVE_GAIN_LPM_PER_V[5]*(-3.0)) - 3.0;
Lightvalve	13:747daba9cf59	592	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[6](-4.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[5](-3.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[7](-4.0)) / ((VALVE_GAIN_LPM_PER_V[5](-3.0)) - VALVE_GAIN_LPM_PER_V[7]*(-4.0)) - 4.0;
Lightvalve	13:747daba9cf59	593	else if (CUR_FLOWRATE >= (VALVE_GAIN_LPM_PER_V[8](-5.0)) && CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[7](-4.0))) VALVE_FF_VOLTAGE = (CUR_FLOWRATE - VALVE_GAIN_LPM_PER_V[9](-5.0)) / ((VALVE_GAIN_LPM_PER_V[7](-4.0)) - VALVE_GAIN_LPM_PER_V[9]*(-5.0)) - 5.0;
Lightvalve	13:747daba9cf59	594	else if (CUR_FLOWRATE < (VALVE_GAIN_LPM_PER_V[8]*(-5.0))) VALVE_FF_VOLTAGE = -5;
Lightvalve	13:747daba9cf59	595	else VALVE_FF_VOLTAGE = 0;
Lightvalve	13:747daba9cf59	596	}
Lightvalve	13:747daba9cf59	597	// VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5;
Lightvalve	14:8e7590227d22	598
Lightvalve	13:747daba9cf59	599	if (CUR_FLOWRATE >= 0) VALVE_FF_VOLTAGE = (double) VELOCITY_COMP_GAIN * 0.001 * VALVE_FF_VOLTAGE * sqrt((double) PRES_SUPPLY - CUR_PRES_A_BAR) * 0.0707; // 0.0707 = 1/sqrt(200.))
Lightvalve	13:747daba9cf59	600	else if (CUR_FLOWRATE < 0) VALVE_FF_VOLTAGE = (double) VELOCITY_COMP_GAIN * 0.001 * VALVE_FF_VOLTAGE * sqrt((double) PRES_SUPPLY - CUR_PRES_B_BAR) * 0.0707;
Lightvalve	14:8e7590227d22	601
Lightvalve	18:b8adf1582ea3	602	V_out = V_out + VALVE_FF_VOLTAGE;
Lightvalve	14:8e7590227d22	603	break;
Lightvalve	14:8e7590227d22	604	}
Lightvalve	14:8e7590227d22	605
Lightvalve	14:8e7590227d22	606	case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: {
Lightvalve	14:8e7590227d22	607	pos.err = pos.ref - (double) pos.sen;
Lightvalve	14:8e7590227d22	608	pos.err_diff = pos.err - pos.err_old;
Lightvalve	14:8e7590227d22	609	pos.err_old = pos.err;
Lightvalve	14:8e7590227d22	610	pos.err_sum += pos.err;
Lightvalve	14:8e7590227d22	611	if (pos.err_sum > 1000) pos.err_sum = 1000;
Lightvalve	14:8e7590227d22	612	if (pos.err_sum<-1000) pos.err_sum = -1000;
Lightvalve	14:8e7590227d22	613	VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01;
Lightvalve	14:8e7590227d22	614
Lightvalve	15:bd0d12728506	615	torq.err = torq.ref - torq.sen;
Lightvalve	14:8e7590227d22	616	torq.err_diff = torq.err - torq.err_old;
Lightvalve	14:8e7590227d22	617	torq.err_old = torq.err;
Lightvalve	14:8e7590227d22	618	torq.err_sum += torq.err;
Lightvalve	14:8e7590227d22	619	if (torq.err_sum > 1000) torq.err_sum = 1000;
Lightvalve	14:8e7590227d22	620	if (torq.err_sum<-1000) torq.err_sum = -1000;
Lightvalve	14:8e7590227d22	621	VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
Lightvalve	14:8e7590227d22	622
Lightvalve	14:8e7590227d22	623	valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ;
Lightvalve	14:8e7590227d22	624	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	19:23b7c1ad8683	625
Lightvalve	14:8e7590227d22	626	break;
Lightvalve	14:8e7590227d22	627	}
Lightvalve	14:8e7590227d22	628
Lightvalve	14:8e7590227d22	629	case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: {
Lightvalve	14:8e7590227d22	630
Lightvalve	14:8e7590227d22	631	break;
Lightvalve	14:8e7590227d22	632	}
Lightvalve	14:8e7590227d22	633
Lightvalve	19:23b7c1ad8683	634
Lightvalve	19:23b7c1ad8683	635	case MODE_TEST_CURRENT_CONTROL: {
Lightvalve	14:8e7590227d22	636	if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
Lightvalve	14:8e7590227d22	637	TMR3_COUNT_IREF = 0;
Lightvalve	19:23b7c1ad8683	638	}
Lightvalve	19:23b7c1ad8683	639	TMR3_COUNT_IREF++;
Lightvalve	19:23b7c1ad8683	640
Lightvalve	19:23b7c1ad8683	641	// Set Current Reference
Lightvalve	14:8e7590227d22	642	double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0;
Lightvalve	14:8e7590227d22	643	double I_REF_MID = 0.0;
Lightvalve	14:8e7590227d22	644	if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
Lightvalve	14:8e7590227d22	645	I_REF = I_REF_MID + 1.0;
Lightvalve	14:8e7590227d22	646	} else {
Lightvalve	14:8e7590227d22	647	I_REF = I_REF_MID - 1.0;
Lightvalve	19:23b7c1ad8683	648	}
Lightvalve	14:8e7590227d22	649	// double T = 1.0; // wave period
Lightvalve	14:8e7590227d22	650	// I_REF = (5. * sin(2. * 3.1415 * (double) TMR3_COUNT_IREF / (double)TMR_FREQ_5k/ T));
Lightvalve	14:8e7590227d22	651	// I_REF = (2.0 * sin(2. * 2. * 3.14 * (double) TMR3_COUNT_IREF / 5000.)+(2.0 * sin(2. * 1. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 5. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.))+(2.0 * sin(2. * 10. * 3.14 * (double)TMR3_COUNT_IREF/ 5000.)));
Lightvalve	19:23b7c1ad8683	652
Lightvalve	14:8e7590227d22	653	if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
Lightvalve	23:59218d4a256d	654	//CAN_TX_PRES((int16_t)(I_REF1000.0), (int16_t) (CUR_CURRENT1000.0)); // to check the datas
Lightvalve	14:8e7590227d22	655	}
Lightvalve	14:8e7590227d22	656	break;
Lightvalve	14:8e7590227d22	657	}
Lightvalve	19:23b7c1ad8683	658
Lightvalve	19:23b7c1ad8683	659	case MODE_TEST_PWM_CONTROL: {
Lightvalve	14:8e7590227d22	660	if (TMR3_COUNT_IREF == TMR_FREQ_5k) {
Lightvalve	14:8e7590227d22	661	TMR3_COUNT_IREF = 0;
Lightvalve	19:23b7c1ad8683	662	}
Lightvalve	19:23b7c1ad8683	663	TMR3_COUNT_IREF++;
Lightvalve	19:23b7c1ad8683	664
Lightvalve	14:8e7590227d22	665	// Set PWM reference
Lightvalve	14:8e7590227d22	666	double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0;
Lightvalve	19:23b7c1ad8683	667	//double I_REF_MID = 0.0;
Lightvalve	14:8e7590227d22	668	if (TMR3_COUNT_IREF < TMR3_CNT_MAX) {
Lightvalve	14:8e7590227d22	669	CUR_PWM = 1000;
Lightvalve	14:8e7590227d22	670	} else {
Lightvalve	14:8e7590227d22	671	CUR_PWM = -1000;
Lightvalve	19:23b7c1ad8683	672	}
Lightvalve	19:23b7c1ad8683	673
Lightvalve	14:8e7590227d22	674	if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
Lightvalve	23:59218d4a256d	675	//CAN_TX_PRES((int16_t)(u_CUR[0]1000.0), (int16_t) (CUR_CURRENT1000.0)); // to check the datas
Lightvalve	14:8e7590227d22	676	}
Lightvalve	13:747daba9cf59	677	break;
Lightvalve	13:747daba9cf59	678	}
Lightvalve	19:23b7c1ad8683	679
Lightvalve	19:23b7c1ad8683	680
Lightvalve	14:8e7590227d22	681	case MODE_CURRENT_CONTROL: {
Lightvalve	14:8e7590227d22	682
Lightvalve	14:8e7590227d22	683	cur.ref = cur.ref; // Unit : mA
Lightvalve	14:8e7590227d22	684	CurrentControl();
Lightvalve	14:8e7590227d22	685	break;
Lightvalve	14:8e7590227d22	686	}
Lightvalve	19:23b7c1ad8683	687
Lightvalve	14:8e7590227d22	688	case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: {
Lightvalve	14:8e7590227d22	689	double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
Lightvalve	14:8e7590227d22	690	double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
Lightvalve	14:8e7590227d22	691	double I_REF_FORCE_FB = 0.0; // I_REF by Force Feedback
Lightvalve	14:8e7590227d22	692	double I_REF_FORCE_FF = 0.0; // I_REF by Force Feedforward
Lightvalve	19:23b7c1ad8683	693
Lightvalve	14:8e7590227d22	694	// feedback input for position control
Lightvalve	14:8e7590227d22	695	pos.err = pos.ref - pos.sen;
Lightvalve	14:8e7590227d22	696	double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.03.141550.0)); // f_cutoff : 50Hz
Lightvalve	14:8e7590227d22	697	double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
Lightvalve	14:8e7590227d22	698	pos.err_diff = (1.0-alpha_update_vel)pos.err_diff + alpha_update_velerr_diff;
Lightvalve	14:8e7590227d22	699	pos.err_old = pos.err;
Lightvalve	14:8e7590227d22	700	I_REF_POS_FB = 0.001((double)P_GAIN_JOINT_POSITION pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
Lightvalve	19:23b7c1ad8683	701
Lightvalve	14:8e7590227d22	702	// feedforward input for position control
Lightvalve	14:8e7590227d22	703	double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
Lightvalve	14:8e7590227d22	704	double K_ff = 1.3;
Lightvalve	14:8e7590227d22	705	double K_v = 0.0;
Lightvalve	14:8e7590227d22	706	if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
Lightvalve	14:8e7590227d22	707	if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
Lightvalve	14:8e7590227d22	708	I_REF_POS_FF = K_ffK_vVel_Act_Ref;
Lightvalve	19:23b7c1ad8683	709
Lightvalve	14:8e7590227d22	710	// feedback input for position control
Lightvalve	14:8e7590227d22	711	I_REF_FORCE_FB = 0.0;
Lightvalve	19:23b7c1ad8683	712
Lightvalve	14:8e7590227d22	713	// feedforward input for position control
Lightvalve	14:8e7590227d22	714	I_REF_FORCE_FF = 0.0;
Lightvalve	19:23b7c1ad8683	715
Lightvalve	14:8e7590227d22	716	cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
Lightvalve	19:23b7c1ad8683	717
Lightvalve	14:8e7590227d22	718	CurrentControl();
Lightvalve	19:23b7c1ad8683	719
Lightvalve	13:747daba9cf59	720	break;
Lightvalve	13:747daba9cf59	721	}
Lightvalve	19:23b7c1ad8683	722
Lightvalve	14:8e7590227d22	723	case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: {
Lightvalve	19:23b7c1ad8683	724	//double T_REF = 0.0; // Torque Reference
Lightvalve	14:8e7590227d22	725	double I_REF_FORCE_FB = 0.; // I_REF by Force Feedback
Lightvalve	14:8e7590227d22	726	double I_REF_VC = 0.; // I_REF for velocity compensation
Lightvalve	19:23b7c1ad8683	727
Lightvalve	14:8e7590227d22	728	// feedback input for position control
Lightvalve	19:23b7c1ad8683	729	//double Joint_Pos_Err = 34.0-(double) pos.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
Lightvalve	19:23b7c1ad8683	730	//double Joint_Vel_Err = 0.0-(double) vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
Lightvalve	19:23b7c1ad8683	731	//double K_spring = 0.7;
Lightvalve	19:23b7c1ad8683	732	//double D_damper = 0.02;
Lightvalve	14:8e7590227d22	733	// T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
Lightvalve	19:23b7c1ad8683	734
Lightvalve	14:8e7590227d22	735	// torque feedback
Lightvalve	15:bd0d12728506	736	torq.err = torq.ref - torq.sen;
Lightvalve	14:8e7590227d22	737	// torq.err_diff = torq.err - torq.err_old;
Lightvalve	14:8e7590227d22	738	// torq.err_old = torq.err;
Lightvalve	14:8e7590227d22	739	torq.err_sum = torq.err_sum + torq.err/(double)TMR_FREQ_5k;
Lightvalve	19:23b7c1ad8683	740	I_REF_FORCE_FB = 0.001((double)P_GAIN_JOINT_TORQUE torq.err + (double)I_GAIN_JOINT_TORQUE * torq.err_sum);
Lightvalve	19:23b7c1ad8683	741
Lightvalve	14:8e7590227d22	742	// velocity compensation for torque control
Lightvalve	19:23b7c1ad8683	743	double Joint_Vel_Act = vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s]
Lightvalve	14:8e7590227d22	744	double K_vc = 1.5; // Velocity comp. gain
Lightvalve	19:23b7c1ad8683	745	double K_v = 0.0; // Valve gain
Lightvalve	14:8e7590227d22	746	if(Joint_Vel_Act > 0) K_v = 1.0/100.0; // open, tuning
Lightvalve	14:8e7590227d22	747	if(Joint_Vel_Act < 0) K_v = 1.0/100.0; // close, tuning
Lightvalve	14:8e7590227d22	748	I_REF_VC = K_vcK_vJoint_Vel_Act;
Lightvalve	19:23b7c1ad8683	749
Lightvalve	14:8e7590227d22	750	cur.ref = I_REF_VC + I_REF_FORCE_FB;
Lightvalve	19:23b7c1ad8683	751	// cur.ref = I_REF_FORCE_FB;
Lightvalve	19:23b7c1ad8683	752
Lightvalve	19:23b7c1ad8683	753	double I_MAX = 10.00; // Maximum Current : 10mV
Lightvalve	14:8e7590227d22	754	double Ka = 1.0/I_GAIN_JOINT_TORQUE;
Lightvalve	14:8e7590227d22	755	if(cur.ref > I_MAX) {
Lightvalve	14:8e7590227d22	756	double I_rem = cur.ref-I_MAX;
Lightvalve	14:8e7590227d22	757	I_rem = Ka*I_rem;
Lightvalve	14:8e7590227d22	758	cur.ref = I_MAX;
Lightvalve	14:8e7590227d22	759	torq.err_sum = torq.err_sum - I_rem/(double)TMR_FREQ_5k;
Lightvalve	14:8e7590227d22	760	} else if(cur.ref < -I_MAX) {
Lightvalve	14:8e7590227d22	761	double I_rem = cur.ref-(-I_MAX);
Lightvalve	19:23b7c1ad8683	762	I_rem = Ka*I_rem;
Lightvalve	14:8e7590227d22	763	cur.ref = -I_MAX;
Lightvalve	14:8e7590227d22	764	torq.err_sum = torq.err_sum - I_rem/(double)TMR_FREQ_5k;
Lightvalve	14:8e7590227d22	765	}
Lightvalve	19:23b7c1ad8683	766
Lightvalve	14:8e7590227d22	767	CurrentControl();
Lightvalve	19:23b7c1ad8683	768
Lightvalve	19:23b7c1ad8683	769
Lightvalve	14:8e7590227d22	770	/*
Lightvalve	14:8e7590227d22	771	double I_REF_POS_FB = 0.0; // I_REF by Position Feedback
Lightvalve	14:8e7590227d22	772	double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward
Lightvalve	14:8e7590227d22	773	double I_REF_FORCE_FB = 0.0; // I_REF by Force Feedback
Lightvalve	14:8e7590227d22	774	double I_REF_FORCE_FF = 0.0; // I_REF by Force Feedforward
Lightvalve	19:23b7c1ad8683	775
Lightvalve	14:8e7590227d22	776	// feedback input for position control
Lightvalve	14:8e7590227d22	777	pos.err = pos.ref - pos.sen;
Lightvalve	14:8e7590227d22	778	double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.03.141550.0)); // f_cutoff : 50Hz
Lightvalve	14:8e7590227d22	779	double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k;
Lightvalve	14:8e7590227d22	780	pos.err_diff = (1.0-alpha_update_vel)pos.err_diff + alpha_update_velerr_diff;
Lightvalve	14:8e7590227d22	781	pos.err_old = pos.err;
Lightvalve	14:8e7590227d22	782	I_REF_POS_FB = 0.001((double)P_GAIN_JOINT_POSITION pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1);
Lightvalve	19:23b7c1ad8683	783
Lightvalve	14:8e7590227d22	784	// feedforward input for position control
Lightvalve	14:8e7590227d22	785	double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s]
Lightvalve	14:8e7590227d22	786	double K_ff = 1.3;
Lightvalve	14:8e7590227d22	787	double K_v = 0.0;
Lightvalve	14:8e7590227d22	788	if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA)
Lightvalve	14:8e7590227d22	789	if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA)
Lightvalve	14:8e7590227d22	790	I_REF_POS_FF = K_ffK_vVel_Act_Ref;
Lightvalve	19:23b7c1ad8683	791
Lightvalve	14:8e7590227d22	792	// feedback input for position control
Lightvalve	14:8e7590227d22	793	I_REF_FORCE_FB = 0.0;
Lightvalve	19:23b7c1ad8683	794
Lightvalve	14:8e7590227d22	795	// feedforward input for position control
Lightvalve	14:8e7590227d22	796	I_REF_FORCE_FF = 0.0;
Lightvalve	19:23b7c1ad8683	797
Lightvalve	14:8e7590227d22	798	cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF;
Lightvalve	19:23b7c1ad8683	799
Lightvalve	14:8e7590227d22	800	CurrentControl();
Lightvalve	14:8e7590227d22	801	*/
Lightvalve	19:23b7c1ad8683	802
Lightvalve	13:747daba9cf59	803	break;
Lightvalve	13:747daba9cf59	804	}
Lightvalve	14:8e7590227d22	805
Lightvalve	14:8e7590227d22	806	case MODE_TORQUE_SENSOR_NULLING: {
Lightvalve	13:747daba9cf59	807	// DAC Voltage reference set
Lightvalve	14:8e7590227d22	808	if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
Lightvalve	15:bd0d12728506	809	CUR_TORQUE_sum += torq.sen;
Lightvalve	19:23b7c1ad8683	810
Lightvalve	14:8e7590227d22	811	if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
Lightvalve	13:747daba9cf59	812	CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0;
Lightvalve	13:747daba9cf59	813	CUR_TORQUE_sum = 0;
Lightvalve	14:8e7590227d22	814
Lightvalve	13:747daba9cf59	815	TORQUE_VREF += 0.0001 * (TORQUE_NULL - CUR_TORQUE_mean);
Lightvalve	19:23b7c1ad8683	816
Lightvalve	13:747daba9cf59	817	if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3;
Lightvalve	13:747daba9cf59	818	if (TORQUE_VREF < 0) TORQUE_VREF = 0;
Lightvalve	19:23b7c1ad8683	819
Lightvalve	16:903b5a4433b4	820	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	821
Lightvalve	16:903b5a4433b4	822	//spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
Lightvalve	16:903b5a4433b4	823	dac_1 = TORQUE_VREF / 3.3;
Lightvalve	13:747daba9cf59	824	}
Lightvalve	13:747daba9cf59	825	} else {
Lightvalve	13:747daba9cf59	826	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	14:8e7590227d22	827	TMR3_COUNT_TORQUE_NULL = 0;
Lightvalve	13:747daba9cf59	828	CUR_TORQUE_sum = 0;
Lightvalve	13:747daba9cf59	829	CUR_TORQUE_mean = 0;
Lightvalve	19:23b7c1ad8683	830
Lightvalve	16:903b5a4433b4	831	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	832
Lightvalve	16:903b5a4433b4	833	//spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
Lightvalve	19:23b7c1ad8683	834
Lightvalve	17:1865016ca2e7	835	//pc.printf("%f\n", TORQUE_VREF);
Lightvalve	16:903b5a4433b4	836	dac_1 = TORQUE_VREF / 3.3;
Lightvalve	14:8e7590227d22	837
Lightvalve	13:747daba9cf59	838	}
Lightvalve	14:8e7590227d22	839	TMR3_COUNT_TORQUE_NULL++;
Lightvalve	13:747daba9cf59	840	break;
Lightvalve	19:23b7c1ad8683	841	}
Lightvalve	14:8e7590227d22	842
Lightvalve	14:8e7590227d22	843	case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
Lightvalve	14:8e7590227d22	844	if (TMR3_COUNT_DEADZONE == 0) {
Lightvalve	13:747daba9cf59	845	if (pos_plus_end == pos_minus_end) need_enc_init = true;
Lightvalve	13:747daba9cf59	846	else temp_time = 0;
Lightvalve	13:747daba9cf59	847	}
Lightvalve	13:747daba9cf59	848	if (need_enc_init) {
Lightvalve	14:8e7590227d22	849	if (TMR3_COUNT_DEADZONE < (int) (0.5 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	850	V_out = VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	851	pos_plus_end = pos.sen;
Lightvalve	14:8e7590227d22	852	} else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
Lightvalve	18:b8adf1582ea3	853	V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	854	pos_minus_end = pos.sen;
Lightvalve	14:8e7590227d22	855	} else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
Lightvalve	13:747daba9cf59	856	temp_time = TMR_FREQ_5k;
Lightvalve	13:747daba9cf59	857	}
Lightvalve	14:8e7590227d22	858
Lightvalve	14:8e7590227d22	859	if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	860	V_out = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	13:747daba9cf59	861	VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
Lightvalve	14:8e7590227d22	862
Lightvalve	14:8e7590227d22	863	} else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9 * (double) TMR_FREQ_5k))) {
Lightvalve	18:b8adf1582ea3	864	V_out = 0;
Lightvalve	13:747daba9cf59	865	CUR_VELOCITY_sum += CUR_VELOCITY;
Lightvalve	14:8e7590227d22	866	} else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
Lightvalve	13:747daba9cf59	867	if (CUR_VELOCITY_sum == 0) DZ_dir = 1;
Lightvalve	13:747daba9cf59	868	else if (CUR_VELOCITY_sum > 0) DZ_dir = 1;
Lightvalve	13:747daba9cf59	869	else if (CUR_VELOCITY_sum < 0) DZ_dir = -1;
Lightvalve	13:747daba9cf59	870	else DZ_temp_cnt2 = DZ_end;
Lightvalve	13:747daba9cf59	871	CUR_VELOCITY_sum = 0;
Lightvalve	14:8e7590227d22	872	} else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	873	if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
Lightvalve	14:8e7590227d22	874
Lightvalve	13:747daba9cf59	875	// Position of Dead Zone
Lightvalve	13:747daba9cf59	876	// (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0)
Lightvalve	14:8e7590227d22	877	// \| / \| / \|/
Lightvalve	13:747daba9cf59	878	// \| ______/ ___\|___/ ______/\|
Lightvalve	13:747daba9cf59	879	// \|/ / \| / \|
Lightvalve	13:747daba9cf59	880	// /\| / \| / \|
Lightvalve	13:747daba9cf59	881	// 0V 0V 0V
Lightvalve	14:8e7590227d22	882
Lightvalve	13:747daba9cf59	883	if (DZ_temp_cnt2 < DZ_end) {
Lightvalve	14:8e7590227d22	884	if (TMR3_COUNT_DEADZONE % 20 != 0) {
Lightvalve	13:747daba9cf59	885	CUR_VELOCITY_sum += CUR_VELOCITY;
Lightvalve	13:747daba9cf59	886	} else {
Lightvalve	18:b8adf1582ea3	887	V_out -= DZ_dir;
Lightvalve	13:747daba9cf59	888	if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
Lightvalve	13:747daba9cf59	889	CUR_VELOCITY_sum = 0;
Lightvalve	13:747daba9cf59	890	}
Lightvalve	13:747daba9cf59	891	if (DZ_temp_cnt == 5) {
Lightvalve	18:b8adf1582ea3	892	if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out;
Lightvalve	18:b8adf1582ea3	893	else VALVE_DEADZONE_PLUS = (int16_t) V_out;
Lightvalve	13:747daba9cf59	894	DZ_dir = -DZ_dir;
Lightvalve	13:747daba9cf59	895	DZ_temp_cnt = 0;
Lightvalve	13:747daba9cf59	896	DZ_temp_cnt2++;
Lightvalve	13:747daba9cf59	897	}
Lightvalve	13:747daba9cf59	898	} else {
Lightvalve	14:8e7590227d22	899	TMR3_COUNT_DEADZONE = -1;
Lightvalve	13:747daba9cf59	900	VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2;
Lightvalve	13:747daba9cf59	901	if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) {
Lightvalve	13:747daba9cf59	902	VALVE_DEADZONE_PLUS = VALVE_CENTER;
Lightvalve	13:747daba9cf59	903	VALVE_DEADZONE_MINUS = VALVE_CENTER;
Lightvalve	13:747daba9cf59	904	}
Lightvalve	18:b8adf1582ea3	905	V_out = 0;
Lightvalve	19:23b7c1ad8683	906
Lightvalve	16:903b5a4433b4	907	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	908
Lightvalve	16:903b5a4433b4	909	//spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
Lightvalve	16:903b5a4433b4	910	//spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
Lightvalve	14:8e7590227d22	911
Lightvalve	13:747daba9cf59	912	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	913	DZ_temp_cnt2 = 0;
Lightvalve	13:747daba9cf59	914	}
Lightvalve	13:747daba9cf59	915	}
Lightvalve	14:8e7590227d22	916	TMR3_COUNT_DEADZONE++;
Lightvalve	13:747daba9cf59	917	break;
Lightvalve	13:747daba9cf59	918	}
Lightvalve	14:8e7590227d22	919
Lightvalve	14:8e7590227d22	920	case MODE_FIND_HOME: {
Lightvalve	19:23b7c1ad8683	921
Lightvalve	19:23b7c1ad8683	922
Lightvalve	14:8e7590227d22	923	////////////////////////////////////////////FIND_HOME_PWM/////////////////////////////////////////////////////
Lightvalve	14:8e7590227d22	924	if (FLAG_FIND_HOME == true) {
Lightvalve	14:8e7590227d22	925	cnt_findhome = 0;
Lightvalve	14:8e7590227d22	926	cnt_vel_findhome = 0;
Lightvalve	14:8e7590227d22	927	//REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
Lightvalve	14:8e7590227d22	928	pos.ref = pos.sen;
Lightvalve	14:8e7590227d22	929	FLAG_FIND_HOME = false;
Lightvalve	14:8e7590227d22	930	}
Lightvalve	14:8e7590227d22	931
Lightvalve	14:8e7590227d22	932	int check_enc = (TMR_FREQ_5k/10);
Lightvalve	14:8e7590227d22	933	if(cnt_findhome%check_enc == 0) {
Lightvalve	14:8e7590227d22	934	FINDHOME_POSITION = pos.sen;
Lightvalve	14:8e7590227d22	935	FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
Lightvalve	14:8e7590227d22	936	FINDHOME_POSITION_OLD = FINDHOME_POSITION;
Lightvalve	14:8e7590227d22	937	}
Lightvalve	14:8e7590227d22	938	cnt_findhome++;
Lightvalve	14:8e7590227d22	939	if(cnt_findhome == 10000) cnt_findhome = 0;
Lightvalve	14:8e7590227d22	940
Lightvalve	14:8e7590227d22	941	if (abs(FINDHOME_VELOCITY) <= 1) {
Lightvalve	14:8e7590227d22	942	cnt_vel_findhome = cnt_vel_findhome + 1;
Lightvalve	14:8e7590227d22	943	} else {
Lightvalve	14:8e7590227d22	944	cnt_vel_findhome = 0;
Lightvalve	14:8e7590227d22	945	}
Lightvalve	14:8e7590227d22	946
Lightvalve	14:8e7590227d22	947	if (cnt_vel_findhome < 2*TMR_FREQ_5k) {
Lightvalve	14:8e7590227d22	948	//REFERENCE_MODE = MODE_REF_NO_ACT;
Lightvalve	14:8e7590227d22	949	if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 1.0;
Lightvalve	14:8e7590227d22	950	else pos.ref = pos.ref - 1.0;
Lightvalve	14:8e7590227d22	951	pos.err = pos.ref - (double) pos.sen;
Lightvalve	14:8e7590227d22	952	V_out = ((double) P_GAIN_JOINT_POSITION * pos.err) * 0.01;
Lightvalve	18:b8adf1582ea3	953	// if (HOMEPOS_OFFSET > 0) V_out = 100;
Lightvalve	18:b8adf1582ea3	954	// else V_out = -100;
Lightvalve	14:8e7590227d22	955	} else {
Lightvalve	14:8e7590227d22	956	ENC_SET(HOMEPOS_OFFSET);
Lightvalve	14:8e7590227d22	957	FLAG_REFERENCE_JOINT_POSITION = 1;
Lightvalve	14:8e7590227d22	958	INIT_REF_POS = HOMEPOS_OFFSET;
Lightvalve	14:8e7590227d22	959	REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k;
Lightvalve	14:8e7590227d22	960	TMR3_COUNT_REFERENCE = 0;
Lightvalve	14:8e7590227d22	961	vel.ref = 0.0;
Lightvalve	14:8e7590227d22	962	//REFERENCE_MODE = MODE_REF_COS_INC;
Lightvalve	14:8e7590227d22	963	CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_PWM;
Lightvalve	14:8e7590227d22	964
Lightvalve	14:8e7590227d22	965	FINDHOME_POSITION = 0;
Lightvalve	14:8e7590227d22	966	FINDHOME_POSITION_OLD = 0;
Lightvalve	14:8e7590227d22	967	FINDHOME_VELOCITY = 0;
Lightvalve	14:8e7590227d22	968	cnt_findhome = 0;
Lightvalve	14:8e7590227d22	969	cnt_vel_findhome = 0;
Lightvalve	14:8e7590227d22	970	}
Lightvalve	19:23b7c1ad8683	971
Lightvalve	14:8e7590227d22	972	/*
Lightvalve	14:8e7590227d22	973	////////////////////////////////////////////FIND_HOME_CURRENT/////////////////////////////////////////////////////
Lightvalve	13:747daba9cf59	974	if (FLAG_FIND_HOME == true) {
Lightvalve	13:747daba9cf59	975	cnt_findhome = 0;
Lightvalve	13:747daba9cf59	976	cnt_vel_findhome = 0;
Lightvalve	13:747daba9cf59	977	REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
Lightvalve	14:8e7590227d22	978	Ref_Joint_Pos = pos.sen;
Lightvalve	13:747daba9cf59	979	FLAG_FIND_HOME = false;
Lightvalve	13:747daba9cf59	980	}
Lightvalve	19:23b7c1ad8683	981
Lightvalve	14:8e7590227d22	982	int cnt_check_enc = (TMR_FREQ_5k/500);
Lightvalve	14:8e7590227d22	983	if(cnt_findhome%cnt_check_enc == 0){
Lightvalve	14:8e7590227d22	984	FINDHOME_POSITION = pos.sen;
Lightvalve	13:747daba9cf59	985	FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
Lightvalve	13:747daba9cf59	986	FINDHOME_POSITION_OLD = FINDHOME_POSITION;
Lightvalve	13:747daba9cf59	987	} cnt_findhome++;
Lightvalve	13:747daba9cf59	988	if(cnt_findhome == 10000) cnt_findhome = 0;
Lightvalve	19:23b7c1ad8683	989
Lightvalve	13:747daba9cf59	990	if (abs(FINDHOME_VELOCITY) <= 1) {
Lightvalve	13:747daba9cf59	991	cnt_vel_findhome = cnt_vel_findhome + 1;
Lightvalve	13:747daba9cf59	992	} else {
Lightvalve	13:747daba9cf59	993	cnt_vel_findhome = 0;
Lightvalve	13:747daba9cf59	994	}
Lightvalve	19:23b7c1ad8683	995
Lightvalve	14:8e7590227d22	996	if (cnt_vel_findhome < 3*TMR_FREQ_5k) { // wait for 3sec
Lightvalve	13:747daba9cf59	997	REFERENCE_MODE = MODE_REF_NO_ACT;
Lightvalve	13:747daba9cf59	998	if (HOMEPOS_OFFSET > 0) Ref_Joint_Pos = Ref_Joint_Pos + 1.0;
Lightvalve	13:747daba9cf59	999	else Ref_Joint_Pos = Ref_Joint_Pos - 1.0;
Lightvalve	14:8e7590227d22	1000	pos.err = Ref_Joint_Pos - pos.sen;
Lightvalve	19:23b7c1ad8683	1001	I_REF = 0.001*((double)pos.err);
Lightvalve	14:8e7590227d22	1002	if(I_REF>5.0) I_REF = 5.0;
Lightvalve	14:8e7590227d22	1003	if(I_REF<-5.0) I_REF = -5.0;
Lightvalve	14:8e7590227d22	1004	cur.ref = I_REF;
Lightvalve	14:8e7590227d22	1005	CurrentControl();
Lightvalve	13:747daba9cf59	1006	} else {
Lightvalve	13:747daba9cf59	1007	ENC_SET(HOMEPOS_OFFSET);
Lightvalve	13:747daba9cf59	1008	FLAG_REFERENCE_JOINT_POSITION = 1;
Lightvalve	13:747daba9cf59	1009	INIT_REF_POS = HOMEPOS_OFFSET;
Lightvalve	13:747daba9cf59	1010	REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k;
Lightvalve	14:8e7590227d22	1011	TMR3_COUNT_REFERENCE = 0;
Lightvalve	13:747daba9cf59	1012	Ref_Joint_Vel = 0.0;
Lightvalve	13:747daba9cf59	1013	REFERENCE_MODE = MODE_REF_COS_INC;
Lightvalve	14:8e7590227d22	1014	CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION;
Lightvalve	19:23b7c1ad8683	1015
Lightvalve	13:747daba9cf59	1016	FINDHOME_POSITION = 0;
Lightvalve	13:747daba9cf59	1017	FINDHOME_POSITION_OLD = 0;
Lightvalve	13:747daba9cf59	1018	FINDHOME_VELOCITY = 0;
Lightvalve	13:747daba9cf59	1019	cnt_findhome = 0;
Lightvalve	13:747daba9cf59	1020	cnt_vel_findhome = 0;
Lightvalve	13:747daba9cf59	1021	}
Lightvalve	14:8e7590227d22	1022	*/
Lightvalve	19:23b7c1ad8683	1023
Lightvalve	13:747daba9cf59	1024	break;
Lightvalve	13:747daba9cf59	1025	}
Lightvalve	14:8e7590227d22	1026
Lightvalve	14:8e7590227d22	1027	case MODE_VALVE_GAIN_SETTING: {
Lightvalve	14:8e7590227d22	1028	if (TMR3_COUNT_FLOWRATE == 0) {
Lightvalve	13:747daba9cf59	1029	if (pos_plus_end == pos_minus_end) need_enc_init = true;
Lightvalve	13:747daba9cf59	1030	else {
Lightvalve	18:b8adf1582ea3	1031	V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	13:747daba9cf59	1032	temp_time = (int) (0.5 * (double) TMR_FREQ_5k);
Lightvalve	13:747daba9cf59	1033	}
Lightvalve	13:747daba9cf59	1034	}
Lightvalve	13:747daba9cf59	1035	if (need_enc_init) {
Lightvalve	14:8e7590227d22	1036	if (TMR3_COUNT_FLOWRATE < (int) (0.5 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1037	V_out = VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1038	pos_plus_end = pos.sen;
Lightvalve	14:8e7590227d22	1039	} else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
Lightvalve	18:b8adf1582ea3	1040	V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1041	pos_minus_end = pos.sen;
Lightvalve	14:8e7590227d22	1042	} else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
Lightvalve	13:747daba9cf59	1043	need_enc_init = false;
Lightvalve	13:747daba9cf59	1044	check_vel_pos_init = (int) (0.9 * (double) (pos_plus_end - pos_minus_end));
Lightvalve	13:747daba9cf59	1045	check_vel_pos_fin = (int) (0.95 * (double) (pos_plus_end - pos_minus_end));
Lightvalve	13:747daba9cf59	1046	check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
Lightvalve	13:747daba9cf59	1047	}
Lightvalve	13:747daba9cf59	1048	temp_time = TMR_FREQ_5k;
Lightvalve	13:747daba9cf59	1049	}
Lightvalve	14:8e7590227d22	1050	TMR3_COUNT_FLOWRATE++;
Lightvalve	14:8e7590227d22	1051	if (TMR3_COUNT_FLOWRATE > temp_time) {
Lightvalve	13:747daba9cf59	1052	if (flag_flowrate % 2 == 0) { // (+)
Lightvalve	18:b8adf1582ea3	1053	VALVE_VOLTAGE = 1000.0 * (double) (flag_flowrate / 2 + 1);
Lightvalve	18:b8adf1582ea3	1054	V_out = VALVE_VOLTAGE;
Lightvalve	14:8e7590227d22	1055	if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
Lightvalve	13:747daba9cf59	1056	fl_temp_cnt++;
Lightvalve	14:8e7590227d22	1057	} else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
Lightvalve	13:747daba9cf59	1058	VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873 * 0.5757 * (double) TMR_FREQ_5k / 10000.0 * (double) check_vel_pos_interv / (double) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6pi/6553610000 0.5757=0.025250.020.0095260*1000
Lightvalve	13:747daba9cf59	1059	// VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
Lightvalve	13:747daba9cf59	1060	fl_temp_cnt2++;
Lightvalve	13:747daba9cf59	1061	}
Lightvalve	13:747daba9cf59	1062	} else if (flag_flowrate % 2 == 1) { // (-)
Lightvalve	13:747daba9cf59	1063	VALVE_VOLTAGE = -1. * (double) (flag_flowrate / 2 + 1);
Lightvalve	18:b8adf1582ea3	1064	V_out = VALVE_VOLTAGE;
Lightvalve	14:8e7590227d22	1065	if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
Lightvalve	13:747daba9cf59	1066	fl_temp_cnt++;
Lightvalve	14:8e7590227d22	1067	} else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
Lightvalve	13:747daba9cf59	1068	VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873 * 0.5757 * (double) TMR_FREQ_5k / 10000.0 * (double) check_vel_pos_interv / (double) fl_temp_cnt / (-VALVE_VOLTAGE);
Lightvalve	13:747daba9cf59	1069	// VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
Lightvalve	13:747daba9cf59	1070	fl_temp_cnt2++;
Lightvalve	13:747daba9cf59	1071	}
Lightvalve	13:747daba9cf59	1072	}
Lightvalve	13:747daba9cf59	1073	if (fl_temp_cnt2 == 100) {
Lightvalve	19:23b7c1ad8683	1074
Lightvalve	16:903b5a4433b4	1075	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	1076
Lightvalve	16:903b5a4433b4	1077	//spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0));
Lightvalve	13:747daba9cf59	1078	cur_vel_sum = 0;
Lightvalve	13:747daba9cf59	1079	fl_temp_cnt = 0;
Lightvalve	13:747daba9cf59	1080	fl_temp_cnt2 = 0;
Lightvalve	13:747daba9cf59	1081	flag_flowrate++;
Lightvalve	13:747daba9cf59	1082	}
Lightvalve	13:747daba9cf59	1083	if (flag_flowrate == 10) {
Lightvalve	18:b8adf1582ea3	1084	V_out = 0;
Lightvalve	13:747daba9cf59	1085	flag_flowrate = 0;
Lightvalve	14:8e7590227d22	1086	TMR3_COUNT_FLOWRATE = 0;
Lightvalve	13:747daba9cf59	1087	valve_gain_repeat_cnt++;
Lightvalve	13:747daba9cf59	1088	if (valve_gain_repeat_cnt >= 1) {
Lightvalve	13:747daba9cf59	1089	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1090	valve_gain_repeat_cnt = 0;
Lightvalve	13:747daba9cf59	1091	}
Lightvalve	14:8e7590227d22	1092
Lightvalve	13:747daba9cf59	1093	}
Lightvalve	13:747daba9cf59	1094	break;
Lightvalve	13:747daba9cf59	1095	}
Lightvalve	14:8e7590227d22	1096
Lightvalve	13:747daba9cf59	1097	}
Lightvalve	14:8e7590227d22	1098
Lightvalve	14:8e7590227d22	1099	case MODE_PRESSURE_SENSOR_NULLING: {
Lightvalve	13:747daba9cf59	1100	// DAC Voltage reference set
Lightvalve	14:8e7590227d22	1101	if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
Lightvalve	14:8e7590227d22	1102	CUR_PRES_A_sum += pres_A.sen;
Lightvalve	14:8e7590227d22	1103	CUR_PRES_B_sum += pres_B.sen;
Lightvalve	14:8e7590227d22	1104
Lightvalve	14:8e7590227d22	1105	if (TMR3_COUNT_PRES_NULL % 10 == 0) {
Lightvalve	13:747daba9cf59	1106	CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0;
Lightvalve	13:747daba9cf59	1107	CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0;
Lightvalve	13:747daba9cf59	1108	CUR_PRES_A_sum = 0;
Lightvalve	13:747daba9cf59	1109	CUR_PRES_B_sum = 0;
Lightvalve	14:8e7590227d22	1110
Lightvalve	16:903b5a4433b4	1111	double VREF_NullingGain = 0.003;
Lightvalve	13:747daba9cf59	1112	PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
Lightvalve	13:747daba9cf59	1113	PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
Lightvalve	14:8e7590227d22	1114
Lightvalve	13:747daba9cf59	1115	if (PRES_A_VREF > 3.3) PRES_A_VREF = 3.3;
Lightvalve	13:747daba9cf59	1116	if (PRES_A_VREF < 0) PRES_A_VREF = 0;
Lightvalve	13:747daba9cf59	1117	if (PRES_B_VREF > 3.3) PRES_B_VREF = 3.3;
Lightvalve	13:747daba9cf59	1118	if (PRES_B_VREF < 0) PRES_B_VREF = 0;
Lightvalve	19:23b7c1ad8683	1119
Lightvalve	16:903b5a4433b4	1120	dac_1 = PRES_A_VREF / 3.3;
Lightvalve	16:903b5a4433b4	1121	dac_2 = PRES_B_VREF / 3.3;
Lightvalve	13:747daba9cf59	1122	}
Lightvalve	13:747daba9cf59	1123	} else {
Lightvalve	13:747daba9cf59	1124	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	14:8e7590227d22	1125	TMR3_COUNT_PRES_NULL = 0;
Lightvalve	13:747daba9cf59	1126	CUR_PRES_A_sum = 0;
Lightvalve	13:747daba9cf59	1127	CUR_PRES_B_sum = 0;
Lightvalve	13:747daba9cf59	1128	CUR_PRES_A_mean = 0;
Lightvalve	13:747daba9cf59	1129	CUR_PRES_B_mean = 0;
Lightvalve	14:8e7590227d22	1130
Lightvalve	16:903b5a4433b4	1131	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	1132
Lightvalve	15:bd0d12728506	1133	dac_1 = PRES_A_VREF / 3.3;
Lightvalve	15:bd0d12728506	1134	dac_2 = PRES_B_VREF / 3.3;
Lightvalve	17:1865016ca2e7	1135	//pc.printf("nulling end");
Lightvalve	13:747daba9cf59	1136	}
Lightvalve	14:8e7590227d22	1137	TMR3_COUNT_PRES_NULL++;
Lightvalve	13:747daba9cf59	1138	break;
Lightvalve	13:747daba9cf59	1139	}
Lightvalve	14:8e7590227d22	1140
Lightvalve	14:8e7590227d22	1141	case MODE_PRESSURE_SENSOR_CALIB: {
Lightvalve	14:8e7590227d22	1142	if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
Lightvalve	18:b8adf1582ea3	1143	V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1144	if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
Lightvalve	13:747daba9cf59	1145	CUR_PRES_A_sum += CUR_PRES_A;
Lightvalve	13:747daba9cf59	1146	}
Lightvalve	14:8e7590227d22	1147	} else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
Lightvalve	18:b8adf1582ea3	1148	V_out = VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1149	if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
Lightvalve	13:747daba9cf59	1150	CUR_PRES_B_sum += CUR_PRES_B;
Lightvalve	13:747daba9cf59	1151	}
Lightvalve	13:747daba9cf59	1152	} else {
Lightvalve	13:747daba9cf59	1153	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	14:8e7590227d22	1154	TMR3_COUNT_PRES_CALIB = 0;
Lightvalve	18:b8adf1582ea3	1155	V_out = 0;
Lightvalve	18:b8adf1582ea3	1156	PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((double) TMR_FREQ_5k - 1.0) - PRES_A_NULL;
Lightvalve	18:b8adf1582ea3	1157	PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.0);
Lightvalve	18:b8adf1582ea3	1158	PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((double) TMR_FREQ_5k - 1.0) - PRES_B_NULL;
Lightvalve	18:b8adf1582ea3	1159	PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.0);
Lightvalve	13:747daba9cf59	1160	CUR_PRES_A_sum = 0;
Lightvalve	13:747daba9cf59	1161	CUR_PRES_B_sum = 0;
Lightvalve	13:747daba9cf59	1162	CUR_PRES_A_mean = 0;
Lightvalve	13:747daba9cf59	1163	CUR_PRES_B_mean = 0;
Lightvalve	19:23b7c1ad8683	1164
Lightvalve	16:903b5a4433b4	1165	ROM_RESET_DATA();
Lightvalve	14:8e7590227d22	1166
Lightvalve	16:903b5a4433b4	1167	//spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
Lightvalve	16:903b5a4433b4	1168	//spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
Lightvalve	13:747daba9cf59	1169	}
Lightvalve	14:8e7590227d22	1170	TMR3_COUNT_PRES_CALIB++;
Lightvalve	13:747daba9cf59	1171	break;
Lightvalve	13:747daba9cf59	1172	}
Lightvalve	14:8e7590227d22	1173
Lightvalve	14:8e7590227d22	1174	case MODE_ROTARY_FRICTION_TUNING: {
Lightvalve	14:8e7590227d22	1175	if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4 + 3. * sin(2 * 3.14159 * 0.5 * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001 * 0.05);
Lightvalve	18:b8adf1582ea3	1176	V_out = PWM_out * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001);
Lightvalve	18:b8adf1582ea3	1177	if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT;
Lightvalve	18:b8adf1582ea3	1178	else V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1179	TMR3_COUNT_ROTARY_FRIC_TUNE++;
Lightvalve	14:8e7590227d22	1180	if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
Lightvalve	14:8e7590227d22	1181	TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
Lightvalve	17:1865016ca2e7	1182	V_out = 0;
Lightvalve	13:747daba9cf59	1183	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1184	}
Lightvalve	13:747daba9cf59	1185	break;
Lightvalve	13:747daba9cf59	1186	}
Lightvalve	14:8e7590227d22	1187
Lightvalve	14:8e7590227d22	1188	case MODE_DDV_POS_VS_PWM_ID: {
Lightvalve	13:747daba9cf59	1189	VALVE_ID_timer = VALVE_ID_timer + 1;
Lightvalve	14:8e7590227d22	1190
Lightvalve	14:8e7590227d22	1191	if(VALVE_ID_timer < TMR_FREQ_5k*1) {
Lightvalve	18:b8adf1582ea3	1192	V_out = 3000.0 * sin(23.14VALVE_ID_timer/TMR_FREQ_5k * 100.0);
Lightvalve	14:8e7590227d22	1193	} else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
Lightvalve	18:b8adf1582ea3	1194	V_out = 1000.0*(ID_index_array[ID_index]);
Lightvalve	14:8e7590227d22	1195	} else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
Lightvalve	13:747daba9cf59	1196	VALVE_POS_TMP = 0;
Lightvalve	13:747daba9cf59	1197	data_num = 0;
Lightvalve	14:8e7590227d22	1198	} else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
Lightvalve	13:747daba9cf59	1199	data_num = data_num + 1;
Lightvalve	13:747daba9cf59	1200	VALVE_POS_TMP = VALVE_POS_TMP + value;
Lightvalve	14:8e7590227d22	1201	} else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
Lightvalve	13:747daba9cf59	1202	VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
Lightvalve	13:747daba9cf59	1203	VALVE_ID_timer = 0;
Lightvalve	13:747daba9cf59	1204	ID_index= ID_index +1;
Lightvalve	13:747daba9cf59	1205	}
Lightvalve	14:8e7590227d22	1206
Lightvalve	17:1865016ca2e7	1207	if(ID_index>=25) {
Lightvalve	13:747daba9cf59	1208	int i;
Lightvalve	13:747daba9cf59	1209	VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
Lightvalve	17:1865016ca2e7	1210	for(i=0; i<25; i++) {
Lightvalve	13:747daba9cf59	1211	VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
Lightvalve	14:8e7590227d22	1212	if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
Lightvalve	13:747daba9cf59	1213	VALVE_MAX_POS = VALVE_POS_AVG[i];
Lightvalve	13:747daba9cf59	1214	VALVE_POS_AVG_OLD = VALVE_MAX_POS;
Lightvalve	14:8e7590227d22	1215	} else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) {
Lightvalve	13:747daba9cf59	1216	VALVE_MIN_POS = VALVE_POS_AVG[i];
Lightvalve	13:747daba9cf59	1217	VALVE_POS_AVG_OLD = VALVE_MIN_POS;
Lightvalve	13:747daba9cf59	1218	}
Lightvalve	13:747daba9cf59	1219	}
Lightvalve	16:903b5a4433b4	1220	ROM_RESET_DATA();
Lightvalve	13:747daba9cf59	1221	ID_index = 0;
Lightvalve	13:747daba9cf59	1222	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1223	}
Lightvalve	14:8e7590227d22	1224
Lightvalve	14:8e7590227d22	1225
Lightvalve	13:747daba9cf59	1226	break;
Lightvalve	13:747daba9cf59	1227	}
Lightvalve	14:8e7590227d22	1228
Lightvalve	14:8e7590227d22	1229	case MODE_DDV_DEADZONE_AND_CENTER: {
Lightvalve	14:8e7590227d22	1230
Lightvalve	13:747daba9cf59	1231	VALVE_DZ_timer = VALVE_DZ_timer + 1;
Lightvalve	14:8e7590227d22	1232	if(first_check == 0) {
Lightvalve	14:8e7590227d22	1233	if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1234	V_out = VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1235	} else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1236	V_out = VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1237	pos_plus_end = pos.sen;
Lightvalve	14:8e7590227d22	1238	} else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1239	V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1240	} else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1241	V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1242	pos_minus_end = pos.sen;
Lightvalve	14:8e7590227d22	1243	} else if(VALVE_DZ_timer < (int) (3.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1244	V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	14:8e7590227d22	1245	} else if(VALVE_DZ_timer < (int) (4.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1246	V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	13:747daba9cf59	1247	data_num = data_num + 1;
Lightvalve	13:747daba9cf59	1248	VALVE_POS_TMP = VALVE_POS_TMP + value;
Lightvalve	14:8e7590227d22	1249	} else if(VALVE_DZ_timer == (int) (4.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1250	V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	13:747daba9cf59	1251	DDV_POS_AVG = VALVE_POS_TMP / data_num;
Lightvalve	14:8e7590227d22	1252	START_POS = pos.sen;
Lightvalve	23:59218d4a256d	1253	//CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num));
Lightvalve	13:747daba9cf59	1254	VALVE_POS_TMP = 0;
Lightvalve	13:747daba9cf59	1255	data_num = 0;
Lightvalve	14:8e7590227d22	1256
Lightvalve	14:8e7590227d22	1257	} else if(VALVE_DZ_timer < (int) (5.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1258	valve_pos.ref = DDV_POS_AVG;
Lightvalve	14:8e7590227d22	1259	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	13:747daba9cf59	1260	//CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS));
Lightvalve	14:8e7590227d22	1261
Lightvalve	14:8e7590227d22	1262	} else if(VALVE_DZ_timer < (int) (6.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1263	valve_pos.ref = DDV_POS_AVG;
Lightvalve	14:8e7590227d22	1264	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	13:747daba9cf59	1265	if(CUR_VELOCITY >= 0)
Lightvalve	13:747daba9cf59	1266	VEL_POINT = VEL_POINT + 1;
Lightvalve	13:747daba9cf59	1267	else
Lightvalve	13:747daba9cf59	1268	VEL_POINT = VEL_POINT - 1;
Lightvalve	14:8e7590227d22	1269	} else if(VALVE_DZ_timer == (int) (6.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1270	valve_pos.ref = DDV_POS_AVG;
Lightvalve	14:8e7590227d22	1271	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	14:8e7590227d22	1272	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1273	// if(VEL_POINT >= 0)
Lightvalve	14:8e7590227d22	1274	// {
Lightvalve	14:8e7590227d22	1275	// DZ_case = 1;
Lightvalve	14:8e7590227d22	1276	// }
Lightvalve	14:8e7590227d22	1277	// else
Lightvalve	14:8e7590227d22	1278	// {
Lightvalve	14:8e7590227d22	1279	// DZ_case = -1;
Lightvalve	14:8e7590227d22	1280	// }
Lightvalve	14:8e7590227d22	1281	// if(abs(FINAL_POS - START_POS) < 100)
Lightvalve	14:8e7590227d22	1282	// {
Lightvalve	14:8e7590227d22	1283	// DZ_case = 0;
Lightvalve	14:8e7590227d22	1284	// }
Lightvalve	14:8e7590227d22	1285
Lightvalve	14:8e7590227d22	1286	if((FINAL_POS - START_POS)>100) {
Lightvalve	13:747daba9cf59	1287	DZ_case = 1;
Lightvalve	14:8e7590227d22	1288	} else if((FINAL_POS - START_POS)<-100) {
Lightvalve	13:747daba9cf59	1289	DZ_case = -1;
Lightvalve	14:8e7590227d22	1290	} else {
Lightvalve	13:747daba9cf59	1291	DZ_case = 0;
Lightvalve	13:747daba9cf59	1292	}
Lightvalve	14:8e7590227d22	1293
Lightvalve	14:8e7590227d22	1294	VEL_POINT = 0;
Lightvalve	13:747daba9cf59	1295	first_check = 1;
Lightvalve	13:747daba9cf59	1296	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1297	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1298	Ref_Valve_Pos_Old = DDV_POS_AVG;
Lightvalve	13:747daba9cf59	1299	DZ_NUM = 1;
Lightvalve	13:747daba9cf59	1300	DZ_index = 1;
Lightvalve	18:b8adf1582ea3	1301	//CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM));
Lightvalve	14:8e7590227d22	1302
Lightvalve	13:747daba9cf59	1303	}
Lightvalve	19:23b7c1ad8683	1304	} else {
Lightvalve	14:8e7590227d22	1305	if((DZ_case == -1 && DZ_NUM == 1) \| (DZ_case == 1 && DZ_NUM == 1)) {
Lightvalve	14:8e7590227d22	1306	if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1307	V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	14:8e7590227d22	1308	} else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1309	START_POS = pos.sen;
Lightvalve	14:8e7590227d22	1310	} else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1311	valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index;
Lightvalve	14:8e7590227d22	1312	if(valve_pos.ref <= VALVE_MIN_POS) {
Lightvalve	14:8e7590227d22	1313	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	14:8e7590227d22	1314	} else if(valve_pos.ref >= VALVE_MAX_POS) {
Lightvalve	14:8e7590227d22	1315	valve_pos.ref = VALVE_MAX_POS;
Lightvalve	13:747daba9cf59	1316	}
Lightvalve	14:8e7590227d22	1317	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	13:747daba9cf59	1318	if(CUR_VELOCITY >= 0)
Lightvalve	13:747daba9cf59	1319	VEL_POINT = VEL_POINT + 1;
Lightvalve	13:747daba9cf59	1320	else
Lightvalve	13:747daba9cf59	1321	VEL_POINT = VEL_POINT - 1;
Lightvalve	14:8e7590227d22	1322	} else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1323	Ref_Valve_Pos_Old = valve_pos.ref;
Lightvalve	14:8e7590227d22	1324	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1325	// if(VEL_POINT >= 0)
Lightvalve	14:8e7590227d22	1326	// DZ_DIRECTION = 1 * DZ_case;
Lightvalve	14:8e7590227d22	1327	// else
Lightvalve	14:8e7590227d22	1328	// DZ_DIRECTION = -1 * DZ_case;
Lightvalve	14:8e7590227d22	1329	// if(abs(FINAL_POS - START_POS) < 100)
Lightvalve	14:8e7590227d22	1330	// DZ_DIRECTION = 1 * DZ_case;
Lightvalve	14:8e7590227d22	1331
Lightvalve	14:8e7590227d22	1332	if((FINAL_POS - START_POS)>100) {
Lightvalve	14:8e7590227d22	1333	DZ_DIRECTION = 1 * DZ_case;
Lightvalve	14:8e7590227d22	1334	} else if((FINAL_POS - START_POS)<-100) {
Lightvalve	14:8e7590227d22	1335	DZ_DIRECTION = -1 * DZ_case;
Lightvalve	14:8e7590227d22	1336	} else {
Lightvalve	13:747daba9cf59	1337	DZ_DIRECTION = 1 * DZ_case;
Lightvalve	13:747daba9cf59	1338	}
Lightvalve	14:8e7590227d22	1339
Lightvalve	14:8e7590227d22	1340	VEL_POINT = 0;
Lightvalve	14:8e7590227d22	1341
Lightvalve	13:747daba9cf59	1342	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1343	DZ_index= DZ_index *2;
Lightvalve	14:8e7590227d22	1344	if(DZ_index >= 128) {
Lightvalve	14:8e7590227d22	1345	FIRST_DZ = valve_pos.ref;
Lightvalve	13:747daba9cf59	1346	DZ_NUM = 2;
Lightvalve	13:747daba9cf59	1347	Ref_Valve_Pos_Old = FIRST_DZ;
Lightvalve	13:747daba9cf59	1348	DZ_index = 1;
Lightvalve	13:747daba9cf59	1349	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1350	}
Lightvalve	14:8e7590227d22	1351
Lightvalve	13:747daba9cf59	1352	//CAN_TX_PRES((int16_t) (1), (int16_t) (DZ_index));
Lightvalve	13:747daba9cf59	1353	}
Lightvalve	14:8e7590227d22	1354	} else if((DZ_case == -1 && DZ_NUM == 2) \| (DZ_case == 1 && DZ_NUM == 2)) {
Lightvalve	14:8e7590227d22	1355	if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1356	V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	14:8e7590227d22	1357	} else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1358	START_POS = pos.sen;
Lightvalve	14:8e7590227d22	1359	} else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1360	valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
Lightvalve	14:8e7590227d22	1361	if(valve_pos.ref <= VALVE_MIN_POS) {
Lightvalve	14:8e7590227d22	1362	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	14:8e7590227d22	1363	} else if(valve_pos.ref >= VALVE_MAX_POS) {
Lightvalve	14:8e7590227d22	1364	valve_pos.ref = VALVE_MAX_POS;
Lightvalve	13:747daba9cf59	1365	}
Lightvalve	14:8e7590227d22	1366	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	13:747daba9cf59	1367	if(CUR_VELOCITY >= 0)
Lightvalve	13:747daba9cf59	1368	VEL_POINT = VEL_POINT + 1;
Lightvalve	13:747daba9cf59	1369	else
Lightvalve	13:747daba9cf59	1370	VEL_POINT = VEL_POINT - 1;
Lightvalve	14:8e7590227d22	1371	} else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1372	Ref_Valve_Pos_Old = valve_pos.ref;
Lightvalve	14:8e7590227d22	1373	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1374	// if(VEL_POINT >= 0)
Lightvalve	14:8e7590227d22	1375	// DZ_DIRECTION = 1 * DZ_case;
Lightvalve	14:8e7590227d22	1376	// else
Lightvalve	14:8e7590227d22	1377	// DZ_DIRECTION = -1 * DZ_case;
Lightvalve	14:8e7590227d22	1378	// if(abs(FINAL_POS - START_POS) < 100)
Lightvalve	14:8e7590227d22	1379	// DZ_DIRECTION = -1 * DZ_case;
Lightvalve	14:8e7590227d22	1380
Lightvalve	14:8e7590227d22	1381	if((FINAL_POS - START_POS)>100) {
Lightvalve	13:747daba9cf59	1382	DZ_DIRECTION = -1;
Lightvalve	14:8e7590227d22	1383	} else if((FINAL_POS - START_POS)<-100) {
Lightvalve	14:8e7590227d22	1384	DZ_DIRECTION = 1;
Lightvalve	14:8e7590227d22	1385	} else {
Lightvalve	13:747daba9cf59	1386	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1387	}
Lightvalve	14:8e7590227d22	1388
Lightvalve	14:8e7590227d22	1389	VEL_POINT = 0;
Lightvalve	14:8e7590227d22	1390
Lightvalve	14:8e7590227d22	1391
Lightvalve	13:747daba9cf59	1392	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1393	DZ_index= DZ_index *2;
Lightvalve	14:8e7590227d22	1394	if(DZ_index >= 128) {
Lightvalve	14:8e7590227d22	1395	SECOND_DZ = valve_pos.ref;
Lightvalve	13:747daba9cf59	1396	DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
Lightvalve	13:747daba9cf59	1397	first_check = 0;
Lightvalve	19:23b7c1ad8683	1398	VALVE_DEADZONE_MINUS = FIRST_DZ;
Lightvalve	19:23b7c1ad8683	1399	VALVE_DEADZONE_PLUS = SECOND_DZ;
Lightvalve	19:23b7c1ad8683	1400
Lightvalve	16:903b5a4433b4	1401	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	1402
Lightvalve	16:903b5a4433b4	1403	//spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
Lightvalve	16:903b5a4433b4	1404	//spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
Lightvalve	18:b8adf1582ea3	1405	//spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
Lightvalve	13:747daba9cf59	1406	//CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
Lightvalve	13:747daba9cf59	1407	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1408	DZ_index = 1;
Lightvalve	13:747daba9cf59	1409	}
Lightvalve	14:8e7590227d22	1410
Lightvalve	23:59218d4a256d	1411	//CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index));
Lightvalve	13:747daba9cf59	1412	}
Lightvalve	14:8e7590227d22	1413	} else if(DZ_case == 0 && DZ_NUM ==1) {
Lightvalve	14:8e7590227d22	1414	if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1415	V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	14:8e7590227d22	1416	} else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1417	START_POS = pos.sen;
Lightvalve	14:8e7590227d22	1418	} else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1419	valve_pos.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index;
Lightvalve	14:8e7590227d22	1420	if(valve_pos.ref <= VALVE_MIN_POS) {
Lightvalve	14:8e7590227d22	1421	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	14:8e7590227d22	1422	} else if(valve_pos.ref >= VALVE_MAX_POS) {
Lightvalve	14:8e7590227d22	1423	valve_pos.ref = VALVE_MAX_POS;
Lightvalve	13:747daba9cf59	1424	}
Lightvalve	14:8e7590227d22	1425	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	13:747daba9cf59	1426	if(CUR_VELOCITY >= 0)
Lightvalve	13:747daba9cf59	1427	VEL_POINT = VEL_POINT + 1;
Lightvalve	13:747daba9cf59	1428	else
Lightvalve	13:747daba9cf59	1429	VEL_POINT = VEL_POINT - 1;
Lightvalve	14:8e7590227d22	1430	} else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1431	Ref_Valve_Pos_Old = valve_pos.ref;
Lightvalve	14:8e7590227d22	1432	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1433	// if(VEL_POINT >= 0)
Lightvalve	14:8e7590227d22	1434	// DZ_DIRECTION = 1;
Lightvalve	14:8e7590227d22	1435	// else
Lightvalve	14:8e7590227d22	1436	// DZ_DIRECTION = -1;
Lightvalve	14:8e7590227d22	1437	// if(abs(FINAL_POS - START_POS) < 100)
Lightvalve	14:8e7590227d22	1438	// DZ_DIRECTION = 1;
Lightvalve	14:8e7590227d22	1439
Lightvalve	14:8e7590227d22	1440	if((FINAL_POS - START_POS)>100) {
Lightvalve	14:8e7590227d22	1441	DZ_DIRECTION = 1;
Lightvalve	14:8e7590227d22	1442	} else if((FINAL_POS - START_POS)<-100) {
Lightvalve	14:8e7590227d22	1443	DZ_DIRECTION = -1;
Lightvalve	14:8e7590227d22	1444	} else {
Lightvalve	13:747daba9cf59	1445	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1446	}
Lightvalve	14:8e7590227d22	1447
Lightvalve	14:8e7590227d22	1448	VEL_POINT = 0;
Lightvalve	13:747daba9cf59	1449	//CAN_TX_PRES((int16_t) (7), (int16_t) (DZ_DIRECTION));
Lightvalve	13:747daba9cf59	1450	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1451	DZ_index= DZ_index *2;
Lightvalve	14:8e7590227d22	1452	if(DZ_index >= 128) {
Lightvalve	14:8e7590227d22	1453	FIRST_DZ = valve_pos.ref;
Lightvalve	13:747daba9cf59	1454	DZ_NUM = 2;
Lightvalve	13:747daba9cf59	1455	Ref_Valve_Pos_Old = FIRST_DZ;
Lightvalve	13:747daba9cf59	1456	DZ_index = 1;
Lightvalve	13:747daba9cf59	1457	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1458	}
Lightvalve	13:747daba9cf59	1459	}
Lightvalve	14:8e7590227d22	1460	} else {
Lightvalve	14:8e7590227d22	1461	if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1462	V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	14:8e7590227d22	1463	} else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1464	START_POS = pos.sen;
Lightvalve	14:8e7590227d22	1465	} else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1466	valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index;
Lightvalve	14:8e7590227d22	1467	if(valve_pos.ref <= VALVE_MIN_POS) {
Lightvalve	14:8e7590227d22	1468	valve_pos.ref = VALVE_MIN_POS;
Lightvalve	14:8e7590227d22	1469	} else if(valve_pos.ref > VALVE_MAX_POS) {
Lightvalve	14:8e7590227d22	1470	valve_pos.ref = VALVE_MAX_POS - 1;
Lightvalve	13:747daba9cf59	1471	}
Lightvalve	14:8e7590227d22	1472	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	13:747daba9cf59	1473	if(CUR_VELOCITY >= 0)
Lightvalve	13:747daba9cf59	1474	VEL_POINT = VEL_POINT + 1;
Lightvalve	13:747daba9cf59	1475	else
Lightvalve	13:747daba9cf59	1476	VEL_POINT = VEL_POINT - 1;
Lightvalve	14:8e7590227d22	1477	} else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1478	Ref_Valve_Pos_Old = valve_pos.ref;
Lightvalve	14:8e7590227d22	1479	FINAL_POS = pos.sen;
Lightvalve	13:747daba9cf59	1480	if(VEL_POINT >= 0)
Lightvalve	13:747daba9cf59	1481	DZ_DIRECTION = -1;
Lightvalve	13:747daba9cf59	1482	else
Lightvalve	13:747daba9cf59	1483	DZ_DIRECTION = 1;
Lightvalve	13:747daba9cf59	1484	if(abs(FINAL_POS - START_POS) < 100)
Lightvalve	13:747daba9cf59	1485	DZ_DIRECTION = 1;
Lightvalve	14:8e7590227d22	1486
Lightvalve	14:8e7590227d22	1487	VEL_POINT = 0;
Lightvalve	14:8e7590227d22	1488
Lightvalve	14:8e7590227d22	1489
Lightvalve	13:747daba9cf59	1490	VALVE_DZ_timer = 0;
Lightvalve	13:747daba9cf59	1491	//CAN_TX_PRES((int16_t) (3), (int16_t) (DZ_index));
Lightvalve	13:747daba9cf59	1492	DZ_index= DZ_index *2;
Lightvalve	14:8e7590227d22	1493	if(DZ_index >= 128) {
Lightvalve	14:8e7590227d22	1494	SECOND_DZ = valve_pos.ref;
Lightvalve	13:747daba9cf59	1495	DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ));
Lightvalve	13:747daba9cf59	1496	first_check = 0;
Lightvalve	13:747daba9cf59	1497	VALVE_DEADZONE_MINUS = FIRST_DZ;
Lightvalve	16:903b5a4433b4	1498	//spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ);
Lightvalve	13:747daba9cf59	1499	VALVE_DEADZONE_PLUS = SECOND_DZ;
Lightvalve	16:903b5a4433b4	1500	ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	1501
Lightvalve	16:903b5a4433b4	1502	//spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ);
Lightvalve	18:b8adf1582ea3	1503	//spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER);
Lightvalve	23:59218d4a256d	1504	//CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ);
Lightvalve	13:747daba9cf59	1505	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1506	DZ_index = 1;
Lightvalve	13:747daba9cf59	1507	}
Lightvalve	14:8e7590227d22	1508
Lightvalve	13:747daba9cf59	1509	//CAN_TX_PRES((int16_t) (4), (int16_t) (DZ_index));
Lightvalve	13:747daba9cf59	1510	}
Lightvalve	13:747daba9cf59	1511	}
Lightvalve	14:8e7590227d22	1512	}
Lightvalve	13:747daba9cf59	1513	break;
Lightvalve	13:747daba9cf59	1514	}
Lightvalve	14:8e7590227d22	1515
Lightvalve	14:8e7590227d22	1516	case MODE_DDV_POS_VS_FLOWRATE: {
Lightvalve	13:747daba9cf59	1517	VALVE_FR_timer = VALVE_FR_timer + 1;
Lightvalve	14:8e7590227d22	1518	if(first_check == 0) {
Lightvalve	14:8e7590227d22	1519	if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1520	V_out = VALVE_VOLTAGE_LIMIT;
Lightvalve	23:59218d4a256d	1521	//CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
Lightvalve	14:8e7590227d22	1522	} else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1523	V_out = VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1524	pos_plus_end = pos.sen;
Lightvalve	14:8e7590227d22	1525	// CAN_TX_PRES((int16_t) (V_out), (int16_t) (7));
Lightvalve	14:8e7590227d22	1526	} else if(VALVE_FR_timer < (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	18:b8adf1582ea3	1527	V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1528	} else if(VALVE_FR_timer == (int) (2.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1529	// CAN_TX_PRES((int16_t) (V_out), (int16_t) (8));
Lightvalve	18:b8adf1582ea3	1530	V_out = -VALVE_VOLTAGE_LIMIT;
Lightvalve	14:8e7590227d22	1531	pos_minus_end = pos.sen;
Lightvalve	13:747daba9cf59	1532	first_check = 1;
Lightvalve	13:747daba9cf59	1533	VALVE_FR_timer = 0;
Lightvalve	14:8e7590227d22	1534	valve_pos.ref = DDV_CENTER;
Lightvalve	13:747daba9cf59	1535	ID_index = 0;
Lightvalve	13:747daba9cf59	1536	max_check = 0;
Lightvalve	13:747daba9cf59	1537	min_check = 0;
Lightvalve	13:747daba9cf59	1538	}
Lightvalve	14:8e7590227d22	1539	} else {
Lightvalve	14:8e7590227d22	1540	if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1541	V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen);
Lightvalve	14:8e7590227d22	1542	} else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) {
Lightvalve	13:747daba9cf59	1543	data_num = 0;
Lightvalve	19:23b7c1ad8683	1544	valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER;
Lightvalve	14:8e7590227d22	1545
Lightvalve	14:8e7590227d22	1546	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	14:8e7590227d22	1547	START_POS = pos.sen;
Lightvalve	14:8e7590227d22	1548	} else if(VALVE_FR_timer < (int) (5.0 * (double) TMR_FREQ_5k)) {
Lightvalve	19:23b7c1ad8683	1549	valve_pos.ref = 10*(ID_index_array[ID_index]) + DDV_CENTER;
Lightvalve	14:8e7590227d22	1550	VALVE_POS_CONTROL(valve_pos.ref);
Lightvalve	13:747daba9cf59	1551	data_num = data_num + 1;
Lightvalve	19:23b7c1ad8683	1552	if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen) > 20000.0) {
Lightvalve	14:8e7590227d22	1553	FINAL_POS = pos.sen;
Lightvalve	14:8e7590227d22	1554	one_period_end = 1;
Lightvalve	13:747daba9cf59	1555	}
Lightvalve	14:8e7590227d22	1556	} else if(VALVE_FR_timer == (int) (5.0 * (double) TMR_FREQ_5k)) {
Lightvalve	14:8e7590227d22	1557	FINAL_POS = pos.sen;
Lightvalve	13:747daba9cf59	1558	one_period_end = 1;
Lightvalve	13:747daba9cf59	1559	}
Lightvalve	14:8e7590227d22	1560
Lightvalve	14:8e7590227d22	1561	if(one_period_end == 1) {
Lightvalve	14:8e7590227d22	1562	if(valve_pos.ref > VALVE_MAX_POS) {
Lightvalve	13:747daba9cf59	1563	max_check = 1;
Lightvalve	14:8e7590227d22	1564	} else if(valve_pos.ref < VALVE_MIN_POS) {
Lightvalve	13:747daba9cf59	1565	min_check = 1;
Lightvalve	13:747daba9cf59	1566	}
Lightvalve	13:747daba9cf59	1567	JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec
Lightvalve	19:23b7c1ad8683	1568
Lightvalve	19:23b7c1ad8683	1569	//ROM_RESET_DATA();
Lightvalve	19:23b7c1ad8683	1570
Lightvalve	16:903b5a4433b4	1571	//spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
Lightvalve	16:903b5a4433b4	1572	//spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF));
Lightvalve	13:747daba9cf59	1573	VALVE_FR_timer = 0;
Lightvalve	13:747daba9cf59	1574	one_period_end = 0;
Lightvalve	13:747daba9cf59	1575	ID_index= ID_index +1;
Lightvalve	14:8e7590227d22	1576	// CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (ID_index));
Lightvalve	19:23b7c1ad8683	1577	V_out = 0.0;
Lightvalve	13:747daba9cf59	1578	}
Lightvalve	14:8e7590227d22	1579
Lightvalve	14:8e7590227d22	1580	if(max_check == 1 && min_check == 1) {
Lightvalve	19:23b7c1ad8683	1581
Lightvalve	16:903b5a4433b4	1582	//spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index));
Lightvalve	13:747daba9cf59	1583	VALVE_POS_NUM = ID_index;
Lightvalve	19:23b7c1ad8683	1584	ROM_RESET_DATA();
Lightvalve	13:747daba9cf59	1585	ID_index = 0;
Lightvalve	13:747daba9cf59	1586	first_check = 0;
Lightvalve	13:747daba9cf59	1587	VALVE_FR_timer = 0;
Lightvalve	14:8e7590227d22	1588	// CAN_TX_PRES((int16_t) (123), (int16_t) (123));
Lightvalve	13:747daba9cf59	1589	CONTROL_MODE = MODE_NO_ACT;
Lightvalve	13:747daba9cf59	1590	}
Lightvalve	13:747daba9cf59	1591	}
Lightvalve	13:747daba9cf59	1592	break;
Lightvalve	13:747daba9cf59	1593	}
Lightvalve	14:8e7590227d22	1594
Lightvalve	12:6f2531038ea4	1595	default:
Lightvalve	12:6f2531038ea4	1596	break;
Lightvalve	12:6f2531038ea4	1597	}
Lightvalve	14:8e7590227d22	1598
Lightvalve	13:747daba9cf59	1599	if (FLAG_VALVE_OUTPUT_CALIB) {
Lightvalve	13:747daba9cf59	1600	// Valve Dead Zone (Mechanical dead-zone canceling)
Lightvalve	14:8e7590227d22	1601	// if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) {
Lightvalve	14:8e7590227d22	1602	// if (V_out > 0.) {
Lightvalve	14:8e7590227d22	1603	// VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_PLUS;
Lightvalve	14:8e7590227d22	1604	// } else if (V_out < 0.) {
Lightvalve	14:8e7590227d22	1605	// VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_MINUS;
Lightvalve	14:8e7590227d22	1606	// } else VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_CENTER;
Lightvalve	14:8e7590227d22	1607	// } else VALVE_PWM_VALVE_DZ = (int) V_out;
Lightvalve	14:8e7590227d22	1608
Lightvalve	14:8e7590227d22	1609	if(V_out>0) V_out = V_out + VALVE_DEADZONE_PLUS;
Lightvalve	14:8e7590227d22	1610	else if(V_out<0) V_out = V_out + VALVE_DEADZONE_MINUS;
Lightvalve	14:8e7590227d22	1611
Lightvalve	14:8e7590227d22	1612	VALVE_PWM_VALVE_DZ = V_out + VALVE_CENTER;
Lightvalve	14:8e7590227d22	1613
Lightvalve	13:747daba9cf59	1614	// Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone)
Lightvalve	18:b8adf1582ea3	1615	double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/5.0*1000.0; // convert PWM duty to mV
Lightvalve	13:747daba9cf59	1616	double CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin);
Lightvalve	14:8e7590227d22	1617
Lightvalve	14:8e7590227d22	1618	if (CUR_PWM_DZ > 0) V_out = (int)CUR_PWM_DZ + 143;
Lightvalve	14:8e7590227d22	1619	else if (CUR_PWM_DZ < 0) V_out = (int)CUR_PWM_DZ - 138;
Lightvalve	14:8e7590227d22	1620	else V_out = CUR_PWM_DZ;
Lightvalve	13:747daba9cf59	1621	} else {
Lightvalve	17:1865016ca2e7	1622	V_out = V_out;
Lightvalve	13:747daba9cf59	1623	}
Lightvalve	14:8e7590227d22	1624
jobuuu	7:e9086c72bb22	1625	/*******************************************************
jobuuu	7:e9086c72bb22	1626	*** PWM
jobuuu	7:e9086c72bb22	1627	********************************************************/
Lightvalve	18:b8adf1582ea3	1628	PWM_out= V_out/SUPPLY_VOLTAGE; // Full duty : 12000.0mV
Lightvalve	13:747daba9cf59	1629
jobuuu	2:a1c0a37df760	1630	// Saturation of output voltage to 5.0V
Lightvalve	17:1865016ca2e7	1631	// if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667
Lightvalve	17:1865016ca2e7	1632	// else if (PWM_out < -0.41667) PWM_out=-0.41667;
Lightvalve	19:23b7c1ad8683	1633
Lightvalve	19:23b7c1ad8683	1634	// Saturation of output voltage to 12.0V
Lightvalve	17:1865016ca2e7	1635	if(PWM_out > 1.0) PWM_out=1.0;
Lightvalve	17:1865016ca2e7	1636	else if (PWM_out < -1.0) PWM_out=-1.0;
Lightvalve	13:747daba9cf59	1637
jobuuu	2:a1c0a37df760	1638	if (PWM_out>0.0) {
jobuuu	2:a1c0a37df760	1639	dtc_v=0.0;
jobuuu	1:e04e563be5ce	1640	dtc_w=PWM_out;
jobuuu	1:e04e563be5ce	1641	} else {
jobuuu	2:a1c0a37df760	1642	dtc_v=-PWM_out;
jobuuu	2:a1c0a37df760	1643	dtc_w=0.0;
jobuuu	1:e04e563be5ce	1644	}
Lightvalve	13:747daba9cf59	1645
jobuuu	1:e04e563be5ce	1646	//pwm
jobuuu	2:a1c0a37df760	1647	TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v);
jobuuu	2:a1c0a37df760	1648	TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w);
Lightvalve	13:747daba9cf59	1649
jobuuu	1:e04e563be5ce	1650	}
Lightvalve	11:82d8768d7351	1651	TIM3->SR = 0x0; // reset the status register
Lightvalve	21:e5f1a43ea6f9	1652
jobuuu	7:e9086c72bb22	1653	}
jobuuu	7:e9086c72bb22	1654
Lightvalve	20:806196fda269	1655
Lightvalve	20:806196fda269	1656
Lightvalve	20:806196fda269	1657	//unsigned long CNT_TMR5 = 0;
Lightvalve	20:806196fda269	1658	//double FREQ_TMR5 = (double)FREQ_500;
Lightvalve	20:806196fda269	1659	//double DT_TMR5 = (double)DT_500;
Lightvalve	23:59218d4a256d	1660	extern "C" void TIM2_IRQHandler(void)
Lightvalve	21:e5f1a43ea6f9	1661	{
Lightvalve	23:59218d4a256d	1662	if (TIM2->SR & TIM_SR_UIF ) {
Lightvalve	21:e5f1a43ea6f9	1663
Lightvalve	20:806196fda269	1664	//CAN ----------------------------------------------------------------------
Lightvalve	20:806196fda269	1665	if (flag_data_request[0] == HIGH) {
Lightvalve	20:806196fda269	1666	//position+velocity
Lightvalve	20:806196fda269	1667	CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
Lightvalve	20:806196fda269	1668	//CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
Lightvalve	20:806196fda269	1669	//CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
Lightvalve	20:806196fda269	1670	//pc.printf("can good");
Lightvalve	20:806196fda269	1671	// CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
Lightvalve	20:806196fda269	1672	}
Lightvalve	20:806196fda269	1673
Lightvalve	20:806196fda269	1674	if (flag_data_request[1] == HIGH) {
Lightvalve	20:806196fda269	1675	//torque
Lightvalve	20:806196fda269	1676	//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
Lightvalve	20:806196fda269	1677	//CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
Lightvalve	20:806196fda269	1678	CAN_TX_TORQUE((int16_t) (cur.sen));
Lightvalve	20:806196fda269	1679	//CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN));
Lightvalve	20:806196fda269	1680	// CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
Lightvalve	20:806196fda269	1681	}
Lightvalve	20:806196fda269	1682
Lightvalve	20:806196fda269	1683	if (flag_data_request[2] == HIGH) {
Lightvalve	20:806196fda269	1684	//pressure A and B
Lightvalve	20:806196fda269	1685	//CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
Lightvalve	20:806196fda269	1686	CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
Lightvalve	20:806196fda269	1687	// CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
Lightvalve	20:806196fda269	1688	// CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)1000.), (int16_t) ((DEADZONE_PLUS + 1.))1000.);
Lightvalve	20:806196fda269	1689	// CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
Lightvalve	20:806196fda269	1690
Lightvalve	20:806196fda269	1691	}
Lightvalve	20:806196fda269	1692
Lightvalve	20:806196fda269	1693	if (flag_data_request[3] == HIGH) {
Lightvalve	20:806196fda269	1694	//PWM
Lightvalve	20:806196fda269	1695	CAN_TX_PWM((int16_t) CUR_PWM);
Lightvalve	20:806196fda269	1696	//CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN));
Lightvalve	20:806196fda269	1697	// CAN_TX_PWM((int16_t) cnt_vel_findhome);
Lightvalve	20:806196fda269	1698	// CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
Lightvalve	20:806196fda269	1699	// CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
Lightvalve	20:806196fda269	1700
Lightvalve	20:806196fda269	1701	}
Lightvalve	20:806196fda269	1702
Lightvalve	20:806196fda269	1703	if (flag_data_request[4] == HIGH) {
Lightvalve	20:806196fda269	1704	//valve position
Lightvalve	20:806196fda269	1705	CAN_TX_VALVE_POSITION((int16_t) (value));
Lightvalve	20:806196fda269	1706	//SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
Lightvalve	20:806196fda269	1707	//SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
Lightvalve	20:806196fda269	1708
Lightvalve	20:806196fda269	1709
Lightvalve	20:806196fda269	1710	//CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
Lightvalve	20:806196fda269	1711	// CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));
Lightvalve	20:806196fda269	1712	// CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
Lightvalve	20:806196fda269	1713	// CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
Lightvalve	20:806196fda269	1714	// CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
Lightvalve	20:806196fda269	1715	}
Lightvalve	20:806196fda269	1716
Lightvalve	20:806196fda269	1717	}
Lightvalve	23:59218d4a256d	1718	TIM2->SR = 0x0; // reset the status register
Lightvalve	20:806196fda269	1719	}
Lightvalve	20:806196fda269	1720
Lightvalve	20:806196fda269	1721
Lightvalve	20:806196fda269	1722
Lightvalve	13:747daba9cf59	1723	void CurrentControl()
Lightvalve	13:747daba9cf59	1724	{
jobuuu	7:e9086c72bb22	1725	cur.err = cur.ref - cur.sen;
jobuuu	7:e9086c72bb22	1726	cur.err_int = cur.err_int + cur.err*DT_TMR4;
jobuuu	7:e9086c72bb22	1727	cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
jobuuu	7:e9086c72bb22	1728	cur.err_old = cur.err;
jobuuu	7:e9086c72bb22	1729
jobuuu	7:e9086c72bb22	1730	double R_model = 150.0; // ohm
jobuuu	7:e9086c72bb22	1731	double L_model = 0.3;
jobuuu	7:e9086c72bb22	1732	double w0 = 2.03.1490.0;
jobuuu	7:e9086c72bb22	1733	double KP_I = L_model*w0;
jobuuu	7:e9086c72bb22	1734	double KI_I = R_model*w0;
jobuuu	7:e9086c72bb22	1735	double KD_I = 0.0;
jobuuu	7:e9086c72bb22	1736
jobuuu	7:e9086c72bb22	1737	double FF_gain = 0.0;
jobuuu	7:e9086c72bb22	1738	V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
jobuuu	7:e9086c72bb22	1739	// V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
jobuuu	7:e9086c72bb22	1740	V_out = V_out + FF_gain * (R_modelcur.ref + L_modelcur.ref_diff); // Unit : mV
Lightvalve	13:747daba9cf59	1741
Lightvalve	13:747daba9cf59	1742	double Ka = 5.0/KP_I;
jobuuu	7:e9086c72bb22	1743	if(V_out > V_MAX) {
jobuuu	7:e9086c72bb22	1744	V_rem = V_out-V_MAX;
jobuuu	7:e9086c72bb22	1745	V_rem = Ka*V_rem;
jobuuu	7:e9086c72bb22	1746	V_out = V_MAX;
jobuuu	7:e9086c72bb22	1747	cur.err_int = cur.err_int - V_rem*DT_5k;
jobuuu	7:e9086c72bb22	1748	} else if(V_out < -V_MAX) {
jobuuu	7:e9086c72bb22	1749	V_rem = V_out-(-V_MAX);
Lightvalve	13:747daba9cf59	1750	V_rem = Ka*V_rem;
jobuuu	7:e9086c72bb22	1751	V_out = -V_MAX;
jobuuu	7:e9086c72bb22	1752	cur.err_int = cur.err_int - V_rem*DT_5k;
Lightvalve	13:747daba9cf59	1753	}
Lightvalve	21:e5f1a43ea6f9	1754	}
Lightvalve	23:59218d4a256d	1755

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	31 May 2021
Imports:	0
Forks:	0
Commits:	263
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@24:ef6e1092e9e6, 2019-09-26 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning