distribution-201229

Dependencies:   mbed FastPWM

Revision:
58:2eade98630e2
Parent:
57:f4819de54e7a
Child:
59:f308b1656d9c
diff -r f4819de54e7a -r 2eade98630e2 main.cpp
--- a/main.cpp	Fri Mar 06 12:14:38 2020 +0000
+++ b/main.cpp	Mon Mar 09 12:43:04 2020 +0000
@@ -194,10 +194,13 @@
     make_delay();
 
     //DAC init
-//    dac_1 = PRES_A_VREF / 3.3f;
-//    dac_2 = PRES_B_VREF / 3.3f;
-    dac_1 = TORQUE_VREF / 3.3f;
-    dac_2 = 0.0f;
+    if (SENSING_MODE == 0) {
+        dac_1 = TORQUE_VREF / 3.3f;
+        dac_2 = 0.0f;
+    } else if (SENSING_MODE == 1) {
+        dac_1 = PRES_A_VREF / 3.3f;
+        dac_2 = PRES_B_VREF / 3.3f;
+    }
     make_delay();
 
     for (int i=0; i<50; i++) {
@@ -206,7 +209,7 @@
         else
             ID_index_array[i] =  (i+1) * 0.5f;
     }
-    
+
     /************************************
     ***     Program is operating!
     *************************************/
@@ -356,21 +359,46 @@
         if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
             ENC_UPDATE();
         }
+        
+        ADC1->CR2  |= 0x40000000;   
+        if (SENSING_MODE == 0) {
+            // Torque Sensing (0~210)bar =============================================
+            float pres_A_new = (((float) ADC1->DR) - 2047.5f);
+            double alpha_update_ft = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 100.0f)); // f_cutoff : 200Hz
+            pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new;
+            torq.sen = pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE;
 
-        //Pressure sensor A
-        ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
-        //while((ADC1->SR & 0b10));
-        float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
-        float pres_A_new = ((float)ADC1->DR);
-        pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
-        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.
+        } else if (SENSING_MODE == 1) {
+            // Pressure Sensing (0~210)bar =============================================
+            float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL);
+            float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL);
+            double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 500Hz
+            pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new;
+            pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new;
+            CUR_PRES_A_BAR = pres_A.sen / PRES_SENSOR_A_PULSE_PER_BAR;
+            CUR_PRES_B_BAR = pres_B.sen / PRES_SENSOR_B_PULSE_PER_BAR;
 
-
-        //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;
+            if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
+                torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.0001f; // mm^3*bar >> Nm
+            } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
+                torq.sen = (PISTON_AREA_A * CUR_PRES_A_BAR - PISTON_AREA_B * CUR_PRES_B_BAR) * 0.1f; // mm^2*bar >> N
+            }
+        }
+        
+//        //Pressure sensor A
+//        ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
+//        //while((ADC1->SR & 0b10));
+//        float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f));
+//        float pres_A_new = ((float)ADC1->DR);
+//        pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
+//        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.
+//
+//
+//        //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;
 
 
         //Current
@@ -381,7 +409,7 @@
         float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA
         cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur);
         //cur.sen = raw_cur;
-        
+
         CNT_TMR4++;
     }
     TIM4->SR = 0x0;  // reset the status register
