distribution-201229

Dependencies:   mbed FastPWM

Revision:
55:b25725257569
Parent:
54:647072f5307a
Child:
56:6f50d9d3bfee
--- a/main.cpp	Tue Feb 25 12:56:39 2020 +0000
+++ b/main.cpp	Wed Feb 26 12:51:52 2020 +0000
@@ -136,7 +136,7 @@
     /*********************************
     ***     Initialization
     *********************************/
-    //LED = 1;
+    LED = 1;
     //pc.baud(9600);
 
     // i2c init
@@ -172,9 +172,9 @@
     make_delay();
 
     // TMR2 init
-//    Init_TMR2();
-//    TIM2->CR1 ^= TIM_CR1_UDIS;
-//    make_delay();
+    Init_TMR2();
+    TIM2->CR1 ^= TIM_CR1_UDIS;
+    make_delay();
 
     // CAN
     can.attach(&CAN_RX_HANDLER);
@@ -183,7 +183,7 @@
 
     //Timer priority
     NVIC_SetPriority(TIM3_IRQn, 2);
-//    NVIC_SetPriority(TIM2_IRQn, 3);
+    NVIC_SetPriority(TIM2_IRQn, 3);
     NVIC_SetPriority(TIM4_IRQn, 3);
 
     //can.reset();
@@ -206,7 +206,7 @@
         else
             ID_index_array[i] =  (i+1) * 0.5f;
     }
-
+    
     /************************************
     ***     Program is operating!
     *************************************/
@@ -234,15 +234,15 @@
         if(REF_JOINT_VEL >= min(JOINT_VEL[i],JOINT_VEL[i+1]) && REF_JOINT_VEL <=  max(JOINT_VEL[i],JOINT_VEL[i+1])) {
             if(i==0) {
                 if(JOINT_VEL[i+1] == JOINT_VEL[i]) {
-                    Ref_Valve_Pos_FF = DDV_CENTER;
+                    Ref_Valve_Pos_FF = (float) VALVE_CENTER;
                 } else {
-                    Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
+                    Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + (float) VALVE_CENTER;
                 }
             } else {
                 if(JOINT_VEL[i+1] == JOINT_VEL[i-1]) {
-                    Ref_Valve_Pos_FF = DDV_CENTER;
+                    Ref_Valve_Pos_FF = (float) VALVE_CENTER;
                 } else {
-                    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])) + DDV_CENTER + (float) (10*ID_index_array[i-1]);
+                    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]);
                 }
             }
             break;
@@ -254,7 +254,7 @@
         Ref_Valve_Pos_FF = (float) VALVE_MIN_POS;
     }
 
-    Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - DDV_CENTER);
+    Ref_Valve_Pos_FF = (float) VELOCITY_COMP_GAIN * 0.01f * (float) (Ref_Valve_Pos_FF - (float) VALVE_CENTER);
     return Ref_Valve_Pos_FF;
 
 }
@@ -367,10 +367,10 @@
 
 
         //Pressure sensor B
