Diff: main.cpp

Revision:

38:118df027d851

Parent:

36:a46e63505ed8

Child:

39:e961db3b93f8

Child:

40:3f2c0619c8c4

--- a/main.cpp	Mon Nov 25 05:04:12 2019 +0000
+++ b/main.cpp	Thu Nov 28 09:18:03 2019 +0000
@@ -206,15 +206,15 @@
     make_delay();
 
     //DAC init
-    dac_1 = PRES_A_VREF / 3.3;
-    dac_2 = PRES_B_VREF / 3.3;
+    dac_1 = PRES_A_VREF / 3.3f;
+    dac_2 = PRES_B_VREF / 3.3f;
     make_delay();
 
     for (int i=0; i<50; i++) {
         if(i%2==0)
-            ID_index_array[i] = - i * 0.5;
+            ID_index_array[i] = - i * 0.5f;
         else
-            ID_index_array[i] =  (i+1) * 0.5;
+            ID_index_array[i] =  (i+1) * 0.5f;
     }
 
     /************************************
@@ -246,9 +246,17 @@
     for(i=0; i<VALVE_POS_NUM; i++) {
         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) {
-                Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_CENTER;
+                if(JOINT_VEL[i+1] == JOINT_VEL[i]){
+                    Ref_Valve_Pos_FF = DDV_CENTER;
+                }else{
+                    Ref_Valve_Pos_FF = ((float) 10/(JOINT_VEL[i+1] - JOINT_VEL[i]) * (REF_JOINT_VEL - JOINT_VEL[i])) + DDV_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]);
+                if(JOINT_VEL[i+1] == JOINT_VEL[i-1]){
+                    Ref_Valve_Pos_FF = DDV_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]);
+                }
             }
             break;
         }
@@ -271,6 +279,12 @@
 {
     int i = 0;
 
+    if(REF_VALVE_POS > VALVE_MAX_POS) {
+        REF_VALVE_POS = VALVE_MAX_POS;
+    } else if(REF_VALVE_POS < VALVE_MIN_POS) {
+        REF_VALVE_POS = VALVE_MIN_POS;
+    }
+
     valve_pos_err = REF_VALVE_POS - value;
     valve_pos_err_diff = valve_pos_err - valve_pos_err_old;
     valve_pos_err_old = valve_pos_err;
@@ -280,11 +294,7 @@
 
     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;
 
-    if(REF_VALVE_POS > VALVE_MAX_POS) {
-        REF_VALVE_POS = VALVE_MAX_POS;
-    } else if(REF_VALVE_POS < VALVE_MIN_POS) {
-        REF_VALVE_POS = VALVE_MIN_POS;
-    }
+    
 
 
 //    if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0])
@@ -399,16 +409,17 @@
         ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
         //while((ADC1->SR & 0b10));
         float alpha_update_pres_A = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
-        float pres_A_new = ((float)ADC1->DR - PRES_A_NULL)  / PRES_SENSOR_A_PULSE_PER_BAR;
+//        float pres_A_new = ((float)ADC1->DR - PRES_A_NULL)  / PRES_SENSOR_A_PULSE_PER_BAR;
+        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)*10000.0f / 2048.0f; //N
+        
 
         //Pressure sensor 1B
-        //ADC2->CR2  |= 0x40000000;                        // adc _ 12bit
-        //while((ADC2->SR & 0b10));
-        float alpha_update_pres_B = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f));
-        float pres_B_new = ((float)ADC2->DR - PRES_B_NULL)  / PRES_SENSOR_B_PULSE_PER_BAR;
-        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)/(2.0f*3.14f*1000.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;
 
 
 
@@ -526,19 +537,17 @@
                 //            if(Ref_Joint_Vel_Act > 0) K_ff = 0.75f; // close
                 //            VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50f;
 
-                // torque feedback
-                //            torq.err = torq.ref - torq.sen;
-                //            torq.err_diff = torq.err - torq.err_old;
-                //            torq.err_old = torq.err;
-                //            torq.err_sum += torq.err;
-                //            if (torq.err_sum > 1000) torq.err_sum = 1000;
-                //            if (torq.err_sum<-1000) torq.err_sum = -1000;
-                //            VALVE_PWM_RAW_TORQ = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
-                //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01f;
-                VALVE_POS_RAW_FORCE_FB = 0.0f;
+                //torque feedback
+                torq.err = - torq.ref + torq.sen;
+                torq.err_diff = torq.err - torq.err_old;
+                torq.err_old = torq.err;
+                torq.err_sum += torq.err;
+                if (torq.err_sum > 1000) torq.err_sum = 1000;
+                if (torq.err_sum<-1000) torq.err_sum = -1000;
+                VALVE_POS_RAW_FORCE_FB = (float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum + (float) D_GAIN_JOINT_TORQUE * torq.err_diff;
+                VALVE_POS_RAW_FORCE_FB = VALVE_POS_RAW_FORCE_FB * 0.01f;
 
                 valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB;
-                //valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER;
 
                 if (valve_pos.ref > DDV_CENTER) {
                     valve_pos.ref = valve_pos.ref + VALVE_DEADZONE_PLUS - DDV_CENTER;
@@ -1116,9 +1125,9 @@
                         CUR_PRES_A_sum = 0;
                         CUR_PRES_B_sum = 0;
 
-                        float VREF_NullingGain = 0.003f;
-                        PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean);
-                        PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean);
+                        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);
 
                         if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
                         if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
@@ -1685,8 +1694,8 @@
 
         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) (pres_A.sen), (int16_t) (PRES_A_VREF * 1000.0)); // 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.);