@@ -398,8 +426,6 @@
 extern "C" void TIM3_IRQHandler(void)
 {
     if (TIM3->SR & TIM_SR_UIF ) {
-        
-        
 
         if (((OPERATING_MODE&0b110)>>1) == 0) {
             K_v = 0.4f; // Moog (LPM >> mA) , 100bar
@@ -426,17 +452,17 @@
             if((float) cnt_trans * DT_TMR3 > 3.0f )
                 MODE_POS_FT_TRANS = 0;
         } else if(MODE_POS_FT_TRANS == 2) {
-            alpha_trans = 1.0;
+            alpha_trans = 1.0f;
             cnt_trans = 0;
         } else {
-            alpha_trans = 0.0;
+            alpha_trans = 0.0f;
             cnt_trans = 0;
         }
 
 
         int UTILITY_MODE = 0;
         int CONTROL_MODE = 0;
-        
+
         if (CONTROL_UTILITY_MODE >= 20 || CONTROL_UTILITY_MODE == 0) {
             UTILITY_MODE = CONTROL_UTILITY_MODE;
             CONTROL_MODE = MODE_NO_ACT;
@@ -472,7 +498,7 @@
                         dac_1 = TORQUE_VREF / 3.3f;
                     }
                 } else {
-                    CONTROL_MODE = MODE_NO_ACT;
+                    CONTROL_UTILITY_MODE = MODE_NO_ACT;
                     TMR3_COUNT_TORQUE_NULL = 0;
                     CUR_TORQUE_sum = 0;
                     CUR_TORQUE_mean = 0;
@@ -713,7 +739,6 @@
 //                }
 //
 //            }
-
             case MODE_PRESSURE_SENSOR_NULLING: {
                 // DAC Voltage reference set
                 if (TMR3_COUNT_PRES_NULL < TMR_FREQ_5k * 2) {
@@ -727,8 +752,8 @@
                         CUR_PRES_B_sum = 0;
 
                         float VREF_NullingGain = 0.0003f;
-                        PRES_A_VREF = PRES_A_VREF - VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
-                        PRES_B_VREF = PRES_B_VREF - VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
+                        PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean;
+                        PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean;
 
                         if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
                         if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
@@ -1156,7 +1181,7 @@
                 }
                 break;
             }
-            
+
             case MODE_SYSTEM_ID: {
                 freq_sysid_Iref = (double) cnt_sysid * DT_TMR3 * 3.;
                 valve_pos.ref = 2500.0f * sin(2.0f * 3.14159f * freq_sysid_Iref * (double) cnt_sysid * DT_TMR3);
@@ -1168,8 +1193,8 @@
                 }
                 break;
             }
-            
-            
+
+
 
             default:
                 break;
@@ -1191,7 +1216,7 @@
                 } else { //SW Valve
                     VALVE_POS_CONTROL(valve_pos.ref);
                 }
-                
+
                 break;
             }
 
@@ -1203,11 +1228,11 @@
                 torq_ref = torq.ref + (K_SPRING * pos.err * 0.01f + D_DAMPER * vel.err * 0.0001f) / ENC_PULSE_PER_POSITION; //[N]
 
                 // torque feedback
-                torq.err = (torq_ref - torq.sen)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE); //[N]
+                torq.err = (torq_ref)/(float)(TORQUE_SENSOR_PULSE_PER_TORQUE)  - torq.sen; //[N]
                 torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[N]
 
                 if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) {
-                    
+
                     double I_REF_POS = 0.0f;
                     double I_REF_FORCE_FB = 0.0f; // I_REF by Force Feedback
                     double I_REF_VC = 0.0f; // I_REF for velocity compensation
@@ -1215,7 +1240,7 @@
                     double temp_vel_pos = 0.0f;
                     double temp_vel_torq = 0.0f;
                     double wn_Pos = 2.0f * PI * 5.0f; // f_cut : 5Hz Position Control
-                    
+
                     if ((OPERATING_MODE && 0x01) == 0) { // Rotary Mode
                         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
                         //                            L when P-gain = 100, f_cut = 10Hz                                 L feedforward velocity
@@ -1275,10 +1300,10 @@
                     VALVE_POS_CONTROL(valve_pos.ref);
 
                 }
-                
+
                 break;
             }
-            
+
             case MODE_VALVE_OPEN_LOOP: {
                 V_out = (float) Vout.ref;
                 break;
@@ -1290,7 +1315,7 @@
 
 
         if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve
-            
+
             ////////////////////////////////////////////////////////////////////////////
             ////////////////////////////  CURRENT CONTROL //////////////////////////////
             ////////////////////////////////////////////////////////////////////////////
@@ -1368,8 +1393,8 @@
             else if (CUR_PWM_lin < 0) V_out = (int) CUR_PWM_lin - 140;
             else V_out = CUR_PWM_lin;
         }
