![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
for learning
Diff: main.cpp
- Revision:
- 19:23b7c1ad8683
- Parent:
- 18:b8adf1582ea3
- Child:
- 20:806196fda269
diff -r b8adf1582ea3 -r 23b7c1ad8683 main.cpp --- a/main.cpp Wed Sep 04 11:02:57 2019 +0000 +++ b/main.cpp Mon Sep 09 06:29:56 2019 +0000 @@ -70,6 +70,22 @@ int timer_while = 0; int while_index = 0; + +extern int CID_RX_CMD; +extern int CID_RX_REF_POSITION; +extern int CID_RX_REF_TORQUE; +extern int CID_RX_REF_PRES_DIFF; +extern int CID_RX_REF_VOUT; +extern int CID_RX_REF_VALVE_POSITION; +extern int CID_RX_REF_CURRENT; + +extern int CID_TX_INFO; +extern int CID_TX_POSITION; +extern int CID_TX_TORQUE; +extern int CID_TX_PRES; +extern int CID_TX_VOUT; +extern int CID_TX_VALVE_POSITION; + // ============================================================================= // ============================================================================= // ============================================================================= @@ -129,8 +145,8 @@ /********************************* *** Initialization *********************************/ - LED = 1; - pc.baud(9600); + //LED = 1; + //pc.baud(9600); // i2c init i2c.frequency(400 * 1000); // 0.4 mHz @@ -147,8 +163,7 @@ make_delay(); //rom - - ROM_INIT_DATA(); + ROM_CALL_DATA(); make_delay(); // ADC init @@ -169,19 +184,18 @@ can.attach(&CAN_RX_HANDLER); CAN_ID_INIT(); make_delay(); - + can.filter(msg.id, 0xFFFFF000, CANStandard); + // spi _ enc spi_enc_set_init(); make_delay(); //DAC init dac_1 = PRES_A_VREF / 3.3; - //dac_1 = 0.0; dac_2 = PRES_B_VREF / 3.3; - //dac_2 = 0.0; make_delay(); - for (int i=0; i<100; i++) { + for (int i=0; i<50; i++) { if(i%2==0) ID_index_array[i] = - i * 0.5; else @@ -192,15 +206,15 @@ *** Program is operating! *************************************/ while(1) { - if(timer_while==1){ + if(timer_while==1000){ //i2c read_field(i2c_slave_addr1); if(DIR_VALVE_ENC < 0) value = 1023 - value; - if(LED==1) { - LED=0; - } else - LED = 1; +// if(LED==1) { +// LED=0; +// } else +// LED = 1; timer_while = 0; } @@ -216,9 +230,9 @@ 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; + Ref_Valve_Pos_FF = ((int) 10.0/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER; } 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]; + Ref_Valve_Pos_FF = ((int) 10.0*(ID_index_array[i+1] - ID_index_array[i-1])/(JOINT_VEL[i+1] - JOINT_VEL[i-1]) * (REF_JOINT_VEL - JOINT_VEL[i-1])) + DDV_CENTER + 10*ID_index_array[i-1]; } break; } @@ -228,6 +242,7 @@ } 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; } + Ref_Valve_Pos_FF_CAN = Ref_Valve_Pos_FF; return Ref_Valve_Pos_FF; } @@ -261,6 +276,9 @@ break; } } + + //VALVE_PWM_RAW_FF = 0.0; + V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB; } @@ -308,201 +326,74 @@ TIMER INTERRUPT *******************************************************************************/ -unsigned long CNT_TMR4 = 0; +//unsigned long CNT_TMR4 = 0; double FREQ_TMR4 = (double)FREQ_10k; double DT_TMR4 = (double)DT_10k; extern "C" void TIM4_IRQHandler(void) { - if ( TIM4->SR & TIM_SR_UIF ) { - /******************************************************* - *** Sensor Read & Data Handling - ********************************************************/ + if (TIM4->SR & TIM_SR_UIF ) { + + /******************************************************* + *** Sensor Read & Data Handling + ********************************************************/ - - //Using LoadCell + + //Using LoadCell // ADC1->CR2 |= 0x40000000; // adc _ 12bit // //while((ADC1->SR & 0b10)); // double alpha_update_torque = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // double torque_new = ((double)ADC1->DR - PRES_A_NULL) / TORQUE_SENSOR_PULSE_PER_TORQUE + 1.0; // torq.sen = torq.sen*(1.0-alpha_update_torque)+torque_new*(alpha_update_torque); - //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; - pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A); + - //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; - pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B); - torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B; + //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; + pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A); + + //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; + pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B); + torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B; - //Current - //ADC3->CR2 |= 0x40000000; // adc _ 12bit + //Current + //ADC3->CR2 |= 0x40000000; // adc _ 12bit // 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); - cur.sen = raw_cur; + 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); + cur.sen = raw_cur; - - - /******************************************************* - *** Timer Counting & etc. - ********************************************************/ - CNT_TMR4++; - } + /******************************************************* + *** Timer Counting & etc. + ********************************************************/ + //CNT_TMR4++; + } TIM4->SR = 0x0; // reset the status register } + + int j =0; -unsigned long CNT_TMR3 = 0; +//unsigned long CNT_TMR3 = 0; double FREQ_TMR3 = (double)FREQ_5k; double DT_TMR3 = (double)DT_5k; extern "C" void TIM3_IRQHandler(void) -{ - if ( TIM3->SR & TIM_SR_UIF ) { - +{ + if (TIM3->SR & TIM_SR_UIF ) { ENC_UPDATE(); - /* - // Reference Loop - switch (REFERENCE_MODE) { - case MODE_REF_NO_ACT: { - break; - } - case MODE_REF_DIRECT: { - if (FLAG_REFERENCE_VALVE_PWM) { - Vout.ref = (double) Vout.ref; - } - if (FLAG_REFERENCE_VALVE_POSITION) { - valve_pos.ref = (double) valve_pos.ref; - } - if (FLAG_REFERENCE_JOINT_POSITION) { - pos.ref = (double) pos.ref; - vel.ref = (double) vel.ref; - } - if (FLAG_REFERENCE_JOINT_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) { - 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) { - 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) { - 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) { - 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; - } - 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; - } - + // CONTROL LOOP ------------------------------------------------------------ - case MODE_REF_LINE_INC: { - if (FLAG_REFERENCE_VALVE_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) { - 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) { - 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) { - torq.ref = ((double) torq.ref - (double) INIT_torq.ref)*((double) TMR3_COUNT_REFERENCE / (double) REF_MOVE_TIME_5k) + (double) INIT_torq.ref; - } - 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; - } - - case MODE_REF_SIN_WAVE: { - if (FLAG_REFERENCE_VALVE_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) { - 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) 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) TMR3_COUNT_REFERENCE / (double) TMR3_COUNT_REFERENCE) + (double) torq.ref; - } - TMR3_COUNT_REFERENCE++; - if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) { - REFERENCE_MODE = MODE_REF_DIRECT; - TMR3_COUNT_REFERENCE = 0; - } - break; - } - - case MODE_REF_SQUARE_WAVE: { - if (FLAG_REFERENCE_VALVE_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) { - 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) 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) 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; - } - TMR3_COUNT_REFERENCE++; - if (TMR3_COUNT_REFERENCE >= TMR3_COUNT_REFERENCE * REF_NUM) { - REFERENCE_MODE = MODE_REF_DIRECT; - TMR3_COUNT_REFERENCE = 0; - } - break; - } - - default: - break; - } - - */ - - // CONTROL LOOP ------------------------------------------------------------ - switch (CONTROL_MODE) { case MODE_NO_ACT: { V_out = 0; @@ -551,7 +442,7 @@ // 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; V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB; @@ -561,9 +452,9 @@ 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_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; + //int DDV_JOINT_CAN = 0; // feedback input for position control pos.err = pos.ref - (double) pos.sen; pos.err_diff = pos.err - pos.err_old; @@ -594,9 +485,8 @@ // VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01; VALVE_POS_RAW_FORCE_FB = 0.0; - 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.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB; + valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER; VALVE_POS_CONTROL(valve_pos.ref); break; } @@ -686,7 +576,7 @@ valve_pos.ref = VALVE_PWM_RAW_POS + VALVE_PWM_RAW_TORQ; VALVE_POS_CONTROL(valve_pos.ref); - + break; } @@ -695,66 +585,66 @@ break; } - - case MODE_TEST_CURRENT_CONTROL: - { + + case MODE_TEST_CURRENT_CONTROL: { if (TMR3_COUNT_IREF == TMR_FREQ_5k) { TMR3_COUNT_IREF = 0; - } TMR3_COUNT_IREF++; - - // Set Current Reference + } + 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 + 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: - { + + case MODE_TEST_PWM_CONTROL: { if (TMR3_COUNT_IREF == TMR_FREQ_5k) { TMR3_COUNT_IREF = 0; - } TMR3_COUNT_IREF++; - + } + TMR3_COUNT_IREF++; + // Set PWM reference double TMR3_CNT_MAX = (double)TMR_FREQ_5k/2.0; - double I_REF_MID = 0.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_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 @@ -762,7 +652,7 @@ 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; @@ -770,51 +660,51 @@ 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_JOINT_POSITION_PRES_CONTROL_CURRENT: { - double T_REF = 0.0; // Torque Reference + //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; + //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 - + // torque feedback torq.err = torq.ref - 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); - + 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 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 + 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 + // 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; @@ -823,20 +713,20 @@ 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; + 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 @@ -844,7 +734,7 @@ 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; @@ -852,18 +742,18 @@ 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; } @@ -871,18 +761,18 @@ // DAC Voltage reference set if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) { CUR_TORQUE_sum += torq.sen; - + 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); - + if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3; if (TORQUE_VREF < 0) TORQUE_VREF = 0; - + ROM_RESET_DATA(); - + //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); dac_1 = TORQUE_VREF / 3.3; } @@ -891,18 +781,18 @@ TMR3_COUNT_TORQUE_NULL = 0; CUR_TORQUE_sum = 0; CUR_TORQUE_mean = 0; - + ROM_RESET_DATA(); - + //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); - + //pc.printf("%f\n", TORQUE_VREF); dac_1 = TORQUE_VREF / 3.3; } TMR3_COUNT_TORQUE_NULL++; break; - } + } case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: { if (TMR3_COUNT_DEADZONE == 0) { @@ -967,9 +857,9 @@ VALVE_DEADZONE_MINUS = VALVE_CENTER; } V_out = 0; - + ROM_RESET_DATA(); - + //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS); //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS); @@ -982,8 +872,8 @@ } case MODE_FIND_HOME: { - - + + ////////////////////////////////////////////FIND_HOME_PWM///////////////////////////////////////////////////// if (FLAG_FIND_HOME == true) { cnt_findhome = 0; @@ -1032,7 +922,7 @@ cnt_findhome = 0; cnt_vel_findhome = 0; } - + /* ////////////////////////////////////////////FIND_HOME_CURRENT///////////////////////////////////////////////////// if (FLAG_FIND_HOME == true) { @@ -1042,7 +932,7 @@ Ref_Joint_Pos = pos.sen; FLAG_FIND_HOME = false; } - + int cnt_check_enc = (TMR_FREQ_5k/500); if(cnt_findhome%cnt_check_enc == 0){ FINDHOME_POSITION = pos.sen; @@ -1050,19 +940,19 @@ 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 < 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; pos.err = Ref_Joint_Pos - pos.sen; - I_REF = 0.001*((double)pos.err); + 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; @@ -1076,7 +966,7 @@ Ref_Joint_Vel = 0.0; REFERENCE_MODE = MODE_REF_COS_INC; CONTROL_MODE = MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION; - + FINDHOME_POSITION = 0; FINDHOME_POSITION_OLD = 0; FINDHOME_VELOCITY = 0; @@ -1084,7 +974,7 @@ cnt_vel_findhome = 0; } */ - + break; } @@ -1135,9 +1025,9 @@ } } if (fl_temp_cnt2 == 100) { - + ROM_RESET_DATA(); - + //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0)); cur_vel_sum = 0; fl_temp_cnt = 0; @@ -1180,7 +1070,7 @@ 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; - + dac_1 = PRES_A_VREF / 3.3; dac_2 = PRES_B_VREF / 3.3; } @@ -1193,7 +1083,7 @@ CUR_PRES_B_mean = 0; ROM_RESET_DATA(); - + dac_1 = PRES_A_VREF / 3.3; dac_2 = PRES_B_VREF / 3.3; //pc.printf("nulling end"); @@ -1225,7 +1115,7 @@ CUR_PRES_B_sum = 0; CUR_PRES_A_mean = 0; CUR_PRES_B_mean = 0; - + ROM_RESET_DATA(); //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0)); @@ -1273,9 +1163,9 @@ VALVE_POS_AVG_OLD = VALVE_POS_AVG[0]; for(i=0; i<25; i++) { VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]); - + //ROM_RESET_DATA(); - + //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) { VALVE_MAX_POS = VALVE_POS_AVG[i]; @@ -1285,9 +1175,9 @@ VALVE_POS_AVG_OLD = VALVE_MIN_POS; } } - + ROM_RESET_DATA(); - + //spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) (VALVE_MAX_POS)); //spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) (VALVE_MIN_POS)); CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS)); @@ -1374,7 +1264,7 @@ //CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (DZ_NUM)); } - }else { + } 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); @@ -1468,9 +1358,11 @@ 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; + VALVE_DEADZONE_PLUS = SECOND_DZ; + ROM_RESET_DATA(); - + //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER); @@ -1566,14 +1458,10 @@ DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ)); first_check = 0; VALVE_DEADZONE_MINUS = FIRST_DZ; - - ROM_RESET_DATA(); - //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); VALVE_DEADZONE_PLUS = SECOND_DZ; - ROM_RESET_DATA(); - + //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); //spi_eeprom_write(RID_DDV_CENTER, DDV_CENTER); CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ); @@ -1616,15 +1504,15 @@ 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; - valve_pos.ref = 50*(ID_index_array[ID_index]) + DDV_CENTER; + valve_pos.ref = 10*(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.ref = 10*(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) pos.sen) > 20000) { + if(abs(0.5 * (double) pos_plus_end + 0.5 * (double) pos_minus_end - (double) pos.sen) > 20000.0) { FINAL_POS = pos.sen; one_period_end = 1; } @@ -1640,23 +1528,23 @@ min_check = 1; } JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec - - ROM_RESET_DATA(); - + + //ROM_RESET_DATA(); + //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF)); //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF)); VALVE_FR_timer = 0; one_period_end = 0; ID_index= ID_index +1; // CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (ID_index)); + V_out = 0.0; } if(max_check == 1 && min_check == 1) { - - ROM_RESET_DATA(); - + //spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index)); VALVE_POS_NUM = ID_index; + ROM_RESET_DATA(); ID_index = 0; first_check = 0; VALVE_FR_timer = 0; @@ -1704,14 +1592,13 @@ //CAN ---------------------------------------------------------------------- //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) { - if (TMR3_COUNT_CAN_TX % 10 == 0) { + if (TMR3_COUNT_CAN_TX % 1000 == 0) { if (flag_data_request[0] == HIGH) { //position+velocity CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0); //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]); - //pc.printf("can good"); // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR); } @@ -1721,6 +1608,7 @@ //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.)); //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM)); CAN_TX_TORQUE((int16_t) (cur.sen)); + //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN)); // CAN_TX_TORQUE((int16_t) DZ_temp_cnt); } @@ -1737,7 +1625,7 @@ if (flag_data_request[3] == HIGH) { //PWM CAN_TX_PWM((int16_t) CUR_PWM); - // CAN_TX_PWM((int16_t) (CONTROL_MODE)); + //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN)); // 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.)); @@ -1762,9 +1650,9 @@ } TMR3_COUNT_CAN_TX++; - - - + + + /******************************************************* *** Valve Control @@ -1787,8 +1675,8 @@ // Saturation of output voltage to 5.0V // if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667 // else if (PWM_out < -0.41667) PWM_out=-0.41667; - - // Saturation of output voltage to 12.0V + + // Saturation of output voltage to 12.0V if(PWM_out > 1.0) PWM_out=1.0; else if (PWM_out < -1.0) PWM_out=-1.0; @@ -1819,7 +1707,7 @@ // can.write(msg); // } - if((CNT_TMR3%5000)==0) { + //if((CNT_TMR3%5000)==0) { // if(LED==1) { // LED=0; // } else @@ -1828,73 +1716,18 @@ // pc.printf("A %f\n", (double) pres_A.sen); // CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[j]); // j++; - } + //} /******************************************************* *** Timer Counting & etc. ********************************************************/ - CNT_TMR3++; + //CNT_TMR3++; + } - TIM3->SR = 0x0; // reset the status register } -/* -void ValveControl(unsigned int ControlMode) -{ - switch (ControlMode) { - case MODE_NO_ACT: // 0 - V_out = 0.0; - break; - case MODE_VALVE_OPEN_LOOP: // 1 - V_out = Vout.ref; - break; - case MODE_VALVE_POSITION_CONTROL: // 2 - CurrentControl(); - break; - case MODE_JOINT_POSITION_TORQUE_CONTROL_PWM: // 3 - V_out = 0.0; - break; - case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: // 4 - double I_REF_POS_FB = 0.0; // I_REF by Position Feedback - double I_REF_POS_FF = 0.0; // I_REF by Position 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; - - cur.ref = I_REF_POS_FF + I_REF_POS_FB; - break; - case MODE_TEST_CURRENT_CONTROL: // 9 - V_out = 0.0; - break; - case MODE_TEST_PWM_CONTROL: // 10 - V_out = 0.0; - break; - case MODE_FIND_HOME: // 22 - V_out = 0.0; - break; - default: - V_out = 0.0; - break; - - } -} -*/ - void CurrentControl() { cur.err = cur.ref - cur.sen;