HydraulicControlBoard_SW - HCB with MPC

Users » jsoh91 » Code » HydraulicControlBoard_SW
Jaesung Oh / Mbed 2 deprecated HydraulicControlBoard_SW
HCB with MPC
Diff: function_utilities/function_utilities.cpp

Revision:: 16:903b5a4433b4
Parent:: 15:bd0d12728506
Child:: 17:1865016ca2e7
--- a/function_utilities/function_utilities.cpp	Fri Aug 30 02:26:11 2019 +0000
+++ b/function_utilities/function_utilities.cpp	Mon Sep 02 13:32:33 2019 +0000
@@ -2,6 +2,12 @@
 #include "SPI_EEP_ENC.h"
 #include "function_utilities.h"
 #include "function_CAN.h"
+#include "stm32f4xx_flash.h"
+#include "FlashWriter.h"
+
+int Rom_Sector = 6;
+//FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
+
 
 /*******************************************************************************
  * VARIABLE
@@ -130,7 +136,9 @@
 double CUR_TORQUE_NM_PRESS;
 
 double PRES_A_VREF;
+double PRES_A_VREF_TEST;
 double PRES_B_VREF;
+double PRES_B_VREF_TEST;
 double TORQUE_VREF;
 
 double VALVE_PWM_RAW;
@@ -188,8 +196,8 @@
 double CUR_PRES_B_mean = 0.0;
 double CUR_TORQUE_sum = 0.0;
 double CUR_TORQUE_mean = 0.0;
-double PRES_A_NULL = 200.0;
-double PRES_B_NULL = 200.0;
+double PRES_A_NULL = 10.0;
+double PRES_B_NULL = 10.0;
 double TORQUE_NULL = 3900;
 
 double Ref_Valve_Pos_Old = 0.0;
@@ -320,395 +328,147 @@
  ******************************************************************************/
 void ROM_INIT_DATA(void)
 {
-//    static int16_t temp_check_num = LATEST_VERSION;
-//    spi_eeprom_ready();
-//    if (spi_eeprom_read(RID_IS_FIRST) != temp_check_num) {
-//        spi_eeprom_ready();
-//        spi_eeprom_write(RID_IS_FIRST, temp_check_num);
-//        ROM_RESET_DATA();
-//    }
+    //ROM_RESET_DATA();
     ROM_CALL_DATA();
-    ROM_RESET_DATA();
 }
 
 void ROM_RESET_DATA(void) {
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_BNO, (int) BNO);
-    spi_eeprom_ready();
-    BNO = spi_eeprom_read(RID_BNO);
-    BNO = 11;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_OPERATING_MODE, (int) OPERATING_MODE);
-    spi_eeprom_ready();
-    OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_CAN_FREQ, (int) CAN_FREQ);
-    spi_eeprom_ready();
-    CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_JOINT_ENC_DIR, (int) DIR_JOINT_ENC);
-    spi_eeprom_ready();
-    DIR_JOINT_ENC = spi_eeprom_read(RID_JOINT_ENC_DIR);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_DIR, (int) DIR_VALVE);
-    spi_eeprom_ready();
-    DIR_VALVE = spi_eeprom_read(RID_VALVE_DIR);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_ENC_DIR, (int) DIR_VALVE_ENC);
-    spi_eeprom_ready();
-    DIR_VALVE_ENC = spi_eeprom_read(RID_VALVE_ENC_DIR);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VOLATGE_SUPPLY, (int) (SUPPLY_VOLTAGE * 10.0));
-    spi_eeprom_ready();
-    SUPPLY_VOLTAGE = (double) spi_eeprom_read(RID_VOLATGE_SUPPLY) / 10.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VOLTAGE_VALVE, (int) (VALVE_VOLTAGE_LIMIT * 10.0));
-    spi_eeprom_ready();
-    VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int) P_GAIN_VALVE_POSITION);
-    spi_eeprom_ready();
-    P_GAIN_VALVE_POSITION = spi_eeprom_read(RID_P_GAIN_VALVE_POSITION);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int) I_GAIN_VALVE_POSITION);
-    spi_eeprom_ready();
-    I_GAIN_VALVE_POSITION = spi_eeprom_read(RID_I_GAIN_VALVE_POSITION);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int) D_GAIN_VALVE_POSITION);
-    spi_eeprom_ready();
-    D_GAIN_VALVE_POSITION = spi_eeprom_read(RID_D_GAIN_VALVE_POSITION);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int) P_GAIN_JOINT_POSITION);
-    spi_eeprom_ready();
-    P_GAIN_JOINT_POSITION = spi_eeprom_read(RID_P_GAIN_JOINT_POSITION);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int) I_GAIN_JOINT_POSITION);
-    spi_eeprom_ready();
-    I_GAIN_JOINT_POSITION = spi_eeprom_read(RID_I_GAIN_JOINT_POSITION);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int) D_GAIN_JOINT_POSITION);
-    spi_eeprom_ready();
-    D_GAIN_JOINT_POSITION = spi_eeprom_read(RID_D_GAIN_JOINT_POSITION);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int) P_GAIN_JOINT_TORQUE);
-    spi_eeprom_ready();
-    P_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_P_GAIN_JOINT_TORQUE);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int) I_GAIN_JOINT_TORQUE);
-    spi_eeprom_ready();
-    I_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_I_GAIN_JOINT_TORQUE);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int) D_GAIN_JOINT_TORQUE);
-    spi_eeprom_ready();
-    D_GAIN_JOINT_TORQUE = spi_eeprom_read(RID_D_GAIN_JOINT_TORQUE);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int) (VALVE_DEADZONE_PLUS));
-    spi_eeprom_ready();
-    VALVE_DEADZONE_PLUS = spi_eeprom_read(RID_VALVE_DEADZONE_PLUS);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int) (VALVE_DEADZONE_MINUS));
-    spi_eeprom_ready();
-    VALVE_DEADZONE_MINUS = spi_eeprom_read(RID_VALVE_DEADZONE_MINUS);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int) VELOCITY_COMP_GAIN);
-    spi_eeprom_ready();
-    VELOCITY_COMP_GAIN = spi_eeprom_read(RID_VELOCITY_COMP_GAIN);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_COMPLIANCE_GAIN, (int) COMPLIANCE_GAIN);
-    spi_eeprom_ready();
-    COMPLIANCE_GAIN = spi_eeprom_read(RID_COMPLIANCE_GAIN);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_CNETER, (int) VALVE_CENTER);
-    spi_eeprom_ready();
-    VALVE_CENTER = spi_eeprom_read(RID_VALVE_CNETER);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_FF, (int) VALVE_FF);
-    spi_eeprom_ready();
-    VALVE_FF = spi_eeprom_read(RID_VALVE_FF);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_BULK_MODULUS, (int) BULK_MODULUS);
-    spi_eeprom_ready();
-    BULK_MODULUS = spi_eeprom_read(RID_BULK_MODULUS);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int) CHAMBER_VOLUME_A);
-    spi_eeprom_ready();
-    CHAMBER_VOLUME_A = spi_eeprom_read(RID_CHAMBER_VOLUME_A);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int) CHAMBER_VOLUME_B);
-    spi_eeprom_ready();
-    CHAMBER_VOLUME_B = spi_eeprom_read(RID_CHAMBER_VOLUME_B);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_PISTON_AREA_A, (int) PISTON_AREA_A);
-    spi_eeprom_ready();
-    PISTON_AREA_A = spi_eeprom_read(RID_PISTON_AREA_A);
+    FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
+    if (!writer.ready()) writer.open();
     
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_PISTON_AREA_B, (int) PISTON_AREA_B);
-    spi_eeprom_ready();
-    PISTON_AREA_B = spi_eeprom_read(RID_PISTON_AREA_B);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_PRES_SUPPLY, (int) PRES_SUPPLY);
-    spi_eeprom_ready();
-    PRES_SUPPLY = spi_eeprom_read(RID_PRES_SUPPLY);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_PRES_RETURN, (int) PRES_RETURN);
-    spi_eeprom_ready();
-    PRES_RETURN = spi_eeprom_read(RID_PRES_RETURN);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int) ENC_LIMIT_MINUS);
-    spi_eeprom_ready();
-    ENC_LIMIT_MINUS = spi_eeprom_read(RID_ENC_LIMIT_MINUS);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int) ENC_LIMIT_PLUS);
-    spi_eeprom_ready();
-    ENC_LIMIT_PLUS = spi_eeprom_read(RID_ENC_LIMIT_PLUS);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_STROKE, (int) STROKE);
-    spi_eeprom_ready();
-    STROKE = spi_eeprom_read(RID_STROKE);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int) VALVE_LIMIT_MINUS);
-    spi_eeprom_ready();
-    VALVE_LIMIT_MINUS = spi_eeprom_read(RID_VALVE_LIMIT_MINUS);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int) VALVE_LIMIT_PLUS);
-    spi_eeprom_ready();
-    VALVE_LIMIT_PLUS = spi_eeprom_read(RID_VALVE_LIMIT_PLUS);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int) ENC_PULSE_PER_POSITION);
-    spi_eeprom_ready();
-    ENC_PULSE_PER_POSITION = spi_eeprom_read(RID_ENC_PULSE_PER_POSITION);
-    
-    TORQUE_SENSOR_PULSE_PER_TORQUE = 4096.0 / 200.0;
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int) TORQUE_SENSOR_PULSE_PER_TORQUE);
-    spi_eeprom_ready();
-    TORQUE_SENSOR_PULSE_PER_TORQUE = spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
-    
-    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
-    spi_eeprom_ready();
-    PRES_SENSOR_A_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR) * 0.01;
+    writer.write(RID_BNO,(int) BNO);           // write at address, 쓸때도 4byte씩 씀
+    writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE);
+    writer.write(RID_CAN_FREQ,(int) CAN_FREQ);
+    writer.write(RID_JOINT_ENC_DIR,(int) DIR_JOINT_ENC);
+    writer.write(RID_VALVE_DIR,(int) DIR_VALVE);
+    writer.write(RID_VALVE_ENC_DIR,(int) DIR_VALVE_ENC);
+    writer.write(RID_VOLATGE_SUPPLY,(int) (SUPPLY_VOLTAGE * 10.0));
+    writer.write(RID_VOLTAGE_VALVE,(int) (VALVE_VOLTAGE_LIMIT * 10.0));
+    writer.write(RID_P_GAIN_VALVE_POSITION,(int) P_GAIN_VALVE_POSITION);
+    writer.write(RID_I_GAIN_VALVE_POSITION,(int) I_GAIN_VALVE_POSITION);
+    writer.write(RID_D_GAIN_VALVE_POSITION,(int) D_GAIN_VALVE_POSITION);
+    writer.write(RID_P_GAIN_JOINT_POSITION,(int) P_GAIN_JOINT_POSITION);
+    writer.write(RID_I_GAIN_JOINT_POSITION,(int) I_GAIN_JOINT_POSITION);
+    writer.write(RID_D_GAIN_JOINT_POSITION,(int) D_GAIN_JOINT_POSITION);
+    writer.write(RID_P_GAIN_JOINT_TORQUE,(int) P_GAIN_JOINT_TORQUE);
+    writer.write(RID_I_GAIN_JOINT_TORQUE,(int) I_GAIN_JOINT_TORQUE);
+    writer.write(RID_D_GAIN_JOINT_TORQUE,(int) D_GAIN_JOINT_TORQUE);
+    writer.write(RID_VALVE_DEADZONE_PLUS,(int) VALVE_DEADZONE_PLUS);
+    writer.write(RID_VALVE_DEADZONE_MINUS,(int) VALVE_DEADZONE_MINUS);
+    writer.write(RID_VELOCITY_COMP_GAIN,(int) VELOCITY_COMP_GAIN);
+    writer.write(RID_COMPLIANCE_GAIN,(int) COMPLIANCE_GAIN);
+    writer.write(RID_VALVE_CNETER,(int) VALVE_CENTER);
+    writer.write(RID_VALVE_FF,(int) VALVE_FF);
+    writer.write(RID_BULK_MODULUS,(int) BNO);
+    writer.write(RID_CHAMBER_VOLUME_A,(int) CHAMBER_VOLUME_A);
+    writer.write(RID_CHAMBER_VOLUME_B,(int) CHAMBER_VOLUME_B);
+    writer.write(RID_PISTON_AREA_A,(int) PISTON_AREA_A);
+    writer.write(RID_PISTON_AREA_B,(int) PISTON_AREA_B);
+    writer.write(RID_PRES_SUPPLY,(int) PRES_SUPPLY);
+    writer.write(RID_PRES_RETURN,(int) PRES_RETURN);
+    writer.write(RID_ENC_LIMIT_MINUS,(int) ENC_LIMIT_MINUS);
+    writer.write(RID_ENC_LIMIT_PLUS,(int) ENC_LIMIT_PLUS);
+    writer.write(RID_STROKE,(int) STROKE);
+    writer.write(RID_VALVE_LIMIT_MINUS,(int) VALVE_LIMIT_MINUS);
+    writer.write(RID_VALVE_LIMIT_PLUS,(int) VALVE_LIMIT_PLUS);
+    writer.write(RID_ENC_PULSE_PER_POSITION,(int) ENC_PULSE_PER_POSITION);
+    writer.write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE,(int) TORQUE_SENSOR_PULSE_PER_TORQUE);
+    writer.write(RID_PRES_SENSOR_A_PULSE_PER_BAR,(int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
+    writer.write(RID_PRES_SENSOR_B_PULSE_PER_BAR,(int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
+    writer.write(RID_FRICTION,(int) (FRICTION * 10.0)); 
+    writer.write(RID_HOMEPOS_OFFSET,(int) HOMEPOS_OFFSET); 
+    writer.write(RID_HOMEPOS_VALVE_OPENING,(int) HOMEPOS_VALVE_OPENING); 
+    writer.write(RID_TORQUE_SENSOR_VREF,(int) (TORQUE_VREF * 1000.0)); 
+    writer.write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0));
+    writer.write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0));
+    writer.write(RID_VALVE_GAIN_PLUS_1,(int) (VALVE_GAIN_LPM_PER_V[0] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_PLUS_2,(int) (VALVE_GAIN_LPM_PER_V[2] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_PLUS_3,(int) (VALVE_GAIN_LPM_PER_V[4] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_PLUS_4,(int) (VALVE_GAIN_LPM_PER_V[6] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_PLUS_5,(int) (VALVE_GAIN_LPM_PER_V[8] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_MINUS_1,(int) (VALVE_GAIN_LPM_PER_V[1] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_MINUS_2,(int) (VALVE_GAIN_LPM_PER_V[3] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_MINUS_3,(int) (VALVE_GAIN_LPM_PER_V[5] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_MINUS_4,(int) (VALVE_GAIN_LPM_PER_V[7] * 100.0)); 
+    writer.write(RID_VALVE_GAIN_MINUS_5,(int) (VALVE_GAIN_LPM_PER_V[9] * 100.0)); 
 
-    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
-    spi_eeprom_ready();
-    PRES_SENSOR_B_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR) * 0.01;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_FRICTION, (int) (FRICTION * 10.0));
-    spi_eeprom_ready();
-    FRICTION = (double) spi_eeprom_read(RID_FRICTION) / 10.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_HOMEPOS_OFFSET, (int) HOMEPOS_OFFSET);
-    spi_eeprom_ready();
-    HOMEPOS_OFFSET = (int) spi_eeprom_read(RID_HOMEPOS_OFFSET);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int) HOMEPOS_VALVE_OPENING);
-    spi_eeprom_ready();
-    HOMEPOS_VALVE_OPENING = (int) spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int) (TORQUE_VREF * 1000.0));
-    spi_eeprom_ready();
-    TORQUE_VREF = (double) spi_eeprom_read(RID_TORQUE_SENSOR_VREF) / 1000.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int) (PRES_A_VREF * 1000.0));
-    spi_eeprom_ready();
-    PRES_A_VREF = (double) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) / 1000.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int) (PRES_B_VREF * 1000.0));
-    spi_eeprom_ready();
-    PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int) (VALVE_GAIN_LPM_PER_V[0] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[0] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_1) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int) (VALVE_GAIN_LPM_PER_V[2] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[2] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_2) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int) (VALVE_GAIN_LPM_PER_V[4] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[4] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_3) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int) (VALVE_GAIN_LPM_PER_V[6] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[6] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_4) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int) (VALVE_GAIN_LPM_PER_V[8] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[8] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_5) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int) (VALVE_GAIN_LPM_PER_V[1] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[1] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_1) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int) (VALVE_GAIN_LPM_PER_V[3] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[3] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_2) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int) (VALVE_GAIN_LPM_PER_V[5] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[5] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_3) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int) (VALVE_GAIN_LPM_PER_V[7] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[7] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_4) / 100.0;
-    
-    spi_eeprom_ready();
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int) (VALVE_GAIN_LPM_PER_V[9] * 100.0));
-    spi_eeprom_ready();
-    VALVE_GAIN_LPM_PER_V[9] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_5) / 100.0;   
+    writer.close();    
     
 }
 
 void ROM_CALL_DATA(void) {
-    int i = 0;
-    BNO = spi_eeprom_read(RID_BNO);
+    BNO = flashReadInt(Rom_Sector, RID_BNO);
     BNO = 11;
-    OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
-    CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
-
-    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 = (double) spi_eeprom_read(RID_VOLATGE_SUPPLY) / 10.0;
-    VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.0;
-
-    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 = spi_eeprom_read(RID_VALVE_DEADZONE_PLUS);
-    VALVE_DEADZONE_MINUS = spi_eeprom_read(RID_VALVE_DEADZONE_MINUS);
-
-    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);
+    OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
+    CAN_FREQ = flashReadInt(Rom_Sector, RID_CAN_FREQ);
+    DIR_JOINT_ENC = flashReadInt(Rom_Sector, RID_JOINT_ENC_DIR);
+    DIR_VALVE = flashReadInt(Rom_Sector, RID_VALVE_DIR);
+    DIR_VALVE_ENC = flashReadInt(Rom_Sector, RID_VALVE_ENC_DIR);
+    SUPPLY_VOLTAGE = (double) (flashReadInt(Rom_Sector, RID_VOLATGE_SUPPLY)) *0.1;
+    VALVE_VOLTAGE_LIMIT = (double) (flashReadInt(Rom_Sector, RID_VOLTAGE_VALVE)) * 0.1;
+    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 = flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_PLUS);
+    VALVE_DEADZONE_MINUS = flashReadInt(Rom_Sector, RID_VALVE_DEADZONE_MINUS);
+    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 = (double)PISTON_AREA_B/(double)PISTON_AREA_A;
-
-    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);
+    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 = (double) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01;
+    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
+    PRES_SENSOR_B_PULSE_PER_BAR = (double) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01;
+    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
+    FRICTION = (double) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1;
+    HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET);
+    HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
+    TORQUE_VREF = (double) (flashReadInt(Rom_Sector, RID_TORQUE_SENSOR_VREF)) *0.001;
+    PRES_A_VREF = (double) flashReadInt(6, RID_PRES_A_SENSOR_VREF) * 0.001;
+    PRES_B_VREF = (double) flashReadInt(6, RID_PRES_B_SENSOR_VREF) * 0.001;
+    VALVE_GAIN_LPM_PER_V[0] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_1)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[2] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_2)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[4] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_3)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[6] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_4)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[8] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_PLUS_5)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[1] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_1)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[3] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_2)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[5] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_3)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[7] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_4)) * 0.01;
+    VALVE_GAIN_LPM_PER_V[9] = (double) (flashReadInt(Rom_Sector, RID_VALVE_GAIN_MINUS_5)) * 0.01;
+    for(int i=0; i<18; i++)
+    {
+        VALVE_POS_VS_PWM[i] = (double) (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 = flashReadInt(Rom_Sector, RID_DDV_CNETER);
+    VALVE_POS_NUM = flashReadInt(Rom_Sector, RID_VALVE_POS_NUM);
 
-    STROKE = spi_eeprom_read(RID_STROKE);
-
-    VALVE_LIMIT_MINUS = spi_eeprom_read(RID_VALVE_LIMIT_MINUS);
-    VALVE_LIMIT_PLUS = spi_eeprom_read(RID_VALVE_LIMIT_PLUS);
-
-    ENC_PULSE_PER_POSITION = spi_eeprom_read(RID_ENC_PULSE_PER_POSITION);
-    TORQUE_SENSOR_PULSE_PER_TORQUE = spi_eeprom_read(RID_TORQUE_SENSOR_PULSE_PER_TORQUE);
-    spi_eeprom_ready();
-    PRES_SENSOR_A_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR) * 0.01;
-    spi_eeprom_ready();
-    PRES_SENSOR_B_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_B_PULSE_PER_BAR) * 0.01;
-
-    FRICTION = (double) spi_eeprom_read(RID_FRICTION) / 10.0;
-    HOMEPOS_OFFSET = (int) spi_eeprom_read(RID_HOMEPOS_OFFSET);
-    HOMEPOS_VALVE_OPENING = (int) spi_eeprom_read(RID_HOMEPOS_VALVE_OPENING);
-
-    TORQUE_VREF = (double) spi_eeprom_read(RID_TORQUE_SENSOR_VREF) / 1000.0;
-
-    PRES_A_VREF = (double) spi_eeprom_read(RID_PRES_A_SENSOR_VREF) / 1000.0;
-    PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.0;
-
-    VALVE_GAIN_LPM_PER_V[0] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_1) / 100.0;
-    VALVE_GAIN_LPM_PER_V[2] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_2) / 100.0;
-    VALVE_GAIN_LPM_PER_V[4] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_3) / 100.0;
-    VALVE_GAIN_LPM_PER_V[6] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_4) / 100.0;
-    VALVE_GAIN_LPM_PER_V[8] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_5) / 100.0;
-    VALVE_GAIN_LPM_PER_V[1] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_1) / 100.0;
-    VALVE_GAIN_LPM_PER_V[3] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_2) / 100.0;
-    VALVE_GAIN_LPM_PER_V[5] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_3) / 100.0;
-    VALVE_GAIN_LPM_PER_V[7] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_4) / 100.0;
-    VALVE_GAIN_LPM_PER_V[9] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_5) / 100.0;
-    
-    for(i=0; i<18; i++)
-    {
-        VALVE_POS_VS_PWM[i] = (double) spi_eeprom_read(RID_VALVE_POS_VS_PWM_0 + i);
-    }
-    for(i=0; i<100; i++)
-    {
-        JOINT_VEL[i] = (int32_t) ( (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 = (int) spi_eeprom_read(RID_VALVE_MAX_POS);
-    VALVE_MIN_POS = (int) spi_eeprom_read(RID_VALVE_MIN_POS);
-    DDV_CENTER = (int) spi_eeprom_read(RID_DDV_CNETER);
-    
-    VALVE_POS_NUM = (int) spi_eeprom_read(RID_VALVE_POS_NUM);
-    
 }
 
 /*******************************************************************************
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	14 Oct 2019
Imports:	1
Forks:	0
Commits:	26
Dependents:	0
Dependencies:	3
Followers:	1
Diff: function_utilities/function_utilities.cpp

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
