Yeseong Jeong
/
HydraulicControlBoard_Start
20210203
Diff: main.cpp
- Revision:
- 17:1865016ca2e7
- Parent:
- 16:903b5a4433b4
- Child:
- 18:b8adf1582ea3
--- a/main.cpp Mon Sep 02 13:32:33 2019 +0000 +++ b/main.cpp Tue Sep 03 11:59:55 2019 +0000 @@ -67,6 +67,9 @@ double PWM_out=0.0; +int timer_while = 0; +int while_index = 0; + // ============================================================================= // ============================================================================= @@ -127,8 +130,6 @@ /********************************* *** Initialization *********************************/ - pc.printf("a00000"); - LED = 1; pc.baud(9600); @@ -192,14 +193,19 @@ *** Program is operating! *************************************/ while(1) { - - //spi _ enc - //int a = spi_enc_read(); - //pc.printf("Message received: %d\n", msg.data[0]); - //pc.printf("Message received: %d\n", 13); - //i2c - //read_field(i2c_slave_addr1); - //wait(1); + if(timer_while==1){ + //i2c + read_field(i2c_slave_addr1); + if(DIR_VALVE_ENC < 0) value = 1023 - value; + + if(LED==1) { + LED=0; + } else + LED = 1; + timer_while = 0; + + } + timer_while ++; } } @@ -304,8 +310,8 @@ *******************************************************************************/ unsigned long CNT_TMR4 = 0; -double FREQ_TMR4 = (double)FREQ_20k; -double DT_TMR4 = (double)DT_20k; +double FREQ_TMR4 = (double)FREQ_10k; +double DT_TMR4 = (double)DT_10k; extern "C" void TIM4_IRQHandler(void) { if ( TIM4->SR & TIM_SR_UIF ) { @@ -313,39 +319,42 @@ *** Sensor Read & Data Handling ********************************************************/ - if((CNT_TMR4%2)==0) { - //Using LoadCell + + //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); - //Pressure sensor 1B - 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 + + //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; + pres_A.sen = pres_A.sen*(1.0-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A); + + //Pressure sensor 1B + //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; + 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; -// int raw_cur = ADC3->DR; - while((ADC3->SR & 0b10)); - double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz - double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA - cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur); -*/ - } + int raw_cur = ADC3->DR; + //while((ADC3->SR & 0b10)); + double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz + double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA + cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur); + cur.sen = raw_cur; + + /******************************************************* *** Timer Counting & etc. @@ -494,6 +503,7 @@ */ // CONTROL LOOP ------------------------------------------------------------ + switch (CONTROL_MODE) { case MODE_NO_ACT: { V_out = 0; @@ -887,7 +897,7 @@ //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); - pc.printf("%f\n", TORQUE_VREF); + //pc.printf("%f\n", TORQUE_VREF); dac_1 = TORQUE_VREF / 3.3; } @@ -1187,7 +1197,7 @@ dac_1 = PRES_A_VREF / 3.3; dac_2 = PRES_B_VREF / 3.3; - pc.printf("nulling end"); + //pc.printf("nulling end"); } TMR3_COUNT_PRES_NULL++; break; @@ -1228,13 +1238,13 @@ case MODE_ROTARY_FRICTION_TUNING: { if (TMR3_COUNT_ROTARY_FRIC_TUNE % (5 * TMR_FREQ_5k) == 0) freq_fric_tune = 4 + 3. * sin(2 * 3.14159 * 0.5 * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001 * 0.05); - VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001); - if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL; - else VALVE_PWM_RAW = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; + V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL * sin(2 * 3.14159 * freq_fric_tune * TMR3_COUNT_ROTARY_FRIC_TUNE * 0.0001); + if (V_out > 0) V_out = VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE * PWM_RESOL; + else V_out = -VALVE_VOLTAGE_LIMIT / SUPPLY_VOLTAGE*PWM_RESOL; TMR3_COUNT_ROTARY_FRIC_TUNE++; if (TMR3_COUNT_ROTARY_FRIC_TUNE > TUNING_TIME * TMR_FREQ_5k) { TMR3_COUNT_ROTARY_FRIC_TUNE = 0; - VALVE_PWM_RAW = 0; + V_out = 0; CONTROL_MODE = MODE_NO_ACT; } break; @@ -1244,9 +1254,9 @@ VALVE_ID_timer = VALVE_ID_timer + 1; if(VALVE_ID_timer < TMR_FREQ_5k*1) { - VALVE_PWM_RAW = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100); + V_out = 3000 * sin(2*3.14*VALVE_ID_timer/TMR_FREQ_5k * 100); } else if(VALVE_ID_timer < TMR_FREQ_5k*2) { - VALVE_PWM_RAW = 1000*(ID_index_array[ID_index]); + V_out = 1000*(ID_index_array[ID_index]); } else if(VALVE_ID_timer == TMR_FREQ_5k*2) { VALVE_POS_TMP = 0; data_num = 0; @@ -1259,13 +1269,13 @@ ID_index= ID_index +1; } - if(ID_index>=18) { + if(ID_index>=25) { int i; VALVE_POS_AVG_OLD = VALVE_POS_AVG[0]; - for(i=0; i<18; i++) { + for(i=0; i<25; i++) { VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]); - ROM_RESET_DATA(); + //ROM_RESET_DATA(); //spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i])); if(VALVE_POS_AVG[i] > VALVE_POS_AVG_OLD) { @@ -1685,7 +1695,7 @@ else if (CUR_PWM_DZ < 0) V_out = (int)CUR_PWM_DZ - 138; else V_out = CUR_PWM_DZ; } else { - CUR_PWM = V_out; + V_out = V_out; } //VALVE_PWM(CUR_PWM, VALVE_VOLTAGE_LIMIT, SUPPLY_VOLTAGE); @@ -1699,16 +1709,16 @@ if (flag_data_request[0] == HIGH) { //position+velocity - CAN_TX_POSITION((int32_t) PRES_A_VREF, (int32_t) PRES_B_VREF); + CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); //pc.printf("can good"); - // CAN_TX_POSITION((int) (PRES_A_VREF * 100.), (int) (PRES_B_VREF * 100.)); // CAN_TX_POSITION((long) CUR_PRES_A_BAR, (long) CUR_PRES_B_BAR); } if (flag_data_request[1] == HIGH) { //torque //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM * 100.)); - CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM)); + //CAN_TX_TORQUE((int16_t) (CUR_TORQUE_NM)); + CAN_TX_TORQUE((int16_t) (cur.sen)); // CAN_TX_TORQUE((int16_t) DZ_temp_cnt); } @@ -1765,14 +1775,20 @@ // if(V_out > 0.0) V_out = 1000.0; // else if(V_out < 0.0) V_out = -1000.0; + //V_out = 2000.0 * sin(2 * 3.14159 * (double) CNT_TMR3 / (double) 5000.0); + /******************************************************* *** PWM ********************************************************/ PWM_out= V_out/12000.0; // Full duty : 12000.0mV // Saturation of output voltage to 5.0V - if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667 - else if (PWM_out < -0.41667) PWM_out=-0.41667; +// if(PWM_out > 0.41667) PWM_out=0.41667; //5.0/12.0 = 0.41667 +// else if (PWM_out < -0.41667) PWM_out=-0.41667; + + // Saturation of output voltage to 12.0V + if(PWM_out > 1.0) PWM_out=1.0; + else if (PWM_out < -1.0) PWM_out=-1.0; if (PWM_out>0.0) { dtc_v=0.0; @@ -1802,16 +1818,16 @@ // } if((CNT_TMR3%5000)==0) { - if(LED==1) { - LED=0; - } else - LED = 1; +// if(LED==1) { +// LED=0; +// } else +// LED = 1; // LED != LED; // 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); - pc.printf("preAVref %d\n", (int) PRES_A_VREF); +// pc.printf("preAVref %d\n", (int) PRES_A_VREF); } /******************************************************* @@ -1819,7 +1835,9 @@ ********************************************************/ CNT_TMR3++; } + TIM3->SR = 0x0; // reset the status register + } /*