Sungwoo Kim
distribution-201229
Diff: main.cpp
- Revision:
- 15:bd0d12728506
- Parent:
- 14:8e7590227d22
- Child:
- 16:903b5a4433b4
--- a/main.cpp Thu Aug 29 07:38:00 2019 +0000
+++ b/main.cpp Fri Aug 30 02:26:11 2019 +0000
@@ -174,7 +174,8 @@
//DAC init
//dac_1 = PRES_A_VREF/3.3;
dac_1 = 0.0;
- dac_2 = PRES_B_VREF/3.3;
+ //dac_2 = PRES_B_VREF/3.3;
+ dac_2 = 0.0;
make_delay();
for (int i=0; i<100; i++) {
@@ -195,7 +196,7 @@
//pc.printf("Message received: %d\n", msg.data[0]);
//pc.printf("Message received: %d\n", 13);
//i2c
- //read_field(i2c_slave_addr1);
+ read_field(i2c_slave_addr1);
}
}
@@ -309,23 +310,29 @@
*** Sensor Read & Data Handling
********************************************************/
- if((CNT_TMR4%2)==0) {
-
+ if((CNT_TMR4%20)==0) {
+ //Using LoadCell
+// ADC1->CR2 |= 0x40000000; // adc _ 12bit
+// //while((ADC1->SR & 0b10));
+// double alpha_update_torque = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
+// double torque_new = ((double)ADC1->DR - PRES_A_NULL) / TORQUE_SENSOR_PULSE_PER_TORQUE + 1.0;
+// torq.sen = torq.sen*(1.0-alpha_update_torque)+torque_new*(alpha_update_torque);
+
//Pressure sensor A
ADC1->CR2 |= 0x40000000; // adc _ 12bit
//while((ADC1->SR & 0b10));
double alpha_update_pres_A = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
double pres_A_new = ((double)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR + 1.0;
pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A);
- //pres_A.sen = (double)ADC1->DR;
-/*
//Pressure sensor 1B
- //ADC2->CR2 |= 0x40000000; // adc _ 12bit
- while((ADC2->SR & 0b10));
+ ADC2->CR2 |= 0x40000000; // adc _ 12bit
+ //while((ADC2->SR & 0b10));
double alpha_update_pres_B = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0));
double pres_B_new = ((double)ADC2->DR - PRES_B_NULL) / PRES_SENSOR_B_PULSE_PER_BAR + 1.0;
pres_B.sen = pres_B.sen*(1.0-alpha_update_pres_B)+pres_B_new*(alpha_update_pres_B);
+ torq.sen = pres_A.sen * (double) PISTON_AREA_A - pres_B.sen * (double) PISTON_AREA_B;
+/*
//Current
ADC3->CR2 |= 0x40000000; // adc _ 12bit
// a1=ADC2->DR;
@@ -525,7 +532,7 @@
PWM_RAW_POS_FF = K_ff*Ref_Vel_Act/0.50;
// torque feedback
- // torq.err = Ref_Joint_Torq - CUR_TORQUE_NM;
+ // torq.err = torq.ref - torq.sen;
// torq.err_diff = torq.err - torq.err_old;
// torq.err_old = torq.err;
// torq.err_sum += torq.err;
@@ -533,9 +540,9 @@
// if (torq.err_sum<-1000) torq.err_sum = -1000;
// VALVE_PWM_RAW_TORQ = (double) P_GAIN_JOINT_TORQUE * torq.err + (double) I_GAIN_JOINT_TORQUE * torq.err_sum + (double) D_GAIN_JOINT_TORQUE * torq.err_diff;
// VALVE_PWM_RAW_TORQ = VALVE_PWM_RAW_TORQ * 0.01;
+
PWM_RAW_FORCE_FB = 0.0;
- // VALVE_PWM_RAW = VALVE_PWM_RAW_FF + VALVE_PWM_RAW_POS + (double) COMPLIANCE_GAIN * 0.01 * VALVE_PWM_RAW_TORQ;
V_out = PWM_RAW_POS_FF + PWM_RAW_POS_FB + PWM_RAW_FORCE_FB;
break;
@@ -566,7 +573,7 @@
// VALVE_POS_RAW_POS_FF = K_ff*Ref_Joint_Vel_Act/0.50;
// torque feedback
- // torq.err = Ref_Joint_Torq - CUR_TORQUE_NM;
+ // torq.err = torq.ref - torq.sen;
// torq.err_diff = torq.err - torq.err_old;
// torq.err_old = torq.err;
// torq.err_sum += torq.err;
@@ -658,7 +665,7 @@
if (pos.err_sum<-1000) pos.err_sum = -1000;
VALVE_PWM_RAW_POS = ((double) P_GAIN_JOINT_POSITION * pos.err + (double) I_GAIN_JOINT_POSITION * pos.err_sum + (double) D_GAIN_JOINT_POSITION * pos.err_diff) * 0.01;
- torq.err = torq.ref - (double) torq.sen;
+ torq.err = torq.ref - torq.sen;
torq.err_diff = torq.err - torq.err_old;
torq.err_old = torq.err;
torq.err_sum += torq.err;
@@ -777,11 +784,9 @@
double K_spring = 0.7;
double D_damper = 0.02;
// T_REF = K_spring * pos.err + D_damper * Joint_Vel_Err; // unit : Nm
-
- T_REF = torq.ref; // unit : Nm
// torque feedback
- torq.err = T_REF - (double) torq.sen;
+ torq.err = torq.ref - torq.sen;
// torq.err_diff = torq.err - torq.err_old;
// torq.err_old = torq.err;
torq.err_sum = torq.err_sum + torq.err/(double)TMR_FREQ_5k;
@@ -854,20 +859,16 @@
case MODE_TORQUE_SENSOR_NULLING: {
// DAC Voltage reference set
if (TMR3_COUNT_TORQUE_NULL < TMR_FREQ_5k * 2) {
- CUR_TORQUE_sum += CUR_TORQUE;
-
+ CUR_TORQUE_sum += torq.sen;
+
if (TMR3_COUNT_TORQUE_NULL % 10 == 0) {
CUR_TORQUE_mean = CUR_TORQUE_sum / 10.0;
CUR_TORQUE_sum = 0;
TORQUE_VREF += 0.0001 * (TORQUE_NULL - CUR_TORQUE_mean);
-
- // TORQUE_VREF = 0;
+
if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3;
if (TORQUE_VREF < 0) TORQUE_VREF = 0;
-
- spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
- spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
}
} else {
CONTROL_MODE = MODE_NO_ACT;
@@ -875,6 +876,8 @@
CUR_TORQUE_sum = 0;
CUR_TORQUE_mean = 0;
spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0));
+
+ pc.printf("%f\n", TORQUE_VREF);
}
TMR3_COUNT_TORQUE_NULL++;
@@ -1152,12 +1155,11 @@
if (PRES_B_VREF > 3.3) PRES_B_VREF = 3.3;
if (PRES_B_VREF < 0) PRES_B_VREF = 0;
- spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
- spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
+// spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
+// spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
}
} else {
CONTROL_MODE = MODE_NO_ACT;
- pc.printf("no_act");
TMR3_COUNT_PRES_NULL = 0;
CUR_PRES_A_sum = 0;
CUR_PRES_B_sum = 0;
@@ -1166,6 +1168,11 @@
spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0));
spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0));
+
+ dac_1 = PRES_A_VREF / 3.3;
+ dac_2 = PRES_B_VREF / 3.3;
+ pc.printf("%f\n", PRES_A_VREF);
+ pc.printf("%f\n", PRES_B_VREF);
}
TMR3_COUNT_PRES_NULL++;
break;
@@ -1644,7 +1651,7 @@
}
//VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE);
- PWM_out = CUR_PWM;
+ //PWM_out = CUR_PWM;
@@ -1670,7 +1677,7 @@
if (flag_data_request[2] == HIGH) {
//pressure A and B
//CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
- CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (PRES_A_VREF)); // CUR_PRES_X : 0(0bar)~4096(210bar)
+ CAN_TX_PRES((int16_t) (pres_A.sen), (int16_t) (pres_B.sen)); // CUR_PRES_X : 0(0bar)~4096(210bar)
// CAN_TX_PRES((int16_t) (CUR_PRES_A_BAR * 100.), (int16_t) (CUR_PRES_B_BAR * 100.));
// CAN_TX_PRES((int16_t) ((DEADZONE_MINUS + 1.)*1000.), (int16_t) ((DEADZONE_PLUS + 1.))*1000.);
// CAN_TX_PRES((int16_t) DZ_dir, (int16_t) ((VALVE_DEADZONE_PLUS + 1.))*1000.);
@@ -1762,9 +1769,10 @@
} else
LED = 1;
// LED != LED;
- pc.printf("flag %f\n", (double) pres_A.sen);
- spi_eeprom_ready();
- pc.printf("romrom %d\n", (int) PRES_SENSOR_A_PULSE_PER_BAR);
+// pc.printf("A %f\n", (double) pres_A.sen);
+// pc.printf("A %f\n", PRES_SENSOR_A_PULSE_PER_BAR);
+// pc.printf("B %f\n", (double) pres_B.sen);
+// pc.printf("B %f\n", PRES_SENSOR_B_PULSE_PER_BAR);
}
/*******************************************************