HydraulicControlBoard_LIGHT_GJ - eeprom_test

Users » GiJeongKim » Code » HydraulicControlBoard_LIGHT_GJ
Kim GiJeong / Mbed 2 deprecated HydraulicControlBoard_LIGHT_GJ
eeprom_test
Diff: function_utilities/function_utilities.cpp

Revision:: 14:8e7590227d22
Parent:: 13:747daba9cf59
Child:: 15:bd0d12728506
--- a/function_utilities/function_utilities.cpp	Tue Aug 27 06:50:54 2019 +0000
+++ b/function_utilities/function_utilities.cpp	Thu Aug 29 07:38:00 2019 +0000
@@ -1,6 +1,7 @@
 #include "setting.h"
 #include "SPI_EEP_ENC.h"
 #include "function_utilities.h"
+#include "function_CAN.h"
 
 /*******************************************************************************
  * VARIABLE
@@ -13,7 +14,7 @@
 uint8_t SETTING_SWITCH = 0;
 uint8_t SETTING_SWITCH_OLD = 0;
 uint8_t REFERENCE_MODE = 0;
-uint16_t CAN_FREQ = 5000;
+uint16_t CAN_FREQ = 500;
 uint8_t DIR_JOINT_ENC = 0;
 uint8_t DIR_VALVE = 0;
 uint8_t DIR_VALVE_ENC = 0;
@@ -65,8 +66,8 @@
 
 int16_t ENC_PULSE_PER_POSITION;
 int16_t TORQUE_SENSOR_PULSE_PER_TORQUE;
-double PRES_SENSOR_A_PULSE_PER_BAR;
-double PRES_SENSOR_B_PULSE_PER_BAR;
+double PRES_SENSOR_A_PULSE_PER_BAR = 4096.0 / 200.0;
+double PRES_SENSOR_B_PULSE_PER_BAR = 4096.0 / 200.0;
 
 int HOMEPOS_OFFSET;
 int HOMEPOS_VALVE_OPENING;
@@ -87,6 +88,7 @@
 int16_t REF_PRES_DIFF;
 int16_t REF_PWM;
 int16_t REF_VALVE_POSITION;
+int16_t REF_CURRENT;
 
 int REF_MOVE_TIME_5k;
 int INIT_REF_PWM;
@@ -95,6 +97,7 @@
 int INIT_REF_VEL;
 int INIT_REF_TORQUE;
 int INIT_REF_PRES_DIFF;
+int INIT_REF_CURRENT;
 
 int CUR_POSITION;
 int CUR_VELOCITY;
@@ -145,21 +148,22 @@
 int DDV_CENTER;
 int VALVE_POS_NUM;
 
-int TMR2_COUNT_FINDHOME;
-int TMR2_COUNT_FLOWRATE;
-int TMR2_COUNT_DEADZONE;
-int TMR2_COUNT_PRES_NULL;
-int TMR2_COUNT_TORQUE_NULL;
-int TMR2_COUNT_PRES_CALIB;
-int TMR2_COUNT_REFERENCE;
-int TMR2_COUNT_JOINT;
-int TMR2_COUNT_ROTARY_FRIC_TUNE;
+int TMR3_COUNT_FINDHOME;
+int TMR3_COUNT_FLOWRATE;
+int TMR3_COUNT_DEADZONE;
+int TMR3_COUNT_PRES_NULL;
+int TMR3_COUNT_TORQUE_NULL;
+int TMR3_COUNT_PRES_CALIB;
+int TMR3_COUNT_REFERENCE;
+int TMR3_COUNT_JOINT;
+int TMR3_COUNT_ROTARY_FRIC_TUNE;
 
 bool FLAG_REFERENCE_VALVE_PWM;
 bool FLAG_REFERENCE_VALVE_POSITION;
 bool FLAG_REFERENCE_JOINT_POSITION;
 bool FLAG_REFERENCE_JOINT_TORQUE;
 bool FLAG_REFERENCE_PRES_DIFF;
+bool FLAG_REFERENCE_CURRENT;
 
 double TUNING_TIME;
 
@@ -188,11 +192,6 @@
 double PRES_B_NULL = 200.0;
 double TORQUE_NULL = 3900;
 
-double Ref_PWM = 0.0;
-double Ref_Valve_Pos = 0.0;
-double Ref_Joint_Pos = 0.0;
-double Ref_Joint_Vel = 0.0;
-double Ref_Joint_Torq = 0.0;
 double Ref_Valve_Pos_Old = 0.0;
 
 int VALVE_ID_timer = 0;
@@ -226,7 +225,7 @@
 char min_check = 0; 
 
 double valve_pos_err = 0.0, valve_pos_err_old = 0.0, valve_pos_err_diff = 0.0, valve_pos_err_sum = 0.0;
-double joint_pos_err = 0.0, joint_pos_err_old = 0.0, joint_pos_err_diff = 0.0, joint_pos_err_sum = 0.0;
+double joint_pos_err = 0.0, joint_pos_err_old = 0.0, joint_pos_err_diff = 0.0, joint_pos_err_diff_fil = 0.0, joint_pos_err_sum = 0.0;
 double joint_torq_err = 0.0, joint_torq_err_old = 0.0, joint_torq_err_diff = 0.0, joint_torq_err_sum = 0.0;
 double VALVE_PWM_RAW_POS = 0.0, VALVE_PWM_RAW_TORQ = 0.0;
 
@@ -272,6 +271,13 @@
 
 bool FLAG_VALVE_OUTPUT_CALIB = false;
 
+uint32_t TMR3_COUNT_CAN_TX = 0;
+
+double I_REF = 0.0;
+int TMR3_COUNT_IREF = 0;
+double CUR_CURRENT = 0.0;
+double u_CUR[3] = {0.0,0.0,0.0};
+
 /*******************************************************************************
  * General math functions
  ******************************************************************************/
