HydraulicControlBoard_Rainbow_v1_2_copy1 - 2011

Users » Lightvalve » Code » HydraulicControlBoard_Rainbow_v1_2_copy1

Sungwoo Kim / Mbed 2 deprecated HydraulicControlBoard_Rainbow_v1_2_copy1

2011

main.cpp@18:b8adf1582ea3, 2019-09-04 (annotated)

Committer:: Lightvalve
Date:: Wed Sep 04 11:02:57 2019 +0000
Revision:: 18:b8adf1582ea3
Parent:: 17:1865016ca2e7
Child:: 19:23b7c1ad8683

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

main.cpp@18:b8adf1582ea3, 2019-09-04 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning