![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
For LIGHT
Diff: main.cpp
- Revision:
- 14:8e7590227d22
- Parent:
- 13:747daba9cf59
- Child:
- 15:bd0d12728506
diff -r 747daba9cf59 -r 8e7590227d22 main.cpp --- a/main.cpp Tue Aug 27 06:50:54 2019 +0000 +++ b/main.cpp Thu Aug 29 07:38:00 2019 +0000 @@ -12,7 +12,9 @@ DigitalOut check_2(PC_3); AnalogOut dac_1(PA_4); AnalogOut dac_2(PA_5); -//AnalogIn adc3(PC_1); +AnalogIn adc1(PC_4); //pressure_1 +AnalogIn adc2(PB_0); //pressure_2 +AnalogIn adc3(PC_1); //current // PWM /////////////////////////////////////////// double dtc_v=0.0; @@ -49,6 +51,12 @@ State pres_A; State pres_B; State cur; +State valve_pos; + +State INIT_Vout; +State INIT_Valve_Pos; +State INIT_Pos; +State INIT_torq; double V_out=0.0; double V_rem=0.0; // for anti-windup @@ -92,6 +100,10 @@ MODE_TEST_CURRENT_CONTROL, //9 MODE_TEST_PWM_CONTROL, //10 + + MODE_CURRENT_CONTROL, //11 + MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT, //12 + MODE_JOINT_POSITION_PRES_CONTROL_CURRENT, //13 //utility MODE_TORQUE_SENSOR_NULLING = 20, //20 @@ -115,6 +127,10 @@ LED = 1; pc.baud(9600); + + //eeprom + ROM_INIT_DATA(); + make_delay(); // i2c init i2c.frequency(400 * 1000); // 0.4 mHz @@ -143,7 +159,7 @@ Init_TMR3(); TIM3->CR1 ^= TIM_CR1_UDIS; make_delay(); - + // CAN can.attach(&CAN_RX_HANDLER); CAN_ID_INIT(); @@ -153,12 +169,11 @@ spi_enc_set_init(); make_delay(); - //eeprom - ROM_INIT_DATA(); - make_delay(); + //DAC init - dac_1 = PRES_A_VREF/3.3; + //dac_1 = PRES_A_VREF/3.3; + dac_1 = 0.0; dac_2 = PRES_B_VREF/3.3; make_delay(); @@ -177,44 +192,39 @@ //spi _ enc //int a = spi_enc_read(); - + //pc.printf("Message received: %d\n", msg.data[0]); + //pc.printf("Message received: %d\n", 13); //i2c - read_field(i2c_slave_addr1); + //read_field(i2c_slave_addr1); } } -int DDV_JOINT_POS_FF(double REF_JOINT_VEL){ - +int DDV_JOINT_POS_FF(double REF_JOINT_VEL) +{ + int i = 0; int Ref_Valve_Pos_FF = DDV_CENTER; - for(i=0; i<VALVE_POS_NUM; i++) - { - if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) - { - if(i==0) - { + for(i=0; i<VALVE_POS_NUM; i++) { + if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <= max(JOINT_VEL[i],JOINT_VEL[i+1])) { + if(i==0) { Ref_Valve_Pos_FF = ((int) 50.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER; - } - else - { + } else { 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]; } break; } } - if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) - { + if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) { Ref_Valve_Pos_FF = VALVE_MAX_POS; - } - else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) - { + } else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) { Ref_Valve_Pos_FF = VALVE_MIN_POS; } return Ref_Valve_Pos_FF; } -void VALVE_POS_CONTROL(double REF_VALVE_POS){ +void VALVE_POS_CONTROL(double REF_VALVE_POS) +{ int i = 0; valve_pos_err = REF_VALVE_POS - value; @@ -225,36 +235,44 @@ if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000; 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; - - if(REF_VALVE_POS > VALVE_MAX_POS) - { + + if(REF_VALVE_POS > VALVE_MAX_POS) { REF_VALVE_POS = VALVE_MAX_POS; - } - else if(REF_VALVE_POS < VALVE_MIN_POS) - { + } else if(REF_VALVE_POS < VALVE_MIN_POS) { REF_VALVE_POS = VALVE_MIN_POS; } - - for(i=0; i<16; i++) - { - 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])) - { - if(i==0) - { + + for(i=0; i<16; i++) { + 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])) { + if(i==0) { VALVE_PWM_RAW_FF = (double) 1000.0/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * (REF_VALVE_POS - VALVE_POS_VS_PWM[i]); - } - else - { + } else { VALVE_PWM_RAW_FF = (double) 1000.0*(ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * (REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000*ID_index_array[i-1]; } break; } } - VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; + V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; } +#define LT_MAX_IDX 57 +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, + -19.0, -18.0, -17.0, -16.0, -15.0, -14.0, -13.0, -12.0, -11.0, -10.0, + -9.0, -8.0, -7.0, -6.0, -5.0, -4.0, -3.0, -2.0, -1.0, 0.0, + 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, + 11.0, 12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, + 25.0, 30.0, 35.0, 40.0, 50.0, 60.0, 80.0, 100.0 + }; // duty +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, + -196.9, -195.0, -192.5, -188.8, -184.5, -180.2, -175.9, -171.5, -166.3, -161.0, + -156.0, -149.5, -139.0, -126.0, -107.0, -87.5, -64.0, -38.5, -9.4, 0.0, + 12.0, 43.5, 69.0, 94.0, 114.0, 132.0, 146.0, 155.5, 162.3, 168.2, + 173.1, 178.2, 182.8, 187.4, 191.8, 196.0, 199.7, 201.9, 203.8, 205.6, + 214.6, 222.5, 230.4, 238.2, 253.3, 268.0, 297.6, 327.7 + }; // mV -double PWM_duty_byLT(double Ref_V) { +double PWM_duty_byLT(double Ref_V) +{ double PWM_duty = 0.0; if(Ref_V<LT_Voltage_Output[0]) { PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5+LT_PWM_duty[0]; @@ -273,7 +291,7 @@ } } } - + return PWM_duty; } @@ -293,24 +311,32 @@ if((CNT_TMR4%2)==0) { - //ADC + //Pressure sensor A + ADC1->CR2 |= 0x40000000; // adc _ 12bit + //while((ADC1->SR & 0b10)); + double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); + double pres_A_new = ((double)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.0; + pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A); + //pres_A.sen = (double)ADC1->DR; +/* + //Pressure sensor 1B + //ADC2->CR2 |= 0x40000000; // adc _ 12bit + while((ADC2->SR & 0b10)); + double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); + double pres_B_new = ((double)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.0; + pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B); + + //Current ADC3->CR2 |= 0x40000000; // adc _ 12bit -// a1=ADC1->DR; // a1=ADC2->DR; // int raw_cur = ADC3->DR; while((ADC3->SR & 0b10)); - double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur); +*/ } - //DAC -// dac_1 = ADC1->DR; -// dac_2 = ADC2->DR; - - - /******************************************************* *** Timer Counting & etc. ********************************************************/ @@ -325,55 +351,54 @@ extern "C" void TIM3_IRQHandler(void) { if ( TIM3->SR & TIM_SR_UIF ) { - + ENC_UPDATE(); - CUR_PRES_A_BAR = (CUR_PRES_A - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.; - CUR_PRES_B_BAR = (CUR_PRES_B - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.; - //CUR_TORQUE_NM = (CUR_TORQUE - TORQUE_NULL) / (double) TORQUE_SENSOR_PULSE_PER_TORQUE; // - CUR_TORQUE_NM = CUR_TORQUE; // - CUR_TORQUE_NM_PRESS = 1. * (CUR_PRES_A_BAR - CUR_PRES_B_BAR); + /* // Reference Loop switch (REFERENCE_MODE) { case MODE_REF_NO_ACT: { break; } - case MODE_REF_DIRECT: { if (FLAG_REFERENCE_VALVE_PWM) { - Ref_PWM = (double) REF_PWM; + Vout.ref = (double) Vout.ref; } if (FLAG_REFERENCE_VALVE_POSITION) { - Ref_Valve_Pos = (double) REF_VALVE_POSITION; + valve_pos.ref = (double) valve_pos.ref; } if (FLAG_REFERENCE_JOINT_POSITION) { - Ref_Joint_Pos = (double) REF_POSITION; - Ref_Joint_Vel = (double) REF_VELOCITY; + pos.ref = (double) pos.ref; + vel.ref = (double) vel.ref; } if (FLAG_REFERENCE_JOINT_TORQUE) { - Ref_Joint_Torq = (double) REF_TORQUE; + torq.ref = (double) torq.ref; + } + if (FLAG_REFERENCE_CURRENT) { + cur.ref = ((double)cur.ref); } break; } - + case MODE_REF_COS_INC: { if (FLAG_REFERENCE_VALVE_PWM) { - Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_PWM; + 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; } if (FLAG_REFERENCE_VALVE_POSITION) { - Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_VALVE_POS; + 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; } if (FLAG_REFERENCE_JOINT_POSITION) { - Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_POS; - Ref_Joint_Vel = 0.0; + 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; + vel.ref = 0.0; } if (FLAG_REFERENCE_JOINT_TORQUE) { - Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*(0.5 - 0.5 * cos(3.14159 * (double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k)) + (double) INIT_REF_TORQUE; + 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; } - TMR2_COUNT_REFERENCE++; - if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) { - REFERENCE_MODE = MODE_REF_DIRECT; - TMR2_COUNT_REFERENCE = 0; + TMR3_COUNT_REFERENCE++; + if (TMR3_COUNT_REFERENCE >= REF_MOVE_TIME_5k) { + //REFERENCE_MODE = MODE_REF_DIRECT; + TMR3_COUNT_REFERENCE = REF_MOVE_TIME_5k; + //TMR3_COUNT_REFERENCE = 0; } break; } @@ -381,217 +406,209 @@ case MODE_REF_LINE_INC: { if (FLAG_REFERENCE_VALVE_PWM) { - Ref_PWM = ((double) REF_PWM - (double) INIT_REF_PWM)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_PWM; + Vout.ref = ((double) Vout.ref - (double) INIT_Vout.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_Vout.ref; } if (FLAG_REFERENCE_VALVE_POSITION) { - Ref_Valve_Pos = ((double) REF_VALVE_POSITION - (double) INIT_REF_VALVE_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_VALVE_POS; + 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; } if (FLAG_REFERENCE_JOINT_POSITION) { - Ref_Joint_Pos = ((double) REF_POSITION - (double) INIT_REF_POS)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS; - Ref_Vel_Test = ((double) REF_POSITION - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k; // pulse/sec - //Ref_Vel_Test = 10; + pos.ref = ((double) pos.ref - (double) INIT_REF_POS)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_POS; + vel.ref = ((double) pos.ref - (double) INIT_REF_POS) / (double) REF_MOVE_TIME_5k * (double) TMR_FREQ_5k; // pulse/sec } if (FLAG_REFERENCE_JOINT_TORQUE) { - Ref_Joint_Torq = ((double) REF_TORQUE - (double) INIT_REF_TORQUE)*((double) TMR2_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_REF_TORQUE; + torq.ref = ((double) torq.ref - (double) INIT_torq.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_torq.ref; } - TMR2_COUNT_REFERENCE++; - if (TMR2_COUNT_REFERENCE >= REF_MOVE_TIME_5k) { + TMR3_COUNT_REFERENCE++; + if (TMR3_COUNT_REFERENCE >= REF_MOVE_TIME_5k) { //REFERENCE_MODE = MODE_REF_DIRECT; - Ref_Vel_Test = 1; - TMR2_COUNT_REFERENCE = REF_MOVE_TIME_5k; - //TMR2_COUNT_REFERENCE = 0; + TMR3_COUNT_REFERENCE = REF_MOVE_TIME_5k; + //TMR3_COUNT_REFERENCE = 0; } break; } case MODE_REF_SIN_WAVE: { if (FLAG_REFERENCE_VALVE_PWM) { - Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM; + Vout.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) Vout.ref; } if (FLAG_REFERENCE_VALVE_POSITION) { - Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION; + valve_pos.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) valve_pos.ref; } if (FLAG_REFERENCE_JOINT_POSITION) { - Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION; + Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) pos.ref; } if (FLAG_REFERENCE_JOINT_TORQUE) { - Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE; + Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) torq.ref; } - TMR2_COUNT_REFERENCE++; - if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) { + TMR3_COUNT_REFERENCE++; + if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) { REFERENCE_MODE = MODE_REF_DIRECT; - TMR2_COUNT_REFERENCE = 0; + TMR3_COUNT_REFERENCE = 0; } break; } case MODE_REF_SQUARE_WAVE: { if (FLAG_REFERENCE_VALVE_PWM) { - Ref_PWM = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_PWM; - if (Ref_PWM >= REF_PWM) Ref_PWM = REF_MAG + REF_PWM; - else Ref_PWM = -REF_MAG + REF_PWM; + Vout.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) Vout.ref; + if (Vout.ref >= Vout.ref) Vout.ref = REF_MAG + Vout.ref; + else Vout.ref = -REF_MAG + Vout.ref; } if (FLAG_REFERENCE_VALVE_POSITION) { - Ref_Valve_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_VALVE_POSITION; - if (Ref_Valve_Pos >= REF_VALVE_POSITION) Ref_Valve_Pos = REF_MAG + REF_VALVE_POSITION; - else Ref_Valve_Pos = -REF_MAG + REF_VALVE_POSITION; + valve_pos.ref = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) valve_pos.ref; + if (valve_pos.ref >= valve_pos.ref) valve_pos.ref = REF_MAG + valve_pos.ref; + else valve_pos.ref = -REF_MAG + valve_pos.ref; } if (FLAG_REFERENCE_JOINT_POSITION) { - Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_POSITION; - if (Ref_Joint_Pos >= REF_POSITION) Ref_Valve_Pos = REF_MAG + REF_POSITION; - else Ref_Joint_Pos = -REF_MAG + REF_POSITION; + Ref_Joint_Pos = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) pos.ref; + if (Ref_Joint_Pos >= pos.ref) valve_pos.ref = REF_MAG + pos.ref; + else Ref_Joint_Pos = -REF_MAG + pos.ref; } if (FLAG_REFERENCE_JOINT_TORQUE) { - Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR2_COUNT_REFERENCE / (double) TMR2_COUNT_REFERENCE) + (double) REF_TORQUE; - if (Ref_Joint_Torq >= REF_TORQUE) Ref_Valve_Pos = REF_MAG + REF_TORQUE; - else Ref_Joint_Torq = -REF_MAG + REF_TORQUE; - } - TMR2_COUNT_REFERENCE++; - if (TMR2_COUNT_REFERENCE >= TMR2_COUNT_REFERENCE * REF_NUM) { - REFERENCE_MODE = MODE_REF_DIRECT; - TMR2_COUNT_REFERENCE = 0; + Ref_Joint_Torq = REF_MAG * sin(2 * 3.14159 * (double) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) torq.ref; + if (Ref_Joint_Torq >= torq.ref) valve_pos.ref = REF_MAG + torq.ref; + else Ref_Joint_Torq = -REF_MAG + torq.ref; } - break; - } - - default: - break; - } - - // CONTROL LOOP ------------------------------------------------------------ - switch (CONTROL_MODE) { - case MODE_NO_ACT: - { - // SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); - VALVE_PWM_RAW = 0; - break; - } - - case MODE_VALVE_OPEN_LOOP: - { - VALVE_PWM_RAW = Ref_PWM; + TMR3_COUNT_REFERENCE++; + if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) { + REFERENCE_MODE = MODE_REF_DIRECT; + TMR3_COUNT_REFERENCE = 0; + } break; } - case MODE_VALVE_POSITION_CONTROL: - { - VALVE_POS_CONTROL(Ref_Valve_Pos); + default: + break; + } + + */ + + // CONTROL LOOP ------------------------------------------------------------ + switch (CONTROL_MODE) { + case MODE_NO_ACT: { + // SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); + V_out = 0; break; } - - case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: - { + + case MODE_VALVE_OPEN_LOOP: { + V_out = Vout.ref; + break; + } + + case MODE_VALVE_POSITION_CONTROL: { + VALVE_POS_CONTROL(valve_pos.ref); + break; + } + + case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: { double PWM_RAW_POS_FB = 0.0; // PWM by Position Feedback double PWM_RAW_POS_FF = 0.0; // PWM by Position Feedforward double PWM_RAW_FORCE_FB = 0.0; // PWM by Force Feedback - + // feedback input for position control - joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION; - joint_pos_err_diff = joint_pos_err - joint_pos_err_old; - joint_pos_err_old = joint_pos_err; - joint_pos_err_sum += joint_pos_err; - if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000; - if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000; - // PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff; - PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * joint_pos_err; + pos.err = pos.ref - (double) pos.sen; + pos.err_diff = pos.err - pos.err_old; + pos.err_old = pos.err; + pos.err_sum += pos.err; + if (pos.err_sum > 1000) pos.err_sum = 1000; + if (pos.err_sum<-1000) pos.err_sum = -1000; + // 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; + PWM_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * pos.err; PWM_RAW_POS_FB = PWM_RAW_POS_FB * 0.01; - + // feedforward input for position control - double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] + double Ref_Vel_Act = vel.ref/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] double K_ff = 0.9; - if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open - if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close - PWM_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50; - + if(Ref_Vel_Act > 0) K_ff = 0.90; // open + if(Ref_Vel_Act > 0) K_ff = 0.75; // close + PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50; + // torque feedback - // joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM; - // joint_torq_err_diff = joint_torq_err - joint_torq_err_old; - // joint_torq_err_old = joint_torq_err; - // joint_torq_err_sum += joint_torq_err; - // if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000; - // if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000; - // VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff; + // torq.err = Ref_Joint_Torq - CUR_TORQUE_NM; + // torq.err_diff = torq.err - torq.err_old; + // torq.err_old = torq.err; + // torq.err_sum += torq.err; + // if (torq.err_sum > 1000) torq.err_sum = 1000; + // if (torq.err_sum<-1000) torq.err_sum = -1000; + // 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; // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01; PWM_RAW_FORCE_FB = 0.0; - - // VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ; - VALVE_PWM_RAW = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB; - + + // VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ; + V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB; + break; } - - case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: - { - TMR2_COUNT_JOINT++; + + case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: { double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback int DDV_JOINT_CAN = 0; // feedback input for position control - joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION; - joint_pos_err_diff = joint_pos_err - joint_pos_err_old; - joint_pos_err_old = joint_pos_err; - joint_pos_err_sum += joint_pos_err; - if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000; - if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000; - VALVE_POS_RAW_POS_FB = (double) P_GAIN_JOINT_POSITION * 0.01 * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff; + pos.err = pos.ref - (double) pos.sen; + pos.err_diff = pos.err - pos.err_old; + pos.err_old = pos.err; + pos.err_sum += pos.err; + if (pos.err_sum > 1000) pos.err_sum = 1000; + if (pos.err_sum<-1000) pos.err_sum = -1000; + 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; VALVE_POS_RAW_POS_FB = VALVE_POS_RAW_POS_FB * 0.01; - - + + //Ref_Joint_Vel = Ref_Vel_Test; // feedforward input for position control - // double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] - // double K_ff = 0.9; - // if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open - // if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close - // VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50; - + // double Ref_Joint_Vel_Act = Ref_Joint_Vel/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] + // double K_ff = 0.9; + // if(Ref_Joint_Vel_Act > 0) K_ff = 0.90; // open + // if(Ref_Joint_Vel_Act > 0) K_ff = 0.75; // close + // VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50; + // torque feedback - // joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM; - // joint_torq_err_diff = joint_torq_err - joint_torq_err_old; - // joint_torq_err_old = joint_torq_err; - // joint_torq_err_sum += joint_torq_err; - // if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000; - // if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000; - // VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff; + // torq.err = Ref_Joint_Torq - CUR_TORQUE_NM; + // torq.err_diff = torq.err - torq.err_old; + // torq.err_old = torq.err; + // torq.err_sum += torq.err; + // if (torq.err_sum > 1000) torq.err_sum = 1000; + // if (torq.err_sum<-1000) torq.err_sum = -1000; + // 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; // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01; VALVE_POS_RAW_FORCE_FB = 0.0; - - Ref_Valve_Pos = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(Ref_Joint_Vel) + VALVE_POS_RAW_FORCE_FB; - //Ref_Valve_Pos = DDV_JOINT_POS_FF(Ref_Joint_Vel); - - VALVE_POS_CONTROL(Ref_Valve_Pos); + + valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB; + //valve_pos.ref = DDV_JOINT_POS_FF(Ref_Joint_Vel); + + VALVE_POS_CONTROL(valve_pos.ref); break; } - - case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: - { - + + case MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING: { + break; } - - case MODE_JOINT_POSITION_PRES_CONTROL_PWM: - { - joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION; - joint_pos_err_diff = joint_pos_err - joint_pos_err_old; - joint_pos_err_old = joint_pos_err; - joint_pos_err_sum += joint_pos_err; - if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000; - if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000; - VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff) * 0.01; - - joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM; - joint_torq_err_diff = joint_torq_err - joint_torq_err_old; - joint_torq_err_old = joint_torq_err; - joint_torq_err_sum += joint_torq_err; - if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000; - if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000; - VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff; - + + case MODE_JOINT_POSITION_PRES_CONTROL_PWM: { + pos.err = pos.ref - (double) pos.sen; + pos.err_diff = pos.err - pos.err_old; + pos.err_old = pos.err; + pos.err_sum += pos.err; + if (pos.err_sum > 1000) pos.err_sum = 1000; + if (pos.err_sum<-1000) pos.err_sum = -1000; + 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; + + torq.err = torq.ref - torq.sen; + torq.err_diff = torq.err - torq.err_old; + torq.err_old = torq.err; + torq.err_sum += torq.err; + if (torq.err_sum > 1000) torq.err_sum = 1000; + if (torq.err_sum<-1000) torq.err_sum = -1000; + 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; + VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01; - - VALVE_PWM_RAW = VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ; - + + V_out = VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ; + CUR_FLOWRATE = (double) CUR_VELOCITY * 0.00009587; CUR_FLOWRATE = CUR_FLOWRATE * 0.5757; // 0.4791=2*pi/65536*5000(pulse/tic to rad/s) 0.5757=0.02525*0.02*0.0095*2*60*1000 (radius * area * 2 * 60(sec --> min) * 1000(m^3 --> L)) if (DIR_VALVE > 0) { @@ -624,126 +641,288 @@ else VALVE_FF_VOLTAGE = 0; } // VALVE_FF_VOLTAGE = CUR_FLOWRATE * 0.5; - + 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.)) 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; + + V_out = V_out + VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE; + break; + } + + case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: { + pos.err = pos.ref - (double) pos.sen; + pos.err_diff = pos.err - pos.err_old; + pos.err_old = pos.err; + pos.err_sum += pos.err; + if (pos.err_sum > 1000) pos.err_sum = 1000; + if (pos.err_sum<-1000) pos.err_sum = -1000; + 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; + + torq.err = torq.ref - (double) torq.sen; + torq.err_diff = torq.err - torq.err_old; + torq.err_old = torq.err; + torq.err_sum += torq.err; + if (torq.err_sum > 1000) torq.err_sum = 1000; + if (torq.err_sum<-1000) torq.err_sum = -1000; + 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; + + valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ; + VALVE_POS_CONTROL(valve_pos.ref); - VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE; + break; + } + + case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: { + + break; + } + + + case MODE_TEST_CURRENT_CONTROL: + { + if (TMR3_COUNT_IREF == TMR_FREQ_5k) { + TMR3_COUNT_IREF = 0; + } TMR3_COUNT_IREF++; + + // Set Current Reference + double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0; + double I_REF_MID = 0.0; + if (TMR3_COUNT_IREF < TMR3_CNT_MAX) { + I_REF = I_REF_MID + 1.0; + } else { + I_REF = I_REF_MID - 1.0; + } +// double T = 1.0; // wave period +// I_REF = (5. * sin(2. * 3.1415 * (double) TMR3_COUNT_IREF / (double)TMR_FREQ_5k/ T)); +// 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.))); + + if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { + CAN_TX_PRES((int16_t)(I_REF*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas + } + break; + } + + case MODE_TEST_PWM_CONTROL: + { + if (TMR3_COUNT_IREF == TMR_FREQ_5k) { + TMR3_COUNT_IREF = 0; + } TMR3_COUNT_IREF++; + + // Set PWM reference + double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0; + double I_REF_MID = 0.0; + if (TMR3_COUNT_IREF < TMR3_CNT_MAX) { + CUR_PWM = 1000; + } else { + CUR_PWM = -1000; + } + + if (TMR3_COUNT_IREF % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { + CAN_TX_PRES((int16_t)(u_CUR[0]*1000.0), (int16_t) (CUR_CURRENT*1000.0)); // to check the datas + } break; } - case MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION: - { - joint_pos_err = Ref_Joint_Pos - (double) CUR_POSITION; - joint_pos_err_diff = joint_pos_err - joint_pos_err_old; - joint_pos_err_old = joint_pos_err; - joint_pos_err_sum += joint_pos_err; - if (joint_pos_err_sum > 1000) joint_pos_err_sum = 1000; - if (joint_pos_err_sum<-1000) joint_pos_err_sum = -1000; - VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * joint_pos_err + (double) I_GAIN_JOINT_POSITION * joint_pos_err_sum + (double) D_GAIN_JOINT_POSITION * joint_pos_err_diff) * 0.01; + + case MODE_CURRENT_CONTROL: { + + cur.ref = cur.ref; // Unit : mA + CurrentControl(); + break; + } + + case MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT: { + double I_REF_POS_FB = 0.0; // I_REF by Position Feedback + double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward + double I_REF_FORCE_FB = 0.0; // I_REF by Force Feedback + double I_REF_FORCE_FF = 0.0; // I_REF by Force Feedforward + + // feedback input for position control + pos.err = pos.ref - pos.sen; + double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz + double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k; + pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff; + pos.err_old = pos.err; + I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1); + + // feedforward input for position control + double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s] + double K_ff = 1.3; + double K_v = 0.0; + if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA) + if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA) + I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref; - joint_torq_err = Ref_Joint_Torq - CUR_TORQUE_NM; - joint_torq_err_diff = joint_torq_err - joint_torq_err_old; - joint_torq_err_old = joint_torq_err; - joint_torq_err_sum += joint_torq_err; - if (joint_torq_err_sum > 1000) joint_torq_err_sum = 1000; - if (joint_torq_err_sum<-1000) joint_torq_err_sum = -1000; - VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * joint_torq_err + (double) I_GAIN_JOINT_TORQUE * joint_torq_err_sum + (double) D_GAIN_JOINT_TORQUE * joint_torq_err_diff; + // feedback input for position control + I_REF_FORCE_FB = 0.0; - Ref_Valve_Pos = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ; + // feedforward input for position control + I_REF_FORCE_FF = 0.0; + + cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF; - valve_pos_err = Ref_Valve_Pos - value; - valve_pos_err_diff = valve_pos_err - valve_pos_err_old; - valve_pos_err_old = valve_pos_err; - valve_pos_err_sum += valve_pos_err; - if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000; - if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000; - - VALVE_PWM_RAW = (double) P_GAIN_VALVE_POSITION * valve_pos_err + (double) I_GAIN_VALVE_POSITION * valve_pos_err_sum + (double) D_GAIN_VALVE_POSITION * valve_pos_err_diff; + CurrentControl(); break; } - case MODE_VALVE_POSITION_PRES_CONTROL_LEARNING: - { + case MODE_JOINT_POSITION_PRES_CONTROL_CURRENT: { + double T_REF = 0.0; // Torque Reference + double I_REF_FORCE_FB = 0.; // I_REF by Force Feedback + double I_REF_VC = 0.; // I_REF for velocity compensation + + // feedback input for position control + double Joint_Pos_Err = 34.0-(double) pos.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] + double Joint_Vel_Err = 0.0-(double) vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] + double K_spring = 0.7; + double D_damper = 0.02; +// T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm + + T_REF = torq.ref; // unit : Nm + + // torque feedback + torq.err = T_REF - (double) torq.sen; + // torq.err_diff = torq.err - torq.err_old; + // torq.err_old = torq.err; + torq.err_sum = torq.err_sum + torq.err/(double)TMR_FREQ_5k; + I_REF_FORCE_FB = 0.001*((double)P_GAIN_JOINT_TORQUE * torq.err + (double)I_GAIN_JOINT_TORQUE * torq.err_sum); + + // velocity compensation for torque control + double Joint_Vel_Act = vel.sen/(double)ENC_PULSE_PER_POSITION; // [pulse/s] >> [deg/s] + double K_vc = 1.5; // Velocity comp. gain + double K_v = 0.0; // Valve gain + if(Joint_Vel_Act > 0) K_v = 1.0/100.0; // open, tuning + if(Joint_Vel_Act < 0) K_v = 1.0/100.0; // close, tuning + I_REF_VC = K_vc*K_v*Joint_Vel_Act; + cur.ref = I_REF_VC + I_REF_FORCE_FB; + // cur.ref = I_REF_FORCE_FB; + + double I_MAX = 10.00; // Maximum Current : 10mV + double Ka = 1.0/I_GAIN_JOINT_TORQUE; + if(cur.ref > I_MAX) { + double I_rem = cur.ref-I_MAX; + I_rem = Ka*I_rem; + cur.ref = I_MAX; + torq.err_sum = torq.err_sum - I_rem/(double)TMR_FREQ_5k; + } else if(cur.ref < -I_MAX) { + double I_rem = cur.ref-(-I_MAX); + I_rem = Ka*I_rem; + cur.ref = -I_MAX; + torq.err_sum = torq.err_sum - I_rem/(double)TMR_FREQ_5k; + } + + CurrentControl(); + + + /* + double I_REF_POS_FB = 0.0; // I_REF by Position Feedback + double I_REF_POS_FF = 0.0; // I_REF by Position Feedforward + double I_REF_FORCE_FB = 0.0; // I_REF by Force Feedback + double I_REF_FORCE_FF = 0.0; // I_REF by Force Feedforward + + // feedback input for position control + pos.err = pos.ref - pos.sen; + double alpha_update_vel = 1.0/(1.0+(double)FREQ_TMR4/(2.0*3.1415*50.0)); // f_cutoff : 50Hz + double err_diff = (pos.err - pos.err_old)*(double)FREQ_5k; + pos.err_diff = (1.0-alpha_update_vel)*pos.err_diff + alpha_update_vel*err_diff; + pos.err_old = pos.err; + I_REF_POS_FB = 0.001*((double)P_GAIN_JOINT_POSITION * pos.err + (double)D_GAIN_JOINT_POSITION * pos.err_diff * 0.1); + + // feedforward input for position control + double Vel_Act_Ref = vel.ref; // [pulse/s] >> [deg/s] + double K_ff = 1.3; + double K_v = 0.0; + if(Vel_Act_Ref > 0) K_v = 1.0/100.0; // open, tuning. (deg/s >> mA) + if(Vel_Act_Ref < 0) K_v = 1.0/100.0; // close, tuning. (deg/s >> mA) + I_REF_POS_FF = K_ff*K_v*Vel_Act_Ref; + + // feedback input for position control + I_REF_FORCE_FB = 0.0; + + // feedforward input for position control + I_REF_FORCE_FF = 0.0; + + cur.ref = I_REF_POS_FF + I_REF_POS_FB + I_REF_FORCE_FB + I_REF_FORCE_FF; + + CurrentControl(); + */ + break; } - - case MODE_TORQUE_SENSOR_NULLING: - { + + case MODE_TORQUE_SENSOR_NULLING: { // DAC Voltage reference set - if (TMR2_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) { + if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) { CUR_TORQUE_sum += CUR_TORQUE; - - if (TMR2_COUNT_TORQUE_NULL % 10 == 0) { + + if (TMR3_COUNT_TORQUE_NULL % 10 == 0) { CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0; CUR_TORQUE_sum = 0; - + TORQUE_VREF += 0.0001 * (TORQUE_NULL - CUR_TORQUE_mean); - + // TORQUE_VREF = 0; if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3; if (TORQUE_VREF < 0) TORQUE_VREF = 0; - + spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0)); spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0)); } } else { CONTROL_MODE = MODE_NO_ACT; - TMR2_COUNT_TORQUE_NULL = 0; + TMR3_COUNT_TORQUE_NULL = 0; CUR_TORQUE_sum = 0; CUR_TORQUE_mean = 0; spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); - + } - TMR2_COUNT_TORQUE_NULL++; + TMR3_COUNT_TORQUE_NULL++; break; - } - - case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: - { - if (TMR2_COUNT_DEADZONE == 0) { + } + + case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: { + if (TMR3_COUNT_DEADZONE == 0) { if (pos_plus_end == pos_minus_end) need_enc_init = true; else temp_time = 0; } if (need_enc_init) { - if (TMR2_COUNT_DEADZONE < (int) (0.5 * (double) TMR_FREQ_5k)) { + if (TMR3_COUNT_DEADZONE < (int) (0.5 * (double) TMR_FREQ_5k)) { VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - pos_plus_end = CUR_POSITION; - } else if (TMR2_COUNT_DEADZONE < TMR_FREQ_5k) { + pos_plus_end = pos.sen; + } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) { VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - pos_minus_end = CUR_POSITION; - } else if (TMR2_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false; + pos_minus_end = pos.sen; + } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false; temp_time = TMR_FREQ_5k; } - - if (temp_time <= TMR2_COUNT_DEADZONE && TMR2_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); + + if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) { + VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0; - - } else if (temp_time <= TMR2_COUNT_DEADZONE && TMR2_COUNT_DEADZONE < (temp_time + (int) (1.9 * (double) TMR_FREQ_5k))) { + + } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9 * (double) TMR_FREQ_5k))) { VALVE_PWM_RAW = 0; CUR_VELOCITY_sum += CUR_VELOCITY; - } else if (TMR2_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) { + } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) { if (CUR_VELOCITY_sum == 0) DZ_dir = 1; else if (CUR_VELOCITY_sum > 0) DZ_dir = 1; else if (CUR_VELOCITY_sum < 0) DZ_dir = -1; else DZ_temp_cnt2 = DZ_end; CUR_VELOCITY_sum = 0; - } else if (TMR2_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) { - if (TMR2_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end; - + } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) { + if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end; + // Position of Dead Zone // (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0) - // | / | / |/ + // | / | / |/ // | ______/ ___|___/ ______/| // |/ / | / | // /| / | / | // 0V 0V 0V - + if (DZ_temp_cnt2 < DZ_end) { - if (TMR2_COUNT_DEADZONE % 20 != 0) { + if (TMR3_COUNT_DEADZONE % 20 != 0) { CUR_VELOCITY_sum += CUR_VELOCITY; } else { VALVE_PWM_RAW -= DZ_dir; @@ -758,7 +937,7 @@ DZ_temp_cnt2++; } } else { - TMR2_COUNT_DEADZONE = -1; + TMR3_COUNT_DEADZONE = -1; VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2; if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) { VALVE_DEADZONE_PLUS = VALVE_CENTER; @@ -767,28 +946,80 @@ VALVE_PWM_RAW = 0; spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS); spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS); - + CONTROL_MODE = MODE_NO_ACT; DZ_temp_cnt2 = 0; } } - TMR2_COUNT_DEADZONE++; + TMR3_COUNT_DEADZONE++; break; } - - case MODE_FIND_HOME: - { + + case MODE_FIND_HOME: { + + + ////////////////////////////////////////////FIND_HOME_PWM///////////////////////////////////////////////////// + if (FLAG_FIND_HOME == true) { + cnt_findhome = 0; + cnt_vel_findhome = 0; + //REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO + pos.ref = pos.sen; + FLAG_FIND_HOME = false; + } + + int check_enc = (TMR_FREQ_5k/10); + if(cnt_findhome%check_enc == 0) { + FINDHOME_POSITION = pos.sen; + FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD; + FINDHOME_POSITION_OLD = FINDHOME_POSITION; + } + cnt_findhome++; + if(cnt_findhome == 10000) cnt_findhome = 0; + + if (abs(FINDHOME_VELOCITY) <= 1) { + cnt_vel_findhome = cnt_vel_findhome + 1; + } else { + cnt_vel_findhome = 0; + } + + if (cnt_vel_findhome < 2*TMR_FREQ_5k) { + //REFERENCE_MODE = MODE_REF_NO_ACT; + if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 1.0; + else pos.ref = pos.ref - 1.0; + pos.err = pos.ref - (double) pos.sen; + V_out = ((double) P_GAIN_JOINT_POSITION * pos.err) * 0.01; + // if (HOMEPOS_OFFSET > 0) VALVE_PWM_RAW = 100; + // else VALVE_PWM_RAW = -100; + } else { + ENC_SET(HOMEPOS_OFFSET); + FLAG_REFERENCE_JOINT_POSITION = 1; + INIT_REF_POS = HOMEPOS_OFFSET; + REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k; + TMR3_COUNT_REFERENCE = 0; + vel.ref = 0.0; + //REFERENCE_MODE = MODE_REF_COS_INC; + CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_PWM; + + FINDHOME_POSITION = 0; + FINDHOME_POSITION_OLD = 0; + FINDHOME_VELOCITY = 0; + cnt_findhome = 0; + cnt_vel_findhome = 0; + } + + /* + ////////////////////////////////////////////FIND_HOME_CURRENT///////////////////////////////////////////////////// if (FLAG_FIND_HOME == true) { cnt_findhome = 0; cnt_vel_findhome = 0; REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO - Ref_Joint_Pos = CUR_POSITION; + Ref_Joint_Pos = pos.sen; FLAG_FIND_HOME = false; } - int check_enc = (TMR_FREQ_5k/10); - if(cnt_findhome%check_enc == 0){ - FINDHOME_POSITION = CUR_POSITION; + int cnt_check_enc = (TMR_FREQ_5k/500); + if(cnt_findhome%cnt_check_enc == 0){ + FINDHOME_POSITION = pos.sen; FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD; FINDHOME_POSITION_OLD = FINDHOME_POSITION; } cnt_findhome++; @@ -800,23 +1031,25 @@ cnt_vel_findhome = 0; } - if (cnt_vel_findhome < 2*TMR_FREQ_5k) { + if (cnt_vel_findhome < 3*TMR_FREQ_5k) { // wait for 3sec REFERENCE_MODE = MODE_REF_NO_ACT; if (HOMEPOS_OFFSET > 0) Ref_Joint_Pos = Ref_Joint_Pos + 1.0; else Ref_Joint_Pos = Ref_Joint_Pos - 1.0; - joint_pos_err = Ref_Joint_Pos - CUR_POSITION; - VALVE_PWM_RAW = ((double) P_GAIN_JOINT_POSITION * joint_pos_err) * 0.01; - // if (HOMEPOS_OFFSET > 0) VALVE_PWM_RAW = 100; - // else VALVE_PWM_RAW = -100; + pos.err = Ref_Joint_Pos - pos.sen; + I_REF = 0.001*((double)pos.err); + if(I_REF>5.0) I_REF = 5.0; + if(I_REF<-5.0) I_REF = -5.0; + cur.ref = I_REF; + CurrentControl(); } else { ENC_SET(HOMEPOS_OFFSET); FLAG_REFERENCE_JOINT_POSITION = 1; INIT_REF_POS = HOMEPOS_OFFSET; REF_MOVE_TIME_5k = 2 * TMR_FREQ_5k; - TMR2_COUNT_REFERENCE = 0; + TMR3_COUNT_REFERENCE = 0; Ref_Joint_Vel = 0.0; REFERENCE_MODE = MODE_REF_COS_INC; - CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_PWM; + CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION; FINDHOME_POSITION = 0; FINDHOME_POSITION_OLD = 0; @@ -824,12 +1057,13 @@ cnt_findhome = 0; cnt_vel_findhome = 0; } + */ + break; } - - case MODE_VALVE_GAIN_SETTING: - { - if (TMR2_COUNT_FLOWRATE == 0) { + + case MODE_VALVE_GAIN_SETTING: { + if (TMR3_COUNT_FLOWRATE == 0) { if (pos_plus_end == pos_minus_end) need_enc_init = true; else { VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; @@ -837,13 +1071,13 @@ } } if (need_enc_init) { - if (TMR2_COUNT_FLOWRATE < (int) (0.5 * (double) TMR_FREQ_5k)) { + if (TMR3_COUNT_FLOWRATE < (int) (0.5 * (double) TMR_FREQ_5k)) { VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - pos_plus_end = CUR_POSITION; - } else if (TMR2_COUNT_FLOWRATE < TMR_FREQ_5k) { + pos_plus_end = pos.sen; + } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) { VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - pos_minus_end = CUR_POSITION; - } else if (TMR2_COUNT_FLOWRATE == TMR_FREQ_5k) { + pos_minus_end = pos.sen; + } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) { need_enc_init = false; check_vel_pos_init = (int) (0.9 * (double) (pos_plus_end - pos_minus_end)); check_vel_pos_fin = (int) (0.95 * (double) (pos_plus_end - pos_minus_end)); @@ -851,14 +1085,14 @@ } temp_time = TMR_FREQ_5k; } - TMR2_COUNT_FLOWRATE++; - if (TMR2_COUNT_FLOWRATE > temp_time) { + TMR3_COUNT_FLOWRATE++; + if (TMR3_COUNT_FLOWRATE > temp_time) { if (flag_flowrate % 2 == 0) { // (+) VALVE_VOLTAGE = 1. * (double) (flag_flowrate / 2 + 1); VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL; - if (CUR_POSITION > (pos_minus_end + check_vel_pos_init) && CUR_POSITION < (pos_minus_end + check_vel_pos_fin)) { + if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) { fl_temp_cnt++; - } else if (CUR_POSITION >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) { + } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) { 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=6*pi/65536*10000 0.5757=0.02525*0.02*0.0095*2*60*1000 // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec fl_temp_cnt2++; @@ -866,9 +1100,9 @@ } else if (flag_flowrate % 2 == 1) { // (-) VALVE_VOLTAGE = -1. * (double) (flag_flowrate / 2 + 1); VALVE_PWM_RAW = VALVE_VOLTAGE / SUPPLY_VOLTAGE*PWM_RESOL; - if (CUR_POSITION < (pos_plus_end - check_vel_pos_init) && CUR_POSITION > (pos_plus_end - check_vel_pos_fin)) { + if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) { fl_temp_cnt++; - } else if (CUR_POSITION <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) { + } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) { 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); // VALVE_GAIN_LPM_PER_V[flag_flowrate] = (double) TMR_FREQ_10k * (double) check_vel_pos_interv / (double) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec fl_temp_cnt2++; @@ -884,74 +1118,73 @@ if (flag_flowrate == 10) { VALVE_PWM_RAW = 0; flag_flowrate = 0; - TMR2_COUNT_FLOWRATE = 0; + TMR3_COUNT_FLOWRATE = 0; valve_gain_repeat_cnt++; if (valve_gain_repeat_cnt >= 1) { CONTROL_MODE = MODE_NO_ACT; valve_gain_repeat_cnt = 0; } - + } break; } - + } - - case MODE_PRESSURE_SENSOR_NULLING: - { + + case MODE_PRESSURE_SENSOR_NULLING: { // DAC Voltage reference set - if (TMR2_COUNT_PRES_NULL < TMR_FREQ_5k * 2) { - CUR_PRES_A_sum += CUR_PRES_A; - CUR_PRES_B_sum += CUR_PRES_B; - - if (TMR2_COUNT_PRES_NULL % 10 == 0) { + if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) { + CUR_PRES_A_sum += pres_A.sen; + CUR_PRES_B_sum += pres_B.sen; + + if (TMR3_COUNT_PRES_NULL % 10 == 0) { CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0; CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0; CUR_PRES_A_sum = 0; CUR_PRES_B_sum = 0; - + double VREF_NullingGain = 0.0003; PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean); PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean); - + if (PRES_A_VREF > 3.3) PRES_A_VREF = 3.3; if (PRES_A_VREF < 0) PRES_A_VREF = 0; if (PRES_B_VREF > 3.3) PRES_B_VREF = 3.3; if (PRES_B_VREF < 0) PRES_B_VREF = 0; - + spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0)); spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0)); } } else { CONTROL_MODE = MODE_NO_ACT; - TMR2_COUNT_PRES_NULL = 0; + pc.printf("no_act"); + TMR3_COUNT_PRES_NULL = 0; CUR_PRES_A_sum = 0; CUR_PRES_B_sum = 0; CUR_PRES_A_mean = 0; CUR_PRES_B_mean = 0; - + spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0)); spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0)); } - TMR2_COUNT_PRES_NULL++; + TMR3_COUNT_PRES_NULL++; break; } - - case MODE_PRESSURE_SENSOR_CALIB: - { - if (TMR2_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) { + + case MODE_PRESSURE_SENSOR_CALIB: { + if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) { VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.; - if (TMR2_COUNT_PRES_CALIB >= TMR_FREQ_5k) { + if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) { CUR_PRES_A_sum += CUR_PRES_A; } - } else if (TMR2_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) { + } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) { VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL / 5.; - if (TMR2_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) { + if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) { CUR_PRES_B_sum += CUR_PRES_B; } } else { CONTROL_MODE = MODE_NO_ACT; - TMR2_COUNT_PRES_CALIB = 0; + TMR3_COUNT_PRES_CALIB = 0; VALVE_PWM_RAW = 0; PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((double) TMR_FREQ_5k - 1.) - PRES_A_NULL; PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((double) PRES_SUPPLY - 1.); @@ -961,73 +1194,57 @@ CUR_PRES_B_sum = 0; CUR_PRES_A_mean = 0; CUR_PRES_B_mean = 0; - + spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0)); spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0)); } - TMR2_COUNT_PRES_CALIB++; + TMR3_COUNT_PRES_CALIB++; break; } - - case MODE_ROTARY_FRICTION_TUNING: - { - if (TMR2_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4 + 3. * sin(2 * 3.14159 * 0.5 * TMR2_COUNT_ROTARY_FRIC_TUNE * 0.0001 * 0.05); - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR2_COUNT_ROTARY_FRIC_TUNE * 0.0001); + + case MODE_ROTARY_FRICTION_TUNING: { + 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); + VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001); if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL; else VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - TMR2_COUNT_ROTARY_FRIC_TUNE++; - if (TMR2_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) { - TMR2_COUNT_ROTARY_FRIC_TUNE = 0; + TMR3_COUNT_ROTARY_FRIC_TUNE++; + if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) { + TMR3_COUNT_ROTARY_FRIC_TUNE = 0; VALVE_PWM_RAW = 0; CONTROL_MODE = MODE_NO_ACT; } break; } - - case MODE_DDV_POS_VS_PWM_ID: - { + + case MODE_DDV_POS_VS_PWM_ID: { VALVE_ID_timer = VALVE_ID_timer + 1; - - if(VALVE_ID_timer < TMR_FREQ_5k*1) - { + + if(VALVE_ID_timer < TMR_FREQ_5k*1) { VALVE_PWM_RAW = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100); - } - else if(VALVE_ID_timer < TMR_FREQ_5k*2) - { + } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { VALVE_PWM_RAW = 1000*(ID_index_array[ID_index]); - } - else if(VALVE_ID_timer == TMR_FREQ_5k*2) - { + } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { VALVE_POS_TMP = 0; data_num = 0; - } - else if(VALVE_ID_timer < TMR_FREQ_5k*3) - { + } else if(VALVE_ID_timer < TMR_FREQ_5k*3) { data_num = data_num + 1; VALVE_POS_TMP = VALVE_POS_TMP + value; - } - else if(VALVE_ID_timer == TMR_FREQ_5k*3) - { + } else if(VALVE_ID_timer == TMR_FREQ_5k*3) { VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num; VALVE_ID_timer = 0; ID_index= ID_index +1; } - - if(ID_index>=18) - { + + if(ID_index>=18) { int i; VALVE_POS_AVG_OLD = VALVE_POS_AVG[0]; - for(i=0;i<18;i++) - { + for(i=0; i<18; i++) { VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]); spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i])); - if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) - { + if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) { VALVE_MAX_POS = VALVE_POS_AVG[i]; VALVE_POS_AVG_OLD = VALVE_MAX_POS; - } - else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) - { + } else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) { VALVE_MIN_POS = VALVE_POS_AVG[i]; VALVE_POS_AVG_OLD = VALVE_MIN_POS; } @@ -1038,103 +1255,77 @@ ID_index = 0; CONTROL_MODE = MODE_NO_ACT; } - - + + break; } - - case MODE_DDV_DEADZONE_AND_CENTER: - { - + + case MODE_DDV_DEADZONE_AND_CENTER: { + VALVE_DZ_timer = VALVE_DZ_timer + 1; - if(first_check == 0) - { - if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - } - else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - pos_plus_end = CUR_POSITION; - } - else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - } - else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - pos_minus_end = CUR_POSITION; - } - else if(VALVE_DZ_timer < (int) (3.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); - } - else if(VALVE_DZ_timer < (int) (4.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); + if(first_check == 0) { + if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + pos_plus_end = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { + V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) { + V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + pos_minus_end = pos.sen; + } else if(VALVE_DZ_timer < (int) (3.0 * (double) TMR_FREQ_5k)) { + V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); + } else if(VALVE_DZ_timer < (int) (4.0 * (double) TMR_FREQ_5k)) { + V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); data_num = data_num + 1; VALVE_POS_TMP = VALVE_POS_TMP + value; - } - else if(VALVE_DZ_timer == (int) (4.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); + } else if(VALVE_DZ_timer == (int) (4.0 * (double) TMR_FREQ_5k)) { + V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); DDV_POS_AVG = VALVE_POS_TMP / data_num; - START_POS = CUR_POSITION; + START_POS = pos.sen; CAN_TX_PRES((int16_t) (DDV_POS_AVG), (int16_t) (data_num)); VALVE_POS_TMP = 0; data_num = 0; - - } - else if(VALVE_DZ_timer < (int) (5.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos = DDV_POS_AVG; - VALVE_POS_CONTROL(Ref_Valve_Pos); + + } else if(VALVE_DZ_timer < (int) (5.0 * (double) TMR_FREQ_5k)) { + valve_pos.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos.ref); //CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS)); - - } - else if(VALVE_DZ_timer < (int) (6.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos = DDV_POS_AVG; - VALVE_POS_CONTROL(Ref_Valve_Pos); + + } else if(VALVE_DZ_timer < (int) (6.0 * (double) TMR_FREQ_5k)) { + valve_pos.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos.ref); if(CUR_VELOCITY >= 0) VEL_POINT = VEL_POINT + 1; else VEL_POINT = VEL_POINT - 1; - } - else if(VALVE_DZ_timer == (int) (6.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos = DDV_POS_AVG; - VALVE_POS_CONTROL(Ref_Valve_Pos); - FINAL_POS = CUR_POSITION; - // if(VEL_POINT >= 0) - // { - // DZ_case = 1; - // } - // else - // { - // DZ_case = -1; - // } - // if(abs(FINAL_POS - START_POS) < 100) - // { - // DZ_case = 0; - // } - - if((FINAL_POS - START_POS)>100) - { + } else if(VALVE_DZ_timer == (int) (6.0 * (double) TMR_FREQ_5k)) { + valve_pos.ref = DDV_POS_AVG; + VALVE_POS_CONTROL(valve_pos.ref); + FINAL_POS = pos.sen; + // if(VEL_POINT >= 0) + // { + // DZ_case = 1; + // } + // else + // { + // DZ_case = -1; + // } + // if(abs(FINAL_POS - START_POS) < 100) + // { + // DZ_case = 0; + // } + + if((FINAL_POS - START_POS)>100) { DZ_case = 1; - } - else if((FINAL_POS - START_POS)<-100) - { + } else if((FINAL_POS - START_POS)<-100) { DZ_case = -1; - } - else - { + } else { DZ_case = 0; } - - VEL_POINT = 0; + + VEL_POINT = 0; first_check = 1; DZ_DIRECTION = 1; VALVE_DZ_timer = 0; @@ -1142,137 +1333,100 @@ DZ_NUM = 1; DZ_index = 1; CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM)); - + } - } - else - { - if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) - { - if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); - } - else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) - { - START_POS = CUR_POSITION; - } - else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; - if(Ref_Valve_Pos <= VALVE_MIN_POS) - { - Ref_Valve_Pos = VALVE_MIN_POS; + }else { + if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) { + if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); + } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { + START_POS = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { + valve_pos.ref = Ref_Valve_Pos_Old - DZ_case * DZ_DIRECTION * 64 / DZ_index; + if(valve_pos.ref <= VALVE_MIN_POS) { + valve_pos.ref = VALVE_MIN_POS; + } else if(valve_pos.ref >= VALVE_MAX_POS) { + valve_pos.ref = VALVE_MAX_POS; } - else if(Ref_Valve_Pos >= VALVE_MAX_POS) - { - Ref_Valve_Pos = VALVE_MAX_POS; - } - VALVE_POS_CONTROL(Ref_Valve_Pos); + VALVE_POS_CONTROL(valve_pos.ref); if(CUR_VELOCITY >= 0) VEL_POINT = VEL_POINT + 1; else VEL_POINT = VEL_POINT - 1; - } - else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos_Old = Ref_Valve_Pos; - FINAL_POS = CUR_POSITION; - // if(VEL_POINT >= 0) - // DZ_DIRECTION = 1 * DZ_case; - // else - // DZ_DIRECTION = -1 * DZ_case; - // if(abs(FINAL_POS - START_POS) < 100) - // DZ_DIRECTION = 1 * DZ_case; - - if((FINAL_POS - START_POS)>100) - { + } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) { + Ref_Valve_Pos_Old = valve_pos.ref; + FINAL_POS = pos.sen; + // if(VEL_POINT >= 0) + // DZ_DIRECTION = 1 * DZ_case; + // else + // DZ_DIRECTION = -1 * DZ_case; + // if(abs(FINAL_POS - START_POS) < 100) + // DZ_DIRECTION = 1 * DZ_case; + + if((FINAL_POS - START_POS)>100) { + DZ_DIRECTION = 1 * DZ_case; + } else if((FINAL_POS - START_POS)<-100) { + DZ_DIRECTION = -1 * DZ_case; + } else { DZ_DIRECTION = 1 * DZ_case; } - else if((FINAL_POS - START_POS)<-100) - { - DZ_DIRECTION = -1 * DZ_case; - } - else - { - DZ_DIRECTION = 1 * DZ_case; - } - - VEL_POINT = 0; - + + VEL_POINT = 0; + VALVE_DZ_timer = 0; DZ_index= DZ_index *2; - if(DZ_index >= 128) - { - FIRST_DZ = Ref_Valve_Pos; + if(DZ_index >= 128) { + FIRST_DZ = valve_pos.ref; DZ_NUM = 2; Ref_Valve_Pos_Old = FIRST_DZ; DZ_index = 1; DZ_DIRECTION = 1; } - + //CAN_TX_PRES((int16_t) (1), (int16_t) (DZ_index)); } - } - else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) - { - if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); - } - else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) - { - START_POS = CUR_POSITION; - } - else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos = Ref_Valve_Pos_Old + DZ_DIRECTION + 64 / DZ_index; - if(Ref_Valve_Pos <= VALVE_MIN_POS) - { - Ref_Valve_Pos = VALVE_MIN_POS; + } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) { + if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = (double) P_GAIN_JOINT_POSITION * 0.01 * (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); + } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { + START_POS = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { + valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION + 64 / DZ_index; + if(valve_pos.ref <= VALVE_MIN_POS) { + valve_pos.ref = VALVE_MIN_POS; + } else if(valve_pos.ref >= VALVE_MAX_POS) { + valve_pos.ref = VALVE_MAX_POS; } - else if(Ref_Valve_Pos >= VALVE_MAX_POS) - { - Ref_Valve_Pos = VALVE_MAX_POS; - } - VALVE_POS_CONTROL(Ref_Valve_Pos); + VALVE_POS_CONTROL(valve_pos.ref); if(CUR_VELOCITY >= 0) VEL_POINT = VEL_POINT + 1; else VEL_POINT = VEL_POINT - 1; - } - else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos_Old = Ref_Valve_Pos; - FINAL_POS = CUR_POSITION; - // if(VEL_POINT >= 0) - // DZ_DIRECTION = 1 * DZ_case; - // else - // DZ_DIRECTION = -1 * DZ_case; - // if(abs(FINAL_POS - START_POS) < 100) - // DZ_DIRECTION = -1 * DZ_case; - - if((FINAL_POS - START_POS)>100) - { + } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) { + Ref_Valve_Pos_Old = valve_pos.ref; + FINAL_POS = pos.sen; + // if(VEL_POINT >= 0) + // DZ_DIRECTION = 1 * DZ_case; + // else + // DZ_DIRECTION = -1 * DZ_case; + // if(abs(FINAL_POS - START_POS) < 100) + // DZ_DIRECTION = -1 * DZ_case; + + if((FINAL_POS - START_POS)>100) { DZ_DIRECTION = -1; - } - else if((FINAL_POS - START_POS)<-100) - { + } else if((FINAL_POS - START_POS)<-100) { + DZ_DIRECTION = 1; + } else { DZ_DIRECTION = 1; } - else - { - DZ_DIRECTION = 1; - } - - VEL_POINT = 0; - - + + VEL_POINT = 0; + + VALVE_DZ_timer = 0; DZ_index= DZ_index *2; - if(DZ_index >= 128) - { - SECOND_DZ = Ref_Valve_Pos; + if(DZ_index >= 128) { + SECOND_DZ = valve_pos.ref; DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ)); first_check = 0; spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); @@ -1282,122 +1436,91 @@ CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; } - + CAN_TX_PRES((int16_t) (2), (int16_t) (DZ_index)); } - } - else if(DZ_case == 0 && DZ_NUM ==1) - { - if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); - } - else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) - { - START_POS = CUR_POSITION; - } - else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index; - if(Ref_Valve_Pos <= VALVE_MIN_POS) - { - Ref_Valve_Pos = VALVE_MIN_POS; + } else if(DZ_case == 0 && DZ_NUM ==1) { + if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); + } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { + START_POS = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { + valve_pos.ref = Ref_Valve_Pos_Old - DZ_DIRECTION * 64 / DZ_index; + if(valve_pos.ref <= VALVE_MIN_POS) { + valve_pos.ref = VALVE_MIN_POS; + } else if(valve_pos.ref >= VALVE_MAX_POS) { + valve_pos.ref = VALVE_MAX_POS; } - else if(Ref_Valve_Pos >= VALVE_MAX_POS) - { - Ref_Valve_Pos = VALVE_MAX_POS; - } - VALVE_POS_CONTROL(Ref_Valve_Pos); + VALVE_POS_CONTROL(valve_pos.ref); if(CUR_VELOCITY >= 0) VEL_POINT = VEL_POINT + 1; else VEL_POINT = VEL_POINT - 1; - } - else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos_Old = Ref_Valve_Pos; - FINAL_POS = CUR_POSITION; - // if(VEL_POINT >= 0) - // DZ_DIRECTION = 1; - // else - // DZ_DIRECTION = -1; - // if(abs(FINAL_POS - START_POS) < 100) - // DZ_DIRECTION = 1; - - if((FINAL_POS - START_POS)>100) - { + } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) { + Ref_Valve_Pos_Old = valve_pos.ref; + FINAL_POS = pos.sen; + // if(VEL_POINT >= 0) + // DZ_DIRECTION = 1; + // else + // DZ_DIRECTION = -1; + // if(abs(FINAL_POS - START_POS) < 100) + // DZ_DIRECTION = 1; + + if((FINAL_POS - START_POS)>100) { + DZ_DIRECTION = 1; + } else if((FINAL_POS - START_POS)<-100) { + DZ_DIRECTION = -1; + } else { DZ_DIRECTION = 1; } - else if((FINAL_POS - START_POS)<-100) - { - DZ_DIRECTION = -1; - } - else - { - DZ_DIRECTION = 1; - } - - VEL_POINT = 0; + + VEL_POINT = 0; //CAN_TX_PRES((int16_t) (7), (int16_t) (DZ_DIRECTION)); VALVE_DZ_timer = 0; DZ_index= DZ_index *2; - if(DZ_index >= 128) - { - FIRST_DZ = Ref_Valve_Pos; + if(DZ_index >= 128) { + FIRST_DZ = valve_pos.ref; DZ_NUM = 2; Ref_Valve_Pos_Old = FIRST_DZ; DZ_index = 1; DZ_DIRECTION = 1; } } - } - else - { - if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); - } - else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) - { - START_POS = CUR_POSITION; - } - else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index; - if(Ref_Valve_Pos <= VALVE_MIN_POS) - { - Ref_Valve_Pos = VALVE_MIN_POS; + } else { + if(VALVE_DZ_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); + } else if(VALVE_DZ_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { + START_POS = pos.sen; + } else if(VALVE_DZ_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { + valve_pos.ref = Ref_Valve_Pos_Old + DZ_DIRECTION * 64 / DZ_index; + if(valve_pos.ref <= VALVE_MIN_POS) { + valve_pos.ref = VALVE_MIN_POS; + } else if(valve_pos.ref > VALVE_MAX_POS) { + valve_pos.ref = VALVE_MAX_POS - 1; } - else if(Ref_Valve_Pos > VALVE_MAX_POS) - { - Ref_Valve_Pos = VALVE_MAX_POS - 1; - } - VALVE_POS_CONTROL(Ref_Valve_Pos); + VALVE_POS_CONTROL(valve_pos.ref); if(CUR_VELOCITY >= 0) VEL_POINT = VEL_POINT + 1; else VEL_POINT = VEL_POINT - 1; - } - else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos_Old = Ref_Valve_Pos; - FINAL_POS = CUR_POSITION; + } else if(VALVE_DZ_timer == (int) (2.0 * (double) TMR_FREQ_5k)) { + Ref_Valve_Pos_Old = valve_pos.ref; + FINAL_POS = pos.sen; if(VEL_POINT >= 0) DZ_DIRECTION = -1; else DZ_DIRECTION = 1; if(abs(FINAL_POS - START_POS) < 100) DZ_DIRECTION = 1; - - VEL_POINT = 0; - - + + VEL_POINT = 0; + + VALVE_DZ_timer = 0; //CAN_TX_PRES((int16_t) (3), (int16_t) (DZ_index)); DZ_index= DZ_index *2; - if(DZ_index >= 128) - { - SECOND_DZ = Ref_Valve_Pos; + if(DZ_index >= 128) { + SECOND_DZ = valve_pos.ref; DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ)); first_check = 0; VALVE_DEADZONE_MINUS = FIRST_DZ; @@ -1409,85 +1532,63 @@ CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; } - + //CAN_TX_PRES((int16_t) (4), (int16_t) (DZ_index)); } } - } + } break; } - - case MODE_DDV_POS_VS_FLOWRATE: - { + + case MODE_DDV_POS_VS_FLOWRATE: { VALVE_FR_timer = VALVE_FR_timer + 1; - if(first_check == 0) - { - if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + if(first_check == 0) { + if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6)); - } - else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - pos_plus_end = CUR_POSITION; - // CAN_TX_PRES((int16_t) (VALVE_PWM_RAW), (int16_t) (7)); - } - else if(VALVE_FR_timer < (int) (2.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - } - else if(VALVE_FR_timer == (int) (2.0 * (double) TMR_FREQ_5k)) - { - // CAN_TX_PRES((int16_t) (VALVE_PWM_RAW), (int16_t) (8)); - VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; - pos_minus_end = CUR_POSITION; + } else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + pos_plus_end = pos.sen; + // CAN_TX_PRES((int16_t) (V_out), (int16_t) (7)); + } else if(VALVE_FR_timer < (int) (2.0 * (double) TMR_FREQ_5k)) { + V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + } else if(VALVE_FR_timer == (int) (2.0 * (double) TMR_FREQ_5k)) { + // CAN_TX_PRES((int16_t) (V_out), (int16_t) (8)); + V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + pos_minus_end = pos.sen; first_check = 1; VALVE_FR_timer = 0; - Ref_Valve_Pos = DDV_CENTER; + valve_pos.ref = DDV_CENTER; ID_index = 0; max_check = 0; min_check = 0; } - } - else - { - if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) - { - VALVE_PWM_RAW = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION); - } - else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) - { + } else { + if(VALVE_FR_timer < (int) (1.0 * (double) TMR_FREQ_5k)) { + V_out = (double) P_GAIN_JOINT_POSITION * 0.01* (0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen); + } else if(VALVE_FR_timer == (int) (1.0 * (double) TMR_FREQ_5k)) { data_num = 0; - Ref_Valve_Pos = 50*(ID_index_array[ID_index]) + DDV_CENTER; - - VALVE_POS_CONTROL(Ref_Valve_Pos); - START_POS = CUR_POSITION; - } - else if(VALVE_FR_timer < (int) (5.0 * (double) TMR_FREQ_5k)) - { - Ref_Valve_Pos = 50*(ID_index_array[ID_index]) + DDV_CENTER; - VALVE_POS_CONTROL(Ref_Valve_Pos); + valve_pos.ref = 50*(ID_index_array[ID_index]) + DDV_CENTER; + + VALVE_POS_CONTROL(valve_pos.ref); + START_POS = pos.sen; + } else if(VALVE_FR_timer < (int) (5.0 * (double) TMR_FREQ_5k)) { + valve_pos.ref = 50*(ID_index_array[ID_index]) + DDV_CENTER; + VALVE_POS_CONTROL(valve_pos.ref); data_num = data_num + 1; - if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) CUR_POSITION) > 20000) - { - FINAL_POS = CUR_POSITION; - one_period_end = 1; + if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen) > 20000) { + FINAL_POS = pos.sen; + one_period_end = 1; } - } - else if(VALVE_FR_timer == (int) (5.0 * (double) TMR_FREQ_5k)) - { - FINAL_POS = CUR_POSITION; + } else if(VALVE_FR_timer == (int) (5.0 * (double) TMR_FREQ_5k)) { + FINAL_POS = pos.sen; one_period_end = 1; } - - if(one_period_end == 1){ - if(Ref_Valve_Pos > VALVE_MAX_POS) - { + + if(one_period_end == 1) { + if(valve_pos.ref > VALVE_MAX_POS) { max_check = 1; - } - else if(Ref_Valve_Pos < VALVE_MIN_POS) - { + } else if(valve_pos.ref < VALVE_MIN_POS) { min_check = 1; } JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec @@ -1496,80 +1597,86 @@ VALVE_FR_timer = 0; one_period_end = 0; ID_index= ID_index +1; - // CAN_TX_PRES((int16_t) (Ref_Valve_Pos), (int16_t) (ID_index)); + // CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (ID_index)); } - - if(max_check == 1 && min_check == 1) - { + + if(max_check == 1 && min_check == 1) { spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index)); VALVE_POS_NUM = ID_index; ID_index = 0; first_check = 0; VALVE_FR_timer = 0; - // CAN_TX_PRES((int16_t) (123), (int16_t) (123)); + // CAN_TX_PRES((int16_t) (123), (int16_t) (123)); CONTROL_MODE = MODE_NO_ACT; } } break; } + default: break; } - + if (FLAG_VALVE_OUTPUT_CALIB) { // Valve Dead Zone (Mechanical dead-zone canceling) - // if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) { - // if (VALVE_PWM_RAW > 0.) { - // VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_DEADZONE_PLUS; - // } else if (VALVE_PWM_RAW < 0.) { - // VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_DEADZONE_MINUS; - // } else VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW + VALVE_CENTER; - // } else VALVE_PWM_VALVE_DZ = (int) VALVE_PWM_RAW; - - if(VALVE_PWM_RAW>0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS; - else if(VALVE_PWM_RAW<0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS; - - VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_CENTER; - + // if (CONTROL_MODE != MODE_VALVE_NULLING_AND_DEADZONE_SETTING) { + // if (V_out > 0.) { + // VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_PLUS; + // } else if (V_out < 0.) { + // VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_DEADZONE_MINUS; + // } else VALVE_PWM_VALVE_DZ = (int) V_out + VALVE_CENTER; + // } else VALVE_PWM_VALVE_DZ = (int) V_out; + + if(V_out>0) V_out = V_out + VALVE_DEADZONE_PLUS; + else if(V_out<0) V_out = V_out + VALVE_DEADZONE_MINUS; + + VALVE_PWM_VALVE_DZ = V_out + VALVE_CENTER; + // Output Voltage Linearization and Dead Zone Cancellation (Electrical dead-zone) double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ/PWM_RESOL*SUPPLY_VOLTAGE*1000.0; // convert PWM duty to mV double CUR_PWM_DZ = PWM_duty_byLT(CUR_PWM_nonlin); - - if (CUR_PWM_DZ > 0) CUR_PWM = (int)CUR_PWM_DZ + 143; - else if (CUR_PWM_DZ < 0) CUR_PWM = (int)CUR_PWM_DZ - 138; - else CUR_PWM = CUR_PWM_DZ; + + if (CUR_PWM_DZ > 0) V_out = (int)CUR_PWM_DZ + 143; + else if (CUR_PWM_DZ < 0) V_out = (int)CUR_PWM_DZ - 138; + else V_out = CUR_PWM_DZ; } else { - CUR_PWM = VALVE_PWM_RAW; + CUR_PWM = V_out; } - - VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE); - - //CAN ---------------------------------------------------------------------- - if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { - + + //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE); + PWM_out = CUR_PWM; + + + + //CAN ---------------------------------------------------------------------- + //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { + if (TMR3_COUNT_CAN_TX % 10 == 0) { + if (flag_data_request[0] == HIGH) { //position+velocity - CAN_TX_POSITION((long) CUR_POSITION, (long) CUR_VELOCITY); + CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); + //pc.printf("can good"); // CAN_TX_POSITION((int) (PRES_A_VREF * 100.), (int) (PRES_B_VREF * 100.)); // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR); } - + if (flag_data_request[1] == HIGH) { //torque //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.)); CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM)); // CAN_TX_TORQUE((int16_t) DZ_temp_cnt); } - + if (flag_data_request[2] == HIGH) { - //pressure A and B - CAN_TX_PRES((int16_t) (CUR_PRES_A), (int16_t) (CUR_PRES_B)); // CUR_PRES_X : 0(0bar)~4096(210bar) + //pressure A and B + //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar) + CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar) // CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.)); // CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.); // CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.); - + } - + if (flag_data_request[3] == HIGH) { //PWM CAN_TX_PWM((int16_t) CUR_PWM); @@ -1577,36 +1684,39 @@ // CAN_TX_PWM((int16_t) cnt_vel_findhome); // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.)); // CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.)); - + } - + if (flag_data_request[4] == HIGH) { //valve position - CAN_TX_VALVE_POSITION((int16_t) (I2C_val)); + CAN_TX_VALVE_POSITION((int16_t) (value)); //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF)); //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF)); - - + + //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM)); // CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE * PWM_RESOL / SUPPLY_VOLTAGE)); // CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION); // CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos); // CAN_TX_VALVE_POSITION((int16_t) flag_flowrate); } - + TMR3_COUNT_CAN_TX = 0; - + } TMR3_COUNT_CAN_TX++; + + + /******************************************************* *** Valve Control ********************************************************/ - ValveControl(CONTROL_MODE); + //ValveControl(CONTROL_MODE); - double t = (double)CNT_TMR4*DT_TMR4; - double T = 5.0; - V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty) +// double t = (double)CNT_TMR4*DT_TMR4; +// double T = 1.0; +// V_out = 1000.0*sin(2.0*PI*t/T); // V_out : -5000.0mV~5000.0mV(full duty) // if(V_out > 0.0) V_out = 1000.0; // else if(V_out < 0.0) V_out = -1000.0; @@ -1634,7 +1744,7 @@ /******************************************************* *** Data Send (CAN) & Print out (UART) ********************************************************/ - //if((CNT_TMR3%40)==0){ +// if((CNT_TMR3%1000)==0){ // msg.id = 50; // msg.len = 4; // int temp_CUR = (int)(cur.sen*1000.0); @@ -1652,6 +1762,9 @@ } else LED = 1; // LED != LED; + pc.printf("flag %f\n", (double) pres_A.sen); + spi_eeprom_ready(); + pc.printf("romrom %d\n", (int) PRES_SENSOR_A_PULSE_PER_BAR); } /******************************************************* @@ -1662,6 +1775,7 @@ TIM3->SR = 0x0; // reset the status register } +/* void ValveControl(unsigned int ControlMode) { switch (ControlMode) { @@ -1714,6 +1828,7 @@ } } +*/ void CurrentControl() {