Diff: main.cpp

Revision:

61:8d36b4042096

Parent:

60:64181f1d3e60

Child:

62:456f87e3124e

--- a/main.cpp	Tue Mar 31 05:12:37 2020 +0000
+++ b/main.cpp	Mon Apr 13 11:55:09 2020 +0000
@@ -131,13 +131,15 @@
     MODE_SYSTEM_ID,                                     //33
 };
 
+
+
 int main()
 {
     /*********************************
     ***     Initialization
     *********************************/
     LED = 1;
-    //pc.baud(9600);
+    pc.baud(9600);
 
     // i2c init
     i2c.frequency(400 * 1000);          // 0.4 mHz
@@ -215,6 +217,7 @@
     *************************************/
     while(1) {
         if(timer_while==1000 && OPERATING_MODE==5) {
+        //if(timer_while==1000) {
             //i2c
             read_field(i2c_slave_addr1);
             if(DIR_VALVE_ENC < 0) value = 1023 - value;
@@ -225,6 +228,7 @@
             timer_while = 0;
         }
         timer_while ++;
+        //pc.printf("%d\n", value);
     }
 }
 
@@ -359,8 +363,8 @@
         if (CNT_TMR4 % (int) ((int) FREQ_TMR4/TMR4_FREQ_10k) == 0) {
             ENC_UPDATE();
         }
-        
-        ADC1->CR2  |= 0x40000000;   
+
+        ADC1->CR2  |= 0x40000000;
         if (SENSING_MODE == 0) {
             // Torque Sensing (0~210)bar =============================================
             float pres_A_new = (((float) ADC1->DR) - 2047.5f);
@@ -384,7 +388,7 @@
                 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));
@@ -616,16 +620,16 @@
                         //REFERENCE_MODE = MODE_REF_NO_ACT;
                         if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 2.0f;
                         else pos.ref = pos.ref - 2.0f;
-                        
+
 //                        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 + (float) VALVE_CENTER;
 //                        VALVE_POS_CONTROL(valve_pos.ref);
-                
+
                         CONTROL_MODE = MODE_JOINT_CONTROL;
                         alpha_trans = 0.0f;
-                    
+
 
                     } else {
                         ENC_SET(HOMEPOS_OFFSET);
@@ -645,7 +649,7 @@
                     vel.ref = 0.0f;
 
                     // input for position control
-                    
+
                     CONTROL_MODE = MODE_JOINT_CONTROL;
                     alpha_trans = 0.0f;
 //                    pos.err = pos.ref - (float)pos.sen;
@@ -921,7 +925,7 @@
                         } else {
                             DZ_case = 0;
                         }
-                        
+
                         CAN_TX_PRES((int16_t) (DZ_case), (int16_t) (6));
 
                         first_check = 1;
@@ -986,7 +990,7 @@
                                 valve_pos.ref = VALVE_MAX_POS;
                             }
                             VALVE_POS_CONTROL(valve_pos.ref);
-        
+
                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
                             Vout.ref = 0.0f;
                         } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
@@ -1031,7 +1035,7 @@
                                 valve_pos.ref = VALVE_MAX_POS;
                             }
                             VALVE_POS_CONTROL(valve_pos.ref);
-                
+
                         } else if(VALVE_DZ_timer == (int) (2.0f * (float) TMR_FREQ_5k)) {
                             Ref_Valve_Pos_Old = valve_pos.ref;
                             FINAL_POS = pos.sen;
@@ -1074,7 +1078,7 @@
                         } else if(VALVE_DZ_timer > (int) (2.0f * (float) TMR_FREQ_5k)) {
                             Ref_Valve_Pos_Old = valve_pos.ref;
                             FINAL_POS = pos.sen;
-                            
+
                             if((FINAL_POS - START_POS)>100) {
                                 DZ_DIRECTION = 1;
                             } else if((FINAL_POS - START_POS)<-100) {
@@ -1091,7 +1095,7 @@
                                 first_check = 0;
                                 VALVE_DEADZONE_MINUS = (float) FIRST_DZ;
                                 VALVE_DEADZONE_PLUS = (float) SECOND_DZ;
-                                
+
                                 ROM_RESET_DATA();
 
                                 CONTROL_UTILITY_MODE = MODE_NO_ACT;
@@ -1296,9 +1300,9 @@
                             torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k;
                         }
                     }
-                    
+
                     VALVE_POS_CONTROL(valve_pos.ref);
-                    
+
                     Vout.ref = (float) P_GAIN_JOINT_POSITION * 0.01f * ((float) pos.err);
                     V_out = (float) Vout.ref;