Diff: main.cpp

Revision:

220:2976cf1b4252

Parent:

219:e00e71ca3e80

Child:

221:2ec0de8590d3

--- a/main.cpp	Tue Apr 27 11:31:49 2021 +0000
+++ b/main.cpp	Mon May 31 07:19:49 2021 +0000
@@ -238,7 +238,7 @@
     make_delay();
 
     ////// bno rom
-//    spi_eeprom_write(RID_BNO, (int16_t) 4);
+//    spi_eeprom_write(RID_BNO, (int16_t) 2);
 //    make_delay();
     ////////
 
@@ -457,20 +457,20 @@
             float pres_A_new, pres_B_new;
             if (DIR_VALVE_ENC > 0) {
                 pres_A_new = (((float)ADC1->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
-                pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR;
+                pres_B_new = (((float)ADC2->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
             } else {
                 pres_A_new = (((float)ADC2->DR) - PRES_A_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR; // unit : bar
-                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_A_PULSE_PER_BAR;
+                pres_B_new = (((float)ADC1->DR) - PRES_B_NULL_pulse)/ PRES_SENSOR_B_PULSE_PER_BAR;
             }
             pres_A.UpdateSen(pres_A_new,FREQ_TMR4,200.0f);
             pres_B.UpdateSen(pres_B_new,FREQ_TMR4,200.0f);
 
             if ((OPERATING_MODE & 0x01) == 0) { // Rotary Actuator
                 float torq_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.0001f; // mm^3*bar >> Nm 
-                torq.UpdateSen(torq_new,FREQ_TMR4,1000.0);  // unit : Nm
+                torq.UpdateSen(torq_new,FREQ_TMR4,1000.0f);  // unit : Nm
             } else if ((OPERATING_MODE & 0x01) == 1) { // Linear Actuator
                 float force_new = (PISTON_AREA_A * pres_A.sen - PISTON_AREA_B * pres_B.sen) * 0.1f; // mm^2*bar >> N
-                force.UpdateSen(force_new,FREQ_TMR4,1000.0);  // unit : N
+                force.UpdateSen(force_new,FREQ_TMR4,1000.0f);  // unit : N
             }
         }
 
@@ -576,7 +576,7 @@
                 static float PresB_pulse_sum = 0.0; 
                 
                 // DAC Voltage reference set
-                float VREF_TuningGain = 0.000003f;
+                float VREF_TuningGain = -0.000003f;
                 if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 5) {
                     LED = 1;
                     if(SENSING_MODE == 0) { // Force Sensor (Loadcell)
@@ -684,10 +684,16 @@
                     }
                 } else if (FINDHOME_STAGE == FINDHOME_ZEROPOSE) {
                     
-                    int T_move = 2*TMR_FREQ_5k;
-                    REF_POSITION_FINDHOME = (0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT;
-
+//                    int T_move = 2*TMR_FREQ_5k;
+                    int T_move = 10000;
+                    REF_POSITION_FINDHOME = ((0.0f - REF_POSITION_FINDHOME_INIT)*0.5f*(1.0f - cos(3.14159f * (float)cnt_findhome / (float)T_move)) + (float)REF_POSITION_FINDHOME_INIT)/ENC_PULSE_PER_POSITION;
+    
                     cnt_findhome++;
+                    
+                    REFERENCE_MODE = MODE_REF_FINDHOME;
+                    CONTROL_MODE = MODE_JOINT_CONTROL;
+                    alpha_trans = 0.0f;
+                    
                     if (cnt_findhome >= T_move) {
                         cnt_findhome = 0;
                         pos.ref = 0.0f;
@@ -1004,9 +1010,11 @@
             // Position, Velocity, and Torque (ID:1200)
             if (flag_data_request[0] == HIGH) {
                 if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
-                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*8.0f));
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (torq.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+//                    CAN_TX_POSITION_FT((int16_t) (PRES_A_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_A.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
                 } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator
-                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*8.0f));
+                    CAN_TX_POSITION_FT((int16_t) (pos.sen*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (force.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
+//                    CAN_TX_POSITION_FT((int16_t) (PRES_B_VREF*10.0f*200.0f), (int16_t) (vel.sen*20.0f), (int16_t) (pres_B.sen*TORQUE_SENSOR_PULSE_PER_TORQUE*10.0f));
                 }
             }