Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 226:82a3ca333004, committed 2021-03-03
- Comitter:
- hyhoon
- Date:
- Wed Mar 03 10:56:25 2021 +0000
- Parent:
- 225:9c0becc196ba
- Child:
- 227:699c3e572283
- Commit message:
- 20210305;
Changed in this revision
--- a/CAN/function_CAN.cpp Thu Feb 04 05:43:47 2021 +0000 +++ b/CAN/function_CAN.cpp Wed Mar 03 10:56:25 2021 +0000 @@ -126,6 +126,7 @@ CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]); if (CONTROL_MODE == 22) { //MODE_FIND_HOME FLAG_FIND_HOME = true; + FINDHOME_STAGE = FINDHOME_INIT; } break; } @@ -290,12 +291,12 @@ 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); + 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*10.0f)); - spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f)); + spi_eeprom_write(RID_VALVE_CNETER, VALVE_CENTER); + spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS); + spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS); break; } @@ -545,10 +546,6 @@ break; } - case CRX_ASK_DDV_VALVE_DEADZONE: { - CAN_TX_DDV_VALVE_DEADZONE(); - break; - } case CRX_LOW_REF: { REFERENCE_MODE = msg.data[1]; @@ -586,7 +583,7 @@ break; } case CRX_SET_HOMEPOS_OFFSET: { - HOMEPOS_OFFSET = (int16_t) (msg.data[1] | msg.data[2] << 8); + HOMEPOS_OFFSET = (int16_t)(msg.data[1] | msg.data[2] << 8); spi_eeprom_write(RID_HOMEPOS_OFFSET, (int16_t) HOMEPOS_OFFSET); break; } @@ -619,86 +616,6 @@ case CRX_SET_ROM: { break; } - case CRX_SET_VALVE_CENTER_OFFSET: { - VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f; - VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET; - spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER); - - break; - } - case CRX_SET_VALVE_DZ_MINUS_OFFSET: { - VALVE_DZ_MINUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f; - VALVE_DEADZONE_MINUS = VALVE_DEADZONE_MINUS + VALVE_DZ_MINUS_OFFSET; - spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS*10.0f)); - - break; - } - case CRX_SET_VALVE_DZ_PLUS_OFFSET: { - VALVE_DZ_PLUS_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f; - VALVE_DEADZONE_PLUS = VALVE_DEADZONE_PLUS + VALVE_DZ_PLUS_OFFSET; - spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS*10.0f)); - break; - } - case CRX_SET_PID_GAIN_OPP: { - 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_MAX_MIN_POS: { - CAN_TX_DDV_VALVE_MAX_MIN_POS(); - break; - } - case CRX_DELAY_TEST: { - flag_delay_test = 1; - break; - } - case CRX_SET_NN_CONTROL_FLAG: { - NN_Control_Flag = (int16_t) msg.data[1]; - CONTROL_UTILITY_MODE = 1; - break; - } - case CRX_SET_FREQ_TEST: { - cnt_freq_test = 0; - CONTROL_UTILITY_MODE = 34; - break; - } - - case CRX_ASK_BUFFER: { - cnt_buffer = (int16_t) (msg.data[1] | msg.data[2] << 8); - CAN_TX_BUFFER(cnt_buffer); - break; - } - - case CRX_SET_STEP_TEST: { - cnt_step_test = 0; - CONTROL_UTILITY_MODE = 37; - break; - } - - case CRX_SET_CHANGE_EVERY_REFERNCE: { - flag_every_reference = 1; - break; - } default: break; @@ -721,8 +638,8 @@ int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8); if((OPERATING_MODE&0b001)==0) { // Rotary Actuator - pos.ref = (double)temp_pos * 1.0f; - vel.ref = (double)temp_vel * 10.0f; + pos.ref = (double)temp_pos * 10.0f; + vel.ref = (double)temp_vel * 256.0f; } else { //Linear Actuator pos.ref = (double)temp_pos * 10.0f; vel.ref = (double)temp_vel * 256.0f; @@ -733,35 +650,35 @@ torq_dot.sen = torq.sen-torq_sen_past; torq.ref_old = torq.ref; torq_sen_past = torq.sen; - - if(CAN_FREQ == -1) { - // Position, Velocity, and Torque (ID:1200) - if (flag_data_request[0] == HIGH) { - if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator - if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f)); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } - } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator - if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); + + if(CAN_FREQ == -1) { + // Position, Velocity, and Torque (ID:1200) + if (flag_data_request[0] == HIGH) { + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator + if (SENSING_MODE == 0) { + CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen*10.0f)); + } else if (SENSING_MODE == 1) { + CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*10.0f), (int16_t) ((pres_B.sen)*10.0f)); + } + } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator + if (SENSING_MODE == 0) { + CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); + } else if (SENSING_MODE == 1) { + CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*10.0f), (int16_t) ((pres_B.sen)*10.0f)); + } } } - } - - // ID:1300 - if (flag_data_request[1] == HIGH) { - CAN_TX_TORQUE((int16_t) 7); //1300 - } - - // Reference Current, Current Current (ID:1700) - if (flag_data_request[2] == HIGH) { - - CAN_TX_SOMETHING((int16_t) (I_REF_fil / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (0), (int16_t) (0)); - //double t_value = 0.0f; + + // ID:1300 + if (flag_data_request[1] == HIGH) { + CAN_TX_TORQUE((int16_t) (pos.ref/10.0f)); //1300 + } + + // Reference Current, Current Current (ID:1700) + if (flag_data_request[2] == HIGH) { + + CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t)HOMEPOS_OFFSET, (int16_t) (I_ERR / mA_PER_pulse)); + //double t_value = 0.0f; // if(value>=(float) VALVE_CENTER) { // t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); // } else { @@ -773,29 +690,29 @@ // } else { // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); // } -// -// +// +// // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 - } - - //If it doesn't rest, below can can not work. - for (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) { - ; + } + + //If it doesn't rest, below can can not work. + for (int can_rest2 = 0; can_rest2 < 10000; can_rest2++) { + ; + } + + // ID:1500 + if (flag_data_request[3] == HIGH) { + //PWM + CAN_TX_PWM((int16_t) (torq.ref)); //1500 + } + + // ID:1600 + if (flag_data_request[4] == HIGH) { + //valve position + CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600 + } + } - - // ID:1500 - if (flag_data_request[3] == HIGH) { - //PWM - CAN_TX_PWM((int16_t) (torq.ref)); //1500 - } - - // ID:1600 - if (flag_data_request[4] == HIGH) { - //valve position - CAN_TX_VALVE_POSITION((int16_t) (a_hat*0.0001f), (int16_t) 0, (int16_t) 0, (int16_t) 0); //1600 - } - - } } else if(address==CID_RX_REF_VALVE_POS) { @@ -829,7 +746,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 7; temp_msg.data[0] = (uint8_t) CTX_SEND_INFO; temp_msg.data[1] = (uint8_t) BNO; temp_msg.data[2] = (uint8_t) CAN_FREQ; @@ -847,7 +764,7 @@ temp_msg.id = CID_TX_INFO; //temp_msg.len = 2; - temp_msg.len = 8; + temp_msg.len = 2; temp_msg.data[0] = (uint8_t) CTX_SEND_BNO; temp_msg.data[1] = (uint8_t) BNO; @@ -859,7 +776,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_OPERATING_MODE; temp_msg.data[1] = (uint8_t) OPERATING_MODE; temp_msg.data[2] = (uint8_t) SENSING_MODE; @@ -874,7 +791,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_CAN_FREQ; temp_msg.data[1] = (uint8_t) CAN_FREQ; temp_msg.data[2] = (uint8_t) (CAN_FREQ >> 8); @@ -887,7 +804,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 2; temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE; temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE; @@ -899,7 +816,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_JOINT_ENC_DIR; temp_msg.data[1] = (uint8_t) DIR_JOINT_ENC; temp_msg.data[2] = (uint8_t) (DIR_JOINT_ENC >> 8); @@ -912,7 +829,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DIR; temp_msg.data[1] = (uint8_t) DIR_VALVE; temp_msg.data[2] = (uint8_t) (DIR_VALVE >> 8); @@ -925,7 +842,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_ENC_DIR; temp_msg.data[1] = (uint8_t) DIR_VALVE_ENC; temp_msg.data[2] = (uint8_t) (DIR_VALVE_ENC >> 8); @@ -940,7 +857,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_SUPPLY; temp_msg.data[1] = (uint8_t) (send_voltage_supply); temp_msg.data[2] = (uint8_t) (send_voltage_supply >> 8); @@ -955,7 +872,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_VOLTAGE_VALVE; temp_msg.data[1] = (uint8_t) send_voltage_valve; temp_msg.data[2] = (uint8_t) (send_voltage_valve >> 8); @@ -1010,14 +927,14 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 7; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE; temp_msg.data[1] = (uint8_t) VALVE_CENTER; temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8); - temp_msg.data[3] = (uint8_t) (int) (VALVE_DEADZONE_PLUS); - temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8); - temp_msg.data[5] = (uint8_t) (int) (VALVE_DEADZONE_MINUS); - temp_msg.data[6] = (uint8_t) ((int) (VALVE_DEADZONE_MINUS) >> 8); + temp_msg.data[3] = (uint8_t) VALVE_DEADZONE_PLUS; + temp_msg.data[4] = (uint8_t) (VALVE_DEADZONE_PLUS >> 8); + temp_msg.data[5] = (uint8_t) VALVE_DEADZONE_MINUS; + temp_msg.data[6] = (uint8_t) (VALVE_DEADZONE_MINUS >> 8); can.write(temp_msg); } @@ -1027,7 +944,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_VELOCITY_COMP_GAIN; temp_msg.data[1] = (uint8_t) VELOCITY_COMP_GAIN; temp_msg.data[2] = (uint8_t) (VELOCITY_COMP_GAIN >> 8); @@ -1040,7 +957,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_COMPLIANCE_GAIN; temp_msg.data[1] = (uint8_t) COMPLIANCE_GAIN; temp_msg.data[2] = (uint8_t) (COMPLIANCE_GAIN >> 8); @@ -1053,7 +970,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_FF; temp_msg.data[1] = (uint8_t) VALVE_FF; temp_msg.data[2] = (uint8_t) (VALVE_FF >> 8); @@ -1066,7 +983,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_BULK_MODULUS; temp_msg.data[1] = (uint8_t) BULK_MODULUS; temp_msg.data[2] = (uint8_t) (BULK_MODULUS >> 8); @@ -1079,7 +996,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_CHAMBER_VOLUME; temp_msg.data[1] = (uint8_t) CHAMBER_VOLUME_A; temp_msg.data[2] = (uint8_t) (CHAMBER_VOLUME_A >> 8); @@ -1094,7 +1011,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_PISTON_AREA; temp_msg.data[1] = (uint8_t) PISTON_AREA_A; temp_msg.data[2] = (uint8_t) (PISTON_AREA_A >> 8); @@ -1109,7 +1026,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_PRES; temp_msg.data[1] = (uint8_t) PRES_SUPPLY; temp_msg.data[2] = (uint8_t) (PRES_SUPPLY >> 8); @@ -1124,7 +1041,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_LIMIT; temp_msg.data[1] = (uint8_t) ENC_LIMIT_MINUS; temp_msg.data[2] = (uint8_t) (ENC_LIMIT_MINUS >> 8); @@ -1139,7 +1056,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_STROKE; temp_msg.data[1] = (uint8_t) STROKE; temp_msg.data[2] = (uint8_t) (STROKE >> 8); @@ -1152,7 +1069,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT; temp_msg.data[1] = (uint8_t) VALVE_MIN_POS; temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8); @@ -1167,7 +1084,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION; int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION); temp_msg.data[1] = (uint8_t) temp_enc_pulse_per_position; @@ -1181,7 +1098,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_TORQUE_SENSOR_PULSE_PER_TORQUE; int temp_torque_sensor_pulse_per_torque = (int) (TORQUE_SENSOR_PULSE_PER_TORQUE * 10000.0f); temp_msg.data[1] = (uint8_t) temp_torque_sensor_pulse_per_torque; @@ -1195,7 +1112,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_PRES_SENSOR_PULSE_PER_BAR; temp_msg.data[1] = (uint8_t) (int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f); temp_msg.data[2] = (uint8_t) ((int) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f) >> 8); @@ -1212,7 +1129,7 @@ send_friction = (int16_t) (FRICTION * 10.0f); temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_FRICTION; temp_msg.data[1] = (uint8_t) send_friction; temp_msg.data[2] = (uint8_t) (send_friction >> 8); @@ -1225,7 +1142,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 6; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_PLUS; temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[0] * 50.0f); temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[2] * 50.0f); @@ -1236,36 +1153,13 @@ can.write(temp_msg); } -void CAN_TX_DDV_VALVE_DEADZONE(void) -{ - CANMessage temp_msg; - float temp_valve_deadzone_minus = 0.0f; - float temp_valve_deadzone_plus = 0.0f; - float temp_ddv_center = 0.0f; - - temp_valve_deadzone_plus = (double)VALVE_DEADZONE_PLUS; - temp_valve_deadzone_minus = (double)VALVE_DEADZONE_MINUS; - temp_ddv_center = (double)VALVE_CENTER; - - temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; - temp_msg.data[0] = (uint8_t) CTX_SEND_DDV_VALVE_DEADZONE; - temp_msg.data[1] = (uint8_t) temp_valve_deadzone_minus; - temp_msg.data[2] = (uint8_t) ((int) (temp_valve_deadzone_minus) >> 8); - temp_msg.data[3] = (uint8_t) (temp_valve_deadzone_plus); - temp_msg.data[4] = (uint8_t) ((int) (temp_valve_deadzone_plus) >> 8); - temp_msg.data[5] = (uint8_t) (temp_ddv_center); - temp_msg.data[6] = (uint8_t) ((int) (temp_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 = 8; + temp_msg.len = 6; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_GAIN_MINUS; temp_msg.data[1] = (uint8_t) (VALVE_GAIN_LPM_PER_V[1] * 50.0f); temp_msg.data[2] = (uint8_t) (VALVE_GAIN_LPM_PER_V[3] * 50.0f); @@ -1281,7 +1175,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 6; temp_msg.data[0] = (uint8_t) CTX_SEND_REFENCE_MODE; temp_msg.data[1] = (uint8_t) REFERENCE_MODE; temp_msg.data[2] = (uint8_t) (int) (REFERENCE_FREQ * 100.0f); @@ -1296,10 +1190,10 @@ { CANMessage temp_msg; int16_t send_homepos_offset; - send_homepos_offset = (int16_t) (HOMEPOS_OFFSET); + send_homepos_offset = (int16_t)HOMEPOS_OFFSET; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_OFFSET; temp_msg.data[1] = (uint8_t) send_homepos_offset; temp_msg.data[2] = (uint8_t) (send_homepos_offset >> 8); @@ -1314,7 +1208,7 @@ send_homepos_valve_opening = (int16_t) (HOMEPOS_VALVE_OPENING); temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_HOMEPOS_VALVE_OPENING; temp_msg.data[1] = (uint8_t) send_homepos_valve_opening; temp_msg.data[2] = (uint8_t) (send_homepos_valve_opening >> 8); @@ -1337,7 +1231,7 @@ PWM_VALVE_ID = ID_index_array[canindex] * 1000; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_VALVE_PWM_VS_VALVE_POS; temp_msg.data[1] = (uint8_t) PWM_VALVE_ID; temp_msg.data[2] = (uint8_t) (PWM_VALVE_ID >> 8); @@ -1362,7 +1256,7 @@ } temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 7; temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE; temp_msg.data[1] = (uint8_t) temp_valve_pos; temp_msg.data[2] = (uint8_t) (temp_valve_pos >> 8); @@ -1381,7 +1275,7 @@ valve_pos_num = (int16_t) VALVE_POS_NUM; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_NUM; temp_msg.data[1] = (uint8_t) valve_pos_num; temp_msg.data[2] = (uint8_t) (valve_pos_num >> 8); @@ -1402,7 +1296,7 @@ temp_ddv_center = VALVE_CENTER; temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 7; temp_msg.data[0] = (uint8_t) CTX_VALVE_MAX_MIN_POS; temp_msg.data[1] = (uint8_t) temp_valve_max_pos; temp_msg.data[2] = (uint8_t) ((int) (temp_valve_max_pos) >> 8); @@ -1423,7 +1317,7 @@ send_ref_array = (int16_t) (ref_array[t_cnt_buffer]); temp_msg.id = CID_TX_INFO; - temp_msg.len = 8; + temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_BUFFER; temp_msg.data[1] = (uint8_t) send_pos_array; temp_msg.data[2] = (uint8_t) (send_pos_array >> 8); @@ -1443,7 +1337,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_POSITION; - temp_msg.len = 8; + temp_msg.len = 6; temp_msg.data[0] = (uint8_t) t_pos; temp_msg.data[1] = (uint8_t) (t_pos >> 8); temp_msg.data[2] = (uint8_t) t_vel; @@ -1474,14 +1368,14 @@ } -void CAN_TX_TORQUE(int16_t t_valve_pos) +void CAN_TX_TORQUE(int16_t t_torque) { CANMessage temp_msg; temp_msg.id = CID_TX_TORQUE; - temp_msg.len = 8; - temp_msg.data[0] = (uint8_t) t_valve_pos; - temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8); + temp_msg.len = 2; + temp_msg.data[0] = (uint8_t) t_torque; + temp_msg.data[1] = (uint8_t) (t_torque >> 8); can.write(temp_msg); } @@ -1491,7 +1385,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_PRES; - temp_msg.len = 8; + temp_msg.len = 4; temp_msg.data[0] = (uint8_t) t_pres_a; temp_msg.data[1] = (uint8_t) (t_pres_a >> 8); temp_msg.data[2] = (uint8_t) t_pres_b; @@ -1505,7 +1399,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_VOUT; - temp_msg.len = 8; + temp_msg.len = 2; temp_msg.data[0] = (uint8_t) t_pwm; temp_msg.data[1] = (uint8_t) (t_pwm >> 8);
--- a/CAN/function_CAN.h Thu Feb 04 05:43:47 2021 +0000 +++ b/CAN/function_CAN.h Wed Mar 03 10:56:25 2021 +0000 @@ -190,7 +190,6 @@ void CAN_TX_HOMEPOS_OFFSET(void); void CAN_TX_HOMPOS_VALVE_OPENING(void); void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex); -void CAN_TX_DDV_VALVE_DEADZONE(void); void CAN_TX_VALVE_POS_VS_FLOWRATE(int8_t canindex); void CAN_TX_VALVE_POS_NUM(void); void CAN_TX_DDV_VALVE_MAX_MIN_POS(void);
--- a/SPI_EEP_ENC/SPI_EEP_ENC.cpp Thu Feb 04 05:43:47 2021 +0000 +++ b/SPI_EEP_ENC/SPI_EEP_ENC.cpp Wed Mar 03 10:56:25 2021 +0000 @@ -68,7 +68,7 @@ enc_cs = 1; // write MDR1 -> 0b10 -> 2-byte counter mode - for(i=0;i<100;i++); + for(i=0;i<10000;i++); enc_cs = 0; temp = enc.write(0b10010000); // WR + MDR1 //temp = enc.write(0b00000010); // 2 byte mode
--- a/function_utilities/function_utilities.cpp Thu Feb 04 05:43:47 2021 +0000 +++ b/function_utilities/function_utilities.cpp Wed Mar 03 10:56:25 2021 +0000 @@ -54,8 +54,8 @@ //float I_GAIN_JOINT_TORQUE_OPP = 0.0; //float D_GAIN_JOINT_TORQUE_OPP = 0.0; -float VALVE_DEADZONE_PLUS; -float VALVE_DEADZONE_MINUS; +int16_t VALVE_DEADZONE_PLUS; +int16_t VALVE_DEADZONE_MINUS; int16_t VELOCITY_COMP_GAIN; int16_t COMPLIANCE_GAIN; @@ -101,7 +101,7 @@ float PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f; float PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f; -int HOMEPOS_OFFSET; +int16_t HOMEPOS_OFFSET; int HOMEPOS_VALVE_OPENING; float FRICTION; @@ -124,7 +124,7 @@ int REF_MOVE_TIME_5k; int INIT_REF_PWM; int INIT_REF_VALVE_POS; -int INIT_REF_POS; +int32_t INIT_REF_POS; int INIT_REF_VEL; int INIT_REF_TORQUE; int INIT_REF_PRES_DIFF; @@ -301,6 +301,7 @@ // Current Control Variables double I_REF = 0.0f; double I_REF_fil = 0.0f; +double I_REF_fil_DZ = 0.0f; double I_ERR = 0.0f; double I_ERR_INT = 0.0f; double I_REF_fil_old = 0.0f; @@ -399,7 +400,7 @@ void ROM_CALL_DATA(void) { BNO = spi_eeprom_read(RID_BNO); - BNO = 2; + BNO = 11; OPERATING_MODE = spi_eeprom_read(RID_OPERATING_MODE); SENSING_MODE = spi_eeprom_read(RID_SENSING_MODE); CURRENT_CONTROL_MODE = spi_eeprom_read(RID_CURRENT_CONTROL_MODE); @@ -419,8 +420,8 @@ 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 = (float) (spi_eeprom_read( RID_VALVE_DEADZONE_PLUS)) * 0.1f; - VALVE_DEADZONE_MINUS = (float) (spi_eeprom_read(RID_VALVE_DEADZONE_MINUS)) * 0.1f; + 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);
--- a/main.cpp Thu Feb 04 05:43:47 2021 +0000 +++ b/main.cpp Wed Mar 03 10:56:25 2021 +0000 @@ -90,6 +90,8 @@ extern int CID_TX_VALVE_POSITION; extern int CID_TX_SOMETHING; +float pres_A_new = 0.0f; +float pres_B_new = 0.0f; /******************************************************************************* * REFERENCE MODE @@ -196,27 +198,39 @@ /********************************* *** Initialization *********************************/ - + HAL_Init(); SystemClock_Config(); - + LED = 0; pc.baud(9600); // 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(); +// 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 + enc_cs = 1; eeprom.format(8,3); eeprom.frequency(5000000); //5M enc.format(8,0); enc.frequency(5000000); //5M + enc_cs = 0; make_delay(); + + // spi _ enc + spi_enc_set_init(); + make_delay(); + + ////// bno rom +// spi_eeprom_write(RID_BNO, (int16_t)1); +// make_delay(); + //////// + // rom ROM_CALL_DATA(); make_delay(); @@ -230,34 +244,41 @@ TIM4->CR1 ^= TIM_CR1_UDIS; make_delay(); - // TMR3 init - Init_TMR3(); - TIM3->CR1 ^= TIM_CR1_UDIS; - make_delay(); - // CAN can.attach(&CAN_RX_HANDLER); CAN_ID_INIT(); make_delay(); + //can.reset(); + can.filter(msg.id, 0xFFFFF000, CANStandard); + + // TMR3 init + Init_TMR3(); + TIM3->CR1 ^= TIM_CR1_UDIS; + make_delay(); + + + //Timer priority NVIC_SetPriority(TIM3_IRQn, 2); NVIC_SetPriority(TIM4_IRQn, 3); - //can.reset(); - can.filter(msg.id, 0xFFFFF000, CANStandard); + - // spi _ enc - spi_enc_set_init(); - make_delay(); + //DAC init if (SENSING_MODE == 0) { dac_1 = TORQUE_VREF / 3.3f; dac_2 = 0.0f; } else if (SENSING_MODE == 1) { - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; + if (DIR_VALVE_ENC > 0) { + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + } else { + dac_1 = PRES_B_VREF / 3.3f; + dac_2 = PRES_A_VREF / 3.3f; + } } make_delay(); @@ -281,7 +302,7 @@ // timer_while ++; //i2c for SW valve - if(OPERATING_MODE == 5){ + if(OPERATING_MODE == 5) { read_field(i2c_slave_addr1); if(DIR_VALVE_ENC < 0) value = 1023 - value; } @@ -408,7 +429,6 @@ extern "C" void TIM4_IRQHandler(void) { if (TIM4->SR & TIM_SR_UIF ) { - /******************************************************* *** Sensor Read & Data Handling ********************************************************/ @@ -426,10 +446,16 @@ pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new; torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE; - // Pressure Sensing (0~210)bar ============================================= + // Pressure Sensing (0~210)bar ============================================= } else if (SENSING_MODE == 1) { - float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL); - float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL); + + if (DIR_VALVE_ENC > 0) { + pres_A_new = (((float)ADC1->DR) - PRES_A_NULL); + pres_B_new = (((float)ADC2->DR) - PRES_B_NULL); + } else { + pres_A_new = (((float)ADC2->DR) - PRES_A_NULL); + pres_B_new = (((float)ADC1->DR) - PRES_B_NULL); + } double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 200Hz pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new; pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new; @@ -466,6 +492,7 @@ { if (TIM3->SR & TIM_SR_UIF ) { + if (((OPERATING_MODE&0b110)>>1) == 0) { K_v = 0.4f; // Moog (LPM >> mA) , 100bar mV_PER_mA = 500.0f; // 5000mV/10mA @@ -548,83 +575,6 @@ break; } -// case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: { -// if (TMR3_COUNT_DEADZONE == 0) { -// if (pos_plus_end == pos_minus_end) need_enc_init = true; -// else temp_time = 0; -// } -// if (need_enc_init) { -// if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) { -// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; -// pos_plus_end = pos.sen; -// } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) { -// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; -// pos_minus_end = pos.sen; -// } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false; -// temp_time = TMR_FREQ_5k; -// } -// -// if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) { -// V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); -// VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0; -// -// } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) { -// V_out = 0; -// CUR_VELOCITY_sum += CUR_VELOCITY; -// } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) { -// if (CUR_VELOCITY_sum == 0) DZ_dir = 1; -// else if (CUR_VELOCITY_sum > 0) DZ_dir = 1; -// else if (CUR_VELOCITY_sum < 0) DZ_dir = -1; -// else DZ_temp_cnt2 = DZ_end; -// CUR_VELOCITY_sum = 0; -// } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) { -// if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end; -// -// // Position of Dead Zone -// // (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0) -// // | / | / |/ -// // | ______/ ___|___/ ______/| -// // |/ / | / | -// // /| / | / | -// // 0V 0V 0V -// -// if (DZ_temp_cnt2 < DZ_end) { -// if (TMR3_COUNT_DEADZONE % 20 != 0) { -// CUR_VELOCITY_sum += CUR_VELOCITY; -// } else { -// V_out -= DZ_dir; -// if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++; -// CUR_VELOCITY_sum = 0; -// } -// if (DZ_temp_cnt == 5) { -// if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out; -// else VALVE_DEADZONE_PLUS = (int16_t) V_out; -// DZ_dir = -DZ_dir; -// DZ_temp_cnt = 0; -// DZ_temp_cnt2++; -// } -// } else { -// TMR3_COUNT_DEADZONE = -1; -// VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2; -// if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) { -// VALVE_DEADZONE_PLUS = VALVE_CENTER; -// VALVE_DEADZONE_MINUS = VALVE_CENTER; -// } -// V_out = 0; -// -// -// -// //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS); -// //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS); -// -// CONTROL_MODE = MODE_NO_ACT; -// DZ_temp_cnt2 = 0; -// } -// } -// TMR3_COUNT_DEADZONE++; -// break; -// } - case MODE_FIND_HOME: { if (FINDHOME_STAGE == FINDHOME_INIT) { cnt_findhome = 0; @@ -648,15 +598,19 @@ } if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec - if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f; - else pos.ref = pos.ref - 12.0f; + if (HOMEPOS_OFFSET > 0) { + pos.ref = pos.ref + 12.0f; + } else { + pos.ref = pos.ref - 12.0f; + } + CONTROL_MODE = MODE_JOINT_CONTROL; alpha_trans = 0.0f; } else { - ENC_SET(HOMEPOS_OFFSET); - INIT_REF_POS = HOMEPOS_OFFSET; + ENC_SET((int32_t)((int32_t)HOMEPOS_OFFSET*10)); + INIT_REF_POS = (int32_t)((int32_t)HOMEPOS_OFFSET*10); REF_POSITION = 0; REF_VELOCITY = 0; FINDHOME_POSITION = 0; @@ -683,6 +637,7 @@ pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] + CONTROL_MODE = MODE_JOINT_CONTROL; if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { @@ -785,7 +740,7 @@ // } // if (fl_temp_cnt2 == 100) { // -// +// // // //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0f)); // cur_vel_sum = 0; @@ -829,8 +784,13 @@ if (PRES_B_VREF > 3.3f) PRES_B_VREF = 3.3f; if (PRES_B_VREF < 0.0f) PRES_B_VREF = 0.0f; - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; + if (DIR_VALVE_ENC > 0) { + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + } else { + dac_1 = PRES_B_VREF / 3.3f; + dac_2 = PRES_A_VREF / 3.3f; + } } } else { CONTROL_UTILITY_MODE = MODE_NO_ACT; @@ -843,8 +803,13 @@ spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0f)); spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0f)); - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; + if (DIR_VALVE_ENC > 0) { + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + } else { + dac_1 = PRES_B_VREF / 3.3f; + dac_2 = PRES_A_VREF / 3.3f; + } } TMR3_COUNT_PRES_NULL++; break; @@ -874,7 +839,7 @@ // CUR_PRES_A_mean = 0; // CUR_PRES_B_mean = 0; // -// +// // // //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f)); // //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f)); @@ -1156,7 +1121,7 @@ first_check = 0; VALVE_DEADZONE_MINUS = (float) FIRST_DZ; VALVE_DEADZONE_PLUS = (float) SECOND_DZ; - + spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER); spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS); spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS); @@ -1359,7 +1324,7 @@ } else if (CURRENT_CONTROL_MODE == 0) { //PWM V_out = valve_pos.ref; } else { - I_REF = valve_pos.ref * 0.001f; + I_REF = (double)(valve_pos.ref * (float)mA_PER_pulse); } break; } @@ -1573,7 +1538,14 @@ double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF; - I_ERR = I_REF_fil - cur.sen; + if (FLAG_VALVE_DEADZONE) { + if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA + else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA + else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA + } else { + I_REF_fil_DZ = I_REF_fil; + } + I_ERR = I_REF_fil_DZ - (double)cur.sen; I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f; @@ -1596,10 +1568,10 @@ double FF_gain = 1.0f; VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT; - I_REF_fil_diff = I_REF_fil - I_REF_fil_old; - I_REF_fil_old = I_REF_fil; + I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old; + I_REF_fil_old = I_REF_fil_DZ; // VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV - VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil); // Unit : mV + VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV double Ka = 3.0f / KP_I; @@ -1623,19 +1595,9 @@ //////////////////////////////////////////////////////////////////////////// ///////////////// Dead Zone Cancellation & Linearization ////////////////// //////////////////////////////////////////////////////////////////////////// - // Dead Zone Cancellation (Mechanical Valve dead-zone) - if (FLAG_VALVE_DEADZONE) { - if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV - else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV - - VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV - - } else { - VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW; - } // Output Voltage Linearization - double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV + double CUR_PWM_nonlin = (double)VALVE_PWM_RAW; // Unit : mV double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000 // Dead Zone Cancellation (Electrical dead-zone) @@ -1688,65 +1650,64 @@ TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v); TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w); - - if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { - // Position, Velocity, and Torque (ID:1200) - if (flag_data_request[0] == HIGH) { - if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator - if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f)); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } - } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator - if (SENSING_MODE == 0) { - CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); - } else if (SENSING_MODE == 1) { - CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); - } - } - - } + if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { + // Position, Velocity, and Torque (ID:1200) + if (flag_data_request[0] == HIGH) { + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator + if (SENSING_MODE == 0) { + CAN_TX_POSITION_FT((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) (torq.sen*10.0f)); + } else if (SENSING_MODE == 1) { + CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen), (int16_t) (vel.sen/10.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); + } + } else if ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator + if (SENSING_MODE == 0) { + CAN_TX_POSITION_FT((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) (torq.sen * 10.0f * (float)(TORQUE_SENSOR_PULSE_PER_TORQUE))); + } else if (SENSING_MODE == 1) { + CAN_TX_POSITION_PRESSURE((int16_t) (pos.sen/10.0f), (int16_t) (vel.sen/256.0f), (int16_t) ((pres_A.sen)*5.0f), (int16_t) ((pres_B.sen)*5.0f)); + } + } - + } + + // ID:1300 if (flag_data_request[1] == HIGH) { CAN_TX_TORQUE((int16_t) 7); //1300 } - // Reference Current, Current Current (ID:1700) + // Reference Current, Current Current (ID:1700) if (flag_data_request[2] == HIGH) { - - CAN_TX_SOMETHING((int16_t) (I_REF_fil / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (0), (int16_t) (0)); + + CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (HOMEPOS_OFFSET), (int16_t) (0)); //double t_value = 0.0f; - // if(value>=(float) VALVE_CENTER) { - // t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - // } else { - // t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - // } - // double t_value_ref = 0.0f; - // if(valve_pos.ref>=(float) VALVE_CENTER) { - // t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - // } else { - // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - // } - // - // - // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 + // if(value>=(float) VALVE_CENTER) { + // t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + // } else { + // t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + // } + // double t_value_ref = 0.0f; + // if(valve_pos.ref>=(float) VALVE_CENTER) { + // t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + // } else { + // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + // } + // + // + // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 } //If it doesn't rest, below can can not work. for (can_rest = 0; can_rest < 10000; can_rest++) { ; } - + // ID:1500 if (flag_data_request[3] == HIGH) { //PWM CAN_TX_PWM((int16_t) (torq.ref)); //1500 } - + // ID:1600 if (flag_data_request[4] == HIGH) { //valve position
--- a/setting.h Thu Feb 04 05:43:47 2021 +0000 +++ b/setting.h Wed Mar 03 10:56:25 2021 +0000 @@ -148,8 +148,8 @@ //extern float I_GAIN_JOINT_TORQUE_OPP; //extern float D_GAIN_JOINT_TORQUE_OPP; -extern float VALVE_DEADZONE_PLUS; -extern float VALVE_DEADZONE_MINUS; +extern int16_t VALVE_DEADZONE_PLUS; +extern int16_t VALVE_DEADZONE_MINUS; extern int16_t VELOCITY_COMP_GAIN; extern int16_t COMPLIANCE_GAIN; @@ -196,7 +196,7 @@ extern float PRES_SENSOR_A_PULSE_PER_BAR; extern float PRES_SENSOR_B_PULSE_PER_BAR; -extern int HOMEPOS_OFFSET; +extern int16_t HOMEPOS_OFFSET; extern int HOMEPOS_VALVE_OPENING; extern float FRICTION; @@ -219,7 +219,7 @@ extern int REF_MOVE_TIME_5k; extern int INIT_REF_PWM; extern int INIT_REF_VALVE_POS; -extern int INIT_REF_POS; +extern int32_t INIT_REF_POS; extern int INIT_REF_VEL; extern int INIT_REF_TORQUE; extern int INIT_REF_PRES_DIFF; @@ -276,8 +276,6 @@ extern float VALVE_CENTER_OFFSET; extern float VALVE_DZ_MINUS_OFFSET; extern float VALVE_DZ_PLUS_OFFSET; -extern float VALVE_DEADZONE_MINUS; -extern float VALVE_DEADZONE_PLUS; extern int TMR3_COUNT_FINDHOME; extern int TMR3_COUNT_FLOWRATE; @@ -392,6 +390,7 @@ // Current Control Variables extern double I_REF; extern double I_REF_fil; +extern double I_REF_fil_DZ; extern double I_ERR; extern double I_ERR_INT; extern double I_REF_fil_old;