Sungwoo Kim
For LIGHT
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++) {
;
}