Kim GiJeong
eeprom_test
Diff: main.cpp
- Revision:
- 67:cfc88fd0591e
- Parent:
- 66:d385a08132d7
- Child:
- 68:135a2d087198
--- 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++) {
;
}