Diff: main.cpp

Revision:

21:e5f1a43ea6f9

Parent:

20:806196fda269

Child:

23:59218d4a256d

--- a/main.cpp	Mon Sep 09 06:57:31 2019 +0000
+++ b/main.cpp	Mon Sep 09 10:10:31 2019 +0000
@@ -121,7 +121,7 @@
 
     MODE_TEST_CURRENT_CONTROL,                          //9
     MODE_TEST_PWM_CONTROL,                              //10
-    
+
     MODE_CURRENT_CONTROL,                               //11
     MODE_JOINT_POSITION_TORQUE_CONTROL_CURRENT,         //12
     MODE_JOINT_POSITION_PRES_CONTROL_CURRENT,           //13
@@ -147,7 +147,7 @@
     *********************************/
     //LED = 1;
     //pc.baud(9600);
-    
+
     // i2c init
     i2c.frequency(400 * 1000);          // 0.4 mHz
     wait_ms(2);                         // Power Up wait
@@ -161,7 +161,7 @@
     enc.format(8,0);
     enc.frequency(5000000); //5M
     make_delay();
-    
+
     //rom
     ROM_CALL_DATA();
     make_delay();
@@ -179,18 +179,18 @@
     Init_TMR3();
     TIM3->CR1 ^= TIM_CR1_UDIS;
     make_delay();
-    
+
     // TMR5 init
     Init_TMR5();
     TIM5->CR1 ^= TIM_CR1_UDIS;
     make_delay();
-    
+
     // CAN
     can.attach(&CAN_RX_HANDLER);
     CAN_ID_INIT();
     make_delay();
     can.filter(msg.id, 0xFFFFF000, CANStandard);
-    
+
     // spi _ enc
     spi_enc_set_init();
     make_delay();
@@ -211,17 +211,17 @@
     ***     Program is operating!
     *************************************/
     while(1) {
-        if(timer_while==1000){
+        if(timer_while==1000) {
             //i2c
             read_field(i2c_slave_addr1);
             if(DIR_VALVE_ENC < 0) value = 1023 - value;
-            
+
 //            if(LED==1) {
 //                LED=0;
 //            } else
 //                LED = 1;
             timer_while = 0;
-            
+
         }
         timer_while ++;
     }
@@ -282,8 +282,6 @@
         }
     }
 
-    //VALVE_PWM_RAW_FF = 0.0;
-
     V_out = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_FB;
 }
 
@@ -337,13 +335,13 @@
 extern "C" void TIM4_IRQHandler(void)
 {
     if (TIM4->SR & TIM_SR_UIF ) {
-        
-    /*******************************************************
-    ***     Sensor Read & Data Handling
-    ********************************************************/
+
+        /*******************************************************
+        ***     Sensor Read & Data Handling
+        ********************************************************/
 
 
-    //Using LoadCell
+        //Using LoadCell
 //            ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
 //            //while((ADC1->SR & 0b10));
 //            double alpha_update_torque = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
@@ -352,38 +350,38 @@
 
 
 
-    //Pressure sensor A
-    ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
-    //while((ADC1->SR & 0b10));
-    double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
-    double pres_A_new = ((double)ADC1->DR - PRES_A_NULL)  / PRES_SENSOR_A_PULSE_PER_BAR;
-    pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+        //Pressure sensor A
+        ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
+        //while((ADC1->SR & 0b10));
+        double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+        double pres_A_new = ((double)ADC1->DR - PRES_A_NULL)  / PRES_SENSOR_A_PULSE_PER_BAR;
+        pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
 
-    //Pressure sensor 1B
-    //ADC2->CR2  |= 0x40000000;                        // adc _ 12bit
-    //while((ADC2->SR & 0b10));
-    double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
-    double pres_B_new = ((double)ADC2->DR - PRES_B_NULL)  / PRES_SENSOR_B_PULSE_PER_BAR;
-    pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
-    torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
+        //Pressure sensor 1B
+        //ADC2->CR2  |= 0x40000000;                        // adc _ 12bit
+        //while((ADC2->SR & 0b10));
+        double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+        double pres_B_new = ((double)ADC2->DR - PRES_B_NULL)  / PRES_SENSOR_B_PULSE_PER_BAR;
+        pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+        torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
 
 
 
-    //Current
-    //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
+        //Current
+        //ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
 //          a1=ADC2->DR;
-    int raw_cur = ADC3->DR;
-    //while((ADC3->SR & 0b10));
-    double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
-    double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
-    cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
-    cur.sen = raw_cur;
+        int raw_cur = ADC3->DR;
+        //while((ADC3->SR & 0b10));
+        double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
+        double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
+        cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
+        cur.sen = raw_cur;
 
-    /*******************************************************
-    ***     Timer Counting & etc.
-    ********************************************************/
-    //CNT_TMR4++;
-    }    
+        /*******************************************************
+        ***     Timer Counting & etc.
+        ********************************************************/
+        //CNT_TMR4++;
+    }
     TIM4->SR = 0x0;  // reset the status register
 }
 
@@ -393,7 +391,7 @@
 double FREQ_TMR3 = (double)FREQ_5k;
 double DT_TMR3 = (double)DT_5k;
 extern "C" void TIM3_IRQHandler(void)
