main.cpp

00001 //Hydraulic Control Board
00002 //distributed by Sungwoo Kim
00003 //       2020/12/28
00004 //revised by Buyoun Cho
00005 //       2021/04/20
00006 
00007 // 유의사항
00008 // 소수 적을때 뒤에 f 꼭 붙이기
00009 // CAN 선은 ground까지 있는 3상 선으로 써야함.
00010 // 전원은 12~24V 인가.
00011 
00012 #include "mbed.h"
00013 #include "FastPWM.h"
00014 #include "INIT_HW.h"
00015 #include "function_CAN.h"
00016 #include "SPI_EEP_ENC.h"
00017 #include "I2C_AS5510.h"
00018 #include "setting.h"
00019 #include "function_utilities.h"
00020 #include "stm32f4xx_flash.h"
00021 #include "FlashWriter.h"
00022 #include <string>
00023 #include <iostream>
00024 #include <cmath>
00025 
00026 using namespace std;
00027 Timer t;
00028 
00029 // dac & check ///////////////////////////////////////////
00030 DigitalOut check(PC_2);
00031 DigitalOut check_2(PC_3);
00032 AnalogOut dac_1(PA_4); // 0.0f ~ 1.0f
00033 AnalogOut dac_2(PA_5); // 0.0f ~ 1.0f
00034 AnalogIn adc1(PC_4); //pressure_1
00035 AnalogIn adc2(PB_0); //pressure_2
00036 AnalogIn adc3(PC_1); //current
00037 
00038 // PWM ///////////////////////////////////////////
00039 float dtc_v=0.0f;
00040 float dtc_w=0.0f;
00041 
00042 // I2C ///////////////////////////////////////////
00043 I2C i2c(PC_9,PA_8); // SDA, SCL (for K22F)
00044 const int i2c_slave_addr1 =  0x56;  // AS5510 address
00045 unsigned int value; // 10bit output of reading sensor AS5510
00046 
00047 // SPI ///////////////////////////////////////////
00048 SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK);
00049 DigitalOut eeprom_cs(PB_12);
00050 SPI enc(PC_12,PC_11,PC_10);
00051 DigitalOut enc_cs(PD_2);
00052 DigitalOut LED(PA_15);
00053 
00054 // UART ///////////////////////////////////////////
00055 Serial pc(PA_9,PA_10); //  _ UART
00056 
00057 // CAN ///////////////////////////////////////////
00058 CAN can(PB_8, PB_9, 1000000);
00059 CANMessage msg;
00060 void onMsgReceived()
00061 {
00062     CAN_RX_HANDLER();
00063 }
00064 
00065 // Variables ///////////////////////////////////////////
00066 State pos;
00067 State vel;
00068 State Vout;
00069 State force;
00070 State torq;         // unit : N
00071 State torq_dot;
00072 State pres_A;       // unit : bar
00073 State pres_B;
00074 State cur;          // unit : mA
00075 State valve_pos;
00076 State valve_pos_raw;
00077 
00078 State INIT_Vout;
00079 State INIT_Valve_Pos;
00080 State INIT_Pos;
00081 State INIT_torq;
00082 
00083 extern int CID_RX_CMD;
00084 extern int CID_RX_REF_POSITION;
00085 extern int CID_RX_REF_OPENLOOP;
00086 extern int CID_RX_REF_PWM;
00087 
00088 extern int CID_TX_INFO;
00089 extern int CID_TX_POS_VEL_TORQ;
00090 extern int CID_TX_PWM;
00091 extern int CID_TX_CURRENT;
00092 extern int CID_TX_VOUT;
00093 extern int CID_TX_VALVE_POSITION;
00094 extern int CID_TX_SOMETHING;
00095 
00096 float temp_P_GAIN = 0.0f;
00097 float temp_I_GAIN = 0.0f;
00098 int temp_VELOCITY_COMP_GAIN = 0;
00099 int logging = 0;
00100 float valve_pos_pulse_can = 0.0f;
00101 
00102 inline float tanh_inv(float y)
00103 {
00104     if(y >= 1.0f - 0.000001f) y = 1.0f - 0.000001f;
00105     if(y <= -1.0f + 0.000001f) y = -1.0f + 0.000001f;
00106     return log(sqrt((1.0f+y)/(1.0f-y)));
00107 }
00108 
00109 
00110 /*******************************************************************************
00111  *  REFERENCE MODE
00112  ******************************************************************************/
00113 enum _REFERENCE_MODE {
00114     MODE_REF_NO_ACT = 0,
00115     MODE_REF_DIRECT,
00116     MODE_REF_FINDHOME
00117 };
00118 
00119 /*******************************************************************************
00120  *  CONTROL MODE
00121  ******************************************************************************/
00122 enum _CONTROL_MODE {
00123     //control mode
00124     MODE_NO_ACT = 0,                                    //0
00125     MODE_VALVE_POSITION_CONTROL,                        //1
00126     MODE_JOINT_CONTROL,                                 //2
00127 
00128     MODE_VALVE_OPEN_LOOP,                               //3
00129     MODE_JOINT_ADAPTIVE_BACKSTEPPING,                   //4
00130     MODE_RL,                                            //5
00131 
00132     MODE_JOINT_POSITION_PRES_CONTROL_PWM,               //6
00133     MODE_JOINT_POSITION_PRES_CONTROL_VALVE_POSITION,    //7
00134     MODE_VALVE_POSITION_PRES_CONTROL_LEARNING,          //8
00135 
00136     MODE_TEST_CURRENT_CONTROL,                          //9
00137     MODE_TEST_PWM_CONTROL,                              //10
00138 
00139     MODE_CURRENT_CONTROL,                               //11
00140     MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT,         //12
00141     MODE_JOINT_POSITION_PRES_CONTROL_CURRENT,           //13
00142     MODE_VALVE_POSITION_TORQUE_CONTROL_LEARNING,        //14
00143 
00144     //utility
00145     MODE_TORQUE_SENSOR_NULLING = 20,                    //20
00146     MODE_VALVE_NULLING_AND_DEADZONE_SETTING,            //21
00147     MODE_FIND_HOME,                                     //22
00148     MODE_VALVE_GAIN_SETTING,                            //23
00149     MODE_PRESSURE_SENSOR_NULLING,                       //24
00150     MODE_PRESSURE_SENSOR_CALIB,                         //25
00151     MODE_ROTARY_FRICTION_TUNING,                        //26
00152 
00153     MODE_DDV_POS_VS_PWM_ID = 30,                        //30
00154     MODE_DDV_DEADZONE_AND_CENTER,                       //31
00155     MODE_DDV_POS_VS_FLOWRATE,                           //32
00156     MODE_SYSTEM_ID,                                     //33
00157     MODE_FREQ_TEST,                                     //34
00158     MODE_SEND_BUFFER,                                   //35
00159     MODE_SEND_OVER,                                     //36
00160     MODE_STEP_TEST,                                     //37
00161 };
00162 
00163 void SystemClock_Config(void)
00164 {
00165     RCC_OscInitTypeDef RCC_OscInitStruct = {0};
00166     RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
00167 
00168     /* Configure the main internal regulator output voltage
00169     */
00170     __HAL_RCC_PWR_CLK_ENABLE();
00171     __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
00172     /* Initializes the CPU, AHB and APB busses clocks
00173     */
00174     RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
00175     RCC_OscInitStruct.HSEState = RCC_HSE_ON;
00176 //    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
00177     RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
00178     RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
00179     RCC_OscInitStruct.PLL.PLLM = 1;//4
00180     RCC_OscInitStruct.PLL.PLLN = 192; //96
00181     RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
00182     RCC_OscInitStruct.PLL.PLLQ = 2;
00183     RCC_OscInitStruct.PLL.PLLR = 2;
00184     if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
00185         //Error_Handler();
00186     }
00187     /** Activate the Over-Drive mode
00188     */
00189     if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
00190         //Error_Handler();
00191     }
00192     /** Initializes the CPU, AHB and APB busses clocks
00193     */
00194     RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
00195                                   |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
00196     RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
00197     RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
00198     RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
00199     RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
00200 
00201     if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) {
00202         //Error_Handler();
00203     }
00204 //    HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
00205 //    HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
00206 //    HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
00207 //    HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
00208 
00209 }
00210 
00211 
00212 //void SystemClock_Config(void)       //External clock
00213 //{
00214 //    RCC_OscInitTypeDef RCC_OscInitStruct;
00215 //    RCC_ClkInitTypeDef RCC_ClkInitStruct;
00216 //
00217 //    /* Configure the main internal regulator output voltage
00218 //    */
00219 //    __PWR_CLK_ENABLE();
00220 //    __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
00221 //    /* Initializes the CPU, AHB and APB busses clocks
00222 //    */
00223 //    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
00224 //    RCC_OscInitStruct.HSIState = RCC_HSE_ON;
00225 ////    RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
00226 //    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
00227 //    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
00228 //    RCC_OscInitStruct.PLL.PLLM = 8;//8
00229 //    RCC_OscInitStruct.PLL.PLLN = 80; //180
00230 //    RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
00231 //    RCC_OscInitStruct.PLL.PLLQ = 4;
00232 //    HAL_RCC_OscConfig(&RCC_OscInitStruct);
00233 //
00234 //    HAL_PWREx_ActivateOverDrive();
00235 ////    RCC_OscInitStruct.PLL.PLLR = 2;
00236 ////    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
00237 ////        //Error_Handler();
00238 ////    }
00239 ////    /** Activate the Over-Drive mode
00240 ////    */
00241 ////    if (HAL_PWREx_EnableOverDrive() != HAL_OK) {
00242 ////        //Error_Handler();
00243 ////    }
00244 //    /** Initializes the CPU, AHB and APB busses clocks
00245 //    */
00246 //    RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_SYSCLK
00247 //                                  |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
00248 //    RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
00249 //    RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
00250 //    RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
00251 //    RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
00252 //    HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
00253 //
00254 //    HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);
00255 //
00256 //    HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
00257 //
00258 //    HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
00259 //}
00260 
00261 int main()
00262 {
00263     /*********************************
00264     ***     Initialization
00265     *********************************/
00266 
00267     HAL_Init();
00268     SystemClock_Config();
00269 
00270     LED = 0;
00271 //    pc.baud(9600);
00272 //
00273 //    // i2c init
00274 //    i2c.frequency(400 * 1000);          // 0.4 mHz
00275 //    wait_ms(2);                         // Power Up wait
00276 //    look_for_hardware_i2c();            // Hardware present
00277 //    init_as5510(i2c_slave_addr1);
00278 //    make_delay();
00279 
00280 
00281     // spi init
00282 //    eeprom_cs = 1;
00283 //    eeprom.format(8,3);
00284 //    eeprom.frequency(5000000); //5M
00285 //    eeprom_cs = 0;
00286 //    make_delay();
00287 //
00288 //    enc_cs = 1;     //sw add
00289 //    enc.format(8,0);
00290 //    enc.frequency(5000000); //10M
00291 //    enc_cs = 0;     //sw add
00292 //
00293 //    make_delay();
00294 //
00295 //    // spi _ enc
00296 //    spi_enc_set_init();
00297 //    make_delay();
00298 //
00299 //    ////// bno rom
00300 //    spi_eeprom_write(RID_BNO, (int16_t) 0);
00301 //    make_delay();
00302     ////////
00303 
00304     // rom
00305     ROM_CALL_DATA();
00306     make_delay();
00307 
00308     // ADC init
00309     Init_ADC();
00310     make_delay();
00311 
00312     // Pwm init
00313     Init_PWM();
00314     TIM4->CR1 ^= TIM_CR1_UDIS;
00315     make_delay();
00316 
00317 //    // CAN
00318 //    can.attach(&CAN_RX_HANDLER);
00319     CAN_ID_INIT();
00320     make_delay();
00321 
00322     //can.reset();
00323 //    can.filter(msg.id, 0xFFFFF000, CANStandard);
00324 
00325 //    // TMR2 init
00326 //    Init_TMR2();
00327 //    TIM2->CR1 ^= TIM_CR1_UDIS;
00328 //    make_delay();
00329 
00330     // TMR3 init
00331     Init_TMR3();
00332     TIM3->CR1 ^= TIM_CR1_UDIS;
00333     make_delay();
00334 
00335 //    //Timer priority
00336     NVIC_SetPriority(TIM3_IRQn, 3);
00337     NVIC_SetPriority(TIM4_IRQn, 2);
00338 //    NVIC_SetPriority(TIM2_IRQn, 2);
00339 //
00340 //
00341 //    //DAC init
00342 //    if (SENSING_MODE == 0) {
00343 //        dac_1 = FORCE_VREF / 3.3f;
00344 //        dac_2 = 0.0f;
00345 //    } else if (SENSING_MODE == 1) {
00346 //        //        if (DIR_VALVE_ENC > 0) {
00347 //        dac_1 = PRES_A_VREF / 3.3f;
00348 //        dac_2 = PRES_B_VREF / 3.3f;
00349 //        //        } else {
00350 //        //            dac_1 = PRES_B_VREF / 3.3f;
00351 //        //            dac_2 = PRES_A_VREF / 3.3f;
00352 //        //        }
00353 //    }
00354 //    make_delay();
00355 
00356 
00357 
00358     TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
00359     TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
00360 
00361 
00362     /************************************
00363     ***     Program is operating!
00364     *************************************/
00365     while(1) {
00366 
00367 //        if (LED > 0) LED = 0;
00368 //        else LED = 1;
00369 //        ADC1->CR2  |= 0x40000000;
00370 //        LVDT_new = ((float)ADC1->DR) - 2047.5f;
00371 //        TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
00372 //        TIM4->CCR1 = (PWM_ARR)*(1.0f-0.3f);
00373     }
00374 }
00375 
00376 
00377 //------------------------------------------------
00378 //     TMR4 : Sensor Read & Data Handling
00379 //-----------------------------------------------
00380 float FREQ_TMR4 = (float)FREQ_20k;
00381 float DT_TMR4 = (float)DT_20k;
00382 long  CNT_TMR4 = 0;
00383 int   TMR4_FREQ_20k = (int)FREQ_20k;
00384 //int toggle = 0;
00385 int PWM_Flag = 0;
00386 int TMR4_timer = 0;
00387 
00388 float LVDT_new = 0.0f;
00389 float LVDT_old = 0.0f;
00390 float LVDT_f_cut = 1000.0f;
00391 float LVDT_LPF = 0.0f;
00392 float LVDT_sum = 0.0f;
00393 
00394 extern "C" void TIM4_IRQHandler(void)
00395 {
00396     if (TIM4->SR & TIM_SR_UIF ) {
00397 
00398 //        if (LED > 0) LED = 0;
00399 //        else LED = 1;
00400 
00401         PWM_Flag++;
00402 
00403         if(PWM_Flag <= 15) {
00404 
00405             if (PWM_Flag >= 4 && PWM_Flag <=13) {
00406                 ADC1->CR2  |= 0x40000000;
00407                 LVDT_new = ((float)ADC1->DR) - 2047.5f;
00408 //                if (ADC1->SR &= ~(ADC_SR_EOC)) {
00409 //                if (LED > 0) LED = 0;
00410 //                else LED = 1;
00411 //                LED = 1;
00412 //                }
00413                 LVDT_sum = LVDT_sum + LVDT_new;
00414                 if (LED > 0) LED = 0;
00415                 else LED = 1;
00416             }
00417 
00418             //pwm
00419             TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
00420             TIM4->CCR1 = (PWM_ARR)*(1.0f-1.0f);
00421         } else if(PWM_Flag <= 500) {
00422             TIM4->CCR2 = (PWM_ARR)*(1.0f-0.0f);
00423             TIM4->CCR1 = (PWM_ARR)*(1.0f-0.0f);
00424         } else {
00425             PWM_Flag = 0;
00426             LVDT_LPF = 0.0f;
00427             force.sen = LVDT_sum * 0.1f;
00428             LVDT_sum = 0.0f;
00429         }
00430 
00431         CNT_TMR4++;
00432     }
00433     TIM4->SR = 0x0;  // reset the status register
00434 }
00435 
00436 
00437 int j =0;
00438 float FREQ_TMR3 = (float)FREQ_5k;
00439 float DT_TMR3 = (float)DT_5k;
00440 int cnt_trans = 0;
00441 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
00442 int can_rest =0;
00443 float force_ref_act_can = 0.0f;
00444 
00445 extern "C" void TIM3_IRQHandler(void)
00446 {
00447     if (TIM3->SR & TIM_SR_UIF ) {
00448 
00449 
00450 
00451         // // =====================================================================
00452 //        // CONTROL LOOP --------------------------------------------------------
00453 //        // =====================================================================
00454 //        int UTILITY_MODE = 0;
00455 //        int CONTROL_MODE = 0;
00456 //
00457 //        if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) {
00458 //            UTILITY_MODE = CONTROL_UTILITY_MODE;
00459 //            CONTROL_MODE = MODE_NO_ACT;
00460 //        } else {
00461 //            CONTROL_MODE = CONTROL_UTILITY_MODE;
00462 //            UTILITY_MODE = MODE_NO_ACT;
00463 //        }
00464 //        // UTILITY MODE ------------------------------------------------------------
00465 //        switch (UTILITY_MODE) {
00466 //            case MODE_NO_ACT: {
00467 //                break;
00468 //            }
00469 //
00470 //            case MODE_TORQUE_SENSOR_NULLING: {
00471 //                static float FORCE_pulse_sum = 0.0;
00472 //                static float PresA_pulse_sum = 0.0;
00473 //                static float PresB_pulse_sum = 0.0;
00474 //
00475 //                // DAC Voltage reference set
00476 //                float VREF_TuningGain = -0.00000003f;
00477 //                if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
00478 //                    if(SENSING_MODE == 0) { // Force Sensor (Loadcell)
00479 //                        FORCE_pulse_sum = FORCE_pulse_sum + force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE;
00480 //                        if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
00481 //                            float FORCE_pluse_mean = FORCE_pulse_sum / 10.0f;
00482 //                            FORCE_pulse_sum = 0.0f;
00483 //
00484 //                            FORCE_VREF += VREF_TuningGain * (0.0f - FORCE_pluse_mean);
00485 //                            if (FORCE_VREF > 3.3f) FORCE_VREF = 3.3f;
00486 //                            if (FORCE_VREF < 0.0f) FORCE_VREF = 0.0f;
00487 //                            dac_1 = FORCE_VREF / 3.3f;
00488 //                        }
00489 //                    } else if (SENSING_MODE == 1) { // Pressure Sensor
00490 //                        PresA_pulse_sum += pres_A.sen*PRES_SENSOR_A_PULSE_PER_BAR;
00491 //                        PresB_pulse_sum += pres_B.sen*PRES_SENSOR_B_PULSE_PER_BAR;
00492 //                        if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
00493 //                            float PresA_pluse_mean = PresA_pulse_sum / 10.0f;
00494 //                            float PresB_pluse_mean = PresB_pulse_sum / 10.0f;
00495 //                            PresA_pulse_sum = 0.0f;
00496 //                            PresB_pulse_sum = 0.0f;
00497 //
00498 //                            PRES_A_VREF += VREF_TuningGain * (0.0f - PresA_pluse_mean);
00499 //                            if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
00500 //                            if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
00501 //                            dac_1 = PRES_A_VREF / 3.3f;
00502 //                            PRES_B_VREF += VREF_TuningGain * (0.0f - PresB_pluse_mean);
00503 //                            if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f;
00504 //                            if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f;
00505 //                            dac_2 = PRES_B_VREF / 3.3f;
00506 //                        }
00507 //                    }
00508 //                    TMR3_COUNT_TORQUE_NULL++;
00509 //                } else {
00510 //                    if(SENSING_MODE == 0 ) { // Force Sensor (Loadcell)
00511 //                        FORCE_pulse_sum = 0.0f;
00512 //                        dac_1 = FORCE_VREF / 3.3f;
00513 //                        spi_eeprom_write(RID_FORCE_SENSOR_VREF, (int16_t)(FORCE_VREF * 1000.0f));
00514 //                    } else if (SENSING_MODE == 1) {
00515 //                        PresA_pulse_sum = 0.0f;
00516 //                        PresB_pulse_sum = 0.0f;
00517 //                        dac_1 = PRES_A_VREF / 3.3f;
00518 //                        dac_2 = PRES_B_VREF / 3.3f;
00519 //                        spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t)(PRES_A_VREF * 1000.0f));
00520 //                        spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t)(PRES_B_VREF * 1000.0f));
00521 //                    }
00522 //                    CONTROL_UTILITY_MODE = MODE_NO_ACT;
00523 //                    TMR3_COUNT_TORQUE_NULL = 0;
00524 //                }
00525 //                break;
00526 //            }
00527 //
00528 //
00529 //            default:
00530 //                break;
00531 //        }
00532 //
00533 //        // CONTROL MODE ------------------------------------------------------------
00534 //        switch (CONTROL_MODE) {
00535 //            case MODE_NO_ACT: {
00536 //                V_out = 0.0f;
00537 //                break;
00538 //            }
00539 //
00540 //            case MODE_VALVE_OPEN_LOOP: {
00541 //                V_out = (float) Vout.ref;
00542 //                break;
00543 //            }
00544 //
00545 //            default:
00546 //                break;
00547 //        }
00548 //
00549 //        if (V_out > 0 ) V_out = (V_out + 180.0f)/0.8588f;
00550 //        else if (V_out < 0) V_out = (V_out - 200.0f)/0.8651f;
00551 //        else V_out = 0.0f;
00552 //
00553 //
00554 //        ////////////////////////////////////////////////////////////////////
00555 //        ///////////////////  PWM Command ///////////////////////////////////
00556 //        ////////////////////////////////////////////////////////////////////
00557 //
00558 //        if(DIR_VALVE<0) {
00559 //            V_out = -V_out;
00560 //        }
00561 //
00562 //        if (V_out >= VALVE_VOLTAGE_LIMIT*1000.0f) {
00563 //            V_out = VALVE_VOLTAGE_LIMIT*1000.0f;
00564 //        } else if(V_out<=-VALVE_VOLTAGE_LIMIT*1000.0f) {
00565 //            V_out = -VALVE_VOLTAGE_LIMIT*1000.0f;
00566 //        }
00567 //        PWM_out= V_out/(SUPPLY_VOLTAGE*1000.0f);
00568 //
00569 //        // Saturation of output voltage
00570 //        if(PWM_out > 1.0f) PWM_out=1.0f;
00571 //        else if (PWM_out < -1.0f) PWM_out=-1.0f;
00572 //
00573 //        if (PWM_out>0.0f) {
00574 //            dtc_v=0.0f;
00575 //            dtc_w=PWM_out;
00576 //        } else {
00577 //            dtc_v=-PWM_out;
00578 //            dtc_w=0.0f;
00579 //        }
00580 //
00581 ////        //pwm
00582 ////        TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v);
00583 ////        TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
00584 //
00585 //        ////////////////////////////////////////////////////////////////////////////
00586 //        //////////////////////  Data transmission through CAN //////////////////////
00587 //        ////////////////////////////////////////////////////////////////////////////
00588 //
00589 //        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
00590 
00591         // Position, Velocity, and Torque (ID:1200)
00592         if (flag_data_request[0] == LOW) {
00593 
00594             if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
00595                 CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
00596 //                    CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
00597 
00598             } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
00599                 CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen));
00600 //                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (valve_pos_can*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
00601             }
00602         }
00603 
00604         // Valve Position (ID:1300)
00605         if (flag_data_request[1] == HIGH) {
00606             CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse));
00607 //                CAN_TX_PWM((int16_t)(TORQUE_SENSOR_PULSE_PER_TORQUE*10000.0f));
00608         }
00609 
00610         // Others : Pressure A, B, Supply Pressure, etc. (for Debugging)  (ID:1400)
00611         if (flag_data_request[2] == HIGH) {
00612             float valve_pos_can = 0.0f;
00613             if(value >= VALVE_ELECTRIC_CENTER) {
00614                 valve_pos_can = 10000.0f*((float)value-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
00615             } else {
00616                 valve_pos_can = -10000.0f*((float)value -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
00617             }
00618             float valve_pos_ref_can = 0.0f;
00619             if(valve_pos.ref >= VALVE_ELECTRIC_CENTER) {
00620                 valve_pos_ref_can = 10000.0f*((float)valve_pos.ref-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER);
00621             } else {
00622                 valve_pos_ref_can = -10000.0f*((float)valve_pos.ref -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER);
00623             }
00624 
00625             valve_pos_ref_can = (float)valve_pos.ref;
00626 
00627             CAN_TX_CURRENT((int16_t) valve_pos_can, (int16_t) valve_pos_pulse_can);
00628         }
00629 
00630         TMR2_COUNT_CAN_TX = 0;
00631 //        }
00632         TMR2_COUNT_CAN_TX++;
00633 
00634     }
00635     TIM3->SR = 0x0;  // reset the status register
00636 
00637 }
00638 
00639 //
00640 //float FREQ_TMR2 = (float)FREQ_400k;
00641 //float DT_TMR2 = (float)DT_400k;
00642 //int TMR2_timer = 0;
00643 //int toggle_old = 0;
00644 //
00645 //float LVDT_new = 0.0f;
00646 //float LVDT_old = 0.0f;
00647 //float LVDT_f_cut = 1000.0f;
00648 //float LVDT_LPF = 0.0f;
00649 //float LVDT_sum = 0.0f;
00650 //
00651 //extern "C" void TIM2_IRQHandler(void)
00652 //{
00653 //    if (TIM2->SR & TIM_SR_UIF ) {
00654 ////        if (LED > 0) LED = 0;
00655 ////        else LED = 1;
00656 ////        LED = 1;
00657 //
00658 //        if (toggle != toggle_old) {
00659 //            TMR2_timer = 0;
00660 //            LVDT_LPF = 0.0f;
00661 //            LVDT_sum = 0.0f;
00662 ////            if (LED > 0) LED = 0;
00663 ////            else LED = 1;
00664 //        }
00665 //
00666 //        if (TMR2_timer >= 6 && TMR2_timer <=45) {
00667 ////        if (TMR2_timer >= 6 && TMR2_timer <=25) {
00668 ////            if (LED > 0) LED = 0;
00669 ////            else LED = 1;
00670 ////            LED = 0;
00671 //            ADC1->CR2  |= 0x40000000;
00672 //            LVDT_new = ((float)ADC1->DR) - 2047.5f;
00673 //            if (ADC1->SR &= ~(ADC_SR_EOC)) {
00674 ////                if (LED > 0) LED = 0;
00675 ////                else LED = 1;
00676 ////                LED = 1;
00677 //            }
00678 ////            float alpha_update = 1.0f / (1.0f + FREQ_TMR2 / (2.0f * 3.14f * LVDT_f_cut)); // f_cutoff : 100Hz
00679 ////            LVDT_LPF = (1.0f - alpha_update) * LVDT_LPF + alpha_update * LVDT_new;
00680 ////            LVDT_sum = LVDT_sum + LVDT_LPF;
00681 //            LVDT_sum = LVDT_sum + LVDT_new;
00682 //
00683 ////            force.UpdateSen((((float)ADC1->DR) - 2047.5f), FREQ_TMR2, 100.0f); // unit : N
00684 ////            this->sen_diff = (sen_new-this->sen)*Freq_update;
00685 ////            float alpha_update = 1.0f / (1.0f + Freq_update / (2.0f * 3.14f * f_cut)); // f_cutoff : 100Hz
00686 ////            this->sen = (1.0f - alpha_update) * this->sen + alpha_update * sen_new;
00687 ////            force.sen = LVDT_new;
00688 //        }
00689 //
00690 //        else if (TMR2_timer == 46) {    //46
00691 ////        if (TMR2_timer == 50) {
00692 ////            LED = 1;
00693 ////            ADC1->CR2  |= 0x40000000;
00694 ////            LVDT_new = ((float)ADC1->DR) - 2047.5f;
00695 ////            force.sen = LVDT_new;
00696 //            force.sen = LVDT_sum * 0.025f;
00697 ////            force.sen = LVDT_sum;
00698 //
00699 ////            ADC1->CR2  |= 0x40000000;
00700 ////            LVDT_new = ((float)ADC1->DR) - 2047.5f;
00701 ////            force.sen = LVDT_new;
00702 ////            force.UpdateSen(LVDT_sum * 0.025f, 1000.0f, 10.0f);
00703 ////            LED = 0;
00704 //        }
00705 ////        LED = 0;
00706 //        toggle_old = toggle;
00707 //        TMR2_timer++;
00708 //    }
00709 //    TIM2->SR = 0x0;  // reset the status register
00710 //
00711 //}