HydraulicControlBoard_LIGHT_GJ - eeprom_test

Users » GiJeongKim » Code » HydraulicControlBoard_LIGHT_GJ

Kim GiJeong / Mbed 2 deprecated HydraulicControlBoard_LIGHT_GJ

eeprom_test

main.cpp@17:1865016ca2e7, 2019-09-03 (annotated)

Committer:: Lightvalve
Date:: Tue Sep 03 11:59:55 2019 +0000
Revision:: 17:1865016ca2e7
Parent:: 16:903b5a4433b4
Child:: 18:b8adf1582ea3

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	20 Jul 2020
Imports:	1
Forks:	0
Commits:	69
Dependents:	0
Dependencies:	2
Followers:	1

main.cpp@17:1865016ca2e7, 2019-09-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning