Diff: function_utilities/function_utilities.cpp

Revision:

170:42c938a40313

Parent:

169:645207e160ca

Child:

173:68c7914679ec

--- a/function_utilities/function_utilities.cpp	Mon Nov 16 02:23:23 2020 +0000
+++ b/function_utilities/function_utilities.cpp	Wed Nov 18 12:03:39 2020 +0000
@@ -92,7 +92,7 @@
 float Pt = 0.0f;    // 0bar = 0Pa
 //float Kv = 0.00000002635f;          // Q = Kv*xv*sqrt(Ps-Pa)    => 100bar full opening 5LPM    (full opening : xv = 1)  [unit] m^3.5/kg^0.5
 float gamma_hat = 1075.0f; // Kv*beta*A/(sqrt(2)*V)   0.00000002635f * 1300000000.0f *  / (sqrt(2.0f)*(1256.6f + 236.4f * 39.75f) * 0.000000001f / 2)     [unit] m^3.5/kg^0.5
-float V = 0.0000053f;           // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
+float V_adapt = 0.0000053f;           // (1256.6f + 236.4f * 39.75f) * 0.000000001f / 2
 float x_4_des_old = 0.0f;
 
 //int16_t VALVE_LIMIT_PLUS;
@@ -344,6 +344,7 @@
 
 int timer_while = 0;
 int while_index = 0;
+int RL_timer = 0;
 
 float K_LPF = 0.0f;
 float D_LPF = 0.0f;
@@ -351,9 +352,51 @@
 float torq_ref_past = 0.0f;
 float output_normalized = 0.0f;
 
+int batch = 0;
+float train_set_x[batch_size] = {0.0f};
+float train_set_error[batch_size] = {0.0f};
+float train_set_count[batch_size] = {0.0f};
+float state_array[batch_size][num_input_RL] = {0.0f};
+float V[batch_size] = {0.0f};
+float r[batch_size] = {0.0f};
+float td_target[batch_size] = {0.0f};
+float delta[batch_size] = {0.0f};
+float advantage[batch_size] = {0.0f};
+float mean = 0.0f;
+float deviation = 0.0f;
+float mean_old = 0.0f;
+float deviation_old = 0.0f;
+float mean_before_SP = 0.0f;
+float deviation_before_SP = 0.0f;
+float mean_before_SP_array[batch_size] = {0.0f};
+float deviation_before_SP_array[batch_size] = {0.0f};
+float mean_array[batch_size] = {0.0f};
+float mean_array_old[batch_size] = {0.0f};
+float deviation_array[batch_size] = {0.0f};
+float deviation_array_old[batch_size] = {0.0f};
+float action = 0.0f;
+float action_array[batch_size] = {0.0f};
+float ratio[batch_size] = {1.0f};
+float pi[batch_size] = {0.0f};
+float pi_old[batch_size] = {0.0f};
+float epsilon = 0.2f;
+float surr1[batch_size] = {0.0f};
+float surr2[batch_size] = {0.0f};
+float loss[batch_size] = {0.0f};
+float loss_batch = 0.0f;
+float gamma = 0.98f;
+float lmbda = 0.95f;
+char Update_Done_Flag = 1;
+char Update_Case = 0;
+float reward_sum = 0.0f;
 
