Sungwoo Kim
/
HydraulicControlBoard_LIGHT
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 67:cfc88fd0591e, committed 2020-06-03
- Comitter:
- Lightvalve
- Date:
- Wed Jun 03 08:46:45 2020 +0000
- Parent:
- 66:d385a08132d7
- Commit message:
- new ankle
Changed in this revision
--- a/CAN/function_CAN.cpp Thu May 21 02:08:49 2020 +0000
+++ b/CAN/function_CAN.cpp Wed Jun 03 08:46:45 2020 +0000
@@ -698,7 +698,7 @@
void CAN_RX_HANDLER()
{
-
+ CAN_TX_PWM((int16_t) 20); //1500
can.read(msg);
unsigned int address = msg.id;
if(address==CID_RX_CMD){
@@ -736,6 +736,7 @@
valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f;
}
}
+
} else if(address==CID_RX_REF_PWM){
int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8);
Vout.ref = (double) temp_ref_pwm;
--- a/function_utilities/function_utilities.cpp Thu May 21 02:08:49 2020 +0000
+++ b/function_utilities/function_utilities.cpp Wed Jun 03 08:46:45 2020 +0000
@@ -376,7 +376,7 @@
{
FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector
if (!writer.ready()) writer.open();
-// BNO = 10;
+// BNO = 11;
writer.write(RID_BNO,(int) BNO); // write at address, 쓸때도 4byte씩 씀
writer.write(RID_OPERATING_MODE,(int) OPERATING_MODE);
writer.write(RID_SENSING_MODE, (int) SENSING_MODE);
@@ -457,9 +457,10 @@
void ROM_CALL_DATA(void)
{
BNO = flashReadInt(Rom_Sector, RID_BNO);
- //BNO = 11;
+// BNO = 11;
OPERATING_MODE = flashReadInt(Rom_Sector, RID_OPERATING_MODE);
SENSING_MODE = flashReadInt(Rom_Sector, RID_SENSING_MODE);
+ SENSING_MODE = 1;
CURRENT_CONTROL_MODE = flashReadInt(Rom_Sector, RID_CURRENT_CONTROL_MODE);
CURRENT_CONTROL_MODE = 1;
FLAG_VALVE_DEADZONE = flashReadInt(Rom_Sector, RID_FLAG_VALVE_DEADZONE);
@@ -505,9 +506,9 @@
//TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 0.41928f; //for ankle
// TORQUE_SENSOR_PULSE_PER_TORQUE = (float) 10000.0f/2048.0f; //for knee
PRES_SENSOR_A_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_A_PULSE_PER_BAR)) * 0.01f;
-// PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f / 200.0f;
+ PRES_SENSOR_A_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
PRES_SENSOR_B_PULSE_PER_BAR = (float) (flashReadInt(Rom_Sector, RID_PRES_SENSOR_B_PULSE_PER_BAR)) * 0.01f;
-// PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f / 200.0f;
+ PRES_SENSOR_B_PULSE_PER_BAR = 4096.0f * 946.0f / 3.3f / 300.0f / 210.0f;
FRICTION = (float) (flashReadInt(Rom_Sector, RID_FRICTION)) * 0.1f;
HOMEPOS_OFFSET = flashReadInt(Rom_Sector, RID_HOMEPOS_OFFSET);
HOMEPOS_VALVE_OPENING = flashReadInt(Rom_Sector, RID_HOMEPOS_VALVE_OPENING);
--- a/main.cpp Thu May 21 02:08:49 2020 +0000
+++ b/main.cpp Wed Jun 03 08:46:45 2020 +0000
@@ -186,8 +186,16 @@
// make_delay();
// CAN
+// can.reset();
+ CAN_ID_INIT();
+// can.filter(msg.id, 0xFFFFF000, CANStandard);
+// can.frequency(1000000); // set CAN bit rate to 1Mbps
+ can.filter(CID_RX_CMD, 0xFFF, CANStandard, 0);
+ can.filter(CID_RX_REF_POSITION, 0xFFF, CANStandard, 1);
+ can.filter(CID_RX_REF_VALVE_POS, 0xFFF, CANStandard, 2);
+ can.filter(CID_RX_REF_PWM, 0xFFF, CANStandard, 3);
can.attach(&CAN_RX_HANDLER);
- CAN_ID_INIT();
+
make_delay();
//Timer priority
@@ -195,8 +203,7 @@
//NVIC_SetPriority(TIM2_IRQn, 3);
NVIC_SetPriority(TIM4_IRQn, 3);
- //can.reset();
- can.filter(msg.id, 0xFFFFF000, CANStandard);
+
// spi _ enc
spi_enc_set_init();
@@ -425,7 +432,7 @@
//int raw_cur = ADC3->DR;
//while((ADC3->SR & 0b10));
float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*500.0f)); // f_cutoff : 500Hz
- float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA
+ float cur_new = ((float)ADC3->DR-2048.0f) / 4096.0f * 3.3f * 10.0f / 1.44f; // unit : mA
cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur);
//cur.sen = raw_cur;
@@ -500,36 +507,76 @@
}
case MODE_TORQUE_SENSOR_NULLING: {
- // DAC Voltage reference set
+
+ if(SENSING_MODE == 0){
+
+ // DAC Voltage reference set
+ if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
+ CUR_TORQUE_sum += torq.sen;
+
+ if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
+ CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f;
+ CUR_TORQUE_sum = 0;
+
+ TORQUE_VREF += 0.0003f * (0.0f - CUR_TORQUE_mean);
+
+ if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
+ if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f;
+
+ //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
+ dac_1 = TORQUE_VREF / 3.3f;
+ }
+ } else {
+ CONTROL_UTILITY_MODE = MODE_NO_ACT;
+ TMR3_COUNT_TORQUE_NULL = 0;
+ CUR_TORQUE_sum = 0;
+ CUR_TORQUE_mean = 0;
+
+ ROM_RESET_DATA();
+
+ dac_1 = TORQUE_VREF / 3.3f;
+ }
+ } else {
if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
- CUR_TORQUE_sum += torq.sen;
+ CUR_PRES_A_sum += pres_A.sen;
+ CUR_PRES_B_sum += pres_B.sen;
if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
- CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0f;
- CUR_TORQUE_sum = 0;
-
- TORQUE_VREF += 0.0003f * (0.0f - CUR_TORQUE_mean);
+ CUR_PRES_A_mean = CUR_PRES_A_sum / 10.0f;
+ CUR_PRES_B_mean = CUR_PRES_B_sum / 10.0f;
+ CUR_PRES_A_sum = 0;
+ CUR_PRES_B_sum = 0;
- if (TORQUE_VREF > 3.3f) TORQUE_VREF = 3.3f;
- if (TORQUE_VREF < 0.0f) TORQUE_VREF = 0.0f;
+ float VREF_NullingGain = 0.0003f;
+ PRES_A_VREF = PRES_A_VREF + VREF_NullingGain * CUR_PRES_A_mean;
+ PRES_B_VREF = PRES_B_VREF + VREF_NullingGain * CUR_PRES_B_mean;
- //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
- dac_1 = TORQUE_VREF / 3.3f;
+ if (PRES_A_VREF > 3.3f) PRES_A_VREF = 3.3f;
+ if (PRES_A_VREF < 0.0f) PRES_A_VREF = 0.0f;
+ 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;
}
} else {
CONTROL_UTILITY_MODE = MODE_NO_ACT;
TMR3_COUNT_TORQUE_NULL = 0;
- CUR_TORQUE_sum = 0;
- CUR_TORQUE_mean = 0;
+ CUR_PRES_A_sum = 0;
+ CUR_PRES_B_sum = 0;
+ CUR_PRES_A_mean = 0;
+ CUR_PRES_B_mean = 0;
ROM_RESET_DATA();
- dac_1 = TORQUE_VREF / 3.3f;
-
+ dac_1 = PRES_A_VREF / 3.3f;
+ dac_2 = PRES_B_VREF / 3.3f;
+ //pc.printf("nulling end");
}
- TMR3_COUNT_TORQUE_NULL++;
- break;
}
+ TMR3_COUNT_TORQUE_NULL++;
+ break;
+ }
// case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: {
// if (TMR3_COUNT_DEADZONE == 0) {
@@ -1604,17 +1651,17 @@
// Position, Velocity, and Torque (ID:1200)
if (flag_data_request[0] == HIGH) {
if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator
- if (SENSING_MODE == 0) {
+ //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 (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) {
+ //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));
- }
+ //} 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 (flag_data_request[1] == HIGH) {
@@ -1643,7 +1690,7 @@
CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar) //1400
}
- //If it doesn't rest, below can can not work.
+// If it doesn't rest, below can can not work.
for (can_rest = 0; can_rest < 10000; can_rest++) {
;
}