![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
LIGHT2
Diff: main.cpp
- Revision:
- 15:bd0d12728506
- Parent:
- 14:8e7590227d22
- Child:
- 16:903b5a4433b4
diff -r 8e7590227d22 -r bd0d12728506 main.cpp --- 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); } /*******************************************************