@@ -312,14 +318,15 @@
 /*******************************************************************************
  * ROM functions
  ******************************************************************************/
-void ROM_INIT_DATA(void) {
+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) {
+    //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();
 }
 
@@ -362,24 +369,30 @@
     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);
-    spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.));
-    spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.));
+    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_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.));
-    spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.));
-    spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[0] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[2] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[4] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[6] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_PLUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[8] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_1, (int16_t) (VALVE_GAIN_LPM_PER_V[1] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_2, (int16_t) (VALVE_GAIN_LPM_PER_V[3] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_3, (int16_t) (VALVE_GAIN_LPM_PER_V[5] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_4, (int16_t) (VALVE_GAIN_LPM_PER_V[7] * 100.));
-    spi_eeprom_write(RID_VALVE_GAIN_MINUS_5, (int16_t) (VALVE_GAIN_LPM_PER_V[9] * 100.));
+    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));
     
     
 }
@@ -387,7 +400,7 @@
 void ROM_CALL_DATA(void) {
     int i = 0;
     BNO = spi_eeprom_read(RID_BNO);
-    BNO = 10;
+    BNO = 11;
     OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE);
     CAN_FREQ = spi_eeprom_read(RID_CAN_FREQ);
 
@@ -395,8 +408,8 @@
     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.;
-    VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.;
+    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);
@@ -443,25 +456,25 @@
     PRES_SENSOR_A_PULSE_PER_BAR = (double) spi_eeprom_read(RID_PRES_SENSOR_A_PULSE_PER_BAR) * 0.01;
     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.;
+    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.;
+    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.;
-    PRES_B_VREF = (double) spi_eeprom_read(RID_PRES_B_SENSOR_VREF) / 1000.;
+    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.;
-    VALVE_GAIN_LPM_PER_V[2] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_2) / 100.;
-    VALVE_GAIN_LPM_PER_V[4] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_3) / 100.;
-    VALVE_GAIN_LPM_PER_V[6] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_4) / 100.;
-    VALVE_GAIN_LPM_PER_V[8] = (double) spi_eeprom_read(RID_VALVE_GAIN_PLUS_5) / 100.;
-    VALVE_GAIN_LPM_PER_V[1] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_1) / 100.;
-    VALVE_GAIN_LPM_PER_V[3] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_2) / 100.;
-    VALVE_GAIN_LPM_PER_V[5] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_3) / 100.;
-    VALVE_GAIN_LPM_PER_V[7] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_4) / 100.;
-    VALVE_GAIN_LPM_PER_V[9] = (double) spi_eeprom_read(RID_VALVE_GAIN_MINUS_5) / 100.;
+    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++)
     {
@@ -520,9 +533,9 @@
     KF_X_21 = KF_G1_21 * KF_X_11 + KF_G1_22 * KF_X_21 + KF_G2_21 * KF_Y_11 + KF_G2_22*KF_Y_21;
     ENC_VEL_KF = (int32_t) KF_X_21;
 
-    CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
+    pos.sen = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
     //    CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur;
-    CUR_VELOCITY = (DIR_JOINT_ENC) * ENC_VEL_KF;
+    vel.sen = (DIR_JOINT_ENC) * ENC_VEL_KF;
 
     //    CUR_POSITION = ENC_pos_cur;
     //    CUR_VELOCITY = ENC_VEL_KF;
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	20 Jul 2020
Imports:	1
Forks:	0
Commits:	69
Dependents:	0
Dependencies:	2
Followers:	1
Diff: function_utilities/function_utilities.cpp

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
