Yeseong Jeong / Mbed 2 deprecated HydraulicControlBoard_Start

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FastPWM

Files at this revision

API Documentation at this revision

Revision 39:e961db3b93f8, committed 2019-12-05

Comitter:
Lightvalve
Date:
Thu Dec 05 14:29:22 2019 +0000
Parent:
38:118df027d851
Commit message:
191205

Changed in this revision

function_utilities/function_utilities.cpp Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
setting.h Show annotated file Show diff for this revision Revisions of this file 
--- a/function_utilities/function_utilities.cpp	Thu Nov 28 09:18:03 2019 +0000
+++ b/function_utilities/function_utilities.cpp	Thu Dec 05 14:29:22 2019 +0000
@@ -440,7 +440,7 @@
 
 void ROM_CALL_DATA(void)
 {
-    BNO = flashReadInt(Rom_Sector, RID_BNO);
+//    BNO = flashReadInt(Rom_Sector, RID_BNO);
     BNO = 0;
     OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
     CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ);
@@ -448,69 +448,69 @@
     DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR);
     DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR);
     SUPPLY_VOLTAGE = (float) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1f;
-    SUPPLY_VOLTAGE = 12000.0f;
-    VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f;
-    VALVE_VOLTAGE_LIMIT = 12000.0f;
-    P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
-    I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
-    D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
-    P_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_POSITION);
-    I_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_POSITION);
-    D_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_POSITION);
-    P_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_TORQUE);
-    I_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_TORQUE);
-    D_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_TORQUE);
-    VALVE_DEADZONE_PLUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_PLUS)) * 0.1f;
-    VALVE_DEADZONE_MINUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS)) * 0.1f;
-    VELOCITY_COMP_GAIN = flashReadInt(Rom_Sector, RID_VELOCITY_COMP_GAIN);
-    COMPLIANCE_GAIN = flashReadInt(Rom_Sector, RID_COMPLIANCE_GAIN);
-    VALVE_CENTER = flashReadInt(Rom_Sector, RID_VALVE_CNETER);
-    VALVE_FF = flashReadInt(Rom_Sector, RID_VALVE_FF);
-    BULK_MODULUS = flashReadInt(Rom_Sector, RID_BULK_MODULUS);
-    CHAMBER_VOLUME_A = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_A);
-    CHAMBER_VOLUME_B = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_B);
-    PISTON_AREA_A = flashReadInt(Rom_Sector, RID_PISTON_AREA_A);
-    PISTON_AREA_B = flashReadInt(Rom_Sector, RID_PISTON_AREA_B);
-    PISTON_AREA_alpha = (float)PISTON_AREA_B/(float)PISTON_AREA_A;
-    PRES_SUPPLY = flashReadInt(Rom_Sector, RID_PRES_SUPPLY);
-    PRES_RETURN = flashReadInt(Rom_Sector, RID_PRES_RETURN);
-    ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS);
-    ENC_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_PLUS);
-    STROKE = flashReadInt(Rom_Sector, RID_STROKE);
-    VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
-    VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
-    ENC_PULSE_PER_POSITION = flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION);
-    TORQUE_SENSOR_PULSE_PER_TORQUE = flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
-    PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
-    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
-    PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
-    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
-    FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f;
-    HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET);
-    HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
-    TORQUE_VREF = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001f;
-    PRES_A_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_A_SENSOR_VREF) * 0.001f;
-    PRES_B_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_B_SENSOR_VREF) * 0.001f;
-    VALVE_GAIN_LPM_PER_V[0] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[2] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[4] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[6] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[8] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[1] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[3] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f;
-    VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f;
-    for(int i=0; i<25; i++) {
-        VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
-    }
-    for(int i=0; i<100; i++) {
-        JOINT_VEL[i] = ( ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
-    }
-    VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS);
-    VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS);
-    DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
-    VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM);
+    SUPPLY_VOLTAGE = 8000.0f;
+//    VALVE_VOLTAGE_LIMIT = (float) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1f;
+    VALVE_VOLTAGE_LIMIT = 8000.0f;
+//    P_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_VALVE_POSITION);
+//    I_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_VALVE_POSITION);
+//    D_GAIN_VALVE_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_VALVE_POSITION);
+//    P_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_POSITION);
+//    I_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_POSITION);
+//    D_GAIN_JOINT_POSITION = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_POSITION);
+//    P_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_P_GAIN_JOINT_TORQUE);
+//    I_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_I_GAIN_JOINT_TORQUE);
+//    D_GAIN_JOINT_TORQUE = flashReadInt(Rom_Sector, RID_D_GAIN_JOINT_TORQUE);
+//    VALVE_DEADZONE_PLUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_PLUS)) * 0.1f;
+//    VALVE_DEADZONE_MINUS = (float) (flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS)) * 0.1f;
+//    VELOCITY_COMP_GAIN = flashReadInt(Rom_Sector, RID_VELOCITY_COMP_GAIN);
+//    COMPLIANCE_GAIN = flashReadInt(Rom_Sector, RID_COMPLIANCE_GAIN);
+//    VALVE_CENTER = flashReadInt(Rom_Sector, RID_VALVE_CNETER);
+//    VALVE_FF = flashReadInt(Rom_Sector, RID_VALVE_FF);
+//    BULK_MODULUS = flashReadInt(Rom_Sector, RID_BULK_MODULUS);
+//    CHAMBER_VOLUME_A = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_A);
+//    CHAMBER_VOLUME_B = flashReadInt(Rom_Sector, RID_CHAMBER_VOLUME_B);
+//    PISTON_AREA_A = flashReadInt(Rom_Sector, RID_PISTON_AREA_A);
+//    PISTON_AREA_B = flashReadInt(Rom_Sector, RID_PISTON_AREA_B);
+//    PISTON_AREA_alpha = (float)PISTON_AREA_B/(float)PISTON_AREA_A;
+//    PRES_SUPPLY = flashReadInt(Rom_Sector, RID_PRES_SUPPLY);
+//    PRES_RETURN = flashReadInt(Rom_Sector, RID_PRES_RETURN);
+//    ENC_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_MINUS);
+//    ENC_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_ENC_LIMIT_PLUS);
+//    STROKE = flashReadInt(Rom_Sector, RID_STROKE);
+//    VALVE_LIMIT_MINUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_MINUS);
+//    VALVE_LIMIT_PLUS = flashReadInt(Rom_Sector, RID_VALVE_LIMIT_PLUS);
+//    ENC_PULSE_PER_POSITION = flashReadInt(Rom_Sector, RID_ENC_PULSE_PER_POSITION);
+//    TORQUE_SENSOR_PULSE_PER_TORQUE = flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
+//    PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
+//    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
+//    PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
+//    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
+//    FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f;
+//    HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET);
+//    HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
+//    TORQUE_VREF = (float) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001f;
+//    PRES_A_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_A_SENSOR_VREF) * 0.001f;
+//    PRES_B_VREF = (float) flashReadInt(Rom_Sector, RID_PRES_B_SENSOR_VREF) * 0.001f;
+//    VALVE_GAIN_LPM_PER_V[0] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[2] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[4] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[6] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[8] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[1] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[3] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[5] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[7] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01f;
+//    VALVE_GAIN_LPM_PER_V[9] = (float) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01f;
+//    for(int i=0; i<25; i++) {
+//        VALVE_POS_VS_PWM[i] = (float) (flashReadInt(Rom_Sector, RID_VALVE_POS_VS_PWM_0 + i));
+//    }
+//    for(int i=0; i<100; i++) {
+//        JOINT_VEL[i] = ( ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((flashReadInt(Rom_Sector, RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
+//    }
+//    VALVE_MAX_POS = flashReadInt(Rom_Sector, RID_VALVE_MAX_POS);
+//    VALVE_MIN_POS = flashReadInt(Rom_Sector, RID_VALVE_MIN_POS);
+//    DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
+//    VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM);
 
 }
 