-//const int num_input = 6;
-//const int num_input_array = num_input*2+1;
+float virt_pos = 0.0f;
+float logging1 = 0.0f;
+float logging2 = 0.0f;
+float logging3 = 0.0f;
+float logging4 = 0.0f;
+float logging5 = 0.0f;
+
 
 /*******************************************************************************
  * General math functions
@@ -481,6 +524,95 @@
 
 void ROM_CALL_DATA(void)
 {
+    BNO = spi_eeprom_read(RID_BNO);
+//    BNO = 11;
+    OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
+    SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE);
+//    SENSING_MODE = 1;
+    CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE);
+//    CURRENT_CONTROL_MODE = 1;
+    FLAG_VALVE_DEADZONE = spi_eeprom_read(RID_FLAG_VALVE_DEADZONE);
+    CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
+//    CAN_FREQ = 500;
+    DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
+    DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
+    DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
+    SUPPLY_VOLTAGE = (float) (spi_eeprom_read(RID_VOLATGE_SUPPLY)) *0.1f;
+    VALVE_VOLTAGE_LIMIT = (float) (spi_eeprom_read(RID_VOLTAGE_VALVE)) * 0.1f;
+    P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
+    I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
+    D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
+    P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
+    I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
+    D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
+    P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
+    I_GAIN_JOINT_TORQUE = spi_eeprom_read( RID_I_GAIN_JOINT_TORQUE);
+    D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
+    VALVE_DEADZONE_PLUS = (float) (spi_eeprom_read( RID_VALVE_DEADZONE_PLUS)) * 0.1f;
+    VALVE_DEADZONE_MINUS = (float) (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS)) * 0.1f;
+    VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
+    COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
+    VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
+    VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
+    BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
+    CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
+    CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
+    PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
+    PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
+    PISTON_AREA_alpha = (float)PISTON_AREA_A/(float)PISTON_AREA_B;
+    alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha;
+    PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
+    PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
+    ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
+    ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
+    STROKE = spi_eeprom_read(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 = (float) (spi_eeprom_read(RID_ENC_PULSE_PER_POSITION)) * 0.1f;
+//    ENC_PULSE_PER_POSITION = (float) 1024.0f;
+    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE)) * 0.0001f;
+    //TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle
+//    TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee
+    PRES_SENSOR_A_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
+//    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
+    PRES_SENSOR_B_PULSE_PER_BAR = (float) (spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
+//    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
+    FRICTION = (float) (spi_eeprom_read(RID_FRICTION)) * 0.1f;
+    HOMEPOS_OFFSET = spi_eeprom_read(RID_HOMEPOS_OFFSET);
+    HOMEPOS_VALVE_OPENING = spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
+    TORQUE_VREF = (float) (spi_eeprom_read(RID_TORQUE_SENSOR_VREF)) *0.001f;
+    PRES_A_VREF = (float) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) * 0.001f;
+    PRES_B_VREF = (float) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) * 0.001f;
+    VALVE_GAIN_LPM_PER_V[0] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_1)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[2] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_2)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[4] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_3)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[6] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_4)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[8] = (float) (spi_eeprom_read(RID_VALVE_GAIN_PLUS_5)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[1] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_1)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[3] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_2)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[5] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_3)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[7] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_4)) * 0.01f;
+    VALVE_GAIN_LPM_PER_V[9] = (float) (spi_eeprom_read(RID_VALVE_GAIN_MINUS_5)) * 0.01f;
+    for(int i=0; i<25; i++) {
+        VALVE_POS_VS_PWM[i] = (float) (spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i));
+    }
+    for(int i=0; i<100; i++) {
+        JOINT_VEL[i] = ( ((spi_eeprom_read( RID_VALVE_POS_VS_FLOWRATE_0 + i)) & 0xFFFF) | ((spi_eeprom_read(RID_VALVE_POS_VS_FLOWRATE_0_1 + i)) & 0xFFFF) << 16 ) ;
+    }
+    VALVE_MAX_POS = spi_eeprom_read(RID_VALVE_MAX_POS);
+    VALVE_MIN_POS = spi_eeprom_read(RID_VALVE_MIN_POS);
+    //DDV_CENTER = (float) (flashReadInt(Rom_Sector, RID_DDV_CENTER)) * 0.1f;
+    VALVE_POS_NUM = spi_eeprom_read(RID_VALVE_POS_NUM);
+    
+    K_SPRING = spi_eeprom_read(RID_K_SPRING);
+    D_DAMPER = spi_eeprom_read(RID_D_DAMPER);
+    
+    
+    
+    
+    
+    /*
+    
     BNO = flashReadInt(Rom_Sector, RID_BNO);
 //    BNO = 1;
     OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
@@ -565,6 +697,8 @@
     D_DAMPER = flashReadInt(Rom_Sector, RID_D_DAMPER);
     
 //    ROM_RESET_DATA();
+
+    */
     
 }