eeprom_test

Dependencies:   mbed FastPWM

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 //200518
00002 #include "mbed.h"
00003 #include "FastPWM.h"
00004 #include "INIT_HW.h"
00005 #include "function_CAN.h"
00006 #include "SPI_EEP_ENC.h"
00007 #include "I2C_AS5510.h"
00008 #include "setting.h"
00009 #include "function_utilities.h"
00010 #include "stm32f4xx_flash.h"
00011 #include "FlashWriter.h"
00012 
00013 ///191008////
00014 
00015 // dac & check ///////////////////////////////////////////
00016 DigitalOut check(PC_2);
00017 DigitalOut check_2(PC_3);
00018 AnalogOut dac_1(PA_4);
00019 AnalogOut dac_2(PA_5);
00020 AnalogIn adc1(PC_4); //pressure_1
00021 AnalogIn adc2(PB_0); //pressure_2
00022 AnalogIn adc3(PC_1); //current
00023 
00024 
00025 // PWM ///////////////////////////////////////////
00026 float dtc_v=0.0f;
00027 float dtc_w=0.0f;
00028 
00029 // I2C ///////////////////////////////////////////
00030 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
00031 const int i2c_slave_addr1 =  0x56;
00032 unsigned int value; // 10bit output of reading sensor AS5510
00033 
00034 // SPI ///////////////////////////////////////////
00035 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
00036 DigitalOut eeprom_cs(PB_12);
00037 //FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
00038 SPI enc(PC_12,PC_11,PC_10);
00039 DigitalOut enc_cs(PD_2);
00040 DigitalOut LED(PA_15);
00041 
00042 // UART ///////////////////////////////////////////
00043 Serial pc(PA_9,PA_10); //  _ UART
00044 
00045 // CAN ///////////////////////////////////////////
00046 CAN can(PB_8, PB_9, 1000000);
00047 CANMessage msg;
00048 void onMsgReceived()
00049 {
00050     CAN_RX_HANDLER();
00051 }
00052 
00053 // Variables ///////////////////////////////////////////
00054 State pos;
00055 State vel;
00056 State Vout;
00057 State torq;
00058 State pres_A;
00059 State pres_B;
00060 State cur;
00061 State valve_pos;
00062 
00063 State INIT_Vout;
00064 State INIT_Valve_Pos;
00065 State INIT_Pos;
00066 State INIT_torq;
00067 
00068 extern int CID_RX_CMD;
00069 extern int CID_RX_REF_POSITION;
00070 extern int CID_RX_REF_VALVE_POS;
00071 extern int CID_RX_REF_PWM;
00072 
00073 extern int CID_TX_INFO;
00074 extern int CID_TX_POSITION;
00075 extern int CID_TX_TORQUE;
00076 extern int CID_TX_PRES;
00077 extern int CID_TX_VOUT;
00078 extern int CID_TX_VALVE_POSITION;
00079 
00080 float I_REF_fil_DZ = 0.0f;
00081 
00082 // =============================================================================
00083 // =============================================================================
00084 // =============================================================================
00085 
00086 /*******************************************************************************
00087  *  REFERENCE MODE
00088  ******************************************************************************/
00089 enum _REFERENCE_MODE {
00090     MODE_REF_NO_ACT = 0,                                //0
00091     MODE_REF_DIRECT,                                //1
00092     MODE_REF_COS_INC,                                  //2
00093     MODE_REF_LINE_INC,                                 //3
00094     MODE_REF_SIN_WAVE,                                  //4
00095     MODE_REF_SQUARE_WAVE,                                  //5
00096 };
00097 
00098 /*******************************************************************************
00099  *  CONTROL MODE
00100  ******************************************************************************/
00101 enum _CONTROL_MODE {
00102     //control mode
00103     MODE_NO_ACT = 0,                                    //0
00104     MODE_VALVE_POSITION_CONTROL,                        //1
00105     MODE_JOINT_CONTROL,                                 //2
00106 
00107     MODE_VALVE_OPEN_LOOP,                               //3
00108     MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION,  //4
00109     MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING,        //5
00110 
00111     MODE_JOINT_POSITION_PRES_CONTROL_PWM,               //6
00112     MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION,    //7
00113     MODE_VALVE_POSITION_PRES_CONTROL_LEARNING,          //8
00114 
00115     MODE_TEST_CURRENT_CONTROL,                          //9
00116     MODE_TEST_PWM_CONTROL,                              //10
00117 
00118     MODE_CURRENT_CONTROL,                               //11
00119     MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT,         //12
00120     MODE_JOINT_POSITION_PRES_CONTROL_CURRENT,           //13
00121 
00122     //utility
00123     MODE_TORQUE_SENSOR_NULLING = 20,                    //20
00124     MODE_VALVE_NULLING_AND_DEADZONE_SETTING,            //21
00125     MODE_FIND_HOME,                                     //22
00126     MODE_VALVE_GAIN_SETTING,                            //23
00127     MODE_PRESSURE_SENSOR_NULLING,                       //24
00128     MODE_PRESSURE_SENSOR_CALIB,                         //25
00129     MODE_ROTARY_FRICTION_TUNING,                        //26
00130 
00131     MODE_DDV_POS_VS_PWM_ID = 30,                           //30
00132     MODE_DDV_DEADZONE_AND_CENTER,                       //31
00133     MODE_DDV_POS_VS_FLOWRATE,                           //32
00134     MODE_SYSTEM_ID,                                     //33
00135 };
00136 
00137 void SystemClock_Config(void);
00138 
00139 int main()
00140 {
00141     /*********************************
00142     ***     Initialization
00143     *********************************/
00144     HAL_Init();
00145 //    SystemClock_Config();
00146     
00147     
00148 //    LED = 1;
00149 //    pc.baud(9600);
00150 
00151     // i2c init
00152     i2c.frequency(400 * 1000);          // 0.4 mHz
00153     wait_ms(2);                         // Power Up wait
00154     look_for_hardware_i2c();            // Hardware present
00155     init_as5510(i2c_slave_addr1);
00156     make_delay();
00157 
00158 //    // spi init
00159     //eeprom.format(8,3);
00160     //eeprom.frequency(5000000); //5M
00161     enc.format(8,0);
00162     enc.frequency(5000000); //5M
00163     make_delay();
00164 
00165     //rom
00166     //ROM_CALL_DATA();
00167     spi_eeprom_call_data();
00168     make_delay();
00169 
00170     // ADC init
00171     Init_ADC();
00172     make_delay();
00173 
00174     // Pwm init
00175     Init_PWM();
00176     TIM4->CR1 ^= TIM_CR1_UDIS;
00177     make_delay();
00178 
00179     // TMR3 init
00180     Init_TMR3();
00181     TIM3->CR1 ^= TIM_CR1_UDIS;
00182     make_delay();
00183 
00184     // TMR2 init
00185 //    Init_TMR2();
00186 //    TIM2->CR1 ^= TIM_CR1_UDIS;
00187 //    make_delay();
00188 
00189     // CAN
00190 //    can.reset();
00191     CAN_ID_INIT();
00192 //    can.filter(msg.id, 0xFFFFF000, CANStandard);
00193 //    can.frequency(1000000); // set CAN bit rate to 1Mbps
00194     can.filter(CID_RX_CMD, 0xFFF, CANStandard, 0);
00195     can.filter(CID_RX_REF_POSITION, 0xFFF, CANStandard, 1);
00196     can.filter(CID_RX_REF_VALVE_POS, 0xFFF, CANStandard, 2);
00197     can.filter(CID_RX_REF_PWM, 0xFFF, CANStandard, 3);
00198     can.attach(&CAN_RX_HANDLER);
00199     
00200     make_delay();
00201 
00202     //Timer priority
00203     NVIC_SetPriority(TIM3_IRQn, 2);
00204     //NVIC_SetPriority(TIM2_IRQn, 3);
00205     NVIC_SetPriority(TIM4_IRQn, 3);
00206 
00207     
00208 
00209     // spi _ enc
00210     spi_enc_set_init();
00211     make_delay();
00212 
00213     //DAC init
00214     if (SENSING_MODE == 0) {
00215         dac_1 = TORQUE_VREF / 3.3f;
00216         dac_2 = 0.0f;
00217     } else if (SENSING_MODE == 1) {
00218         dac_1 = PRES_A_VREF / 3.3f;
00219         dac_2 = PRES_B_VREF / 3.3f;
00220     }
00221     make_delay();
00222 
00223     for (int i=0; i<50; i++) {
00224         if(i%2==0)
00225             ID_index_array[i] = - i * 0.5f;
00226         else
00227             ID_index_array[i] =  (i+1) * 0.5f;
00228     }
00229 
00230     /************************************
00231     ***     Program is operating!
00232     *************************************/
00233     while(1) {
00234         if(timer_while==1000 && OPERATING_MODE==5) {
00235 //        if(timer_while==1000) {
00236             //i2c
00237             read_field(i2c_slave_addr1);
00238             if(DIR_VALVE_ENC < 0) value = 1023 - value;
00239             //            if(LED==1) {
00240             //                LED=0;
00241             //            } else
00242             //                LED = 1;
00243             timer_while = 0;
00244         }
00245         timer_while ++;
00246         //pc.printf("%d\n", value);
00247     }
00248 }
00249 
00250 float DDV_JOINT_POS_FF(float REF_JOINT_VEL)
00251 {
00252 
00253     int i = 0;
00254     float Ref_Valve_Pos_FF = 0.0f;
00255     for(i=0; i<VALVE_POS_NUM; i++) {
00256         if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <=  max(JOINT_VEL[i],JOINT_VEL[i+1])) {
00257             if(i==0) {
00258                 if(JOINT_VEL[i+1] == JOINT_VEL[i]) {
00259                     Ref_Valve_Pos_FF = (float) VALVE_CENTER;
00260                 } else {
00261                     Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + (float) VALVE_CENTER;
00262                 }
00263             } else {
00264                 if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) {
00265                     Ref_Valve_Pos_FF = (float) VALVE_CENTER;
00266                 } else {
00267                     Ref_Valve_Pos_FF = ((float) 10*(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])) + (float) VALVE_CENTER + (float) (10*ID_index_array[i-1]);
00268                 }
00269             }
00270             break;
00271         }
00272     }
00273     if(REF_JOINT_VEL > max(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
00274         Ref_Valve_Pos_FF = (float) VALVE_MAX_POS;
00275     } else if(REF_JOINT_VEL < min(JOINT_VEL[VALVE_POS_NUM-1], JOINT_VEL[VALVE_POS_NUM-2])) {
00276         Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
00277     }
00278 
00279     Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER);
00280     return Ref_Valve_Pos_FF;
00281 
00282 }
00283 
00284 
00285 void VALVE_POS_CONTROL(float REF_VALVE_POS)
00286 {
00287     int i = 0;
00288 
00289     if(REF_VALVE_POS > VALVE_MAX_POS) {
00290         REF_VALVE_POS = VALVE_MAX_POS;
00291     } else if(REF_VALVE_POS < VALVE_MIN_POS) {
00292         REF_VALVE_POS = VALVE_MIN_POS;
00293     }
00294 
00295     valve_pos_err = REF_VALVE_POS - value;
00296     valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
00297     valve_pos_err_old = valve_pos_err;
00298     valve_pos_err_sum += valve_pos_err;
00299     if (valve_pos_err_sum > 1000) valve_pos_err_sum = 1000;
00300     if (valve_pos_err_sum<-1000) valve_pos_err_sum = -1000;
00301 
00302     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;
00303 
00304     for(i=0; i<24; i++) {
00305         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])) {
00306             if(i==0) {
00307                 VALVE_PWM_RAW_FF = (float) 1000.0f / (float) (VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i]);
00308             } else {
00309                 VALVE_PWM_RAW_FF = (float) 1000.0f* (float) (ID_index_array[i+1] - ID_index_array[i-1])/(VALVE_POS_VS_PWM[i+1] - VALVE_POS_VS_PWM[i-1]) * ((float) REF_VALVE_POS - VALVE_POS_VS_PWM[i-1]) + 1000.0f * (float) ID_index_array[i-1];
00310             }
00311             break;
00312         }
00313     }
00314     Vout.ref = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
00315 }
00316 
00317 #define LT_MAX_IDX  57
00318 float LT_PWM_duty[LT_MAX_IDX] = {-100.0f, -80.0f, -60.0f, -50.0f, -40.0f, -35.0f, -30.0f, -25.0f, -20.0f,
00319                                  -19.0f, -18.0f, -17.0f, -16.0f, -15.0f, -14.0f, -13.0f, -12.0f, -11.0f, -10.0f,
00320                                  -9.0f, -8.0f, -7.0f, -6.0f, -5.0f, -4.0f, -3.0f, -2.0f, -1.0f, 0.0f,
00321                                  1.0f, 2.0f, 3.0f, 4.0f, 5.0f, 6.0f, 7.0f, 8.0f, 9.0f, 10.0f,
00322                                  11.0f, 12.0f, 13.0f, 14.0f, 15.0f, 16.0f, 17.0f, 18.0f, 19.0f, 20.0f,
00323                                  25.0f, 30.0f, 35.0f, 40.0f, 50.0f, 60.0f, 80.0f, 100.0f
00324                                 };  // duty
00325 float LT_Voltage_Output[LT_MAX_IDX] = {-230.0f, -215.0f, -192.5f, -185.0f, -177.5f, -170.0f, -164.0f, -160.0f, -150.0f,
00326                                        -150.0f, -145.0f, -145.0f, -145.0f, -135.0f, -135.0f, -135.0f, -127.5f, -127.5f, -115.0f,
00327                                        -115.0f, -115.0F, -100.0f, -100.0f, -100.0f, -60.0f, -60.0f, -10.0f, -5.0f, 0.0f,
00328                                        7.5f, 14.0f, 14.0f, 14.0f, 42.5f, 42.5f, 42.5f, 80.0f, 80.0f, 105.0f,
00329                                        105.0f, 105.0f, 120.0f, 120.0f, 120.0f, 131.0f, 131.0f, 140.0f, 140.0f, 140.0f,
00330                                        155.0f, 160.0f, 170.0f, 174.0f, 182.0f, 191.0f, 212.0f, 230.0f
00331                                       }; // mV
00332 
00333 float PWM_duty_byLT(float Ref_V)
00334 {
00335     float PWM_duty = 0.0f;
00336     if(Ref_V<LT_Voltage_Output[0]) {
00337         PWM_duty = (Ref_V-LT_Voltage_Output[0])/1.5f+LT_PWM_duty[0];
00338     } else if (Ref_V>=LT_Voltage_Output[LT_MAX_IDX-1]) {
00339         PWM_duty = (Ref_V-LT_Voltage_Output[LT_MAX_IDX-1])/1.5f+LT_PWM_duty[LT_MAX_IDX-1];
00340     } else {
00341         int idx = 0;
00342         for(idx=0; idx<LT_MAX_IDX-1; idx++) {
00343             float ini_x = LT_Voltage_Output[idx];
00344             float fin_x = LT_Voltage_Output[idx+1];
00345             float ini_y = LT_PWM_duty[idx];
00346             float fin_y = LT_PWM_duty[idx+1];
00347             if(Ref_V>=ini_x && Ref_V<fin_x) {
00348                 PWM_duty = (fin_y-ini_y)/(fin_x-ini_x)*(Ref_V-ini_x) + ini_y;
00349                 break;
00350             }
00351         }
00352     }
00353 
00354     return PWM_duty;
00355 }
00356 
00357 
00358 
00359 
00360 
00361 /*******************************************************************************
00362                             TIMER INTERRUPT
00363 *******************************************************************************/
00364 
00365 float FREQ_TMR4 = (float)FREQ_20k;
00366 float DT_TMR4 = (float)DT_20k;
00367 long  CNT_TMR4 = 0;
00368 int   TMR4_FREQ_10k = (int)FREQ_10k;
00369 extern "C" void TIM4_IRQHandler(void)
00370 {
00371     if (TIM4->SR & TIM_SR_UIF ) {
00372 
00373         /*******************************************************
00374         ***     Sensor Read & Data Handling
00375         ********************************************************/
00376 
00377         //Encoder
00378         if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
00379             ENC_UPDATE();
00380         }
00381 
00382         ADC1->CR2  |= 0x40000000;
00383         if (SENSING_MODE == 0) {
00384             // Torque Sensing (0~210)bar =============================================
00385             float pres_A_new = (((float) ADC1->DR) - 2047.5f);
00386             double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 200Hz
00387             pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
00388             torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
00389 
00390 
00391 //        float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
00392 ////        float pres_A_new = ((float)ADC1->DR - PRES_A_NULL)  / PRES_SENSOR_A_PULSE_PER_BAR;
00393 //        float pres_A_new = ((float)ADC1->DR);
00394 //        pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
00395 //        torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047
00396 
00397 
00398         } else if (SENSING_MODE == 1) {
00399             // Pressure Sensing (0~210)bar =============================================
00400             float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
00401             float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
00402             double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 500Hz
00403             pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new;
00404             pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new;
00405             CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR;
00406             CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR;
00407 
00408             if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
00409                 torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.0001f; // mm^3*bar >> Nm
00410             } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
00411                 torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N
00412             }
00413         }
00414 
00415 //        //Pressure sensor A
00416 //        ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
00417 //        //while((ADC1->SR & 0b10));
00418 //        float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
00419 //        float pres_A_new = ((float)ADC1->DR);
00420 //        pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
00421 //        torq.sen = - (pres_A.sen-2048.0f); //pulse -2047~2047    //SW just changed the sign to correct the direction of loadcell on LIGHT. Correct later.
00422 //
00423 //
00424 //        //Pressure sensor B
00425 //        float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
00426 //        float pres_B_new = ((float)ADC2->DR);
00427 //        pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
00428 //        //torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
00429 
00430 
00431         //Current
00432         //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
00433         //int raw_cur = ADC3->DR;
00434         //while((ADC3->SR & 0b10));
00435         float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*500.0f)); // f_cutoff : 500Hz
00436         float cur_new = ((float)ADC3->DR-2048.0f) / 4096.0f * 3.3f * 10.0f / 1.44f; // unit : mA
00437         cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur);
00438         //cur.sen = raw_cur;
00439 
00440         CNT_TMR4++;
00441     }
00442     TIM4->SR = 0x0;  // reset the status register
00443 }
00444 
00445 
00446 int j =0;
00447 float FREQ_TMR3 = (float)FREQ_5k;
00448 float DT_TMR3 = (float)DT_5k;
00449 int cnt_trans = 0;
00450 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
00451 int can_rest =0;
00452 
00453 extern "C" void TIM3_IRQHandler(void)
00454 {
00455     if (TIM3->SR & TIM_SR_UIF ) {
00456 
00457         if (((OPERATING_MODE&0b110)>>1) == 0) {
00458             K_v = 0.4f; // Moog (LPM >> mA) , 100bar
00459             mV_PER_mA = 500.0f; // 5000mV/10mA
00460             mV_PER_pulse = 0.5f; // 5000mV/10000pulse
00461             mA_PER_pulse = 0.001f; // 10mA/10000pulse
00462         } else if (((OPERATING_MODE&0b110)>>1) == 1) {
00463             K_v = 0.5f; // KNR (LPM >> mA) , 100bar
00464             mV_PER_mA = 166.6666f; // 5000mV/30mA
00465             mV_PER_pulse = 0.5f; // 5000mV/10000pulse
00466             mA_PER_pulse = 0.003f; // 30mA/10000pulse
00467         }
00468 
00469         if(MODE_POS_FT_TRANS == 1) {
00470             alpha_trans = (float)(1.0f - cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
00471             cnt_trans++;
00472             torq.err_sum = 0;
00473             if((float)cnt_trans * DT_TMR3 > 3.0f)
00474                 MODE_POS_FT_TRANS = 2;
00475         } else if(MODE_POS_FT_TRANS == 3) {
00476             alpha_trans = (float)(1.0f + cos(3.141592f * (float)cnt_trans * DT_TMR3 /3.0f))/2.0f;
00477             cnt_trans++;
00478             torq.err_sum = 0;
00479             if((float) cnt_trans * DT_TMR3 > 3.0f )
00480                 MODE_POS_FT_TRANS = 0;
00481         } else if(MODE_POS_FT_TRANS == 2) {
00482             alpha_trans = 1.0f;
00483             cnt_trans = 0;
00484         } else {
00485             alpha_trans = 0.0f;
00486             cnt_trans = 0;
00487         }
00488 
00489 
00490         int UTILITY_MODE = 0;
00491         int CONTROL_MODE = 0;
00492 
00493         if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) {
00494             UTILITY_MODE = CONTROL_UTILITY_MODE;
00495             CONTROL_MODE = MODE_NO_ACT;
00496         } else {
00497             CONTROL_MODE = CONTROL_UTILITY_MODE;
00498             UTILITY_MODE = MODE_NO_ACT;
00499         }
00500 
00501 
00502 
00503         // UTILITY MODE ------------------------------------------------------------
00504 
00505         switch (UTILITY_MODE) {
00506             case MODE_NO_ACT: {
00507                 break;
00508             }
00509 
00510             case MODE_TORQUE_SENSOR_NULLING: {
00511 
00512                 if(SENSING_MODE == 0){
00513 
00514                     // DAC Voltage reference set
00515                     if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
00516                         CUR_TORQUE_sum += torq.sen;
00517 
00518                         if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
00519                             CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f;
00520                             CUR_TORQUE_sum = 0;
00521 
00522                             TORQUE_VREF += 0.0003f * (0.0f - CUR_TORQUE_mean);
00523 
00524                             if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
00525                             if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f;
00526 
00527                             //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
00528                             dac_1 = TORQUE_VREF / 3.3f;
00529                         }
00530                     } else {
00531                         CONTROL_UTILITY_MODE = MODE_NO_ACT;
00532                         TMR3_COUNT_TORQUE_NULL = 0;
00533                         CUR_TORQUE_sum = 0;
00534                         CUR_TORQUE_mean = 0;
00535 
00536                         ROM_RESET_DATA();
00537 
00538                         dac_1 = TORQUE_VREF / 3.3f;
00539                     }
00540             } else {
00541                 if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
00542                     CUR_PRES_A_sum += pres_A.sen;
00543                     CUR_PRES_B_sum += pres_B.sen;
00544 
00545                     if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
00546                         CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f;
00547                         CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f;
00548                         CUR_PRES_A_sum = 0;
00549                         CUR_PRES_B_sum = 0;
00550 
00551                         float VREF_NullingGain = 0.0003f;
00552                         PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean;
00553                         PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean;
00554 
00555                         if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
00556                         if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
00557                         if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
00558                         if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
00559 
00560                         dac_1 = PRES_A_VREF / 3.3f;
00561                         dac_2 = PRES_B_VREF / 3.3f;
00562                     }
00563                 } else {
00564                     CONTROL_UTILITY_MODE = MODE_NO_ACT;
00565                     TMR3_COUNT_TORQUE_NULL = 0;
00566                     CUR_PRES_A_sum = 0;
00567                     CUR_PRES_B_sum = 0;
00568                     CUR_PRES_A_mean = 0;
00569                     CUR_PRES_B_mean = 0;
00570 
00571                     ROM_RESET_DATA();
00572 
00573                     dac_1 = PRES_A_VREF / 3.3f;
00574                     dac_2 = PRES_B_VREF / 3.3f;
00575                     //pc.printf("nulling end");
00576                 }
00577             }
00578             TMR3_COUNT_TORQUE_NULL++;
00579             break;
00580         }
00581 
00582 //            case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
00583 //                if (TMR3_COUNT_DEADZONE == 0) {
00584 //                    if (pos_plus_end == pos_minus_end) need_enc_init = true;
00585 //                    else temp_time = 0;
00586 //                }
00587 //                if (need_enc_init) {
00588 //                    if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) {
00589 //                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
00590 //                        pos_plus_end = pos.sen;
00591 //                    } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) {
00592 //                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
00593 //                        pos_minus_end = pos.sen;
00594 //                    } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false;
00595 //                    temp_time = TMR_FREQ_5k;
00596 //                }
00597 //
00598 //                if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) {
00599 //                    V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen);
00600 //                    VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0;
00601 //
00602 //                } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) {
00603 //                    V_out = 0;
00604 //                    CUR_VELOCITY_sum += CUR_VELOCITY;
00605 //                } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) {
00606 //                    if (CUR_VELOCITY_sum == 0) DZ_dir = 1;
00607 //                    else if (CUR_VELOCITY_sum > 0) DZ_dir = 1;
00608 //                    else if (CUR_VELOCITY_sum < 0) DZ_dir = -1;
00609 //                    else DZ_temp_cnt2 = DZ_end;
00610 //                    CUR_VELOCITY_sum = 0;
00611 //                } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) {
00612 //                    if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end;
00613 //
00614 //                    // Position of Dead Zone
00615 //                    //  (CUR_VELOCITY < 0)  (CUR_VELOCITY == 0)  (CUR_VELOCITY > 0)
00616 //                    //     |        /                 |    /                      |/
00617 //                    //     | ______/               ___|___/                ______/|
00618 //                    //     |/                     /   |                   /       |
00619 //                    //    /|                     /    |                  /        |
00620 //                    //     0V                         0V                          0V
00621 //
00622 //                    if (DZ_temp_cnt2 < DZ_end) {
00623 //                        if (TMR3_COUNT_DEADZONE % 20 != 0) {
00624 //                            CUR_VELOCITY_sum += CUR_VELOCITY;
00625 //                        } else {
00626 //                            V_out -= DZ_dir;
00627 //                            if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++;
00628 //                            CUR_VELOCITY_sum = 0;
00629 //                        }
00630 //                        if (DZ_temp_cnt == 5) {
00631 //                            if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out;
00632 //                            else VALVE_DEADZONE_PLUS = (int16_t) V_out;
00633 //                            DZ_dir = -DZ_dir;
00634 //                            DZ_temp_cnt = 0;
00635 //                            DZ_temp_cnt2++;
00636 //                        }
00637 //                    } else {
00638 //                        TMR3_COUNT_DEADZONE = -1;
00639 //                        VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2;
00640 //                        if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) {
00641 //                            VALVE_DEADZONE_PLUS = VALVE_CENTER;
00642 //                            VALVE_DEADZONE_MINUS = VALVE_CENTER;
00643 //                        }
00644 //                        V_out = 0;
00645 //
00646 //                        ROM_RESET_DATA();
00647 //
00648 //                        //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS);
00649 //                        //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS);
00650 //
00651 //                        CONTROL_MODE = MODE_NO_ACT;
00652 //                        DZ_temp_cnt2 = 0;
00653 //                    }
00654 //                }
00655 //                TMR3_COUNT_DEADZONE++;
00656 //                break;
00657 //            }
00658 
00659             case MODE_FIND_HOME: {
00660                 if (FINDHOME_STAGE == FINDHOME_INIT) {
00661                     cnt_findhome = 0;
00662                     cnt_vel_findhome = 0;
00663                     //REFERENCE_MODE = MODE_REF_NO_ACT; // Stop taking reference data from PODO
00664                     pos.ref = pos.sen;
00665                     vel.ref = 0.0f;
00666                     I_ERR_INT = 0.0f;
00667                     FINDHOME_STAGE = FINDHOME_GOTOLIMIT;
00668                 } else if (FINDHOME_STAGE == FINDHOME_GOTOLIMIT) {
00669                     int cnt_check_enc = (TMR_FREQ_5k/20);
00670                     if(cnt_findhome%cnt_check_enc == 0) {
00671                         FINDHOME_POSITION = pos.sen;
00672                         FINDHOME_VELOCITY = FINDHOME_POSITION - FINDHOME_POSITION_OLD;
00673                         FINDHOME_POSITION_OLD = FINDHOME_POSITION;
00674                     }
00675                     cnt_findhome++;
00676 
00677                     if (abs(FINDHOME_VELOCITY) <= 1) {
00678                         cnt_vel_findhome = cnt_vel_findhome + 1;
00679                     } else {
00680                         cnt_vel_findhome = 0;
00681                     }
00682 
00683                     if ((cnt_vel_findhome < 3*TMR_FREQ_5k) &&  cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec
00684                         //REFERENCE_MODE = MODE_REF_NO_ACT;
00685                         if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f;
00686                         else pos.ref = pos.ref - 12.0f;
00687 
00688                         pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
00689 
00690                         if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
00691 
00692                         double I_REF_POS = 0.0f;
00693                         double temp_vel_pos = 0.0f;
00694                         double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
00695 
00696                         if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
00697                             temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // rad/s
00698                         } else if ((OPERATING_MODE && 0x01) == 1) {
00699                             temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // mm/s
00700                         }
00701                         if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
00702                         else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
00703 
00704                         I_REF = I_REF_POS;
00705                         I_ERR_INT = 0.0f;
00706 
00707 
00708                         } else {
00709                             float VALVE_POS_RAW_FORCE_FB = 0.0f;
00710                             VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
00711 
00712                             if (VALVE_POS_RAW_FORCE_FB >= 0) {
00713                                 valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
00714                             } else {
00715                                 valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
00716                             }
00717 
00718                             VALVE_POS_CONTROL(valve_pos.ref);
00719 
00720                             V_out = (float) Vout.ref;
00721 
00722                         }
00723 
00724 
00725 
00726 //                        pos.err = pos.ref_home_pos - pos.sen;
00727 //                        float VALVE_POS_RAW_POS_FB = 0.0f;
00728 //                        VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
00729 //                        valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
00730 //                        VALVE_POS_CONTROL(valve_pos.ref);
00731 
00732                         cnt_findhome_wait = 0;
00733 //                        CONTROL_MODE = MODE_JOINT_CONTROL;
00734                         alpha_trans = 0.0f;
00735 
00736 
00737                     } else {
00738                         ENC_SET(HOMEPOS_OFFSET);
00739 //                        ENC_SET_ZERO();
00740                         INIT_REF_POS = HOMEPOS_OFFSET;
00741                         REF_POSITION = 0;
00742                         REF_VELOCITY = 0;
00743                         FINDHOME_POSITION = 0;
00744                         FINDHOME_POSITION_OLD = 0;
00745                         FINDHOME_VELOCITY = 0;
00746 
00747                         cnt_findhome = 0;
00748                         cnt_vel_findhome = 0;
00749                         FINDHOME_STAGE = FINDHOME_ZEROPOSE;
00750 
00751                         pos.ref = 0.0f;
00752                         vel.ref = 0.0f;
00753                         pos.ref_home_pos = 0.0f;
00754                         vel.ref_home_pos = 0.0f;
00755                         
00756                         I_ERR_INT = 0.0f;
00757                         
00758 //                        cnt_findhome_wait = 0;
00759 //                        FINDHOME_STAGE = FINDHOME_INIT;
00760 //                        CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
00761                     }
00762                     
00763 //                } else if (FINDHOME_STAGE == FINDHOME_WAIT)     {
00764 //                    cnt_findhome_wait = cnt_findhome_wait + 1;
00765 //                    if(cnt_findhome_wait >  3*TMR_FREQ_5k){
00766 //                        FINDHOME_STAGE = FINDHOME_ZEROPOSE;
00767 //                        pos.sen = 0.0f;
00768 //                        vel.sen = 0.0f;
00769 //                    }
00770                     
00771                 } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
00772                     int T_move = 2*TMR_FREQ_5k;
00773                     pos.ref = (0.0f - (float)INIT_REF_POS)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)INIT_REF_POS;
00774                     vel.ref = 0.0f;
00775                     
00776                     // input for position control
00777                 
00778 //                    CONTROL_MODE = MODE_JOINT_CONTROL;
00779                     pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
00780 
00781                     if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
00782 
00783                         double I_REF_POS = 0.0f;
00784 
00785                         double temp_vel_pos = 0.0f;
00786                         double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
00787 
00788                         if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
00789                             temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // rad/s
00790                         } else if ((OPERATING_MODE && 0x01) == 1) {
00791                             temp_vel_pos = 0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err; // mm/s
00792                         }
00793                         if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
00794                         else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
00795 
00796                         I_REF = I_REF_POS;
00797                         I_ERR_INT = 0.0f;
00798 
00799 
00800 
00801                     } else {
00802                         float VALVE_POS_RAW_FORCE_FB = 0.0f;
00803                         VALVE_POS_RAW_FORCE_FB = DDV_JOINT_POS_FF(vel.sen) + (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
00804 
00805                         if (VALVE_POS_RAW_FORCE_FB >= 0) {
00806                             valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
00807                         } else {
00808                             valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
00809                         }
00810 
00811                         VALVE_POS_CONTROL(valve_pos.ref);
00812 
00813                         V_out = (float) Vout.ref;
00814 
00815                     }
00816                     
00817 //                    pos.err = pos.ref - (float)pos.sen;
00818 //                    float VALVE_POS_RAW_POS_FB = 0.0f;
00819 //                    VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
00820 //                    valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
00821 //                    VALVE_POS_CONTROL(valve_pos.ref);
00822 
00823                     cnt_findhome++;
00824                     if (cnt_findhome >= T_move) {
00825                         //REFERENCE_MODE = MODE_REF_DIRECT;
00826                         cnt_findhome = 0;
00827                         pos.ref = 0.0f;
00828                         vel.ref = 0.0f;
00829                         pos.ref_home_pos = 0.0f;
00830                         vel.ref_home_pos = 0.0f;
00831                         FINDHOME_STAGE = FINDHOME_INIT;
00832                         CONTROL_UTILITY_MODE = MODE_JOINT_CONTROL;
00833                     }
00834                 }
00835 
00836                 break;
00837             }
00838 
00839 //            case MODE_VALVE_GAIN_SETTING: {
00840 //                if (TMR3_COUNT_FLOWRATE == 0) {
00841 //                    if (pos_plus_end == pos_minus_end) need_enc_init = true;
00842 //                    else {
00843 //                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
00844 //                        temp_time = (int) (0.5f * (float) TMR_FREQ_5k);
00845 //                    }
00846 //                }
00847 //                if (need_enc_init) {
00848 //                    if (TMR3_COUNT_FLOWRATE < (int) (0.5f * (float) TMR_FREQ_5k)) {
00849 //                        V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
00850 //                        pos_plus_end = pos.sen;
00851 //                    } else if (TMR3_COUNT_FLOWRATE < TMR_FREQ_5k) {
00852 //                        V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
00853 //                        pos_minus_end = pos.sen;
00854 //                    } else if (TMR3_COUNT_FLOWRATE == TMR_FREQ_5k) {
00855 //                        need_enc_init = false;
00856 //                        check_vel_pos_init = (int) (0.9f * (float) (pos_plus_end - pos_minus_end));
00857 //                        check_vel_pos_fin = (int) (0.95f * (float) (pos_plus_end - pos_minus_end));
00858 //                        check_vel_pos_interv = check_vel_pos_fin - check_vel_pos_init;
00859 //                    }
00860 //                    temp_time = TMR_FREQ_5k;
00861 //                }
00862 //                TMR3_COUNT_FLOWRATE++;
00863 //                if (TMR3_COUNT_FLOWRATE > temp_time) {
00864 //                    if (flag_flowrate % 2 == 0) { // (+)
00865 //                        VALVE_VOLTAGE = 1000.0f * (float) (flag_flowrate / 2 + 1);
00866 //                        V_out = VALVE_VOLTAGE;
00867 //                        if (pos.sen > (pos_minus_end + check_vel_pos_init) && pos.sen < (pos_minus_end + check_vel_pos_fin)) {
00868 //                            fl_temp_cnt++;
00869 //                        } else if (pos.sen >= (pos_minus_end + check_vel_pos_fin) && CUR_VELOCITY == 0) {
00870 //                            VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0 * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // 0.9587=6*pi/65536*10000 0.5757=0.02525*0.02*0.0095*2*60*1000
00871 //                            //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / VALVE_VOLTAGE; // PULSE/sec
00872 //                            fl_temp_cnt2++;
00873 //                        }
00874 //                    } else if (flag_flowrate % 2 == 1) { // (-)
00875 //                        VALVE_VOLTAGE = -1. * (float) (flag_flowrate / 2 + 1);
00876 //                        V_out = VALVE_VOLTAGE;
00877 //                        if (pos.sen < (pos_plus_end - check_vel_pos_init) && pos.sen > (pos_plus_end - check_vel_pos_fin)) {
00878 //                            fl_temp_cnt++;
00879 //                        } else if (pos.sen <= (pos_plus_end - check_vel_pos_fin) && CUR_VELOCITY == 0) {
00880 //                            VALVE_GAIN_LPM_PER_V[flag_flowrate] = 0.95873f * 0.5757f * (float) TMR_FREQ_5k / 10000.0f * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE);
00881 //                            //                        VALVE_GAIN_LPM_PER_V[flag_flowrate] = (float) TMR_FREQ_10k * (float) check_vel_pos_interv / (float) fl_temp_cnt / (-VALVE_VOLTAGE); // PULSE/sec
00882 //                            fl_temp_cnt2++;
00883 //                        }
00884 //                    }
00885 //                    if (fl_temp_cnt2 == 100) {
00886 //
00887 //                        ROM_RESET_DATA();
00888 //
00889 //                        //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f));
00890 //                        cur_vel_sum = 0;
00891 //                        fl_temp_cnt = 0;
00892 //                        fl_temp_cnt2 = 0;
00893 //                        flag_flowrate++;
00894 //                    }
00895 //                    if (flag_flowrate == 10) {
00896 //                        V_out = 0;
00897 //                        flag_flowrate = 0;
00898 //                        TMR3_COUNT_FLOWRATE = 0;
00899 //                        valve_gain_repeat_cnt++;
00900 //                        if (valve_gain_repeat_cnt >= 1) {
00901 //                            CONTROL_MODE = MODE_NO_ACT;
00902 //                            valve_gain_repeat_cnt = 0;
00903 //                        }
00904 //
00905 //                    }
00906 //                    break;
00907 //                }
00908 //
00909 //            }
00910             case MODE_PRESSURE_SENSOR_NULLING: {
00911                 // DAC Voltage reference set
00912                 if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
00913                     CUR_PRES_A_sum += pres_A.sen;
00914                     CUR_PRES_B_sum += pres_B.sen;
00915 
00916                     if (TMR3_COUNT_PRES_NULL % 10 == 0) {
00917                         CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f;
00918                         CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f;
00919                         CUR_PRES_A_sum = 0;
00920                         CUR_PRES_B_sum = 0;
00921 
00922                         float VREF_NullingGain = 0.0003f;
00923                         PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean;
00924                         PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean;
00925 
00926                         if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
00927                         if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
00928                         if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
00929                         if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
00930 
00931                         dac_1 = PRES_A_VREF / 3.3f;
00932                         dac_2 = PRES_B_VREF / 3.3f;
00933                     }
00934                 } else {
00935                     CONTROL_UTILITY_MODE = MODE_NO_ACT;
00936                     TMR3_COUNT_PRES_NULL = 0;
00937                     CUR_PRES_A_sum = 0;
00938                     CUR_PRES_B_sum = 0;
00939                     CUR_PRES_A_mean = 0;
00940                     CUR_PRES_B_mean = 0;
00941 
00942                     ROM_RESET_DATA();
00943 
00944                     dac_1 = PRES_A_VREF / 3.3f;
00945                     dac_2 = PRES_B_VREF / 3.3f;
00946                     //pc.printf("nulling end");
00947                 }
00948                 TMR3_COUNT_PRES_NULL++;
00949                 break;
00950             }
00951 
00952 //            case MODE_PRESSURE_SENSOR_CALIB: {
00953 //                if (TMR3_COUNT_PRES_CALIB < 2 * TMR_FREQ_5k) {
00954 //                    V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
00955 //                    if (TMR3_COUNT_PRES_CALIB >= TMR_FREQ_5k) {
00956 //                        CUR_PRES_A_sum += CUR_PRES_A;
00957 //                    }
00958 //                } else if (TMR3_COUNT_PRES_CALIB < 4 * TMR_FREQ_5k) {
00959 //                    V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
00960 //                    if (TMR3_COUNT_PRES_CALIB >= 3 * TMR_FREQ_5k) {
00961 //                        CUR_PRES_B_sum += CUR_PRES_B;
00962 //                    }
00963 //                } else {
00964 //                    CONTROL_MODE = MODE_NO_ACT;
00965 //                    TMR3_COUNT_PRES_CALIB = 0;
00966 //                    V_out = 0;
00967 //                    PRES_SENSOR_A_PULSE_PER_BAR = CUR_PRES_A_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_A_NULL;
00968 //                    PRES_SENSOR_A_PULSE_PER_BAR = PRES_SENSOR_A_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
00969 //                    PRES_SENSOR_B_PULSE_PER_BAR = CUR_PRES_B_sum / ((float) TMR_FREQ_5k - 1.0f) - PRES_B_NULL;
00970 //                    PRES_SENSOR_B_PULSE_PER_BAR = PRES_SENSOR_B_PULSE_PER_BAR / ((float) PRES_SUPPLY - 1.0f);
00971 //                    CUR_PRES_A_sum = 0;
00972 //                    CUR_PRES_B_sum = 0;
00973 //                    CUR_PRES_A_mean = 0;
00974 //                    CUR_PRES_B_mean = 0;
00975 //
00976 //                    ROM_RESET_DATA();
00977 //
00978 //                    //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f));
00979 //                    //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f));
00980 //                }
00981 //                TMR3_COUNT_PRES_CALIB++;
00982 //                break;
00983 //            }
00984 
00985 //            case MODE_ROTARY_FRICTION_TUNING: {
00986 //                if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4.0f + 3.0f * sin(2 * 3.14159f * 0.5f * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f * 0.05f);
00987 //                V_out = PWM_out * sin(2 * 3.14159f * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001f);
00988 //                if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT * 1000.0f;
00989 //                else V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f;
00990 //                TMR3_COUNT_ROTARY_FRIC_TUNE++;
00991 //                if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) {
00992 //                    TMR3_COUNT_ROTARY_FRIC_TUNE = 0;
00993 //                    V_out = 0.0f;
00994 //                    CONTROL_MODE = MODE_NO_ACT;
00995 //                }
00996 //                break;
00997 //            }
00998 
00999             case MODE_DDV_POS_VS_PWM_ID: {
01000                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
01001                 VALVE_ID_timer = VALVE_ID_timer + 1;
01002 
01003                 if(VALVE_ID_timer < TMR_FREQ_5k*1) {
01004                     Vout.ref = 3000.0f * sin(2.0f*3.14f*VALVE_ID_timer/TMR_FREQ_5k * 100.0f);
01005                 } else if(VALVE_ID_timer < TMR_FREQ_5k*2) {
01006                     Vout.ref = 1000.0f*(ID_index_array[ID_index]);
01007                 } else if(VALVE_ID_timer == TMR_FREQ_5k*2) {
01008                     VALVE_POS_TMP = 0;
01009                     data_num = 0;
01010                 } else if(VALVE_ID_timer < TMR_FREQ_5k*3) {
01011                     data_num = data_num + 1;
01012                     VALVE_POS_TMP = VALVE_POS_TMP + value;
01013                 } else if(VALVE_ID_timer == TMR_FREQ_5k*3) {
01014                     Vout.ref = 0.0f;
01015                 } else {
01016                     VALVE_POS_AVG[ID_index] = VALVE_POS_TMP / data_num;
01017                     VALVE_ID_timer = 0;
01018                     ID_index= ID_index +1;
01019                 }
01020 
01021                 if(ID_index>=25) {
01022                     int i;
01023                     VALVE_POS_AVG_OLD = VALVE_POS_AVG[0];
01024                     for(i=0; i<25; i++) {
01025                         VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]);
01026                         if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) {
01027                             VALVE_MAX_POS = VALVE_POS_AVG[i];
01028                             VALVE_POS_AVG_OLD = VALVE_MAX_POS;
01029                         } else if(VALVE_POS_AVG[i] < VALVE_POS_AVG_OLD) {
01030                             VALVE_MIN_POS = VALVE_POS_AVG[i];
01031                             VALVE_POS_AVG_OLD = VALVE_MIN_POS;
01032                         }
01033                     }
01034                     ROM_RESET_DATA();
01035                     ID_index = 0;
01036                     CONTROL_UTILITY_MODE = MODE_NO_ACT;
01037                 }
01038 
01039 
01040                 break;
01041             }
01042 
01043             case MODE_DDV_DEADZONE_AND_CENTER: {
01044                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
01045                 VALVE_DZ_timer = VALVE_DZ_timer + 1;
01046                 if(first_check == 0) {
01047                     if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
01048                         Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
01049                     } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
01050                         Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
01051                         pos_plus_end = pos.sen;
01052                     } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
01053                         Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
01054                     } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
01055                         Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
01056                         pos_minus_end = pos.sen;
01057                     } else if(VALVE_DZ_timer < (int) (3.0f * (float) TMR_FREQ_5k)) {
01058                         Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
01059                     } else if(VALVE_DZ_timer < (int) (4.0f * (float) TMR_FREQ_5k)) {
01060                         Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
01061                         data_num = data_num + 1;
01062                         VALVE_POS_TMP = VALVE_POS_TMP + value;
01063                     } else if(VALVE_DZ_timer == (int) (4.0f * (float) TMR_FREQ_5k)) {
01064                         Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
01065                         DDV_POS_AVG = VALVE_POS_TMP / data_num;
01066                         START_POS = pos.sen;
01067                         VALVE_POS_TMP = 0;
01068                         data_num = 0;
01069 
01070                     } else if(VALVE_DZ_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
01071                         valve_pos.ref = DDV_POS_AVG;
01072                         VALVE_POS_CONTROL(valve_pos.ref);
01073 
01074                     } else if(VALVE_DZ_timer < (int) (6.0f * (float) TMR_FREQ_5k)) {
01075                         valve_pos.ref = DDV_POS_AVG;
01076                         VALVE_POS_CONTROL(valve_pos.ref);
01077 
01078                     } else if(VALVE_DZ_timer == (int) (6.0f * (float) TMR_FREQ_5k)) {
01079                         valve_pos.ref = DDV_POS_AVG;
01080                         VALVE_POS_CONTROL(valve_pos.ref);
01081                         FINAL_POS = pos.sen;
01082 
01083                         if((FINAL_POS - START_POS)>200) {
01084                             DZ_case = 1;
01085                         } else if((FINAL_POS - START_POS)<-200) {
01086                             DZ_case = -1;
01087                         } else {
01088                             DZ_case = 0;
01089                         }
01090 
01091                         CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
01092 
01093                         first_check = 1;
01094                         DZ_DIRECTION = 1;
01095                         VALVE_DZ_timer = 0;
01096                         Ref_Valve_Pos_Old = DDV_POS_AVG;
01097                         DZ_NUM = 1;
01098                         DZ_index = 1;
01099 
01100                     }
01101                 } else {
01102                     if((DZ_case == -1 && DZ_NUM == 1) | (DZ_case == 1 && DZ_NUM == 1)) {
01103                         if(VALVE_DZ_timer < (int) (1.0 * (float) TMR_FREQ_5k)) {
01104                             Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
01105                             //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
01106                             //CONTROL_MODE = MODE_JOINT_CONTROL;
01107                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
01108                             START_POS = pos.sen;
01109                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
01110                             valve_pos.ref = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
01111                             if(valve_pos.ref <= VALVE_MIN_POS) {
01112                                 valve_pos.ref = VALVE_MIN_POS;
01113                             } else if(valve_pos.ref >= VALVE_MAX_POS) {
01114                                 valve_pos.ref = VALVE_MAX_POS;
01115                             }
01116                             VALVE_POS_CONTROL(valve_pos.ref);
01117 
01118                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
01119                             Ref_Valve_Pos_Old = valve_pos.ref;
01120                             FINAL_POS = pos.sen;
01121 
01122                             if((FINAL_POS - START_POS)>100) {
01123                                 DZ_DIRECTION = 1 * DZ_case;
01124                             } else if((FINAL_POS - START_POS)<-100) {
01125                                 DZ_DIRECTION = -1 * DZ_case;
01126                             } else {
01127                                 DZ_DIRECTION = 1 * DZ_case;
01128                             }
01129 
01130                             VALVE_DZ_timer = 0;
01131                             DZ_index= DZ_index *2;
01132                             if(DZ_index >= 128) {
01133                                 FIRST_DZ = valve_pos.ref;
01134                                 DZ_NUM = 2;
01135                                 Ref_Valve_Pos_Old = FIRST_DZ;
01136                                 DZ_index = 1;
01137                                 DZ_DIRECTION = 1;
01138                             }
01139                         }
01140                     } else if((DZ_case == -1 && DZ_NUM == 2) | (DZ_case == 1 && DZ_NUM == 2)) {
01141                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
01142                             Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
01143                             //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
01144                             //CONTROL_MODE = MODE_JOINT_CONTROL;
01145                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
01146                             START_POS = pos.sen;
01147                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
01148                             valve_pos.ref = Ref_Valve_Pos_Old  - DZ_case * DZ_DIRECTION * 64 / DZ_index;
01149                             if(valve_pos.ref <= VALVE_MIN_POS) {
01150                                 valve_pos.ref = VALVE_MIN_POS;
01151                             } else if(valve_pos.ref >= VALVE_MAX_POS) {
01152                                 valve_pos.ref = VALVE_MAX_POS;
01153                             }
01154                             VALVE_POS_CONTROL(valve_pos.ref);
01155 
01156                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
01157                             Vout.ref = 0.0f;
01158                         } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
01159                             Ref_Valve_Pos_Old = valve_pos.ref;
01160                             FINAL_POS = pos.sen;
01161 
01162                             if((FINAL_POS - START_POS)>100) {
01163                                 DZ_DIRECTION = 1 * DZ_case;
01164                             } else if((FINAL_POS - START_POS)<-100) {
01165                                 DZ_DIRECTION = -1 * DZ_case;
01166                             } else {
01167                                 DZ_DIRECTION = -1 * DZ_case;
01168                             }
01169 
01170                             VALVE_DZ_timer = 0;
01171                             DZ_index= DZ_index * 2;
01172                             if(DZ_index >= 128) {
01173                                 SECOND_DZ = valve_pos.ref;
01174                                 VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
01175                                 first_check = 0;
01176                                 VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
01177                                 VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
01178 
01179                                 ROM_RESET_DATA();
01180 
01181                                 CONTROL_UTILITY_MODE = MODE_NO_ACT;
01182                                 DZ_index = 1;
01183                             }
01184                         }
01185                     } else if(DZ_case == 0 && DZ_NUM ==1) {
01186                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
01187                             Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
01188                             //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
01189                             //CONTROL_MODE = MODE_JOINT_CONTROL;
01190                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
01191                             START_POS = pos.sen;
01192                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
01193                             valve_pos.ref = Ref_Valve_Pos_Old  - DZ_DIRECTION * 64 / DZ_index;
01194                             if(valve_pos.ref <= VALVE_MIN_POS) {
01195                                 valve_pos.ref = VALVE_MIN_POS;
01196                             } else if(valve_pos.ref >= VALVE_MAX_POS) {
01197                                 valve_pos.ref = VALVE_MAX_POS;
01198                             }
01199                             VALVE_POS_CONTROL(valve_pos.ref);
01200 
01201                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
01202                             Ref_Valve_Pos_Old = valve_pos.ref;
01203                             FINAL_POS = pos.sen;
01204 
01205                             if((FINAL_POS - START_POS)>100) {
01206                                 DZ_DIRECTION = 1;
01207                             } else if((FINAL_POS - START_POS)<-100) {
01208                                 DZ_DIRECTION = -1;
01209                             } else {
01210                                 DZ_DIRECTION = 1;
01211                             }
01212                             VALVE_DZ_timer = 0;
01213                             DZ_index= DZ_index *2;
01214                             if(DZ_index >= 128) {
01215                                 FIRST_DZ = valve_pos.ref;
01216                                 DZ_NUM = 2;
01217                                 Ref_Valve_Pos_Old = FIRST_DZ;
01218                                 DZ_index = 1;
01219                                 DZ_DIRECTION = 1;
01220                             }
01221                         }
01222                     } else {
01223                         if(VALVE_DZ_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
01224                             Vout.ref = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
01225                             //pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
01226                             //CONTROL_MODE = MODE_JOINT_CONTROL;
01227                         } else if(VALVE_DZ_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
01228                             START_POS = pos.sen;
01229                         } else if(VALVE_DZ_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
01230                             valve_pos.ref = Ref_Valve_Pos_Old  + DZ_DIRECTION * 64 / DZ_index;
01231                             if(valve_pos.ref <= VALVE_MIN_POS) {
01232                                 valve_pos.ref = VALVE_MIN_POS;
01233                             } else if(valve_pos.ref > VALVE_MAX_POS) {
01234                                 valve_pos.ref = VALVE_MAX_POS - 1;
01235                             }
01236                             VALVE_POS_CONTROL(valve_pos.ref);
01237 
01238                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
01239                             Vout.ref = 0.0f;
01240                         } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
01241                             Ref_Valve_Pos_Old = valve_pos.ref;
01242                             FINAL_POS = pos.sen;
01243 
01244                             if((FINAL_POS - START_POS)>100) {
01245                                 DZ_DIRECTION = 1;
01246                             } else if((FINAL_POS - START_POS)<-100) {
01247                                 DZ_DIRECTION = -1;
01248                             } else {
01249                                 DZ_DIRECTION = 1;
01250                             }
01251 
01252                             VALVE_DZ_timer = 0;
01253                             DZ_index= DZ_index *2;
01254                             if(DZ_index >= 128) {
01255                                 SECOND_DZ = valve_pos.ref;
01256                                 VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
01257                                 first_check = 0;
01258                                 VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
01259                                 VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
01260 
01261                                 ROM_RESET_DATA();
01262 
01263                                 CONTROL_UTILITY_MODE = MODE_NO_ACT;
01264                                 DZ_index = 1;
01265                             }
01266                         }
01267                     }
01268                 }
01269                 break;
01270             }
01271 
01272             case MODE_DDV_POS_VS_FLOWRATE: {
01273                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
01274                 VALVE_FR_timer = VALVE_FR_timer + 1;
01275                 if(first_check == 0) {
01276                     if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
01277                         Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
01278                         //CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
01279                     } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
01280                         Vout.ref = VALVE_VOLTAGE_LIMIT * 1000.0f;
01281                         pos_plus_end = pos.sen;
01282                         //                    CAN_TX_PRES((int16_t) (V_out), (int16_t) (7));
01283                     } else if(VALVE_FR_timer < (int) (2.0f * (float) TMR_FREQ_5k)) {
01284                         Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
01285                     } else if(VALVE_FR_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
01286                         //                    CAN_TX_PRES((int16_t) (V_out), (int16_t) (8));
01287                         Vout.ref = -VALVE_VOLTAGE_LIMIT * 1000.0f;
01288                         pos_minus_end = pos.sen;
01289                         first_check = 1;
01290                         VALVE_FR_timer = 0;
01291                         valve_pos.ref = (float) VALVE_CENTER;
01292                         ID_index = 0;
01293                         max_check = 0;
01294                         min_check = 0;
01295                     }
01296                 } else {
01297                     if(VALVE_FR_timer < (int) (1.0f * (float) TMR_FREQ_5k)) {
01298                         //V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen)/(float) ENC_PULSE_PER_POSITION;
01299                         pos.ref = 0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end;
01300                         CONTROL_MODE = MODE_JOINT_CONTROL;
01301                     } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
01302                         data_num = 0;
01303                         valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
01304 
01305                         VALVE_POS_CONTROL(valve_pos.ref);
01306                         START_POS = pos.sen;
01307                     } else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
01308                         valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
01309                         VALVE_POS_CONTROL(valve_pos.ref);
01310                         data_num = data_num + 1;
01311                         if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) {
01312                             FINAL_POS = pos.sen;
01313                             one_period_end = 1;
01314                         }
01315                     } else if(VALVE_FR_timer == (int) (5.0f * (float) TMR_FREQ_5k)) {
01316                         FINAL_POS = pos.sen;
01317                         one_period_end = 1;
01318                         V_out = 0.0f;
01319                     }
01320 
01321                     if(one_period_end == 1) {
01322                         if(valve_pos.ref > VALVE_MAX_POS) {
01323                             max_check = 1;
01324                         } else if(valve_pos.ref < VALVE_MIN_POS) {
01325                             min_check = 1;
01326                         }
01327                         JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k;   //  pulse/sec
01328 
01329                         VALVE_FR_timer = 0;
01330                         one_period_end = 0;
01331                         ID_index= ID_index +1;
01332                         V_out = 0.0f;
01333                     }
01334 
01335                     if(max_check == 1 && min_check == 1) {
01336 
01337                         VALVE_POS_NUM = ID_index;
01338                         ROM_RESET_DATA();
01339                         ID_index = 0;
01340                         first_check = 0;
01341                         VALVE_FR_timer = 0;
01342                         CONTROL_UTILITY_MODE = MODE_NO_ACT;
01343 //                        CAN_TX_PRES((int16_t) (VALVE_FR_timer), (int16_t) (6));
01344                     }
01345                 }
01346                 break;
01347             }
01348 
01349             case MODE_SYSTEM_ID: {
01350                 freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.;
01351                 valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3);
01352                 CONTROL_MODE = MODE_VALVE_OPEN_LOOP;
01353                 cnt_sysid++;
01354                 if (freq_sysid_Iref >= 300) {
01355                     cnt_sysid = 0;
01356                     CONTROL_UTILITY_MODE = MODE_NO_ACT;
01357                 }
01358                 break;
01359             }
01360 
01361 
01362 
01363             default:
01364                 break;
01365         }
01366 
01367         // CONTROL MODE ------------------------------------------------------------
01368 
01369         switch (CONTROL_MODE) {
01370             case MODE_NO_ACT: {
01371                 V_out = 0.0f;
01372                 break;
01373             }
01374 
01375             case MODE_VALVE_POSITION_CONTROL: {
01376                 if (OPERATING_MODE == 5) { //SW Valve
01377                     VALVE_POS_CONTROL(valve_pos.ref);
01378                     V_out = Vout.ref;
01379                 } else if (CURRENT_CONTROL_MODE == 0) { //PWM
01380                     V_out = valve_pos.ref;
01381                 } else { 
01382                     I_REF = valve_pos.ref * 0.001f;
01383                 }
01384 
01385                 break;
01386             }
01387 
01388             case MODE_JOINT_CONTROL: {
01389                 double torq_ref = 0.0f;
01390                 pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm]
01391                 vel.err = (vel.ref - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
01392 //                vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s]
01393                 pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm]
01394                 
01395                 //K & D Low Pass Filter
01396                 float alpha_K_D = 1.0f/(1.0f + 5000.0f/(2.0f*3.14f*30.0f)); // f_cutoff : 30Hz
01397                 K_LPF = K_LPF*(1.0f-alpha_K_D)+K_SPRING*(alpha_K_D);
01398                 D_LPF = D_LPF*(1.0f-alpha_K_D)+D_DAMPER*(alpha_K_D);
01399                 
01400                 torq_ref = torq.ref + K_LPF * pos.err - D_LPF * vel.sen / ENC_PULSE_PER_POSITION; //[N]
01401 
01402                 // torque feedback
01403                 torq.err = torq_ref - torq.sen; //[N]
01404                 torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
01405 
01406                 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
01407 
01408                     double I_REF_POS = 0.0f;
01409                     double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
01410                     double I_REF_VC = 0.0f; // I_REF for velocity compensation
01411 
01412                     double temp_vel_pos = 0.0f;
01413                     double temp_vel_torq = 0.0f;
01414                     double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
01415 
01416                     if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
01417                         temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
01418                         //                            L when P-gain = 100, f_cut = 10Hz                                 L feedforward velocity
01419                     } else if ((OPERATING_MODE && 0x01) == 1) {
01420                         temp_vel_pos = (0.01f * (double) P_GAIN_JOINT_POSITION * wn_Pos * pos.err + 0.01f * (double) I_GAIN_JOINT_POSITION * wn_Pos * pos.err_sum + 0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / ENC_PULSE_PER_POSITION); // mm/s
01421                         //                            L when P-gain = 100, f_cut = 10Hz                                 L feedforward velocity
01422                     }
01423                     if (temp_vel_pos > 0.0f) I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
01424                     else I_REF_POS = temp_vel_pos * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
01425 
01426                     // velocity compensation for torque control
01427                     if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
01428                         I_REF_FORCE_FB = 0.001f * ((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum);
01429                         //                temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION) * PI / 180.0; // rad/s
01430                         temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION) * PI / 180.0f; // rad/s
01431                         //                                                          L feedforward velocity
01432                     } else if ((OPERATING_MODE && 0x01) == 1) {
01433                         I_REF_FORCE_FB = 0.001f * 0.01f*((double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum); // Linear Actuators are more sensitive.
01434                         //                temp_vel_torq = (0.01 * (double) VELOCITY_COMP_GAIN * (double) CUR_VELOCITY / (double) ENC_PULSE_PER_POSITION); // mm/s
01435                         temp_vel_torq = (0.01f * (double) VELOCITY_COMP_GAIN * vel.ref / (double) ENC_PULSE_PER_POSITION); // mm/s
01436                         //                                                          L feedforward velocity
01437                     }
01438                     if (temp_vel_torq > 0.0f) I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_A * 0.00006f / (K_v * sqrt(2.0f * alpha3 / (alpha3 + 1.0f))));
01439                     else I_REF_VC = temp_vel_torq * ((double) PISTON_AREA_B * 0.00006f / (K_v * sqrt(2.0f / (alpha3 + 1.0f))));
01440                     //                                                  L   velocity(rad/s or mm/s) >> I_ref(mA)
01441                     //            Ref_Joint_FT_dot = (Ref_Joint_FT_Nm - Ref_Joint_FT_Nm_old) / TMR_DT_5k;
01442                     //            Ref_Joint_FT_Nm_old = Ref_Joint_FT_Nm;
01443 
01444                     I_REF = (1.0f - alpha_trans) * I_REF_POS + alpha_trans * (I_REF_VC + I_REF_FORCE_FB);
01445                     
01446                     // Anti-windup for FT
01447                     if (I_GAIN_JOINT_TORQUE != 0) {
01448                         double I_MAX = 10.0f; // Maximum Current : 10mV
01449                         double Ka = 2.0f / ((double) I_GAIN_JOINT_TORQUE * 0.001f);
01450                         if (I_REF > I_MAX) {
01451                             double I_rem = I_REF - I_MAX;
01452                             I_rem = Ka*I_rem;
01453                             I_REF = I_MAX;
01454                             torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
01455                         } else if (I_REF < -I_MAX) {
01456                             double I_rem = I_REF - (-I_MAX);
01457                             I_rem = Ka*I_rem;
01458                             I_REF = -I_MAX;
01459                             torq.err_sum = torq.err_sum - I_rem /(float) TMR_FREQ_5k;
01460                         }
01461                     }
01462 
01463                 } else {
01464                     float VALVE_POS_RAW_FORCE_FB = 0.0f;
01465 
01466                     VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) * 0.01f
01467                                                           + DDV_JOINT_POS_FF(vel.sen))+ (1.0f-alpha_trans) * (P_GAIN_JOINT_POSITION * 0.01f * pos.err + DDV_JOINT_POS_FF(vel.ref));
01468 
01469                     if (VALVE_POS_RAW_FORCE_FB >= 0) {
01470                         valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_PLUS;
01471                     } else {
01472                         valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS;
01473                     }
01474 
01475                     if(I_GAIN_JOINT_TORQUE != 0) {
01476                         double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * 100.0f;
01477                         if(valve_pos.ref>VALVE_MAX_POS) {
01478                             double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS;
01479                             valve_pos_rem = valve_pos_rem * Ka;
01480                             valve_pos.ref = VALVE_MAX_POS;
01481                             torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
01482                         } else if(valve_pos.ref < VALVE_MIN_POS) {
01483                             double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS;
01484                             valve_pos_rem = valve_pos_rem * Ka;
01485                             valve_pos.ref = VALVE_MIN_POS;
01486                             torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
01487                         }
01488                     }
01489 
01490                     VALVE_POS_CONTROL(valve_pos.ref);
01491 
01492 //                    Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err);
01493                     V_out = (float) Vout.ref;
01494 
01495                 }
01496 
01497                 break;
01498             }
01499 
01500             case MODE_VALVE_OPEN_LOOP: {
01501                 V_out = (float) Vout.ref;
01502                 break;
01503             }
01504 
01505             default:
01506                 break;
01507         }
01508 
01509 
01510         if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
01511 
01512             ////////////////////////////////////////////////////////////////////////////
01513             ////////////////////////////  CURRENT CONTROL //////////////////////////////
01514             ////////////////////////////////////////////////////////////////////////////
01515             
01516             if (CURRENT_CONTROL_MODE) {
01517                 
01518                 // Moog Valve Current Control Gain
01519                 double R_model = 500.0f; // ohm
01520                 double L_model = 1.2f;
01521                 double w0 = 2.0f * 3.14f * 150.0f;
01522                 double KP_I = 0.1f * L_model*w0;
01523                 double KI_I = 0.1f * R_model*w0;
01524 
01525                 // KNR Valve Current Control Gain
01526                 if (((OPERATING_MODE & 0b110)>>1) == 1) { // KNR Valve
01527                     R_model = 163.0f; // ohm
01528                     L_model = 1.0f;
01529                     w0 = 2.0f * 3.14f * 80.0f;
01530                     KP_I = 1.0f * L_model*w0;
01531                     KI_I = 0.08f * R_model*w0;
01532                 }
01533                 
01534                 double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz
01535                 I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF;
01536                 
01537                 FLAG_VALVE_DEADZONE = 1;
01538                 
01539                 if (FLAG_VALVE_DEADZONE) {
01540                     if (I_REF_fil > 0) I_REF_fil_DZ = I_REF_fil + VALVE_DEADZONE_PLUS / 1000.0f; // unit: mA
01541                     else if (I_REF_fil < 0) I_REF_fil_DZ = I_REF_fil + VALVE_DEADZONE_MINUS / 1000.0f; // unit: mA
01542                     else I_REF_fil_DZ = I_REF_fil + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f / 1000.0f; // unit: mA
01543                 } else {
01544                     I_REF_fil_DZ = I_REF_fil;
01545                 }
01546 
01547                 I_ERR = I_REF_fil_DZ - cur.sen;
01548                 I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f;
01549 
01550 
01551                 
01552 
01553                 double FF_gain = 1.0f;
01554 
01555                 VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT;
01556                 //        VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model*I_REF); // Unit : mV
01557                 I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old;
01558                 I_REF_fil_old = I_REF_fil_DZ;
01559 //                VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV
01560                 VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV
01561                 double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV
01562 
01563                 double Ka = 3.0f / KP_I;
01564                 if (VALVE_PWM_RAW > V_MAX) {
01565                     V_rem = VALVE_PWM_RAW - V_MAX;
01566                     V_rem = Ka*V_rem;
01567                     VALVE_PWM_RAW = V_MAX;
01568                     I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
01569                 } else if (VALVE_PWM_RAW < -V_MAX) {
01570                     V_rem = VALVE_PWM_RAW - (-V_MAX);
01571                     V_rem = Ka*V_rem;
01572                     VALVE_PWM_RAW = -V_MAX;
01573                     I_ERR_INT = I_ERR_INT - V_rem * 0.0002f;
01574                 }
01575                 Cur_Valve_Open_pulse = cur.sen / mA_PER_pulse;
01576             } else {
01577                 VALVE_PWM_RAW = I_REF * mV_PER_mA;
01578                 Cur_Valve_Open_pulse = I_REF / mA_PER_pulse;
01579             }
01580 
01581             ////////////////////////////////////////////////////////////////////////////
01582             /////////////////  Dead Zone Cancellation & Linearization //////////////////
01583             ////////////////////////////////////////////////////////////////////////////
01584             // Dead Zone Cancellation (Mechanical Valve dead-zone)
01585 //            if (FLAG_VALVE_DEADZONE) {
01586 //                if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
01587 //                else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
01588 //                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
01589 //            } else {
01590 //                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
01591 //            }
01592 
01593             // VALVE_DEADZONE_PLUS : Current[mA] to start extraction
01594             // VALVE_DEADZONE_MINUS : Current[mA] to start contraction
01595 //            double R_model = 500.0f;
01596 //            FLAG_VALVE_DEADZONE = true;
01597 //            if (FLAG_VALVE_DEADZONE) {
01598 //                if (VALVE_PWM_RAW > 0) VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * R_model / 1000.0f; // unit: mV
01599 //                else if (VALVE_PWM_RAW < 0) VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * R_model / 1000.0f; // unit: mV
01600 //                else VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f* R_model / 1000.0f;
01601 //            } else {
01602                 VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
01603 //            }
01604 
01605             // Output Voltage Linearization
01606             double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV
01607             double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin);  // -8000~8000
01608 
01609             // Dead Zone Cancellation (Electrical dead-zone)
01610             if (CUR_PWM_lin > 0) V_out = (float) (CUR_PWM_lin + 169.0f);
01611             else if (CUR_PWM_lin < 0) V_out = (float) (CUR_PWM_lin - 174.0f);
01612             else V_out = (float) (CUR_PWM_lin);
01613         }
01614         
01615 //        if(V_out > 0.0f) V_out = (float) (V_out + 169.0f);
01616 //        else if(V_out < 0.0f) V_out = (float) (V_out - 174.0f);
01617 //        else V_out = V_out;
01618     
01619         /*******************************************************
01620         ***     PWM
01621         ********************************************************/
01622         if(DIR_VALVE<0){
01623             V_out = -V_out;
01624         }
01625         
01626         if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) {
01627             V_out = VALVE_VOLTAGE_LIMIT*1000.0f;
01628         } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
01629             V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
01630         }
01631         PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f); // Full duty : 12000.0mV
01632 
01633         // Saturation of output voltage to 12.0V
01634         if(PWM_out > 1.0f) PWM_out=1.0f;
01635         else if (PWM_out < -1.0f) PWM_out=-1.0f;
01636 
01637         if (PWM_out>0.0f) {
01638             dtc_v=0.0f;
01639             dtc_w=PWM_out;
01640         } else {
01641             dtc_v=-PWM_out;
01642             dtc_w=0.0f;
01643         }
01644 
01645         //pwm
01646         TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
01647         TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
01648 
01649 
01650         if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
01651 
01652             // Position, Velocity, and Torque (ID:1200)
01653             if (flag_data_request[0] == HIGH) {
01654                 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
01655                     //if (SENSING_MODE == 0) {
01656                         CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
01657                     //} else if (SENSING_MODE == 1) {
01658                     //    CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
01659                     //}
01660                 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
01661                     //if (SENSING_MODE == 0) {
01662                         CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE)));
01663                     //} else if (SENSING_MODE == 1) {
01664                     //    CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
01665                     //}
01666                 }
01667             }
01668             if (flag_data_request[1] == HIGH) {
01669                 //valve position
01670                 double t_value = 0;
01671 //                if(valve_pos.ref>=(float) VALVE_CENTER) {
01672 //                    t_value = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
01673 //                } else {
01674 //                    t_value = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
01675 //                }
01676                 if(OPERATING_MODE==5) {
01677                     t_value = (double) value;
01678                 } else if(CURRENT_CONTROL_MODE==1) {
01679                     t_value = cur.sen;
01680                 } else {
01681                     t_value = V_out;
01682                 }
01683 //                CAN_TX_TORQUE((int16_t) (I_REF_fil_DZ * 1000.0f)); //1300
01684                 CAN_TX_TORQUE((int16_t) (cur.sen * 1000.0f)); //1300
01685                 //CAN_TX_TORQUE((int16_t) (I_REF * 1000.0f)); //1300
01686             }
01687 
01688 
01689             if (flag_data_request[2] == HIGH) {
01690                 //pressure A and B
01691                 CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400
01692             }
01693 
01694 //            If it doesn't rest, below can can not work.
01695             for (can_rest = 0; can_rest < 10000; can_rest++) {
01696                 ;
01697             }
01698 
01699             if (flag_data_request[3] == HIGH) {
01700                 //PWM
01701                 CAN_TX_PWM((int16_t) value); //1500
01702             }
01703             //for (i = 0; i < 10000; i++) {
01704 //                ;
01705 //            }
01706             if (flag_data_request[4] == HIGH) {
01707                 //valve position
01708                 CAN_TX_VALVE_POSITION((int16_t) (CAN_FREQ), (int16_t) (D_DAMPER), (int16_t) OPERATING_MODE); //1600
01709             }
01710 
01711             // Others : Reference position, Reference FT, PWM, Current  (ID:1300)
01712 //        if (flag_data_request[1] == HIGH) {
01713 //            CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3));
01714 //        }
01715             //if (flag_delay_test == 1){
01716             //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
01717             //}
01718 
01719             TMR2_COUNT_CAN_TX = 0;
01720         }
01721         TMR2_COUNT_CAN_TX++;
01722 
01723     }
01724     TIM3->SR = 0x0;  // reset the status register
01725 
01726 }
01727 
01728 void SystemClock_Config(void)
01729 {
01730     RCC_OscInitTypeDef RCC_OscInitStruct = {0};
01731     RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
01732 
01733     /** Configure the main internal regulator output voltage
01734     */
01735     __HAL_RCC_PWR_CLK_ENABLE();
01736     __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
01737     /** Initializes the CPU, AHB and APB busses clocks
01738     */
01739     RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
01740     RCC_OscInitStruct.HSIState = RCC_HSI_ON;
01741     RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
01742     RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
01743     RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
01744     RCC_OscInitStruct.PLL.PLLM = 8;  //8
01745     RCC_OscInitStruct.PLL.PLLN = 180;//180
01746     RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
01747     RCC_OscInitStruct.PLL.PLLQ = 2;
01748     RCC_OscInitStruct.PLL.PLLR = 2;
01749     if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
01750         //Error_Handler();
01751     }
01752     /** Activate the Over-Drive mode
01753     */
01754     if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
01755         //Error_Handler();
01756     }
01757     /** Initializes the CPU, AHB and APB busses clocks
01758     */
01759     RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
01760                                   |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
01761     RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
01762     RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
01763     RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
01764     RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
01765 
01766     if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
01767         //Error_Handler();
01768     }
01769     HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSI, RCC_MCODIV_1);
01770     HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_SYSCLK, RCC_MCODIV_2);
01771 }