--- a/main.cpp	Thu Nov 28 09:18:03 2019 +0000
+++ b/main.cpp	Thu Dec 05 14:29:22 2019 +0000
@@ -143,6 +143,9 @@
     MODE_DDV_POS_VS_FLOWRATE,                           //32
 };
 
+int messi = 0;
+float end_pos = 0.0f;
+
 int main()
 {
     /*********************************
@@ -152,11 +155,11 @@
     //pc.baud(9600);
 
     // i2c init
-    i2c.frequency(400 * 1000);          // 0.4 mHz
-    wait_ms(2);                         // Power Up wait
-    look_for_hardware_i2c();            // Hardware present
-    init_as5510(i2c_slave_addr1);
-    make_delay();
+    //i2c.frequency(400 * 1000);          // 0.4 mHz
+//    wait_ms(2);                         // Power Up wait
+//    look_for_hardware_i2c();            // Hardware present
+//    init_as5510(i2c_slave_addr1);
+//    make_delay();
 
 //    // spi init
     //eeprom.format(8,3);
@@ -169,9 +172,9 @@
     ROM_CALL_DATA();
     make_delay();
 
-    // ADC init
-    Init_ADC();
-    make_delay();
+//    // ADC init
+//    Init_ADC();
+//    make_delay();
 
     // Pwm init
     Init_PWM();
@@ -206,32 +209,27 @@
     make_delay();
 
     //DAC init
-    dac_1 = PRES_A_VREF / 3.3f;
-    dac_2 = PRES_B_VREF / 3.3f;
-    make_delay();
+//    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.5f;
-        else
-            ID_index_array[i] =  (i+1) * 0.5f;
-    }
+//    for (int i=0; i<50; i++) {
+//        if(i%2==0)
+//            ID_index_array[i] = - i * 0.5f;
+//        else
+//            ID_index_array[i] =  (i+1) * 0.5f;
+//    }
+    
+    ENC_UPDATE();
+    pos.ref = pos.sen;
+    
 
     /************************************
     ***     Program is operating!
     *************************************/
     while(1) {
-        if(timer_while==1000) {
-            //pc.printf("cmd %d\n ");
-            //i2c
-            read_field(i2c_slave_addr1);
-            if(DIR_VALVE_ENC < 0) value = 1023 - value;
-
-//            if(LED==1) {
-//                LED=0;
-//            } else
-//                LED = 1;
-            timer_while = 0;
+        if(timer_while==100000) {
+            messi = 1;
 
         }
         timer_while ++;
@@ -452,12 +450,51 @@
 {
     if (TIM3->SR & TIM_SR_UIF ) {
         ENC_UPDATE();
+        
+        if(messi == 1){
+            pos.ref = pos.ref + 0.007f;//0.009 -> 10ml/30min
+            pos.err = pos.ref - (float) pos.sen;
+            pos.err_diff = pos.err - pos.err_old;
+            pos.err_old = pos.err;
+            pos.err_sum += pos.err;
+            if (pos.err_sum > 1000) pos.err_sum = 1000;
+            if (pos.err_sum<-1000) pos.err_sum = -1000;
+            V_out = (float) 500.0f + (float) 3000.0f * 0.01f * pos.err + (float) 0.0f * 0.01f * pos.err_sum + (float) 0.0f * pos.err_diff;
+            
+            if (abs(pos.ref-pos.sen) > 1000){
+                V_out = 0;
+                pos.ref = pos.sen;
+                end_pos = pos.sen;
+                messi = 2;
+            } 
+        }
+        if(messi ==2){
+            pos.ref = pos.ref - 2.0f;
+            pos.err = pos.ref - (float) pos.sen;
+            pos.err_diff = pos.err - pos.err_old;
+            pos.err_old = pos.err;
+            pos.err_sum += pos.err;
+            if (pos.err_sum > 1000) pos.err_sum = 1000;
+            if (pos.err_sum<-1000) pos.err_sum = -1000;
+            V_out = (float) 3000.0f * 0.01f * pos.err + (float) 0.0f * 0.01f * pos.err_sum + (float) 0.0f * pos.err_diff;
+            
+           // if (abs(pos.ref-pos.sen) > 2000){
+//                V_out = 0;
+//                messi = 0;
+//            } 
+            
+            if (abs(end_pos-pos.sen) > 81920){
+                V_out = 0;
+                messi = 0;
+            } 
+        }
+        
 
         // CONTROL LOOP ------------------------------------------------------------
 
         switch (CONTROL_MODE) {
             case MODE_NO_ACT: {
-                V_out = 0.0f;
+                V_out = V_out;
                 break;
             }
 

--- a/setting.h	Thu Nov 28 09:18:03 2019 +0000
+++ b/setting.h	Thu Dec 05 14:29:22 2019 +0000
@@ -7,7 +7,8 @@
 //#define PWM_ARR 0x465         // loop 80k, pwm 40k 
 //#define PWM_ARR     0x8CA       // loop 40k, pwm 20k
 //#define PWM_ARR     0x1194      // loop 20k, pwm 10k
-#define PWM_ARR     0x2328    // loop 10k, pwm 5k
+//#define PWM_ARR     0x2328    // loop 10k, pwm 5k
+#define PWM_ARR     0x465 
 #define TMR3_COUNT  0x4650      // loop 5k
 #define TMR2_COUNT  0x2710      // loop 500hz with prescale 18