Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2_copy1
2011
Diff: main.cpp
- Revision:
- 16:903b5a4433b4
- Parent:
- 15:bd0d12728506
- Child:
- 17:1865016ca2e7
diff -r bd0d12728506 -r 903b5a4433b4 main.cpp --- a/main.cpp Fri Aug 30 02:26:11 2019 +0000 +++ b/main.cpp Mon Sep 02 13:32:33 2019 +0000 @@ -6,6 +6,8 @@ #include "I2C_AS5510.h" #include "setting.h" #include "function_utilities.h" +#include "stm32f4xx_flash.h" +#include "FlashWriter.h" // dac & check /////////////////////////////////////////// DigitalOut check(PC_2); @@ -26,8 +28,9 @@ unsigned int value; // 10bit output of reading sensor AS5510 // SPI /////////////////////////////////////////// -SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK); -DigitalOut eeprom_cs(PB_12); +//SPI eeprom(PB_15, PB_14, PB_13); // EEPROM //(SPI_MOSI, SPI_MISO, SPI_SCK); +//DigitalOut eeprom_cs(PB_12); +//FlashWriter writer(6);//2부터 7까지 되는듯 아마 sector SPI enc(PC_12,PC_11,PC_10); DigitalOut enc_cs(PD_2); DigitalOut LED(PA_15); @@ -124,14 +127,11 @@ /********************************* *** Initialization *********************************/ - + pc.printf("a00000"); + LED = 1; pc.baud(9600); - //eeprom - ROM_INIT_DATA(); - make_delay(); - // i2c init i2c.frequency(400 * 1000); // 0.4 mHz wait_ms(2); // Power Up wait @@ -140,11 +140,16 @@ make_delay(); // // spi init - eeprom.format(8,3); - eeprom.frequency(5000000); //5M + //eeprom.format(8,3); + //eeprom.frequency(5000000); //5M enc.format(8,0); enc.frequency(5000000); //5M make_delay(); + + //rom + + ROM_INIT_DATA(); + make_delay(); // ADC init Init_ADC(); @@ -169,13 +174,11 @@ spi_enc_set_init(); make_delay(); - - //DAC init - //dac_1 = PRES_A_VREF/3.3; - dac_1 = 0.0; - //dac_2 = PRES_B_VREF/3.3; - dac_2 = 0.0; + dac_1 = PRES_A_VREF / 3.3; + //dac_1 = 0.0; + dac_2 = PRES_B_VREF / 3.3; + //dac_2 = 0.0; make_delay(); for (int i=0; i<100; i++) { @@ -185,7 +188,6 @@ ID_index_array[i] = (i+1) * 0.5; } - /************************************ *** Program is operating! *************************************/ @@ -196,7 +198,8 @@ //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); + //wait(1); } } @@ -310,7 +313,7 @@ *** Sensor Read & Data Handling ********************************************************/ - if((CNT_TMR4%20)==0) { + if((CNT_TMR4%2)==0) { //Using LoadCell // ADC1->CR2 |= 0x40000000; // adc _ 12bit // //while((ADC1->SR & 0b10)); @@ -493,7 +496,6 @@ // CONTROL LOOP ------------------------------------------------------------ switch (CONTROL_MODE) { case MODE_NO_ACT: { - // SPI_VREF_DAC_WRITE(PRES_A_VREF, PRES_B_VREF, TORQUE_VREF, 0); V_out = 0; break; } @@ -869,15 +871,24 @@ if (TORQUE_VREF > 3.3) TORQUE_VREF = 3.3; if (TORQUE_VREF < 0) TORQUE_VREF = 0; + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); + dac_1 = TORQUE_VREF / 3.3; } } else { CONTROL_MODE = MODE_NO_ACT; TMR3_COUNT_TORQUE_NULL = 0; CUR_TORQUE_sum = 0; CUR_TORQUE_mean = 0; - spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_TORQUE_SENSOR_VREF, (int16_t) (TORQUE_VREF * 1000.0)); pc.printf("%f\n", TORQUE_VREF); + dac_1 = TORQUE_VREF / 3.3; } TMR3_COUNT_TORQUE_NULL++; @@ -947,8 +958,11 @@ VALVE_DEADZONE_MINUS = VALVE_CENTER; } VALVE_PWM_RAW = 0; - spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS); - spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, VALVE_DEADZONE_PLUS); + //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, VALVE_DEADZONE_MINUS); CONTROL_MODE = MODE_NO_ACT; DZ_temp_cnt2 = 0; @@ -1112,7 +1126,10 @@ } } if (fl_temp_cnt2 == 100) { - spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0)); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_VALVE_GAIN_PLUS_1 + flag_flowrate, (int16_t) (VALVE_GAIN_LPM_PER_V[flag_flowrate] * 100.0)); cur_vel_sum = 0; fl_temp_cnt = 0; fl_temp_cnt2 = 0; @@ -1146,7 +1163,7 @@ CUR_PRES_A_sum = 0; CUR_PRES_B_sum = 0; - double VREF_NullingGain = 0.0003; + double VREF_NullingGain = 0.003; PRES_A_VREF -= VREF_NullingGain * (PRES_A_NULL - CUR_PRES_A_mean); PRES_B_VREF -= VREF_NullingGain * (PRES_B_NULL - CUR_PRES_B_mean); @@ -1154,9 +1171,9 @@ if (PRES_A_VREF < 0) PRES_A_VREF = 0; 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)); + + dac_1 = PRES_A_VREF / 3.3; + dac_2 = PRES_B_VREF / 3.3; } } else { CONTROL_MODE = MODE_NO_ACT; @@ -1166,13 +1183,11 @@ CUR_PRES_A_mean = 0; CUR_PRES_B_mean = 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)); - + ROM_RESET_DATA(); + 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); + pc.printf("nulling end"); } TMR3_COUNT_PRES_NULL++; break; @@ -1201,9 +1216,11 @@ CUR_PRES_B_sum = 0; CUR_PRES_A_mean = 0; CUR_PRES_B_mean = 0; + + ROM_RESET_DATA(); - spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0)); - spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0)); + //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0)); + //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0)); } TMR3_COUNT_PRES_CALIB++; break; @@ -1247,7 +1264,10 @@ VALVE_POS_AVG_OLD = VALVE_POS_AVG[0]; for(i=0; i<18; i++) { VALVE_POS_VS_PWM[i] = (int16_t) (VALVE_POS_AVG[i]); - spi_eeprom_write(RID_VALVE_POS_VS_PWM_0 + i, (int16_t) (VALVE_POS_AVG[i])); + + 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) { VALVE_MAX_POS = VALVE_POS_AVG[i]; VALVE_POS_AVG_OLD = VALVE_MAX_POS; @@ -1256,8 +1276,11 @@ VALVE_POS_AVG_OLD = VALVE_MIN_POS; } } - spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) (VALVE_MAX_POS)); - spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) (VALVE_MIN_POS)); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) (VALVE_MAX_POS)); + //spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) (VALVE_MIN_POS)); CAN_TX_PRES((int16_t) (VALVE_MAX_POS), (int16_t) (VALVE_MIN_POS)); ID_index = 0; CONTROL_MODE = MODE_NO_ACT; @@ -1436,9 +1459,12 @@ SECOND_DZ = valve_pos.ref; DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ)); first_check = 0; - spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); - spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); - spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); + //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); + //spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER); //CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ); CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; @@ -1531,10 +1557,16 @@ DDV_CENTER = (int) (0.5 * (double) (FIRST_DZ) + 0.5 * (double) (SECOND_DZ)); first_check = 0; VALVE_DEADZONE_MINUS = FIRST_DZ; - spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, FIRST_DZ); VALVE_DEADZONE_PLUS = SECOND_DZ; - spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); - spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, SECOND_DZ); + //spi_eeprom_write(RID_DDV_CNETER, DDV_CENTER); CAN_TX_PRES((int16_t) FIRST_DZ, (int16_t) SECOND_DZ); CONTROL_MODE = MODE_NO_ACT; DZ_index = 1; @@ -1599,8 +1631,11 @@ min_check = 1; } JOINT_VEL[ID_index] = (FINAL_POS - START_POS) / data_num * TMR_FREQ_5k; // pulse/sec - spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF)); - spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF)); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0 + ID_index, (int16_t) (JOINT_VEL[ID_index] & 0xFFFF)); + //spi_eeprom_write(RID_VALVE_POS_VS_FLOWRATE_0_1 + ID_index, (int16_t) ((JOINT_VEL[ID_index] >> 16) & 0xFFFF)); VALVE_FR_timer = 0; one_period_end = 0; ID_index= ID_index +1; @@ -1608,7 +1643,10 @@ } if(max_check == 1 && min_check == 1) { - spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index)); + + ROM_RESET_DATA(); + + //spi_eeprom_write(RID_VALVE_POS_NUM, (int16_t) (ID_index)); VALVE_POS_NUM = ID_index; ID_index = 0; first_check = 0; @@ -1661,7 +1699,7 @@ if (flag_data_request[0] == HIGH) { //position+velocity - CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); + CAN_TX_POSITION((int32_t) PRES_A_VREF, (int32_t) PRES_B_VREF); //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); @@ -1773,6 +1811,7 @@ // 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); } /*******************************************************