distribution-201229

Dependencies:   mbed FastPWM

Revision:
15:bd0d12728506
Parent:
14:8e7590227d22
Child:
16:903b5a4433b4
--- a/main.cpp	Thu Aug 29 07:38:00 2019 +0000
+++ b/main.cpp	Fri Aug 30 02:26:11 2019 +0000
@@ -174,7 +174,8 @@
     //DAC init
     //dac_1 = PRES_A_VREF/3.3;
     dac_1 = 0.0;
-    dac_2 = PRES_B_VREF/3.3;
+    //dac_2 = PRES_B_VREF/3.3;
+    dac_2 = 0.0;
     make_delay();
 
     for (int i=0; i<100; i++) {
@@ -195,7 +196,7 @@
         //pc.printf("Message received: %d\n", msg.data[0]);
         //pc.printf("Message received: %d\n", 13);
         //i2c
-        //read_field(i2c_slave_addr1);
+        read_field(i2c_slave_addr1);
     }
 }
 
@@ -309,23 +310,29 @@
         ***     Sensor Read & Data Handling
         ********************************************************/
 
-        if((CNT_TMR4%2)==0) {
-
+        if((CNT_TMR4%20)==0) {
+            //Using LoadCell
+//            ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
+//            //while((ADC1->SR & 0b10));
+//            double alpha_update_torque = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+//            double torque_new = ((double)ADC1->DR - PRES_A_NULL)  / TORQUE_SENSOR_PULSE_PER_TORQUE + 1.0;
+//            torq.sen = torq.sen*(1.0-alpha_update_torque)+torque_new*(alpha_update_torque);
+            
             //Pressure sensor A
             ADC1->CR2  |= 0x40000000;                        // adc _ 12bit
             //while((ADC1->SR & 0b10));
             double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
             double pres_A_new = ((double)ADC1->DR - PRES_A_NULL)  / PRES_SENSOR_A_PULSE_PER_BAR + 1.0;
             pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
-            //pres_A.sen = (double)ADC1->DR;
-/*            
             //Pressure sensor 1B
-            //ADC2->CR2  |= 0x40000000;                        // adc _ 12bit
-            while((ADC2->SR & 0b10));
+            ADC2->CR2  |= 0x40000000;                        // adc _ 12bit
+            //while((ADC2->SR & 0b10));
             double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
             double pres_B_new = ((double)ADC2->DR - PRES_B_NULL)  / PRES_SENSOR_B_PULSE_PER_BAR + 1.0;
             pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+            torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
             
+/*                      
             //Current
             ADC3->CR2  |= 0x40000000;                        // adc _ 12bit
 //          a1=ADC2->DR;
@@ -525,7 +532,7 @@
                 PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50;
 
                 // torque feedback
-                //            torq.err = Ref_Joint_Torq - CUR_TORQUE_NM;
+                //            torq.err = torq.ref - torq.sen;
                 //            torq.err_diff = torq.err - torq.err_old;
                 //            torq.err_old = torq.err;
                 //            torq.err_sum += torq.err;
@@ -533,9 +540,9 @@
                 //            if (torq.err_sum<-1000) torq.err_sum = -1000;
                 //            VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
                 //            VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
+                
                 PWM_RAW_FORCE_FB = 0.0;
 
-                //            VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
                 V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
 
                 break;
@@ -566,7 +573,7 @@
                 //            VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
 
                 // torque feedback
-                //            torq.err = Ref_Joint_Torq - CUR_TORQUE_NM;
+                //            torq.err = torq.ref - torq.sen;
                 //            torq.err_diff = torq.err - torq.err_old;
                 //            torq.err_old = torq.err;
                 //            torq.err_sum += torq.err;
@@ -658,7 +665,7 @@
                 if (pos.err_sum<-1000) pos.err_sum = -1000;
                 VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01;
 
-                torq.err = torq.ref - (double) torq.sen;
+                torq.err = torq.ref - torq.sen;
                 torq.err_diff = torq.err - torq.err_old;
                 torq.err_old = torq.err;
                 torq.err_sum += torq.err;
@@ -777,11 +784,9 @@
                 double K_spring = 0.7;
                 double D_damper = 0.02;
 //              T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
-                
-                T_REF = torq.ref; // unit : Nm
             
                 // torque feedback
-                torq.err = T_REF - (double) torq.sen;
+                torq.err = torq.ref - torq.sen;
                 //            torq.err_diff = torq.err - torq.err_old;
                 //            torq.err_old = torq.err;
                 torq.err_sum = torq.err_sum + torq.err/(double)TMR_FREQ_5k;
@@ -854,20 +859,16 @@
             case MODE_TORQUE_SENSOR_NULLING: {
                 // DAC Voltage reference set
                 if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
-                    CUR_TORQUE_sum += CUR_TORQUE;
-
+                    CUR_TORQUE_sum += torq.sen;
+                    
                     if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
                         CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0;
                         CUR_TORQUE_sum = 0;
 
                         TORQUE_VREF += 0.0001 * (TORQUE_NULL - CUR_TORQUE_mean);
-
-                        //                    TORQUE_VREF = 0;
+                        
                         if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3;
                         if (TORQUE_VREF < 0) TORQUE_VREF = 0;
-
-                        spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
-                        spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
                     }
                 } else {
                     CONTROL_MODE = MODE_NO_ACT;
@@ -875,6 +876,8 @@
                     CUR_TORQUE_sum = 0;
                     CUR_TORQUE_mean = 0;
                     spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
+                    
+                    pc.printf("%f\n", TORQUE_VREF);
 
                 }
                 TMR3_COUNT_TORQUE_NULL++;
@@ -1152,12 +1155,11 @@
                         if (PRES_B_VREF > 3.3) PRES_B_VREF = 3.3;
                         if (PRES_B_VREF < 0) PRES_B_VREF = 0;
 
-                        spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
-                        spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
+//                        spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
+//                        spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
                     }
                 } else {
                     CONTROL_MODE = MODE_NO_ACT;
-                    pc.printf("no_act");
                     TMR3_COUNT_PRES_NULL = 0;
                     CUR_PRES_A_sum = 0;
                     CUR_PRES_B_sum = 0;
@@ -1166,6 +1168,11 @@
 
                     spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
                     spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
+                    
+                    dac_1 = PRES_A_VREF / 3.3;
+                    dac_2 = PRES_B_VREF / 3.3;
+                    pc.printf("%f\n", PRES_A_VREF);
+                    pc.printf("%f\n", PRES_B_VREF);
                 }
                 TMR3_COUNT_PRES_NULL++;
                 break;
@@ -1644,7 +1651,7 @@
         }
 
         //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
-        PWM_out = CUR_PWM;
+        //PWM_out = CUR_PWM;
 
 
 
@@ -1670,7 +1677,7 @@
             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_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) (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.);
@@ -1762,9 +1769,10 @@
             } else
                 LED = 1;
 //            LED != LED;
-            pc.printf("flag %f\n", (double) pres_A.sen);
-            spi_eeprom_ready();
-            pc.printf("romrom %d\n", (int) PRES_SENSOR_A_PULSE_PER_BAR);
+//            pc.printf("A %f\n", (double) pres_A.sen);
+//            pc.printf("A %f\n", PRES_SENSOR_A_PULSE_PER_BAR);
+//            pc.printf("B %f\n", (double) pres_B.sen);
+//            pc.printf("B %f\n", PRES_SENSOR_B_PULSE_PER_BAR);
         }
 
         /*******************************************************