-{ 
+{
     if (TIM3->SR & TIM_SR_UIF ) {
         ENC_UPDATE();
 
@@ -1593,70 +1591,70 @@
         //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
         //PWM_out = CUR_PWM;
 
-/*
+        /*
 
-        //CAN ----------------------------------------------------------------------
-        //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
-        if (TMR3_COUNT_CAN_TX % 1000 == 0) {
+                //CAN ----------------------------------------------------------------------
+                //if (TMR3_COUNT_CAN_TX % (int) (TMR_FREQ_5k / CAN_FREQ) == 0) {
+                if (TMR3_COUNT_CAN_TX % 1000 == 0) {
 
-            if (flag_data_request[0] == HIGH) {
-                //position+velocity
-                CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
-                //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
-                //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
-                //pc.printf("can good");
-                //                                    CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
-            }
+                    if (flag_data_request[0] == HIGH) {
+                        //position+velocity
+                        CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen);
+                        //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0);
+                        //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]);
+                        //pc.printf("can good");
+                        //                                    CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR);
+                    }
 
-            if (flag_data_request[1] == HIGH) {
-                //torque
-                //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
-                //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
-                CAN_TX_TORQUE((int16_t) (cur.sen));
-                //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN));
-                //            CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
-            }
+                    if (flag_data_request[1] == HIGH) {
+                        //torque
+                        //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.));
+                        //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM));
+                        CAN_TX_TORQUE((int16_t) (cur.sen));
+                        //CAN_TX_TORQUE((int16_t) (Ref_Valve_Pos_FF_CAN));
+                        //            CAN_TX_TORQUE((int16_t) DZ_temp_cnt);
+                    }
 
-            if (flag_data_request[2] == HIGH) {
-                //pressure A and B
-                //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
-                CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
-                //                        CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
-                //            CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
-                //            CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
+                    if (flag_data_request[2] == HIGH) {
+                        //pressure A and B
+                        //CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+                        CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+                        //                        CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
+                        //            CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
+                        //            CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
 
-            }
+                    }
 
-            if (flag_data_request[3] == HIGH) {
-                //PWM
-                CAN_TX_PWM((int16_t) CUR_PWM);
-                //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN));
-                //            CAN_TX_PWM((int16_t) cnt_vel_findhome);
-                //            CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
-                //                        CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
+                    if (flag_data_request[3] == HIGH) {
+                        //PWM
+                        CAN_TX_PWM((int16_t) CUR_PWM);
+                        //CAN_TX_PWM((int16_t) (Ref_Valve_Pos_FF_CAN));
+                        //            CAN_TX_PWM((int16_t) cnt_vel_findhome);
+                        //            CAN_TX_PWM((int16_t) (VALVE_VOLTAGE * 1000.));
+                        //                        CAN_TX_PWM((int16_t) (VALVE_VOLTAGE_VALVE_DZ * 1000.));
 
-            }
+                    }
 
-            if (flag_data_request[4] == HIGH) {
-                //valve position
-                CAN_TX_VALVE_POSITION((int16_t) (value));
-                //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
-                //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
+                    if (flag_data_request[4] == HIGH) {
+                        //valve position
+                        CAN_TX_VALVE_POSITION((int16_t) (value));
+                        //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF));
+                        //SPI_ROM_WRITE(RID_VALVE_POS_VS_FLOWRATE_0_1, (int16_t) ((0xFEF1>>16) & 0xFFFF));
 
 
-                //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
-                //                        CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));
-                //            CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
-                //            CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
-                //            CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
-            }
+                        //CAN_TX_VALVE_POSITION((int16_t) (VALVE_POS_NUM));
+                        //                        CAN_TX_VALVE_POSITIOfxN((int16_t) (VALVE_FF_VOLTAGE / SUPPLY_VOLTAGE));
+                        //            CAN_TX_VALVE_POSITION((int16_t) P_GAIN_JOINT_POSITION);
+                        //            CAN_TX_VALVE_POSITION((int16_t) Ref_Joint_Pos);
+                        //            CAN_TX_VALVE_POSITION((int16_t) flag_flowrate);
+                    }
 
-            TMR3_COUNT_CAN_TX = 0;
+                    TMR3_COUNT_CAN_TX = 0;
 
-        }
-        TMR3_COUNT_CAN_TX++;
+                }
+                TMR3_COUNT_CAN_TX++;
 
-*/
+        */
 
 
         /*******************************************************
@@ -1727,10 +1725,10 @@
         ***     Timer Counting & etc.
         ********************************************************/
         //CNT_TMR3++;
-    
+
     }
     TIM3->SR = 0x0;  // reset the status register
-    
+
 }
 
 
@@ -1739,9 +1737,9 @@
 //double FREQ_TMR5 = (double)FREQ_500;
 //double DT_TMR5 = (double)DT_500;
 extern "C" void TIM5_IRQHandler(void)
-{ 
+{
     if (TIM5->SR & TIM_SR_UIF ) {
-        
+
         //CAN ----------------------------------------------------------------------
         if (flag_data_request[0] == HIGH) {
             //position+velocity
@@ -1832,4 +1830,4 @@
         V_out = -V_MAX;
         cur.err_int = cur.err_int - V_rem*DT_5k;
     }
-}
\ No newline at end of file
+}