Revision 15:bd0d12728506, committed 2019-08-30

Comitter:

Lightvalve

Date:

Fri Aug 30 02:26:11 2019 +0000

Parent:

14:8e7590227d22

Child:

16:903b5a4433b4

Commit message:

190830

Changed in this revision

--- a/CAN/function_CAN.cpp	Thu Aug 29 07:38:00 2019 +0000
+++ b/CAN/function_CAN.cpp	Fri Aug 30 02:26:11 2019 +0000
@@ -119,9 +119,9 @@
             //pc.printf("can middle %d\n", request_type);
 
 //            if (flag_data_request[1] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
-            if (flag_data_request[1] == HIGH) dac_1 = PRES_A_VREF/3.3;
+            //if (flag_data_request[1] == HIGH) dac_1 = PRES_A_VREF/3.3;
 //            if (flag_data_request[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
-            if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3;
+            //if (flag_data_request[2] == HIGH) dac_2 = PRES_B_VREF/3.3;
 
             break;
         }

--- a/INIT_HW/INIT_HW.cpp	Thu Aug 29 07:38:00 2019 +0000
+++ b/INIT_HW/INIT_HW.cpp	Fri Aug 30 02:26:11 2019 +0000
@@ -70,7 +70,7 @@
     //PWM Setup
 
     TIM3->PSC = 0x0;                                            // no prescaler, timer counts up in sync with the peripheral clock
-    TIM3->ARR = TMR3_COUNT;                                          // set auto reload, 40 khz
+    TIM3->ARR = TMR3_COUNT;                                          // set auto reload, 5 khz
     TIM3->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
     TIM3->CR1 |= TIM_CR1_CEN;                                   // enable TIM4
 }
\ No newline at end of file

--- a/function_utilities/function_utilities.cpp	Thu Aug 29 07:38:00 2019 +0000
+++ b/function_utilities/function_utilities.cpp	Fri Aug 30 02:26:11 2019 +0000
@@ -320,80 +320,296 @@
  ******************************************************************************/
 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();
-    //}
+//    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_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);
+    
+    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;
 
-    spi_eeprom_write(RID_BNO, (int16_t) BNO);
-    spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE);
-    spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
-    spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
-    spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
-    spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
-    spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.));
-    spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.));
-    spi_eeprom_write(RID_P_GAIN_VALVE_POSITION, (int16_t) P_GAIN_VALVE_POSITION);
-    spi_eeprom_write(RID_I_GAIN_VALVE_POSITION, (int16_t) I_GAIN_VALVE_POSITION);
-    spi_eeprom_write(RID_D_GAIN_VALVE_POSITION, (int16_t) D_GAIN_VALVE_POSITION);
-    spi_eeprom_write(RID_P_GAIN_JOINT_POSITION, (int16_t) P_GAIN_JOINT_POSITION);
-    spi_eeprom_write(RID_I_GAIN_JOINT_POSITION, (int16_t) I_GAIN_JOINT_POSITION);
-    spi_eeprom_write(RID_D_GAIN_JOINT_POSITION, (int16_t) D_GAIN_JOINT_POSITION);
-    spi_eeprom_write(RID_P_GAIN_JOINT_TORQUE, (int16_t) P_GAIN_JOINT_TORQUE);
-    spi_eeprom_write(RID_I_GAIN_JOINT_TORQUE, (int16_t) I_GAIN_JOINT_TORQUE);
-    spi_eeprom_write(RID_D_GAIN_JOINT_TORQUE, (int16_t) D_GAIN_JOINT_TORQUE);
-    spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
-    spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
-    spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
-    spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
-    spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER);
-    spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
-    spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
-    spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
-    spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
-    spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
-    spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
-    spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
-    spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
-    spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
-    spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
-    spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
-    spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int16_t) VALVE_LIMIT_MINUS);
-    spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int16_t) VALVE_LIMIT_PLUS);
-    spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) ENC_PULSE_PER_POSITION);
-    spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) TORQUE_SENSOR_PULSE_PER_TORQUE);
+    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();
-    PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
-    PRES_SENSOR_A_PULSE_PER_BAR = 1.0;
-    //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0));
-    spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int) (6));
+    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();
-    PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
-    spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0));
-    spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.));
-    spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
-    spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
-    spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_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));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.0));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.0));
+    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;   
     
 }
 
@@ -453,7 +669,9 @@
 
     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;

--- 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);
         }
 
         /*******************************************************