Sungwoo Kim
asdgas
Dependencies: mbed Eigen FastPWM
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;