Yeseong Jeong
/
HydraulicControlBoard_Start
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 11:82d8768d7351, committed 2019-08-23
- Comitter:
- Lightvalve
- Date:
- Fri Aug 23 12:28:08 2019 +0000
- Parent:
- 10:83a6baa77a2e
- Child:
- 12:6f2531038ea4
- Commit message:
- 190823
Changed in this revision
--- a/CAN/function_CAN.cpp Fri Aug 23 01:02:32 2019 +0000
+++ b/CAN/function_CAN.cpp Fri Aug 23 12:28:08 2019 +0000
@@ -1,4 +1,7 @@
#include "function_CAN.h"
+#include "setting.h"
+#include "function_utilities.h"
+#include "SPI_EEP_ENC.h"
// CAN ID Setting Variables
int CID_RX_CMD = 100;
@@ -15,26 +18,570 @@
int CID_TX_VOUT = 1500;
int CID_TX_VALVE_POSITION = 1600;
-// Board Information
-int BNO = 0;
-int CONTROL_MODE = 0;
-double P_GAIN_JOINT_POSITION = 0.0;
-double I_GAIN_JOINT_POSITION = 0.0;
-double D_GAIN_JOINT_POSITION = 0.0;
-double P_GAIN_JOINT_TORQUE = 0.0;
-double I_GAIN_JOINT_TORQUE = 0.0;
-double D_GAIN_JOINT_TORQUE = 0.0;
+
+/*******************************************************************************
+ * CAN functions
+ ******************************************************************************/
+void CAN_ID_INIT(void) {
+ CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD);
+ CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION);
+ CID_RX_REF_TORQUE = (int) (BNO + INIT_CID_RX_REF_TORQUE);
+ CID_RX_REF_PRES_DIFF = (int) (BNO + INIT_CID_RX_REF_PRES_DIFF);
+ CID_RX_REF_VOUT = (int) (BNO + INIT_CID_RX_REF_VOUT);
+ CID_RX_REF_VALVE_POSITION = (int) (BNO + INIT_CID_RX_REF_VALVE_POSITION);
+
+ CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO);
+ CID_TX_POSITION = (int) (BNO + INIT_CID_TX_POSITION);
+ CID_TX_TORQUE = (int) (BNO + INIT_CID_TX_TORQUE);
+ CID_TX_PRES = (int) (BNO + INIT_CID_TX_PRES);
+ CID_TX_VOUT = (int) (BNO + INIT_CID_TX_VOUT);
+ CID_TX_VALVE_POSITION = (int) (BNO + INIT_CID_TX_VALVE_POSITION);
+}
void ReadCMD(char CMD)
{
switch(CMD){
- case CRX_ASK_INFO:
-// CAN_TX_INFO();
- break;
- case CRX_ASK_BNO:
-// CAN_TX_BNO();
- break;
+ case CRX_ASK_INFO: {
+ CAN_TX_INFO();
+ break;
+ }
+ case CRX_ASK_BNO: {
+ CAN_TX_BNO();
+ break;
+ }
+ case CRX_SET_BNO: {
+ BNO = (int16_t) msg.data[1];
+ spi_eeprom_write(RID_BNO, (int16_t) BNO);
+ CAN_ID_INIT(); // can id init
+ break;
+ }
+ case CRX_ASK_OPERATING_MODE: {
+ CAN_TX_OPERATING_MODE();
+ break;
+ }
+ case CRX_SET_OPERATING_MODE: {
+ OPERATING_MODE = (int16_t) msg.data[1];
+ spi_eeprom_write(RID_OPERATING_MODE, (int16_t) OPERATING_MODE);
+ break;
+ }
+ case CRX_SET_ENC_ZERO: {
+ ENC_SET_ZERO();
+
+ break;
+ }
+ case CRX_SET_FET_ON: {
+ LAT_MOT_EN = (int16_t) msg.data[1];;
+
+ break;
+ }
+
+ case CRX_SET_REF_UPDATE_ENABLE: {
+ flag_ref_enable = (int16_t) msg.data[1];
+
+ break;
+ }
+
+ case CRX_ASK_CAN_FREQ: {
+ CAN_TX_CAN_FREQ();
+
+ break;
+ }
+
+ case CRX_SET_CAN_FREQ: {
+ CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ);
+ break;
+ }
+
+ case CRX_ASK_CONTROL_MODE: {
+ CAN_TX_CONTROL_MODE();
+
+ break;
+ }
+
+ case CRX_SET_CONTROL_MODE: {
+ CONTROL_MODE = (int16_t) (msg.data[1]);
+ if (CONTROL_MODE == MODE_FIND_HOME) {
+ FLAG_FIND_HOME = true;
+ }
+ break;
+ }
+
+ case CRX_SET_DATA_REQUEST: {
+ int request_type = msg.data[2];
+ flag_data_request[request_type] = msg.data[1];
+
+ 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[2] == HIGH) SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); // set DAC
+
+ break;
+ }
+
+ case CRX_ASK_JOINT_ENC_DIR: {
+ CAN_TX_JOINT_ENC_DIR();
+
+ break;
+ }
+
+ case CRX_SET_JOINT_ENC_DIR: {
+ DIR_JOINT_ENC = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ if (DIR_JOINT_ENC >= 0)
+ DIR_JOINT_ENC = 1;
+ else
+ DIR_JOINT_ENC = -1;
+ spi_eeprom_write(RID_JOINT_ENC_DIR, (int16_t) DIR_JOINT_ENC);
+
+ break;
+ }
+
+ case CRX_ASK_VALVE_DIR: {
+ CAN_TX_VALVE_DIR();
+
+ break;
+ }
+
+ case CRX_SET_VALVE_DIR: {
+ DIR_VALVE = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ if (DIR_VALVE >= 0)
+ DIR_VALVE = 1;
+ else
+ DIR_VALVE = -1;
+ spi_eeprom_write(RID_VALVE_DIR, (int16_t) DIR_VALVE);
+
+ break;
+ }
+
+ case CRX_ASK_VALVE_ENC_DIR: {
+ CAN_TX_VALVE_ENC_DIR();
+
+ break;
+ }
+
+ case CRX_SET_VALVE_ENC_DIR: {
+ DIR_VALVE_ENC = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ if (DIR_VALVE_ENC >= 0)
+ DIR_VALVE_ENC = 1;
+ else
+ DIR_VALVE_ENC = -1;
+
+ spi_eeprom_write(RID_VALVE_ENC_DIR, (int16_t) DIR_VALVE_ENC);
+
+ break;
+ }
+
+ case CRX_ASK_VOLTAGE_SUPPLY: {
+ CAN_TX_VOLTAGE_SUPPLY();
+
+ break;
+ }
+
+ case CRX_SET_VOLTAGE_SUPPLY: {
+ SUPPLY_VOLTAGE = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.;
+ spi_eeprom_write(RID_VOLATGE_SUPPLY, (int16_t) (SUPPLY_VOLTAGE * 10.));
+
+ break;
+ }
+
+ case CRX_ASK_VOLTAGE_VALVE: {
+ CAN_TX_VOLTAGE_VALVE();
+
+ break;
+ }
+
+ case CRX_SET_VOLTAGE_VALVE: {
+ VALVE_VOLTAGE_LIMIT = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.;
+ spi_eeprom_write(RID_VOLTAGE_VALVE, (int16_t) (VALVE_VOLTAGE_LIMIT * 10.));
+
+
+ break;
+ }
+ ///////////////////////////////////////////////////////////////////////////////////////////////////////////
+ case CRX_SET_HOMEPOS: {
+ CONTROL_MODE = 22;
+ SETTING_SWITCH = (int16_t) (msg.data[1]);
+ break;
+ }
+
+ case CRX_ASK_PID_GAIN: {
+ CAN_TX_PID_GAIN(msg.data[1]);
+
+ break;
+ }
+
+ case CRX_SET_PID_GAIN: {
+ if (msg.data[1] == 0) {
+ P_GAIN_VALVE_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
+ I_GAIN_VALVE_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
+ D_GAIN_VALVE_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
+
+ 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);
+
+
+ } else if (msg.data[1] == 1) {
+ P_GAIN_JOINT_POSITION = (int16_t) (msg.data[2] | msg.data[3] << 8);
+ I_GAIN_JOINT_POSITION = (int16_t) (msg.data[4] | msg.data[5] << 8);
+ D_GAIN_JOINT_POSITION = (int16_t) (msg.data[6] | msg.data[7] << 8);
+
+ 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);
+ } else if (msg.data[1] == 2) {
+ P_GAIN_JOINT_TORQUE = (int16_t) (msg.data[2] | msg.data[3] << 8);
+ I_GAIN_JOINT_TORQUE = (int16_t) (msg.data[4] | msg.data[5] << 8);
+ D_GAIN_JOINT_TORQUE = (int16_t) (msg.data[6] | msg.data[7] << 8);
+
+ 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);
+
+
+ }
+
+ break;
+ }
+
+ case CRX_ASK_VALVE_DEADZONE: {
+ CAN_TX_VALVE_DEADZONE();
+
+ break;
+ }
+
+ case CRX_SET_VALVE_DEADZONE: {
+ VALVE_CENTER = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ VALVE_DEADZONE_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
+ VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8);
+
+ spi_eeprom_write(RID_VALVE_CNETER, (int16_t) (VALVE_CENTER));
+ spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS));
+ spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS));
+
+ break;
+ }
+
+ case CRX_ASK_VELOCITY_COMP_GAIN: {
+ CAN_TX_VELOCITY_COMP_GAIN();
+
+ break;
+ }
+
+ case CRX_SET_VELOCITY_COMP_GAIN: {
+ VELOCITY_COMP_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_VELOCITY_COMP_GAIN, (int16_t) VELOCITY_COMP_GAIN);
+
+ break;
+ }
+
+ case CRX_ASK_COMPLIANCE_GAIN: {
+ CAN_TX_COMPLIANCE_GAIN();
+
+ break;
+ }
+
+ case CRX_SET_COMPLIANCE_GAIN: {
+ COMPLIANCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_COMPLIANCE_GAIN, (int16_t) COMPLIANCE_GAIN);
+
+ break;
+ }
+
+ case CRX_ASK_VALVE_FF: {
+ CAN_TX_VALVE_FF();
+
+ break;
+ }
+
+ case CRX_SET_VALVE_FF: {
+ VALVE_FF = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_VALVE_FF, (int16_t) VALVE_FF);
+
+ break;
+ }
+
+ case CRX_ASK_BULK_MODULUS: {
+ CAN_TX_BULK_MODULUS();
+
+ break;
+ }
+
+ case CRX_SET_BULK_MODULUS: {
+ BULK_MODULUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_BULK_MODULUS, (int16_t) BULK_MODULUS);
+
+ break;
+ }
+
+ case CRX_ASK_CHAMBER_VOLUME: {
+ CAN_TX_CHAMBER_VOLUME();
+
+ break;
+ }
+
+ case CRX_SET_CHAMBER_VOLUME: {
+ CHAMBER_VOLUME_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ CHAMBER_VOLUME_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
+
+ spi_eeprom_write(RID_CHAMBER_VOLUME_A, (int16_t) CHAMBER_VOLUME_A);
+ spi_eeprom_write(RID_CHAMBER_VOLUME_B, (int16_t) CHAMBER_VOLUME_B);
+
+ break;
+ }
+
+ case CRX_ASK_PISTON_AREA: {
+ CAN_TX_PISTON_AREA();
+
+ break;
+ }
+
+ case CRX_SET_PISTON_AREA: {
+ PISTON_AREA_A = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ PISTON_AREA_B = (int16_t) (msg.data[3] | msg.data[4] << 8);
+ PISTON_AREA_alpha = (double)PISTON_AREA_B/(double)PISTON_AREA_A;
+
+ spi_eeprom_write(RID_PISTON_AREA_A, (int16_t) PISTON_AREA_A);
+ spi_eeprom_write(RID_PISTON_AREA_B, (int16_t) PISTON_AREA_B);
+ break;
+ }
+
+ case CRX_ASK_PRES: {
+ CAN_TX_PRES_A_AND_B();
+ SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0);
+ break;
+ }
+
+ case CRX_SET_PRES: {
+ PRES_SUPPLY = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ PRES_RETURN = (int16_t) (msg.data[3] | msg.data[4] << 8);
+
+ spi_eeprom_write(RID_PRES_SUPPLY, (int16_t) PRES_SUPPLY);
+ spi_eeprom_write(RID_PRES_RETURN, (int16_t) PRES_RETURN);
+
+
+ break;
+ }
+
+ case CRX_ASK_ENC_LIMIT: {
+ CAN_TX_ENC_LIMIT();
+
+ break;
+ }
+
+ case CRX_SET_ENC_LIMIT: {
+ ENC_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ ENC_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
+
+ spi_eeprom_write(RID_ENC_LIMIT_MINUS, (int16_t) ENC_LIMIT_MINUS);
+ spi_eeprom_write(RID_ENC_LIMIT_PLUS, (int16_t) ENC_LIMIT_PLUS);
+
+ break;
+ }
+
+ case CRX_ASK_STROKE: {
+ CAN_TX_STROKE();
+ break;
+ }
+
+ case CRX_SET_STROKE: {
+ STROKE = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_STROKE, (int16_t) STROKE);
+
+ break;
+ }
+
+ case CRX_ASK_VALVE_LIMIT: {
+ CAN_TX_VALVE_LIMIT();
+
+ break;
+ }
+
+ case CRX_SET_VALVE_LIMIT: {
+ VALVE_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8);
+ VALVE_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8);
+
+ spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int16_t) VALVE_LIMIT_MINUS);
+ spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int16_t) VALVE_LIMIT_PLUS);
+
+ break;
+ }
+
+ case CRX_ASK_ENC_PULSE_PER_POSITION: {
+ CAN_TX_ENC_PULSE_PER_POSITION();
+
+ break;
+ }
+
+ case CRX_SET_ENC_PULSE_PER_POSITION: {
+ ENC_PULSE_PER_POSITION = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_ENC_PULSE_PER_POSITION, (int16_t) ENC_PULSE_PER_POSITION);
+
+ break;
+ }
+
+ case CRX_ASK_TORQUE_SENSOR_PULSE_PER_TORQUE: {
+ CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE();
+
+ break;
+ }
+
+ case CRX_SET_TORQUE_SENSOR_PULSE_PER_TORQUE: {
+ TORQUE_SENSOR_PULSE_PER_TORQUE = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_TORQUE_SENSOR_PULSE_PER_TORQUE, (int16_t) TORQUE_SENSOR_PULSE_PER_TORQUE);
+
+ break;
+ }
+
+ case CRX_ASK_PRES_SENSOR_PULSE_PER_PRES: {
+ CAN_TX_PRES_SENSOR_PULSE_PER_PRES();
+
+ break;
+ }
+
+ case CRX_SET_PRES_SENSOR_PULSE_PER_PRES: {
+ PRES_SENSOR_A_PULSE_PER_BAR = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) * 0.01;
+ PRES_SENSOR_B_PULSE_PER_BAR = (double) ((int16_t) (msg.data[3] | msg.data[4] << 8)) * 0.01;
+
+ 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.));
+
+ break;
+ }
+
+ case CRX_ASK_FRICTION: {
+ CAN_TX_FRICTION();
+
+ break;
+ }
+
+ case CRX_SET_FRICTION: {
+ FRICTION = (double) ((int16_t) (msg.data[1] | msg.data[2] << 8)) / 10.;
+
+ spi_eeprom_write(RID_FRICTION, (int16_t) (FRICTION * 10.));
+
+ break;
+ }
+
+ case CRX_ASK_VALVE_GAIN_PLUS: {
+ CAN_TX_VALVE_GAIN_PLUS();
+
+ break;
+ }
+ case CRX_SET_VALVE_GAIN_PLUS: {
+ VALVE_GAIN_LPM_PER_V[0] = (double) msg.data[1] / 50.;
+ VALVE_GAIN_LPM_PER_V[2] = (double) msg.data[2] / 50.;
+ VALVE_GAIN_LPM_PER_V[4] = (double) msg.data[3] / 50.;
+ VALVE_GAIN_LPM_PER_V[6] = (double) msg.data[4] / 50.;
+ VALVE_GAIN_LPM_PER_V[8] = (double) msg.data[5] / 50.;
+
+ 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.));
+
+ break;
+ }
+
+ case CRX_ASK_VALVE_GAIN_MINUS: {
+ CAN_TX_VALVE_GAIN_MINUS();
+
+ break;
+ }
+ case CRX_SET_VALVE_GAIN_MINUS: {
+ VALVE_GAIN_LPM_PER_V[1] = (double) msg.data[1] / 50.;
+ VALVE_GAIN_LPM_PER_V[3] = (double) msg.data[2] / 50.;
+ VALVE_GAIN_LPM_PER_V[5] = (double) msg.data[3] / 50.;
+ VALVE_GAIN_LPM_PER_V[7] = (double) msg.data[4] / 50.;
+ VALVE_GAIN_LPM_PER_V[9] = (double) msg.data[5] / 50.;
+
+ 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.));
+
+ break;
+ }
+ case CRX_ASK_DDV_VALVE_DEADZONE: {
+ CAN_TX_DDV_VALVE_DEADZONE();
+ break;
+ }
+
+ case CRX_LOW_REF: {
+ REFERENCE_MODE = msg.data[1];
+
+ REF_NUM = msg.data[2];
+ REF_PERIOD = (double) ((int16_t) (msg.data[3] | msg.data[4] << 8)) / 100.;
+ if (REF_PERIOD <= 0.) REF_MOVE_TIME_5k = TMR_FREQ_5k / CAN_FREQ;
+ else REF_MOVE_TIME_5k = (int) (REF_PERIOD * (double) TMR_FREQ_5k);
+ REF_MAG = (double) ((int16_t) (msg.data[5] | msg.data[6] << 8)) / 100.;
+
+ break;
+ }
+
+ case CRX_JUMP_STATUS: {
+ MODE_JUMP_STATUS = msg.data[1];
+
+ break;
+ }
+
+ case CRX_SET_ERR_CLEAR: {
+
+ for (i = 0; i < num_err; i++) {
+ flag_err[i] = FALSE;
+ flag_err_old[i] = FALSE;
+ }
+
+ flag_err_rt = FALSE;
+
+
+ break;
+ }
+
+ case CRX_ASK_HOMEPOS_OFFSET: {
+ CAN_TX_HOMEPOS_OFFSET();
+ break;
+ }
+ case CRX_SET_HOMEPOS_OFFSET: {
+ HOMEPOS_OFFSET = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET);
+ break;
+ }
+
+ case CRX_ASK_HOMEPOS_VALVE_OPENING: {
+ CAN_TX_HOMPOS_VALVE_OPENING();
+ break;
+ }
+ case CRX_SET_HOMEPOS_VALVE_OPENING: {
+ HOMEPOS_VALVE_OPENING = (int16_t) (msg.data[1] | msg.data[2] << 8);
+
+ spi_eeprom_write(RID_HOMEPOS_VALVE_OPENING, (int16_t) HOMEPOS_VALVE_OPENING);
+ break;
+ }
+
+ case CRX_ASK_VALVE_PWM_VS_VALVE_POS: {
+ can_index = (int16_t) msg.data[1];
+ CAN_TX_VALVE_PWM_VS_VALVE_POS(can_index);
+ break;
+ }
+ case CRX_ASK_VALVE_POS_VS_FLOWRATE: {
+ can_index = (int16_t) msg.data[1];
+ CAN_TX_VALVE_POS_VS_FLOWRATE(can_index);
+ break;
+ }
+ case CRX_ASK_VALVE_POS_NUM: {
+ CAN_TX_VALVE_POS_NUM();
+ break;
+ }
default:
break;
}
@@ -57,8 +604,8 @@
int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8);
torq.ref = (double)temp_torq;
} else if(address==CID_RX_REF_PRES_DIFF) {
-// int16_t temp_presdiff = (int16_t) (msg.data[0] | msg.data[1] << 8);
-// torq.ref = (double)temp_presdiff;
+ int16_t temp_presdiff = (int16_t) (msg.data[0] | msg.data[1] << 8);
+ torq.ref = (double)temp_presdiff;
} else if(address==CID_RX_REF_VOUT) {
int16_t temp_PWM = (int16_t) (msg.data[0] | msg.data[1] << 8);
Vout.ref = (double)temp_PWM;
@@ -72,9 +619,9 @@
/******************************************************************************
Information Transmission Functions
*******************************************************************************/
-/*
-inline void CAN_TX_INFO(void) {
- CANMessage temp_msg;
+
+void CAN_TX_INFO(void) {
+ CANmsg.data temp_msg;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 7;
@@ -89,8 +636,8 @@
can.write(temp_msg);
}
-inline void CAN_TX_BNO(void) {
- CANMessage temp_msg;
+void CAN_TX_BNO(void) {
+ CANmsg.data temp_msg;
temp_msg.id = CID_TX_INFO;
temp_msg.len = 2;
@@ -100,7 +647,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_OPERATING_MODE(void) {
+void CAN_TX_OPERATING_MODE(void) {
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -111,8 +658,7 @@
can.write(temp_msg);
}
-
-inline void CAN_TX_CAN_FREQ(void) {
+void CAN_TX_CAN_FREQ(void) {
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -123,8 +669,8 @@
can.write(temp_msg);
}
-*/
-inline void CAN_TX_CONTROL_MODE(void) {
+
+void CAN_TX_CONTROL_MODE(void) {
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -134,8 +680,8 @@
can.write(temp_msg);
}
-/*
-inline void CAN_TX_JOINT_ENC_DIR(void) {
+
+void CAN_TX_JOINT_ENC_DIR(void) {
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -147,7 +693,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_VALVE_DIR(void) {
+void CAN_TX_VALVE_DIR(void) {
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -159,7 +705,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_VALVE_ENC_DIR(void) {
+void CAN_TX_VALVE_ENC_DIR(void) {
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -171,7 +717,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_VOLTAGE_SUPPLY(void) {
+void CAN_TX_VOLTAGE_SUPPLY(void) {
long send_voltage_supply = (long) (SUPPLY_VOLTAGE * 10);
CANMessage temp_msg;
@@ -185,7 +731,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_VOLTAGE_VALVE(void) {
+void CAN_TX_VOLTAGE_VALVE(void) {
long send_voltage_valve = (long) (VALVE_VOLTAGE_LIMIT * 10);
CANMessage temp_msg;
@@ -198,9 +744,9 @@
can.write(temp_msg);
}
-*/
+
-inline void CAN_TX_PID_GAIN(int t_type) {
+void CAN_TX_PID_GAIN(int t_type) {
// t_type = 0 : valve position control gain
// t_type = 1 : joint position control gain
// t_type = 2 : joint torque control gain
@@ -236,8 +782,8 @@
can.write(temp_msg);
}
-/*
-inline void CAN_TX_VALVE_DEADZONE(void) {
+
+void CAN_TX_VALVE_DEADZONE(void) {
CANMessage temp_msg;
temp_msg.id = CID_TX_INFO;
@@ -253,9 +799,139 @@
can.write(temp_msg);
}
-inline void CAN_TX_ENC_PULSE_PER_POSITION(void) {
+void CAN_TX_VELOCITY_COMP_GAIN(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 3;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_VELOCITY_COMP_GAIN;
+ temp_msg.data[1] = (unsigned char) VELOCITY_COMP_GAIN;
+ temp_msg.data[2] = (unsigned char) (VELOCITY_COMP_GAIN >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_COMPLIANCE_GAIN(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 3;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_COMPLIANCE_GAIN;
+ temp_msg.data[1] = (unsigned char) COMPLIANCE_GAIN;
+ temp_msg.data[2] = (unsigned char) (COMPLIANCE_GAIN >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_VALVE_FF(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 3;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_FF;
+ temp_msg.data[1] = (unsigned char) VALVE_FF;
+ temp_msg.data[2] = (unsigned char) (VALVE_FF >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_BULK_MODULUS(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 3;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_BULK_MODULUS;
+ temp_msg.data[1] = (unsigned char) BULK_MODULUS;
+ temp_msg.data[2] = (unsigned char) (BULK_MODULUS >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_CHAMBER_VOLUME(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 5;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_CHAMBER_VOLUME;
+ temp_msg.data[1] = (unsigned char) CHAMBER_VOLUME_A;
+ temp_msg.data[2] = (unsigned char) (CHAMBER_VOLUME_A >> 8);
+ temp_msg.data[3] = (unsigned char) CHAMBER_VOLUME_B;
+ temp_msg.data[4] = (unsigned char) (CHAMBER_VOLUME_B >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_PISTON_AREA(void) {
CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 5;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_PISTON_AREA;
+ temp_msg.data[1] = (unsigned char) PISTON_AREA_A;
+ temp_msg.data[2] = (unsigned char) (PISTON_AREA_A >> 8);
+ temp_msg.data[3] = (unsigned char) PISTON_AREA_B;
+ temp_msg.data[4] = (unsigned char) (PISTON_AREA_B >> 8);
+ can.write(temp_msg);
+}
+
+void CAN_TX_PRES_A_AND_B(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 5;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_PRES;
+ temp_msg.data[1] = (unsigned char) PRES_SUPPLY;
+ temp_msg.data[2] = (unsigned char) (PRES_SUPPLY >> 8);
+ temp_msg.data[3] = (unsigned char) PRES_RETURN;
+ temp_msg.data[4] = (unsigned char) (PRES_RETURN >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_ENC_LIMIT(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 5;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_ENC_LIMIT;
+ temp_msg.data[1] = (unsigned char) ENC_LIMIT_MINUS;
+ temp_msg.data[2] = (unsigned char) (ENC_LIMIT_MINUS >> 8);
+ temp_msg.data[3] = (unsigned char) ENC_LIMIT_PLUS;
+ temp_msg.data[4] = (unsigned char) (ENC_LIMIT_PLUS >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_STROKE(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 3;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_STROKE;
+ temp_msg.data[1] = (unsigned char) STROKE;
+ temp_msg.data[2] = (unsigned char) (STROKE >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_VALVE_LIMIT(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 5;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_LIMIT;
+ temp_msg.data[1] = (unsigned char) VALVE_LIMIT_MINUS;
+ temp_msg.data[2] = (unsigned char) (VALVE_LIMIT_MINUS >> 8);
+ temp_msg.data[3] = (unsigned char) VALVE_LIMIT_PLUS;
+ temp_msg.data[4] = (unsigned char) (VALVE_LIMIT_PLUS >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_ENC_PULSE_PER_POSITION(void) {
+ CANMessage temp_msg;
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (unsigned char) CTX_SEND_ENC_PULSE_PER_POSITION;
@@ -265,9 +941,9 @@
can.write(temp_msg);
}
-inline void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void) {
+void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void) {
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (unsigned char) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE;
@@ -277,9 +953,9 @@
can.write(temp_msg);
}
-inline void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void) {
+void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void) {
CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 5;
temp_msg.data[0] = (unsigned char) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR;
@@ -291,12 +967,86 @@
can.write(temp_msg);
}
-inline void CAN_TX_HOMEPOS_OFFSET(void) {
+void CAN_TX_FRICTION(void) {
+ CANMessage temp_msg;
+ long send_friction;
+ send_friction = (long) (FRICTION * 10.);
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 3;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_FRICTION;
+ temp_msg.data[1] = (unsigned char) send_friction;
+ temp_msg.data[2] = (unsigned char) (send_friction >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_VALVE_GAIN_PLUS(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 6;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_GAIN_PLUS;
+ temp_msg.data[1] = (unsigned char) (VALVE_GAIN_LPM_PER_V[0] * 50.);
+ temp_msg.data[2] = (unsigned char) (VALVE_GAIN_LPM_PER_V[2] * 50.);
+ temp_msg.data[3] = (unsigned char) (VALVE_GAIN_LPM_PER_V[4] * 50.);
+ temp_msg.data[4] = (unsigned char) (VALVE_GAIN_LPM_PER_V[6] * 50.);
+ temp_msg.data[5] = (unsigned char) (VALVE_GAIN_LPM_PER_V[8] * 50.);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_DDV_VALVE_DEADZONE(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 7;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_DDV_VALVE_DEADZONE;
+ temp_msg.data[1] = (unsigned char) VALVE_DEADZONE_MINUS;
+ temp_msg.data[2] = (unsigned char) ((int) (VALVE_DEADZONE_MINUS) >> 8);
+ temp_msg.data[3] = (unsigned char) (VALVE_DEADZONE_PLUS);
+ temp_msg.data[4] = (unsigned char) ((int) (VALVE_DEADZONE_PLUS) >> 8);
+ temp_msg.data[5] = (unsigned char) (DDV_CENTER);
+ temp_msg.data[6] = (unsigned char) ((int) (DDV_CENTER) >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_VALVE_GAIN_MINUS(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 6;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_VALVE_GAIN_MINUS;
+ temp_msg.data[1] = (unsigned char) (VALVE_GAIN_LPM_PER_V[1] * 50.);
+ temp_msg.data[2] = (unsigned char) (VALVE_GAIN_LPM_PER_V[3] * 50.);
+ temp_msg.data[3] = (unsigned char) (VALVE_GAIN_LPM_PER_V[5] * 50.);
+ temp_msg.data[4] = (unsigned char) (VALVE_GAIN_LPM_PER_V[7] * 50.);
+ temp_msg.data[5] = (unsigned char) (VALVE_GAIN_LPM_PER_V[9] * 50.);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_REFENCE_MODE(void) {
+ CANMessage temp_msg;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 6;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_REFENCE_MODE;
+ temp_msg.data[1] = (unsigned char) REFERENCE_MODE;
+ temp_msg.data[2] = (unsigned char) (int) (REFERENCE_FREQ * 100.);
+ temp_msg.data[3] = (unsigned char) ((int) (REFERENCE_FREQ * 100.) >> 8);
+ temp_msg.data[4] = (unsigned char) (int) (REFERENCE_MAG * 100.);
+ temp_msg.data[5] = (unsigned char) ((int) (REFERENCE_MAG * 100.) >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_HOMEPOS_OFFSET(void) {
+ CANMessage temp_msg;
long send_homepos_offset;
send_homepos_offset = (long) (HOMEPOS_OFFSET);
-
- CANMessage temp_msg;
-
+
temp_msg.id = CID_TX_INFO;
temp_msg.len = 3;
temp_msg.data[0] = (unsigned char) CTX_SEND_HOMEPOS_OFFSET;
@@ -305,12 +1055,78 @@
can.write(temp_msg);
}
-*/
+
+void CAN_TX_HOMPOS_VALVE_OPENING(void) {
+ CANMessage temp_msg;
+ long send_homepos_valve_opening;
+ send_homepos_valve_opening = (long) (HOMEPOS_VALVE_OPENING);
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 3;
+ temp_msg.data[0] = (unsigned char) CTX_SEND_HOMEPOS_VALVE_OPENING;
+ temp_msg.data[1] = (unsigned char) send_homepos_valve_opening;
+ temp_msg.data[2] = (unsigned char) (send_homepos_valve_opening >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_VALVE_PWM_VS_VALVE_POS(char canindex) {
+ CANMessage temp_msg;
+ int16_t valve_pos_vs_pwm;
+ valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]);
+
+ int16_t PWM_VALVE_ID = ID_index_array[canindex] * 1000;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 5;
+ temp_msg.data[0] = (unsigned char) CTX_VALVE_PWM_VS_VALVE_POS;
+ temp_msg.data[1] = (unsigned char) PWM_VALVE_ID;
+ temp_msg.data[2] = (unsigned char) (PWM_VALVE_ID >> 8);
+ temp_msg.data[3] = (unsigned char) valve_pos_vs_pwm;
+ temp_msg.data[4] = (unsigned char) (valve_pos_vs_pwm >> 8);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_VALVE_POS_VS_FLOWRATE(char canindex) {
+ CANMessage temp_msg;
+ int32_t valve_pos_vs_flowrate;
+ valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]);
+
+ int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 50 + DDV_CENTER;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 7;
+ temp_msg.data[0] = (unsigned char) CTX_VALVE_POS_VS_FLOWRATE;
+ temp_msg.data[1] = (unsigned char) VALVE_POS_VALVE_ID;
+ temp_msg.data[2] = (unsigned char) (VALVE_POS_VALVE_ID >> 8);
+ temp_msg.data[3] = (unsigned char) valve_pos_vs_flowrate;
+ temp_msg.data[4] = (unsigned char) (valve_pos_vs_flowrate >> 8);
+ temp_msg.data[5] = (unsigned char) (valve_pos_vs_flowrate >> 16);
+ temp_msg.data[6] = (unsigned char) (valve_pos_vs_flowrate >> 24);
+
+ can.write(temp_msg);
+}
+
+void CAN_TX_VALVE_POS_NUM(void) {
+ CANMessage temp_msg;
+ int32_t valve_pos_num;
+ valve_pos_num = (int16_t) VALVE_POS_NUM;
+
+ temp_msg.id = CID_TX_INFO;
+ temp_msg.len = 3;
+ temp_msg.data[0] = (unsigned char) CTX_VALVE_POS_NUM;
+ temp_msg.data[1] = (unsigned char) valve_pos_num;
+ temp_msg.data[2] = (unsigned char) (valve_pos_num >> 8);
+
+ can.write(temp_msg);
+}
+
/******************************************************************************
Sensor & State Transmission Functions
*******************************************************************************/
-inline void CAN_TX_POSITION(long t_pos, long t_vel) {
+void CAN_TX_POSITION(long t_pos, long t_vel) {
CANMessage temp_msg;
temp_msg.id = CID_TX_POSITION;
@@ -327,7 +1143,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_TORQUE(int16_t t_torque) {
+void CAN_TX_TORQUE(int16_t t_torque) {
CANMessage temp_msg;
temp_msg.id = CID_TX_TORQUE;
@@ -338,7 +1154,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b) {
+void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b) {
CANMessage temp_msg;
temp_msg.id = CID_TX_PRES;
@@ -351,7 +1167,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_PWM(int16_t t_pwm) {
+void CAN_TX_PWM(int16_t t_pwm) {
CANMessage temp_msg;
temp_msg.id = CID_TX_POSITION;
@@ -362,7 +1178,7 @@
can.write(temp_msg);
}
-inline void CAN_TX_VALVE_POSITION(int16_t t_valve_pos) {
+void CAN_TX_VALVE_POSITION(int16_t t_valve_pos) {
CANMessage temp_msg;
temp_msg.id = CID_TX_VALVE_POSITION;
--- a/CAN/function_CAN.h Fri Aug 23 01:02:32 2019 +0000
+++ b/CAN/function_CAN.h Fri Aug 23 12:28:08 2019 +0000
@@ -6,6 +6,22 @@
extern CAN can;
extern CANMessage msg;
+// INIT_CID
+#define INIT_CID_RX_CMD 100
+#define INIT_CID_RX_REF_POSITION 200
+#define INIT_CID_RX_REF_TORQUE 300
+#define INIT_CID_RX_REF_PRES_DIFF 400
+#define INIT_CID_RX_REF_VOUT 500
+#define INIT_CID_RX_REF_VALVE_POSITION 600
+
+#define INIT_CID_TX_INFO 1100
+#define INIT_CID_TX_POSITION 1200
+#define INIT_CID_TX_TORQUE 1300
+#define INIT_CID_TX_PRES 1400
+#define INIT_CID_TX_VOUT 1500
+#define INIT_CID_TX_VALVE_POSITION 1600
+
+
// CID_RX_CMD - RX CMD type
#define CRX_ASK_INFO 0
#define CRX_ASK_BNO 1
@@ -72,6 +88,11 @@
#define CRX_SET_HOMEPOS_OFFSET 140
#define CRX_ASK_HOMEPOS_VALVE_OPENING 41
#define CRX_SET_HOMEPOS_VALVE_OPENING 141
+#define CRX_ASK_DDV_VALVE_DEADZONE 42
+#define CRX_SET_DDV_VALVE_DEADZONE 142
+#define CRX_ASK_VALVE_PWM_VS_VALVE_POS 43
+#define CRX_ASK_VALVE_POS_VS_FLOWRATE 44
+#define CRX_ASK_VALVE_POS_NUM 45
#define CRX_SET_ERR_CLEAR 150
#define CRX_JUMP_STATUS 255
@@ -109,41 +130,54 @@
#define CTX_SEND_REFENCE_MODE 39
#define CTX_SEND_HOMEPOS_OFFSET 40
#define CTX_SEND_HOMEPOS_VALVE_OPENING 41
+#define CTX_SEND_DDV_VALVE_DEADZONE 42
+#define CTX_VALVE_PWM_VS_VALVE_POS 43
+#define CTX_VALVE_POS_VS_FLOWRATE 44
+#define CTX_VALVE_POS_NUM 45
+
+// Sensor & State Transmission
+void CAN_TX_POSITION(long t_pos, long t_vel);
+void CAN_TX_TORQUE(int16_t t_torque);
+void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b);
+void CAN_TX_PWM(int16_t t_pwm);
+void CAN_TX_VALVE_POSITION(int16_t t_valve_pos);
// Information Transmission
-//inline void CAN_TX_INFO(void);
-//inline void CAN_TX_BNO(void);
-//inline void CAN_TX_OPERATING_MODE(void);
-//inline void CAN_TX_CAN_FREQ(void);
-//inline void CAN_TX_CONTROL_MODE(void);
-//inline void CAN_TX_JOINT_ENC_DIR(void);
-//inline void CAN_TX_VALVE_DIR(void);
-//inline void CAN_TX_VALVE_ENC_DIR(void);
-//inline void CAN_TX_VOLTAGE_SUPPLY(void);
-//inline void CAN_TX_VOLTAGE_VALVE(void);
-//inline void CAN_TX_PID_GAIN(int t_type);
-//inline void CAN_TX_VALVE_DEADZONE(void);
-//inline void CAN_TX_ENC_PULSE_PER_POSITION(void);
-//inline void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void);
-//inline void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void);
-//inline void CAN_TX_HOMEPOS_OFFSET(void);
-
-// Sensor & State Transmission
-inline void CAN_TX_POSITION(long t_pos, long t_vel);
-inline void CAN_TX_TORQUE(int16_t t_torque);
-inline void CAN_TX_PRES(int16_t t_pres_a, int16_t t_pres_b);
-inline void CAN_TX_PWM(int16_t t_pwm);
-inline void CAN_TX_VALVE_POSITION(int16_t t_valve_pos);
-
-// Board Information Variables
-extern int BNO;
-extern int CONTROL_MODE;
-extern double P_GAIN_JOINT_POSITION;
-extern double I_GAIN_JOINT_POSITION;
-extern double D_GAIN_JOINT_POSITION;
-extern double P_GAIN_JOINT_TORQUE;
-extern double I_GAIN_JOINT_TORQUE;
-extern double D_GAIN_JOINT_TORQUE;
+void CAN_TX_INFO(void);
+void CAN_TX_BNO(void);
+void CAN_TX_OPERATING_MODE(void);
+void CAN_TX_CAN_FREQ(void);
+void CAN_TX_CONTROL_MODE(void);
+void CAN_TX_JOINT_ENC_DIR(void);
+void CAN_TX_VALVE_DIR(void);
+void CAN_TX_VALVE_ENC_DIR(void);
+void CAN_TX_VOLTAGE_SUPPLY(void);
+void CAN_TX_VOLTAGE_VALVE(void);
+void CAN_TX_PID_GAIN(int t_type);
+void CAN_TX_VALVE_DEADZONE(void);
+void CAN_TX_VELOCITY_COMP_GAIN(void);
+void CAN_TX_COMPLIANCE_GAIN(void);
+void CAN_TX_VALVE_FF(void);
+void CAN_TX_BULK_MODULUS(void);
+void CAN_TX_CHAMBER_VOLUME(void);
+void CAN_TX_PISTON_AREA(void);
+void CAN_TX_PRES_A_AND_B(void);
+void CAN_TX_ENC_LIMIT(void);
+void CAN_TX_STROKE(void);
+void CAN_TX_VALVE_LIMIT(void);
+void CAN_TX_ENC_PULSE_PER_POSITION(void);
+void CAN_TX_TORQUE_SENSOR_PULSE_PER_TORQUE(void);
+void CAN_TX_PRES_SENSOR_PULSE_PER_PRES(void);
+void CAN_TX_FRICTION(void);
+void CAN_TX_VALVE_GAIN_PLUS(void);
+void CAN_TX_VALVE_GAIN_MINUS(void);
+void CAN_TX_REFENCE_MODE(void);
+void CAN_TX_HOMEPOS_OFFSET(void);
+void CAN_TX_HOMPOS_VALVE_OPENING(void);
+void CAN_TX_VALVE_PWM_VS_VALVE_POS(char canindex);
+void CAN_TX_DDV_VALVE_DEADZONE(void);
+void CAN_TX_VALVE_POS_VS_FLOWRATE(char canindex);
+void CAN_TX_VALVE_POS_NUM(void);
class State
{
@@ -180,5 +214,8 @@
// CAN Receive Functions
void ReadCMD();
void CAN_RX_HANDLER();
+void CAN_ID_INIT(void);
+
+extern int ID_index_array[100];
#endif
--- a/INIT_HW/INIT_HW.cpp Fri Aug 23 01:02:32 2019 +0000
+++ b/INIT_HW/INIT_HW.cpp Fri Aug 23 12:28:08 2019 +0000
@@ -52,3 +52,25 @@
TIM4->CR1 |= TIM_CR1_CEN; // enable TIM4
}
+
+void Init_TMR3(){
+ RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; // enable TIM3 clock
+
+ //ISR Setup
+
+ NVIC_EnableIRQ(TIM3_IRQn); //Enable TIM3 IRQ
+
+ TIM3->DIER |= TIM_DIER_UIE; // enable update interrupt
+ TIM3->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode
+ TIM3->CR1 |= TIM_CR1_UDIS;
+ TIM3->CR1 |= TIM_CR1_ARPE; // autoreload on,
+ TIM3->RCR |= 0x001; // update event once per up/down count of TIM3
+ TIM3->EGR |= TIM_EGR_UG;
+
+ //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->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on.
+ TIM3->CR1 |= TIM_CR1_CEN; // enable TIM4
+}
\ No newline at end of file
--- a/INIT_HW/INIT_HW.h Fri Aug 23 01:02:32 2019 +0000 +++ b/INIT_HW/INIT_HW.h Fri Aug 23 12:28:08 2019 +0000 @@ -6,5 +6,6 @@ void Init_ADC(void); void Init_PWM(); +void Init_TMR3(); #endif
--- a/SPI_EEP_ENC/SPI_EEP_ENC.h Fri Aug 23 01:02:32 2019 +0000 +++ b/SPI_EEP_ENC/SPI_EEP_ENC.h Fri Aug 23 12:28:08 2019 +0000 @@ -12,4 +12,159 @@ void spi_enc_set_init(void); int spi_enc_read(void); +void SPI_VREF_DAC_WRITE_CHANNEL(unsigned int channel, unsigned int mode, unsigned int value); +void SPI_VREF_DAC_WRITE(double VA, double VB, double VC, double VD); +void SPI_VREF_DAC_SET_ZERO(void); + +/******************************************************************************* + * ROM DATA ADDRESS ID + ******************************************************************************/ + +#define RID_BNO 0 +#define RID_OPERATING_MODE 1 +#define RID_CAN_FREQ 2 +#define RID_JOINT_ENC_DIR 3 +#define RID_VALVE_DIR 4 +#define RID_VALVE_ENC_DIR 5 +#define RID_VOLATGE_SUPPLY 6 +#define RID_VOLTAGE_VALVE 7 + +#define RID_P_GAIN_VALVE_POSITION 8 +#define RID_I_GAIN_VALVE_POSITION 9 +#define RID_D_GAIN_VALVE_POSITION 10 + +#define RID_P_GAIN_JOINT_POSITION 11 +#define RID_I_GAIN_JOINT_POSITION 12 +#define RID_D_GAIN_JOINT_POSITION 13 + +#define RID_P_GAIN_JOINT_TORQUE 14 +#define RID_I_GAIN_JOINT_TORQUE 15 +#define RID_D_GAIN_JOINT_TORQUE 16 + +#define RID_VALVE_DEADZONE_PLUS 17 +#define RID_VALVE_DEADZONE_MINUS 18 + +#define RID_VELOCITY_COMP_GAIN 19 +#define RID_COMPLIANCE_GAIN 20 + +#define RID_VALVE_CNETER 21 + +#define RID_VALVE_FF 22 + +#define RID_BULK_MODULUS 23 + +#define RID_CHAMBER_VOLUME_A 24 +#define RID_CHAMBER_VOLUME_B 25 + +#define RID_PISTON_AREA_A 26 +#define RID_PISTON_AREA_B 27 + +#define RID_PRES_SUPPLY 28 +#define RID_PRES_RETURN 29 + +#define RID_ENC_LIMIT_PLUS 30 +#define RID_ENC_LIMIT_MINUS 31 + +#define RID_STROKE 32 + +#define RID_VALVE_LIMIT_PLUS 34 +#define RID_VALVE_LIMIT_MINUS 35 + +#define RID_ENC_PULSE_PER_POSITION 36 +#define RID_TORQUE_SENSOR_PULSE_PER_TORQUE 37 +#define RID_PRES_SENSOR_A_PULSE_PER_BAR 38 +#define RID_PRES_SENSOR_B_PULSE_PER_BAR 39 + +#define RID_FRICTION 40 +#define RID_HOMEPOS_OFFSET 41 +#define RID_HOMEPOS_VALVE_OPENING 42 + +#define RID_TORQUE_SENSOR_VREF 45 + +#define RID_PRES_A_SENSOR_VREF 50 +#define RID_PRES_B_SENSOR_VREF 51 + +#define RID_VALVE_GAIN_PLUS_1 60 +#define RID_VALVE_GAIN_MINUS_1 61 +#define RID_VALVE_GAIN_PLUS_2 62 +#define RID_VALVE_GAIN_MINUS_2 63 +#define RID_VALVE_GAIN_PLUS_3 64 +#define RID_VALVE_GAIN_MINUS_3 65 +#define RID_VALVE_GAIN_PLUS_4 66 +#define RID_VALVE_GAIN_MINUS_4 67 +#define RID_VALVE_GAIN_PLUS_5 68 +#define RID_VALVE_GAIN_MINUS_5 69 + +#define RID_VALVE_POS_VS_PWM_0 70 +#define RID_VALVE_POS_VS_PWM_1 71 +#define RID_VALVE_POS_VS_PWM_2 72 +#define RID_VALVE_POS_VS_PWM_3 73 +#define RID_VALVE_POS_VS_PWM_4 74 +#define RID_VALVE_POS_VS_PWM_5 75 +#define RID_VALVE_POS_VS_PWM_6 76 +#define RID_VALVE_POS_VS_PWM_7 77 +#define RID_VALVE_POS_VS_PWM_8 78 +#define RID_VALVE_POS_VS_PWM_9 79 +#define RID_VALVE_POS_VS_PWM_10 80 +#define RID_VALVE_POS_VS_PWM_11 81 +#define RID_VALVE_POS_VS_PWM_12 82 +#define RID_VALVE_POS_VS_PWM_13 83 +#define RID_VALVE_POS_VS_PWM_14 84 +#define RID_VALVE_POS_VS_PWM_15 85 +#define RID_VALVE_POS_VS_PWM_16 86 +#define RID_VALVE_POS_VS_PWM_17 87 + +#define RID_VALVE_MAX_POS 90 +#define RID_VALVE_MIN_POS 91 + +#define RID_VALVE_POS_NUM 92 +#define RID_DDV_CNETER 93 + +#define RID_IS_FIRST 99 + + +#define RID_VALVE_POS_VS_FLOWRATE_0 100 +#define RID_VALVE_POS_VS_FLOWRATE_1 101 +#define RID_VALVE_POS_VS_FLOWRATE_2 102 +#define RID_VALVE_POS_VS_FLOWRATE_3 103 +#define RID_VALVE_POS_VS_FLOWRATE_4 104 +#define RID_VALVE_POS_VS_FLOWRATE_5 105 +#define RID_VALVE_POS_VS_FLOWRATE_6 106 +#define RID_VALVE_POS_VS_FLOWRATE_7 107 +#define RID_VALVE_POS_VS_FLOWRATE_8 108 +#define RID_VALVE_POS_VS_FLOWRATE_9 109 +#define RID_VALVE_POS_VS_FLOWRATE_10 110 +#define RID_VALVE_POS_VS_FLOWRATE_11 111 +#define RID_VALVE_POS_VS_FLOWRATE_12 112 +#define RID_VALVE_POS_VS_FLOWRATE_13 113 +#define RID_VALVE_POS_VS_FLOWRATE_14 114 +#define RID_VALVE_POS_VS_FLOWRATE_15 115 +#define RID_VALVE_POS_VS_FLOWRATE_16 116 +#define RID_VALVE_POS_VS_FLOWRATE_17 117 +#define RID_VALVE_POS_VS_FLOWRATE_18 118 +#define RID_VALVE_POS_VS_FLOWRATE_19 119 +#define RID_VALVE_POS_VS_FLOWRATE_20 120 + +#define RID_VALVE_POS_VS_FLOWRATE_0_1 200 +#define RID_VALVE_POS_VS_FLOWRATE_1_1 201 +#define RID_VALVE_POS_VS_FLOWRATE_2_1 202 +#define RID_VALVE_POS_VS_FLOWRATE_3_1 203 +#define RID_VALVE_POS_VS_FLOWRATE_4_1 204 +#define RID_VALVE_POS_VS_FLOWRATE_5_1 205 +#define RID_VALVE_POS_VS_FLOWRATE_6_1 206 +#define RID_VALVE_POS_VS_FLOWRATE_7_1 207 +#define RID_VALVE_POS_VS_FLOWRATE_8_1 208 +#define RID_VALVE_POS_VS_FLOWRATE_9_1 209 +#define RID_VALVE_POS_VS_FLOWRATE_10_1 210 +#define RID_VALVE_POS_VS_FLOWRATE_11_1 211 +#define RID_VALVE_POS_VS_FLOWRATE_12_1 212 +#define RID_VALVE_POS_VS_FLOWRATE_13_1 213 +#define RID_VALVE_POS_VS_FLOWRATE_14_1 214 +#define RID_VALVE_POS_VS_FLOWRATE_15_1 215 +#define RID_VALVE_POS_VS_FLOWRATE_16_1 216 +#define RID_VALVE_POS_VS_FLOWRATE_17_1 217 +#define RID_VALVE_POS_VS_FLOWRATE_18_1 218 +#define RID_VALVE_POS_VS_FLOWRATE_19_1 219 +#define RID_VALVE_POS_VS_FLOWRATE_20_1 220 + #endif //_SPI_H_ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/function_utilities/function_utilities.cpp Fri Aug 23 12:28:08 2019 +0000
@@ -0,0 +1,456 @@
+#include "setting.h"
+#include "SPI_EEP_ENC.h"
+#include "function_utilities.h"
+
+/*******************************************************************************
+ * VARIABLE
+ ******************************************************************************/
+
+// Board Information
+uint8_t BNO = 0;
+uint8_t CONTROL_MODE = 0;
+uint8_t OPERATING_MODE = 0;
+uint8_t SETTING_SWITCH = 0;
+uint8_t SETTING_SWITCH_OLD = 0;
+uint8_t REFERENCE_MODE = 0;
+uint16_t CAN_FREQ = 5000;
+uint8_t DIR_JOINT_ENC = 0;
+uint8_t DIR_VALVE = 0;
+uint8_t DIR_VALVE_ENC = 0;
+
+double SUPPLY_VOLTAGE = 12.0;
+double VALVE_VOLTAGE_LIMIT = 5.0;
+
+double P_GAIN_VALVE_POSITION = 0.0;
+double I_GAIN_VALVE_POSITION= 0.0;
+double D_GAIN_VALVE_POSITION= 0.0;
+double P_GAIN_JOINT_POSITION = 0.0;
+double I_GAIN_JOINT_POSITION = 0.0;
+double D_GAIN_JOINT_POSITION = 0.0;
+double P_GAIN_JOINT_TORQUE = 0.0;
+double I_GAIN_JOINT_TORQUE = 0.0;
+double D_GAIN_JOINT_TORQUE = 0.0;
+
+int16_t VALVE_DEADZONE_PLUS;
+int16_t VALVE_DEADZONE_MINUS;
+
+int16_t VELOCITY_COMP_GAIN;
+int16_t COMPLIANCE_GAIN;
+
+int16_t VALVE_CENTER;
+
+int16_t VALVE_FF;
+
+int16_t BULK_MODULUS;
+
+int16_t CHAMBER_VOLUME_A;
+int16_t CHAMBER_VOLUME_B;
+
+int16_t PISTON_AREA_A;
+int16_t PISTON_AREA_B;
+double PISTON_AREA_alpha;
+
+
+int16_t PRES_SUPPLY;
+int16_t PRES_RETURN;
+
+int16_t ENC_LIMIT_PLUS;
+int16_t ENC_LIMIT_MINUS;
+
+int16_t STROKE;
+
+
+int16_t VALVE_LIMIT_PLUS;
+int16_t VALVE_LIMIT_MINUS;
+
+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;
+
+int HOMEPOS_OFFSET;
+int HOMEPOS_VALVE_OPENING;
+
+double FRICTION;
+double REF_PERIOD;
+double REF_MAG;
+int REF_NUM;
+
+
+double DAC_REF;
+double DAC_RESOL;
+double PWM_RESOL;
+
+int REF_POSITION;
+int REF_VELOCITY;
+int16_t REF_TORQUE;
+int16_t REF_PRES_DIFF;
+int16_t REF_PWM;
+int16_t REF_VALVE_POSITION;
+
+int REF_MOVE_TIME_5k;
+int INIT_REF_PWM;
+int INIT_REF_VALVE_POS;
+int INIT_REF_POS;
+int INIT_REF_VEL;
+int INIT_REF_TORQUE;
+int INIT_REF_PRES_DIFF;
+
+int CUR_POSITION;
+int CUR_VELOCITY;
+double CUR_TORQUE;
+double CUR_PRES_A;
+double CUR_PRES_B;
+int CUR_PWM;
+int CUR_VALVE_POSITION;
+
+unsigned int TMR2_COUNT_LED1;
+unsigned int TMR2_COUNT_LED2;
+unsigned int TMR2_COUNT_CAN_TX;
+
+int num_err;
+int flag_err[8];
+int flag_err_old[8];
+int flag_err_rt;
+
+int flag_ref_enable;
+
+int flag_data_request[5];
+
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////// SEUNGHOON ADD ///////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+
+double CUR_PRES_A_BAR;
+double CUR_PRES_B_BAR;
+double CUR_TORQUE_NM;
+double CUR_TORQUE_NM_PRESS;
+
+double PRES_A_VREF;
+double PRES_B_VREF;
+double TORQUE_VREF;
+
+double VALVE_PWM_RAW;
+double VALVE_PWM_RAW_FB;
+double VALVE_PWM_RAW_FF;
+int VALVE_PWM_VALVE_DZ;
+int VALVE_INPUT_PWM;
+
+double VALVE_GAIN_LPM_PER_V[10];
+double VALVE_POS_VS_PWM[18];
+long JOINT_VEL[100];
+
+int VALVE_MAX_POS;
+int VALVE_MIN_POS;
+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;
+
+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;
+
+double TUNING_TIME;
+
+double REFERENCE_FREQ;
+double REFERENCE_MAG;
+
+bool FLAG_FIND_HOME;
+
+int MODE_JUMP_STATUS;
+enum _JUMP_STATUS {
+ JUMP_NO_ACT = 0, //0
+ JUMP_START, //1
+ JUMP_TAKEOFF, //2
+ JUMP_FLYING, //3
+ JUMP_LANDING, //4
+};
+
+
+
+
+/*******************************************************************************
+ * General math functions
+ ******************************************************************************/
+
+
+double dabs(double tx) {
+ if (tx >= 0.0)
+ return tx;
+ else
+ return -tx;
+}
+
+double change_int_to_efloat(int input) {
+ int i = 0;
+
+ double output = 0;
+ int vn = (int) ((double) input / 10.);
+ int en = input % 10;
+
+ double temp = 1.;
+ for (i = 0; i < en; i++)
+ temp *= 0.1;
+
+ output = (double) vn*temp;
+ return output;
+}
+
+void make_delay(void) {
+ int i = 0;
+
+ for (i = 0; i < 100000; i++) {
+ ;
+ }
+
+}
+
+
+/*******************************************************************************
+ * ROM functions
+ ******************************************************************************/
+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_CALL_DATA();
+}
+
+void ROM_RESET_DATA(void) {
+
+ 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);
+ 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_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.));
+
+
+}
+
+void ROM_CALL_DATA(void) {
+ int i = 0;
+ BNO = spi_eeprom_read(RID_BNO);
+ BNO = 10;
+ 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.;
+ VALVE_VOLTAGE_LIMIT = (double) spi_eeprom_read(RID_VOLTAGE_VALVE) / 10.;
+
+ 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);
+ 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);
+
+ 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);
+ 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.;
+ 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.;
+
+ 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.;
+
+ 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.;
+
+ 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] = (long) ( (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);
+
+}
+
+/*******************************************************************************
+ * ENCODER functions
+ ******************************************************************************/
+// A-KHA
+#define KF_G1_11 0.083920206005350
+#define KF_G1_12 0.000013905329560
+#define KF_G1_21 -0.000575742328210
+#define KF_G1_22 0.799999939711725
+// K
+#define KF_G2_11 0.916079793994650
+#define KF_G2_12 0.000002878711641
+#define KF_G2_21 0.000575742328210
+#define KF_G2_22 0.199999945139809
+
+double KF_Y_11 = 0.;
+double KF_Y_21 = 0.;
+double KF_X_11 = 0.;
+double KF_X_21 = 0.;
+
+long ENC_pos_old = 0, ENC_pos_cur = 0, ENC_pos_diff = 0;
+long ENC_RAW = 0, ENC_VEL_RAW = 0, ENC_VEL_KF = 0;
+long enc_offset = 0;
+
+void ENC_UPDATE(void) {
+
+ ENC_pos_cur = spi_enc_read();
+ ENC_pos_diff = ENC_pos_cur - ENC_pos_old;
+
+ if (ENC_pos_diff > 1300 || ENC_pos_diff<-1300) {
+ //MOT_E_STOP(0);
+ }
+
+ ENC_VEL_RAW = (long) (ENC_pos_diff * TMR_FREQ_5k);
+
+ KF_Y_11 = ENC_pos_cur;
+ KF_Y_21 = ENC_VEL_RAW;
+ KF_X_11 = KF_G1_11 * KF_X_11 + KF_G1_12 * KF_X_21 + KF_G2_11 * KF_Y_11 + KF_G2_12*KF_Y_21;
+ 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 = (long) KF_X_21;
+
+ CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur + enc_offset;
+ // CUR_POSITION = (DIR_JOINT_ENC) * ENC_pos_cur;
+ CUR_VELOCITY = (DIR_JOINT_ENC) * ENC_VEL_KF;
+
+ // CUR_POSITION = ENC_pos_cur;
+ // CUR_VELOCITY = ENC_VEL_KF;
+
+ ENC_pos_old = ENC_pos_cur;
+}
+
+void ENC_SET_ZERO(void) {
+
+ spi_enc_set_clear();
+ CUR_POSITION = 0;
+ ENC_pos_old = ENC_pos_cur = 0;
+
+}
+
+void ENC_SET(long value) {
+
+ spi_enc_set_clear();
+ enc_offset = value;
+ CUR_POSITION = value;
+ ENC_pos_old = ENC_pos_cur = value;
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/function_utilities/function_utilities.h Fri Aug 23 12:28:08 2019 +0000 @@ -0,0 +1,18 @@ +#ifndef _FUNCTION_UTILITIES_H +#define _FUNCTION_UTILITIES_H + +double dabs(double tx); +double change_int_to_efloat(int input); +void make_delay(void); + +void ROM_RESET_DATA(void); +void ROM_CALL_DATA(void); +void ROM_INIT_DATA(void); + +void ENC_UPDATE(void); +void ENC_SET_ZERO(void); +void ENC_SET(long value); +void VALVE_POSITION_INIT(void); +void VALVE_PWM(int pwm, double vol_max, double vol_in); + +#endif \ No newline at end of file
--- a/main.cpp Fri Aug 23 01:02:32 2019 +0000
+++ b/main.cpp Fri Aug 23 12:28:08 2019 +0000
@@ -5,6 +5,7 @@
#include "SPI_EEP_ENC.h"
#include "I2C_AS5510.h"
#include "setting.h"
+#include "function_utilities.h"
// dac & check ///////////////////////////////////////////
DigitalOut check(PC_2);
@@ -27,7 +28,7 @@
DigitalOut eeprom_cs(PB_12);
SPI enc(PC_12,PC_11,PC_10);
DigitalOut enc_cs(PD_2);
-DigitalOut indi_led(PA_15);
+DigitalOut LED(PA_15);
// UART ///////////////////////////////////////////
Serial pc(PA_9,PA_10); // _ UART
@@ -35,6 +36,10 @@
// CAN ///////////////////////////////////////////
CAN can(PB_8, PB_9, 1000000);
CANMessage msg;
+void onMsgReceived()
+{
+ CAN_RX_HANDLER();
+}
// Variables ///////////////////////////////////////////
State pos;
@@ -50,7 +55,7 @@
double V_MAX = 12000.0; // Maximum Voltage : 12V = 12000mV
double PWM_out=0.0;
-
+int ID_index_array[100] = {0};
// =============================================================================
// =============================================================================
// =============================================================================
@@ -61,44 +66,61 @@
*** Initialization
*********************************/
- indi_led = 0;
+ LED = 1;
pc.baud(9600);
-
- //Timer t;
- //t.start();
- //t.stop();
- //pc.printf("The time taken was %f seconds\n",t.read());
// i2c init
i2c.frequency(400 * 1000); // 0.4 mHz
wait_ms(2); // Power Up wait
look_for_hardware_i2c(); // Hardware present
init_as5510(i2c_slave_addr1);
+ make_delay();
// // spi init
eeprom.format(8,3);
eeprom.frequency(5000000); //5M
enc.format(8,0);
enc.frequency(5000000); //5M
+ make_delay();
// ADC init
Init_ADC();
+ make_delay();
// Pwm init
Init_PWM();
TIM4->CR1 ^= TIM_CR1_UDIS;
+ make_delay();
-// //SPI
-// spi_eeprom_ready();
-// spi_eeprom_write(0x1,0x112);
-// spi_eeprom_ready();
-// int i = spi_eeprom_read(0x1);
+ // TMR3 init
+ Init_TMR3();
+ TIM3->CR1 ^= TIM_CR1_UDIS;
+ make_delay();
// CAN
can.attach(&CAN_RX_HANDLER);
+ CAN_ID_INIT();
+ make_delay();
// spi _ enc
spi_enc_set_init();
+ make_delay();
+
+ //eeprom
+ ROM_INIT_DATA();
+ make_delay();
+
+ //DAC init
+ dac_1 = PRES_A_VREF/3.3;
+ dac_2 = PRES_B_VREF/3.3;
+ make_delay();
+
+ for (int i=0; i<100; i++){
+ if(i%2==0)
+ ID_index_array[i] = - i * 0.5;
+ else
+ ID_index_array[i] = (i+1) * 0.5;
+ }
/************************************
@@ -107,16 +129,10 @@
while(1) {
//spi _ enc
- int a = spi_enc_read();
+ //int a = spi_enc_read();
+
//i2c
read_field(i2c_slave_addr1);
-
- check_2=0;
-
- pc.printf("%f\n",1234567);
-// pc.printf("%d\n",a1);
-
-// wait(0.01f);
}
}
@@ -138,31 +154,39 @@
********************************************************/
if((CNT_TMR4%2)==0){
- //spi
- // eeprom.write(0xff);
- // eeprom.write(0xff);
- // ready();
- // read(1);
-
- //i2c
- //// read_field(i2c_slave_addr1);
//ADC
ADC3->CR2 |= 0x40000000; // adc _ 12bit
- // a1=ADC1->DR;
- // a1=ADC2->DR;
-// int raw_cur = ADC3->DR;
+// a1=ADC1->DR;
+// a1=ADC2->DR;
+// int raw_cur = ADC3->DR;
while((ADC3->SR & 0b10));
double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz
double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA
cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur);
-
}
//DAC
// dac_1 = ADC1->DR;
// dac_2 = ADC2->DR;
+
+
+
+ /*******************************************************
+ *** Timer Counting & etc.
+ ********************************************************/
+ CNT_TMR4++;
+ }
+ TIM4->SR = 0x0; // reset the status register
+}
+
+unsigned long CNT_TMR3 = 0;
+double FREQ_TMR3 = (double)FREQ_5k;
+double DT_TMR3 = (double)DT_5k;
+extern "C" void TIM3_IRQHandler(void)
+{
+ if ( TIM3->SR & TIM_SR_UIF ) {
/*******************************************************
*** Valve Control
@@ -199,30 +223,47 @@
/*******************************************************
*** Data Send (CAN) & Print out (UART)
********************************************************/
- if((CNT_TMR4%40)==0){
- msg.id = 50;
- msg.len = 4;
- int temp_CUR = (int)(cur.sen*1000.0);
- msg.data[0]=0x00FF&temp_CUR;
- msg.data[1]=0x00FF&(temp_CUR>>8);
- int temp_PWM = (int)(V_out);
- msg.data[2]=0x00FF&temp_PWM;
- msg.data[3]=0x00FF&(temp_PWM>>8);
- can.write(msg);
+ //if((CNT_TMR3%40)==0){
+// msg.id = 50;
+// msg.len = 4;
+// int temp_CUR = (int)(cur.sen*1000.0);
+// msg.data[0]=0x00FF&temp_CUR;
+// msg.data[1]=0x00FF&(temp_CUR>>8);
+// int temp_PWM = (int)(V_out);
+// msg.data[2]=0x00FF&temp_PWM;
+// msg.data[3]=0x00FF&(temp_PWM>>8);
+// can.write(msg);
+// }
+
+ if((CNT_TMR3%5000)==0){
+ if(LED==1)
+ {
+ LED=0;
+ }
+ else
+ LED = 1;
+// LED != LED;
}
-// if((CNT_TMR4%4000)==0){
-// pc.printf("%d\n",a1);
-// }
-
/*******************************************************
*** Timer Counting & etc.
********************************************************/
- CNT_TMR4++;
+ CNT_TMR3++;
}
- TIM4->SR = 0x0; // reset the status register
+ TIM3->SR = 0x0; // reset the status register
}
+/*******************************************************************************
+ * REFERENCE MODE
+ ******************************************************************************/
+enum _REFERENCE_MODE{
+ MODE_REF_NO_ACT = 0, //0
+ MODE_REF_DIRECT, //1
+ MODE_REF_COS_INC, //2
+ MODE_REF_LINE_INC, //3
+ MODE_REF_SIN_WAVE, //4
+ MODE_REF_SQUARE_WAVE, //5
+};
/*******************************************************************************
* CONTROL MODE
@@ -252,6 +293,10 @@
MODE_PRESSURE_SENSOR_NULLING, //24
MODE_PRESSURE_SENSOR_CALIB, //25
MODE_ROTARY_FRICTION_TUNING, //26
+
+ MODE_DDV_POS_VS_PWM_ID = 30, //30
+ MODE_DDV_DEADZONE_AND_CENTER, //31
+ MODE_DDV_POS_VS_FLOWRATE, //32
};
void ValveControl(unsigned int ControlMode)
--- a/setting.h Fri Aug 23 01:02:32 2019 +0000 +++ b/setting.h Fri Aug 23 12:28:08 2019 +0000 @@ -1,8 +1,6 @@ #include "mbed.h" #include "FastPWM.h" -#define PI 3.141592 - // pwm #define PIN_V PB_7 #define PIN_W PB_6 @@ -10,6 +8,7 @@ //#define PWM_ARR 0x8CA // loop 40k, pwm 20k #define PWM_ARR 0x1194 // loop 20k, pwm 10k //#define PWM_ARR 0x2328 // loop 10k, pwm 5k +#define TMR3_COUNT 0x4650 // loop 5k #define FREQ_5k 5000.0 #define FREQ_10k 10000.0 @@ -53,3 +52,215 @@ void CurrentControl(); void ValveControl(unsigned int ControlMode); + + +/******************************************************************************* + * COMMON Settings + ******************************************************************************/ +#define LATEST_VERSION 19032 + + + +/******************************************************************************* + * COMMON CONSTANTS + ******************************************************************************/ +#define RAD_30 0.523598775598299 +#define RAD_60 1.047197551196598 +#define RAD_120 2.094395102393195 +#define RAD_180 3.141592653589793 +#define RAD_240 4.188790204786391 +#define RAD_300 5.235987755982989 +#define RAD_360 6.283185307179586 + +#define SYSFREQ 200000000 +#define PBCLK 100000000 + +//#define TMR_FREQ_10k 10000 +#define TMR_FREQ_5k 5000 + +#define FALSE 0 +#define TRUE 1 +#define OUTPUT 0 +#define INPUT 1 +#define LOW 0 +#define HIGH 1 +#define DIGITAL 0 +#define ANALOG 1 + +#define PI 3.141592653589793 +#define D2R 0.017453292519943 +#define R2D 57.295779513082323 + + +/******************************************************************************* + * HEADER INCLUDE + ******************************************************************************/ + +/******************************************************************************* + * VARIABLE + ******************************************************************************/ + +// Board Information +extern uint8_t BNO; +extern uint8_t CONTROL_MODE; +extern uint8_t OPERATING_MODE; +extern uint8_t SETTING_SWITCH; +extern uint8_t SETTING_SWITCH_OLD; +extern uint8_t REFERENCE_MODE; +extern uint16_t CAN_FREQ; +extern uint8_t DIR_JOINT_ENC; +extern uint8_t DIR_VALVE; +extern uint8_t DIR_VALVE_ENC; + +extern double SUPPLY_VOLTAGE; +extern double VALVE_VOLTAGE_LIMIT; + +extern double P_GAIN_VALVE_POSITION; +extern double I_GAIN_VALVE_POSITION; +extern double D_GAIN_VALVE_POSITION; +extern double P_GAIN_JOINT_POSITION; +extern double I_GAIN_JOINT_POSITION; +extern double D_GAIN_JOINT_POSITION; +extern double P_GAIN_JOINT_TORQUE; +extern double I_GAIN_JOINT_TORQUE; +extern double D_GAIN_JOINT_TORQUE; + +extern int16_t VALVE_DEADZONE_PLUS; +extern int16_t VALVE_DEADZONE_MINUS; + +extern int16_t VELOCITY_COMP_GAIN; +extern int16_t COMPLIANCE_GAIN; + +extern int16_t VALVE_CENTER; + +extern int16_t VALVE_FF; + +extern int16_t BULK_MODULUS; + +extern int16_t CHAMBER_VOLUME_A; +extern int16_t CHAMBER_VOLUME_B; + +extern int16_t PISTON_AREA_A; +extern int16_t PISTON_AREA_B; +extern double PISTON_AREA_alpha; + + +extern int16_t PRES_SUPPLY; +extern int16_t PRES_RETURN; + +extern int16_t ENC_LIMIT_PLUS; +extern int16_t ENC_LIMIT_MINUS; + +extern int16_t STROKE; + + +extern int16_t VALVE_LIMIT_PLUS; +extern int16_t VALVE_LIMIT_MINUS; + +extern int16_t ENC_PULSE_PER_POSITION; +extern int16_t TORQUE_SENSOR_PULSE_PER_TORQUE; +extern double PRES_SENSOR_A_PULSE_PER_BAR; +extern double PRES_SENSOR_B_PULSE_PER_BAR; + +extern int HOMEPOS_OFFSET; +extern int HOMEPOS_VALVE_OPENING; + +extern double FRICTION; +extern double REF_PERIOD; +extern double REF_MAG; +extern int REF_NUM; + + +extern double DAC_REF; +extern double DAC_RESOL; +extern double PWM_RESOL; + +extern int REF_POSITION; +extern int REF_VELOCITY; +extern int16_t REF_TORQUE; +extern int16_t REF_PRES_DIFF; +extern int16_t REF_PWM; +extern int16_t REF_VALVE_POSITION; + +extern int REF_MOVE_TIME_5k; +extern int INIT_REF_PWM; +extern int INIT_REF_VALVE_POS; +extern int INIT_REF_POS; +extern int INIT_REF_VEL; +extern int INIT_REF_TORQUE; +extern int INIT_REF_PRES_DIFF; + +extern int CUR_POSITION; +extern int CUR_VELOCITY; +extern double CUR_TORQUE; +extern double CUR_PRES_A; +extern double CUR_PRES_B; +extern int CUR_PWM; +extern int CUR_VALVE_POSITION; + +extern unsigned int TMR2_COUNT_LED1; +extern unsigned int TMR2_COUNT_LED2; +extern unsigned int TMR2_COUNT_CAN_TX; + +extern int num_err; +extern int flag_err[8]; +extern int flag_err_old[8]; +extern int flag_err_rt; + +extern int flag_ref_enable; + +extern int flag_data_request[5]; + +//////////////////////////////////////////////////////////////////////////////// +////////////////////////////// SEUNGHOON ADD /////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////// + +extern double CUR_PRES_A_BAR; +extern double CUR_PRES_B_BAR; +extern double CUR_TORQUE_NM; +extern double CUR_TORQUE_NM_PRESS; + +extern double PRES_A_VREF; +extern double PRES_B_VREF; +extern double TORQUE_VREF; + +extern double VALVE_PWM_RAW; +extern double VALVE_PWM_RAW_FB; +extern double VALVE_PWM_RAW_FF; +extern int VALVE_PWM_VALVE_DZ; +extern int VALVE_INPUT_PWM; + +extern double VALVE_GAIN_LPM_PER_V[10]; +extern double VALVE_POS_VS_PWM[18]; +extern long JOINT_VEL[100]; + +extern int VALVE_MAX_POS; +extern int VALVE_MIN_POS; +extern int DDV_CENTER; +extern int VALVE_POS_NUM; + +extern int TMR2_COUNT_FINDHOME; +extern int TMR2_COUNT_FLOWRATE; +extern int TMR2_COUNT_DEADZONE; +extern int TMR2_COUNT_PRES_NULL; +extern int TMR2_COUNT_TORQUE_NULL; +extern int TMR2_COUNT_PRES_CALIB; +extern int TMR2_COUNT_REFERENCE; +extern int TMR2_COUNT_JOINT; +extern int TMR2_COUNT_ROTARY_FRIC_TUNE; + +extern bool FLAG_REFERENCE_VALVE_PWM; +extern bool FLAG_REFERENCE_VALVE_POSITION; +extern bool FLAG_REFERENCE_JOINT_POSITION; +extern bool FLAG_REFERENCE_JOINT_TORQUE; +extern bool FLAG_REFERENCE_PRES_DIFF; + +extern double TUNING_TIME; + +extern double REFERENCE_FREQ; +extern double REFERENCE_MAG; + +extern bool FLAG_FIND_HOME; + +extern int MODE_JUMP_STATUS; +