-        
-        
+
+
 
         /*******************************************************
         ***     PWM
@@ -1404,15 +1429,15 @@
             if (flag_data_request[0] == HIGH) {
                 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));
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (pres_A.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));
+                        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));
                     }
                 } 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));
+                        CAN_TX_POSITION_FT((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f));
                     } else if (SENSING_MODE == 1) {
-                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) (pres_A.sen*10.0f), (int16_t) (pres_B.sen*10.0f));
+                        CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/4.0f), (int16_t) (vel.sen/100.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f));
                     }
                 }
             }
@@ -1424,27 +1449,27 @@
 //                } else {
 //                    t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER);
 //                }
-                if(OPERATING_MODE==5){
+                if(OPERATING_MODE==5) {
                     t_value = (double) value;
-                }else if(CURRENT_CONTROL_MODE==1){
+                } else if(CURRENT_CONTROL_MODE==1) {
                     t_value = cur.sen;
-                }else{
+                } else {
                     t_value = V_out;
                 }
                 CAN_TX_TORQUE((int16_t) (t_value)); //1300
             }
-            
-            
+
+
             if (flag_data_request[2] == HIGH) {
                 //pressure A and B
                 CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400
             }
-            
+
             //If it doesn't rest, below can can not work.
             for (can_rest = 0; can_rest < 10000; can_rest++) {
                 ;
             }
-            
+
             if (flag_data_request[3] == HIGH) {
                 //PWM
                 CAN_TX_PWM((int16_t) cur.sen); //1500
@@ -1462,7 +1487,7 @@
 //            CAN_TX_SOMETHING((int) (FORCE_VREF), (int16_t) (1), (int16_t) (2), (int16_t) (3));
 //        }
             //if (flag_delay_test == 1){
-                //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
+            //CAN_TX_PRES((int16_t) (0),(int16_t) torq_ref);
             //}
 
             TMR2_COUNT_CAN_TX = 0;
@@ -1472,115 +1497,4 @@
     }
     TIM3->SR = 0x0;  // reset the status register
 
-}
-
-
-
-//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] == 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] == LOW) {
-//            //valve position
-//            double t_value = 0;
-//            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)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER);
-//            }
-//            CAN_TX_TORQUE((int16_t) (t_value));
-//        //}
-//
-//        //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)
-//
-//        //}
-//
-//        //if (flag_data_request[3] == LOW) {
-//            //PWM
-//        int i = 0;
-//        for (i = 0; i < 10000; i++) {
-//            ;
-//        }
-//            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
-//
-//        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) ((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);
-//        //}
-//
-//
-//    }
-//    TIM2->SR = 0x0;  // reset the status register
-//}
-
-
-//
-//void CurrentControl()
-//{
-//    cur.err = cur.ref - cur.sen;
-//    cur.err_int = cur.err_int + cur.err*DT_TMR4;
-//    cur.err_diff = (cur.err - cur.err_old)*FREQ_TMR4;
-//    cur.err_old = cur.err;
-//
-//    float R_model = 150.0f; // ohm
-//    float L_model = 0.3f;
-//    float w0 = 2.0f*3.14f*90.0f;
-//    float KP_I = L_model*w0;
-//    float KI_I = R_model*w0;
-//    float KD_I = 0.0f;
-//
-//    float FF_gain = 0.0f;
-//    V_out = (int) (KP_I * cur.err + KI_I * cur.err_int + KD_I * cur.err_diff);
-//    //          V_out = V_out + FF_gain * (R_model*I_REF); // Unit : mV
-//    V_out = V_out + FF_gain * (R_model*cur.ref + L_model*cur.ref_diff); // Unit : mV
-//
-//    float Ka = 5.0f/KP_I;
-//    if(V_out > V_MAX) {
-//        V_rem = V_out-V_MAX;
-//        V_rem = Ka*V_rem;
-//        V_out = V_MAX;
-//        cur.err_int = cur.err_int - V_rem*DT_5k;
-//    } else if(V_out < -V_MAX) {
-//        V_rem = V_out-(-V_MAX);
-//        V_rem = Ka*V_rem;
-//        V_out = -V_MAX;
-//        cur.err_int = cur.err_int - V_rem*DT_5k;
-//    }
-//}
-
-
-
-
-
-
-
-
+}
\ No newline at end of file