-//        float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
-//        float pres_B_new = ((float)ADC2->DR);
-//        pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
-//        torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
+        float alpha_update_pres_B = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
+        float pres_B_new = ((float)ADC2->DR);
+        pres_B.sen = pres_B.sen*(1.0f-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+        //torq.sen = pres_A.sen * (float) PISTON_AREA_A - pres_B.sen * (float) PISTON_AREA_B;
 
 
         //Current
@@ -393,11 +393,13 @@
 float DT_TMR3 = (float)DT_5k;
 int cnt_trans = 0;
 double VALVE_POS_RAW_FORCE_FB_LOGGING = 0.0f;
-int canfreq = CAN_FREQUENCY;
+int can_rest =0;
 
 extern "C" void TIM3_IRQHandler(void)
 {
     if (TIM3->SR & TIM_SR_UIF ) {
+        
+        
 
         if ((OPERATING_MODE&0b110) == 0) {
             K_v = 0.4f; // Moog (LPM >> mA) , 100bar
@@ -591,7 +593,7 @@
                         pos.err = pos.ref_home_pos - pos.sen;
                         float VALVE_POS_RAW_POS_FB = 0.0f;
                         VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * pos.err/(float) ENC_PULSE_PER_POSITION * 0.01f;
-                        valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
+                        valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
                         VALVE_POS_CONTROL(valve_pos.ref);
 
                         //float wn_Pos = 2.0f*PI*5.0f; // f_cut : 10Hz Position Control
@@ -621,7 +623,7 @@
                     pos.err = pos.ref_home_pos - (float)pos.sen;
                     float VALVE_POS_RAW_POS_FB = 0.0f;
                     VALVE_POS_RAW_POS_FB = (float) P_GAIN_JOINT_POSITION * 0.01f * pos.err/(float) ENC_PULSE_PER_POSITION;
-                    valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
+                    valve_pos.ref = VALVE_POS_RAW_POS_FB + (float) VALVE_CENTER;
                     VALVE_POS_CONTROL(valve_pos.ref);
 
                     cnt_findhome++;
@@ -982,7 +984,7 @@
                             DZ_index= DZ_index *2;
                             if(DZ_index >= 128) {
                                 SECOND_DZ = valve_pos.ref;
-                                DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ);
+                                VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
                                 first_check = 0;
                                 VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
                                 VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
@@ -1067,7 +1069,7 @@
                             DZ_index= DZ_index *2;
                             if(DZ_index >= 128) {
                                 SECOND_DZ = valve_pos.ref;
-                                DDV_CENTER = 0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ);
+                                VALVE_CENTER = (int) (0.5f * (float) (FIRST_DZ) + 0.5f * (float) (SECOND_DZ));
                                 first_check = 0;
                                 VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
                                 VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
@@ -1099,7 +1101,7 @@
                         pos_minus_end = pos.sen;
                         first_check = 1;
                         VALVE_FR_timer = 0;
-                        valve_pos.ref = DDV_CENTER;
+                        valve_pos.ref = (float) VALVE_CENTER;
                         ID_index = 0;
                         max_check = 0;
                         min_check = 0;
@@ -1109,12 +1111,12 @@
                         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;
                     } else if(VALVE_FR_timer == (int) (1.0f * (float) TMR_FREQ_5k)) {
                         data_num = 0;
-                        valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
+                        valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
 
                         VALVE_POS_CONTROL(valve_pos.ref);
                         START_POS = pos.sen;
                     } else if(VALVE_FR_timer < (int) (5.0f * (float) TMR_FREQ_5k)) {
-                        valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + DDV_CENTER;
+                        valve_pos.ref = 10.0f*((float) ID_index_array[ID_index]) + (float) VALVE_CENTER;
                         VALVE_POS_CONTROL(valve_pos.ref);
                         data_num = data_num + 1;
                         if(abs(0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen) > 20000.0f) {
@@ -1276,7 +1278,7 @@
                 double KI_I = 0.1f * R_model*w0;
 
                 // KNR Valve Current Control Gain
-                if ((OPERATING_MODE & 0x02) == 1) { // KNR Valve
+                if ((OPERATING_MODE & 0b110) == 1) { // KNR Valve
                     R_model = 163.0f; // ohm
                     L_model = 1.0f;
                     w0 = 2.0f * 3.14f * 80.0f;
@@ -1319,7 +1321,7 @@
                 if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV
                 else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV
 
-                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + VALVE_CENTER * mV_PER_pulse; // unit: mV
+                VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV
 
             } else {
                 VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW;
@@ -1364,19 +1366,17 @@
         TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w);
 
 
+        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) {
 
-        
-        if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/canfreq) == 0) {
-//        if (TMR2_COUNT_CAN_TX % 10 == 0) {
             // Position, Velocity, and Torque (ID:1200)
-            if (flag_data_request[0] == HIGH) {
-                if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
+            if (flag_data_request[0] == LOW) {
+                if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
                     if (SENSING_MODE == 0) {
                         CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f));
                     } else if (SENSING_MODE == 1) {
                         CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
                     }
-                } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
+                } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
                     if (SENSING_MODE == 0) {
                         CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
                     } else if (SENSING_MODE == 1) {
@@ -1384,30 +1384,38 @@
                     }
                 }
             }
-            if (flag_data_request[1] == HIGH) {
+            if (flag_data_request[1] == LOW) {
                 //valve position
                 double t_value = 0;
-                if(value>=DDV_CENTER) {
-                    t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+                if(value>=(float) VALVE_CENTER) {
+                    t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER);
                 } else {
-                    t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+                    t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
                 }
-                CAN_TX_TORQUE((int16_t) (t_value));
+                CAN_TX_TORQUE((int16_t) (t_value)); //1300
             }
-
-            if (flag_data_request[2] == HIGH) {
+            
+            
+            if (flag_data_request[2] == LOW) {
                 //pressure A and B
-                CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // 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) //1400
             }
-
-            if (flag_data_request[3] == HIGH) {
+            
+            //If it doesn't rest, below can can not work.
+            for (can_rest = 0; can_rest < 10000; can_rest++) {
+                ;
+            }
+            
+            if (flag_data_request[3] == LOW) {
                 //PWM
-                CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
+                CAN_TX_PWM((int16_t) cur.sen); //1500
             }
-
-            if (flag_data_request[4] == HIGH) {
+            //for (i = 0; i < 10000; i++) {
+//                ;
+//            }
+            if (flag_data_request[4] == LOW) {
                 //valve position
-                CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING);
+                CAN_TX_VALVE_POSITION((int16_t) (CAN_FREQ), (int16_t) (D_DAMPER), (int16_t) OPERATING_MODE); //1600
             }
 
             // Others : Reference position, Reference FT, PWM, Current  (ID:1300)
@@ -1422,8 +1430,6 @@
         }
         TMR2_COUNT_CAN_TX++;
 
-
-
     }
     TIM3->SR = 0x0;  // reset the status register
 
@@ -1434,58 +1440,63 @@
 //unsigned long CNT_TMR5 = 0;
 //float FREQ_TMR5 = (float)FREQ_500;
 //float DT_TMR5 = (float)DT_500;
+
 //extern "C" void TIM2_IRQHandler(void)
 //{
 //    LED = 1;
 //    if (TIM2->SR & TIM_SR_UIF ) {
 //
 //        //CAN ----------------------------------------------------------------------
-//        if (flag_data_request[0] == HIGH) {
+//        //if (flag_data_request[0] == LOW) {
 //            //position+velocity
 //            CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (torq.sen*10.0f));
-//        }
+//        //}
 //
-//        if (flag_data_request[1] == HIGH) {
+//        //if (flag_data_request[1] == LOW) {
 //            //valve position
 //            double t_value = 0;
-//            if(value>=DDV_CENTER) {
-//                t_value = 10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER);
+//            if(value>=(double)VALVE_CENTER) {
+//                t_value = 10000.0f*((double)value-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER);
 //            } else {
-//                t_value = -10000.0f*((double)value-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER);
+//                t_value = -10000.0f*((double)value-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
 //            }
 //            CAN_TX_TORQUE((int16_t) (t_value));
-//        }
+//        //}
 //
-//        if (flag_data_request[2] == HIGH) {
+//        //if (flag_data_request[2] == LOW) {
 //            //pressure A and B
 //            CAN_TX_PRES((int16_t) (valve_pos.ref), (int16_t) (MODE_POS_FT_TRANS * 100.0f)); // 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] == LOW) {
+//            //PWM
+//        int i = 0;
+//        for (i = 0; i < 10000; i++) {
+//            ;
 //        }
-//
-//        if (flag_data_request[3] == HIGH) {
-//            //PWM
 //            CAN_TX_PWM((int16_t) VALVE_DEADZONE_PLUS);
 //            //            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] == LOW) {
+//            //valve position
 //
-//        if (flag_data_request[4] == HIGH) {
-//            //valve position
+//        for (i = 0; i < 10000; i++) {
+//            ;
+//        }
 //            CAN_TX_VALVE_POSITION((int16_t) (K_SPRING), (int16_t) (D_DAMPER), (int16_t) VALVE_POS_RAW_FORCE_FB_LOGGING);
-//            //CAN_TX_VALVE_POSITION((int16_t) (DDV_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
+//            //CAN_TX_VALVE_POSITION((int16_t) ((float) VALVE_CENTER * 10.0f), (int16_t) valve_pos.ref, (int16_t) V_out);
 //
 //            //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);
-//        }
+//        //}
 //
 //
 //    }