![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: main.cpp
- Revision:
- 226:82a3ca333004
- Parent:
- 225:9c0becc196ba
- Child:
- 227:699c3e572283
diff -r 9c0becc196ba -r 82a3ca333004 main.cpp --- a/main.cpp Thu Feb 04 05:43:47 2021 +0000 +++ b/main.cpp Wed Mar 03 10:56:25 2021 +0000 @@ -90,6 +90,8 @@ extern int CID_TX_VALVE_POSITION; extern int CID_TX_SOMETHING; +float pres_A_new = 0.0f; +float pres_B_new = 0.0f; /******************************************************************************* * REFERENCE MODE @@ -196,27 +198,39 @@ /********************************* *** Initialization *********************************/ - + HAL_Init(); SystemClock_Config(); - + LED = 0; pc.baud(9600); // i2c init - i2c.frequency(400 * 1000); // 0.4 mHz - wait_ms(2); // Power Up wait - look_for_hardware_i2c(); // Hardware present - init_as5510(i2c_slave_addr1); - make_delay(); +// i2c.frequency(400 * 1000); // 0.4 mHz +// wait_ms(2); // Power Up wait +// look_for_hardware_i2c(); // Hardware present +// init_as5510(i2c_slave_addr1); +// make_delay(); // spi init + enc_cs = 1; eeprom.format(8,3); eeprom.frequency(5000000); //5M enc.format(8,0); enc.frequency(5000000); //5M + enc_cs = 0; make_delay(); + + // spi _ enc + spi_enc_set_init(); + make_delay(); + + ////// bno rom +// spi_eeprom_write(RID_BNO, (int16_t)1); +// make_delay(); + //////// + // rom ROM_CALL_DATA(); make_delay(); @@ -230,34 +244,41 @@ TIM4->CR1 ^= TIM_CR1_UDIS; make_delay(); - // TMR3 init - Init_TMR3(); - TIM3->CR1 ^= TIM_CR1_UDIS; - make_delay(); - // CAN can.attach(&CAN_RX_HANDLER); CAN_ID_INIT(); make_delay(); + //can.reset(); + can.filter(msg.id, 0xFFFFF000, CANStandard); + + // TMR3 init + Init_TMR3(); + TIM3->CR1 ^= TIM_CR1_UDIS; + make_delay(); + + + //Timer priority NVIC_SetPriority(TIM3_IRQn, 2); NVIC_SetPriority(TIM4_IRQn, 3); - //can.reset(); - can.filter(msg.id, 0xFFFFF000, CANStandard); + - // spi _ enc - spi_enc_set_init(); - make_delay(); + //DAC init if (SENSING_MODE == 0) { dac_1 = TORQUE_VREF / 3.3f; dac_2 = 0.0f; } else if (SENSING_MODE == 1) { - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; + if (DIR_VALVE_ENC > 0) { + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + } else { + dac_1 = PRES_B_VREF / 3.3f; + dac_2 = PRES_A_VREF / 3.3f; + } } make_delay(); @@ -281,7 +302,7 @@ // timer_while ++; //i2c for SW valve - if(OPERATING_MODE == 5){ + if(OPERATING_MODE == 5) { read_field(i2c_slave_addr1); if(DIR_VALVE_ENC < 0) value = 1023 - value; } @@ -408,7 +429,6 @@ extern "C" void TIM4_IRQHandler(void) { if (TIM4->SR & TIM_SR_UIF ) { - /******************************************************* *** Sensor Read & Data Handling ********************************************************/ @@ -426,10 +446,16 @@ pres_A.sen = (1.0f - alpha_update_ft) * pres_A.sen + alpha_update_ft * pres_A_new; torq.sen = -pres_A.sen / TORQUE_SENSOR_PULSE_PER_TORQUE; - // Pressure Sensing (0~210)bar ============================================= + // Pressure Sensing (0~210)bar ============================================= } else if (SENSING_MODE == 1) { - float pres_A_new = (((float)ADC1->DR) - PRES_A_NULL); - float pres_B_new = (((float)ADC2->DR) - PRES_B_NULL); + + if (DIR_VALVE_ENC > 0) { + pres_A_new = (((float)ADC1->DR) - PRES_A_NULL); + pres_B_new = (((float)ADC2->DR) - PRES_B_NULL); + } else { + pres_A_new = (((float)ADC2->DR) - PRES_A_NULL); + pres_B_new = (((float)ADC1->DR) - PRES_B_NULL); + } double alpha_update_pres = 1.0f / (1.0f + FREQ_TMR4 / (2.0f * 3.14f * 200.0f)); // f_cutoff : 200Hz pres_A.sen = (1.0f - alpha_update_pres) * pres_A.sen + alpha_update_pres * pres_A_new; pres_B.sen = (1.0f - alpha_update_pres) * pres_B.sen + alpha_update_pres * pres_B_new; @@ -466,6 +492,7 @@ { if (TIM3->SR & TIM_SR_UIF ) { + if (((OPERATING_MODE&0b110)>>1) == 0) { K_v = 0.4f; // Moog (LPM >> mA) , 100bar mV_PER_mA = 500.0f; // 5000mV/10mA @@ -548,83 +575,6 @@ break; } -// case MODE_VALVE_NULLING_AND_DEADZONE_SETTING: { -// if (TMR3_COUNT_DEADZONE == 0) { -// if (pos_plus_end == pos_minus_end) need_enc_init = true; -// else temp_time = 0; -// } -// if (need_enc_init) { -// if (TMR3_COUNT_DEADZONE < (int) (0.5f * (float) TMR_FREQ_5k)) { -// V_out = VALVE_VOLTAGE_LIMIT * 1000.0f; -// pos_plus_end = pos.sen; -// } else if (TMR3_COUNT_DEADZONE < TMR_FREQ_5k) { -// V_out = -VALVE_VOLTAGE_LIMIT * 1000.0f; -// pos_minus_end = pos.sen; -// } else if (TMR3_COUNT_DEADZONE == TMR_FREQ_5k) need_enc_init = false; -// temp_time = TMR_FREQ_5k; -// } -// -// if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + TMR_FREQ_5k)) { -// V_out = (float) P_GAIN_JOINT_POSITION * (0.5f * (float) pos_plus_end + 0.5f * (float) pos_minus_end - (float) pos.sen); -// VALVE_CENTER = VALVE_DEADZONE_PLUS = VALVE_DEADZONE_MINUS = 0; -// -// } else if (temp_time <= TMR3_COUNT_DEADZONE && TMR3_COUNT_DEADZONE < (temp_time + (int) (1.9f * (float) TMR_FREQ_5k))) { -// V_out = 0; -// CUR_VELOCITY_sum += CUR_VELOCITY; -// } else if (TMR3_COUNT_DEADZONE == (temp_time + 2 * TMR_FREQ_5k)) { -// if (CUR_VELOCITY_sum == 0) DZ_dir = 1; -// else if (CUR_VELOCITY_sum > 0) DZ_dir = 1; -// else if (CUR_VELOCITY_sum < 0) DZ_dir = -1; -// else DZ_temp_cnt2 = DZ_end; -// CUR_VELOCITY_sum = 0; -// } else if (TMR3_COUNT_DEADZONE > (temp_time + 2 * TMR_FREQ_5k)) { -// if (TMR3_COUNT_DEADZONE > (temp_time + 10 * TMR_FREQ_5k)) DZ_temp_cnt2 = DZ_end; -// -// // Position of Dead Zone -// // (CUR_VELOCITY < 0) (CUR_VELOCITY == 0) (CUR_VELOCITY > 0) -// // | / | / |/ -// // | ______/ ___|___/ ______/| -// // |/ / | / | -// // /| / | / | -// // 0V 0V 0V -// -// if (DZ_temp_cnt2 < DZ_end) { -// if (TMR3_COUNT_DEADZONE % 20 != 0) { -// CUR_VELOCITY_sum += CUR_VELOCITY; -// } else { -// V_out -= DZ_dir; -// if (CUR_VELOCITY_sum * DZ_dir < 0) DZ_temp_cnt++; -// CUR_VELOCITY_sum = 0; -// } -// if (DZ_temp_cnt == 5) { -// if (DZ_dir >= 0) VALVE_DEADZONE_MINUS = (int16_t) V_out; -// else VALVE_DEADZONE_PLUS = (int16_t) V_out; -// DZ_dir = -DZ_dir; -// DZ_temp_cnt = 0; -// DZ_temp_cnt2++; -// } -// } else { -// TMR3_COUNT_DEADZONE = -1; -// VALVE_CENTER = VALVE_DEADZONE_PLUS / 2 + VALVE_DEADZONE_MINUS / 2; -// if (VALVE_DEADZONE_PLUS < VALVE_DEADZONE_MINUS) { -// VALVE_DEADZONE_PLUS = VALVE_CENTER; -// VALVE_DEADZONE_MINUS = VALVE_CENTER; -// } -// V_out = 0; -// -// -// -// //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; -// } -// } -// TMR3_COUNT_DEADZONE++; -// break; -// } - case MODE_FIND_HOME: { if (FINDHOME_STAGE == FINDHOME_INIT) { cnt_findhome = 0; @@ -648,15 +598,19 @@ } if ((cnt_vel_findhome < 3*TMR_FREQ_5k) && cnt_findhome < 10*TMR_FREQ_5k) { // wait for 3sec - if (HOMEPOS_OFFSET > 0) pos.ref = pos.ref + 12.0f; - else pos.ref = pos.ref - 12.0f; + if (HOMEPOS_OFFSET > 0) { + pos.ref = pos.ref + 12.0f; + } else { + pos.ref = pos.ref - 12.0f; + } + CONTROL_MODE = MODE_JOINT_CONTROL; alpha_trans = 0.0f; } else { - ENC_SET(HOMEPOS_OFFSET); - INIT_REF_POS = HOMEPOS_OFFSET; + ENC_SET((int32_t)((int32_t)HOMEPOS_OFFSET*10)); + INIT_REF_POS = (int32_t)((int32_t)HOMEPOS_OFFSET*10); REF_POSITION = 0; REF_VELOCITY = 0; FINDHOME_POSITION = 0; @@ -683,6 +637,7 @@ pos.err = (pos.ref - pos.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm] vel.err = (0.0f - vel.sen)/(float)(ENC_PULSE_PER_POSITION); //[mm/s] pos.err_sum += pos.err/(float) TMR_FREQ_5k; //[mm] + CONTROL_MODE = MODE_JOINT_CONTROL; if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { @@ -785,7 +740,7 @@ // } // 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.0f)); // cur_vel_sum = 0; @@ -829,8 +784,13 @@ 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; + if (DIR_VALVE_ENC > 0) { + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + } else { + dac_1 = PRES_B_VREF / 3.3f; + dac_2 = PRES_A_VREF / 3.3f; + } } } else { CONTROL_UTILITY_MODE = MODE_NO_ACT; @@ -843,8 +803,13 @@ spi_eeprom_write(RID_PRES_A_SENSOR_VREF, (int16_t) (PRES_A_VREF * 1000.0f)); spi_eeprom_write(RID_PRES_B_SENSOR_VREF, (int16_t) (PRES_B_VREF * 1000.0f)); - dac_1 = PRES_A_VREF / 3.3f; - dac_2 = PRES_B_VREF / 3.3f; + if (DIR_VALVE_ENC > 0) { + dac_1 = PRES_A_VREF / 3.3f; + dac_2 = PRES_B_VREF / 3.3f; + } else { + dac_1 = PRES_B_VREF / 3.3f; + dac_2 = PRES_A_VREF / 3.3f; + } } TMR3_COUNT_PRES_NULL++; break; @@ -874,7 +839,7 @@ // CUR_PRES_A_mean = 0; // CUR_PRES_B_mean = 0; // -// +// // // //spi_eeprom_write(RID_PRES_SENSOR_A_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_A_PULSE_PER_BAR * 100.0f)); // //spi_eeprom_write(RID_PRES_SENSOR_B_PULSE_PER_BAR, (int16_t) (PRES_SENSOR_B_PULSE_PER_BAR * 100.0f)); @@ -1156,7 +1121,7 @@ first_check = 0; VALVE_DEADZONE_MINUS = (float) FIRST_DZ; VALVE_DEADZONE_PLUS = (float) SECOND_DZ; - + spi_eeprom_write(RID_VALVE_CNETER, (int16_t) VALVE_CENTER); spi_eeprom_write(RID_VALVE_MAX_POS, (int16_t) VALVE_MAX_POS); spi_eeprom_write(RID_VALVE_MIN_POS, (int16_t) VALVE_MIN_POS); @@ -1359,7 +1324,7 @@ } else if (CURRENT_CONTROL_MODE == 0) { //PWM V_out = valve_pos.ref; } else { - I_REF = valve_pos.ref * 0.001f; + I_REF = (double)(valve_pos.ref * (float)mA_PER_pulse); } break; } @@ -1573,7 +1538,14 @@ double alpha_update_Iref = 1.0f / (1.0f + 5000.0f / (2.0f * 3.14f * 300.0f)); // f_cutoff : 500Hz I_REF_fil = (1.0f - alpha_update_Iref) * I_REF_fil + alpha_update_Iref*I_REF; - I_ERR = I_REF_fil - cur.sen; + if (FLAG_VALVE_DEADZONE) { + if (I_REF_fil > 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_PLUS*mA_PER_pulse; // unit: mA + else if (I_REF_fil < 0.0f) I_REF_fil_DZ = I_REF_fil + (double)VALVE_DEADZONE_MINUS*mA_PER_pulse; // unit: mA + else I_REF_fil_DZ = I_REF_fil + (double)(VALVE_DEADZONE_PLUS+VALVE_DEADZONE_MINUS)/2.0f*mA_PER_pulse; // unit: mA + } else { + I_REF_fil_DZ = I_REF_fil; + } + I_ERR = I_REF_fil_DZ - (double)cur.sen; I_ERR_INT = I_ERR_INT + (I_ERR) * 0.0002f; @@ -1596,10 +1568,10 @@ double FF_gain = 1.0f; VALVE_PWM_RAW = KP_I * 2.0f * I_ERR + KI_I * 2.0f* I_ERR_INT; - I_REF_fil_diff = I_REF_fil - I_REF_fil_old; - I_REF_fil_old = I_REF_fil; + I_REF_fil_diff = I_REF_fil_DZ - I_REF_fil_old; + I_REF_fil_old = I_REF_fil_DZ; // VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil + L_model * I_REF_fil_diff * 5000.0f); // Unit : mV - VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil); // Unit : mV + VALVE_PWM_RAW = VALVE_PWM_RAW + FF_gain * (R_model * I_REF_fil_DZ); // Unit : mV double V_MAX = 12000.0f; // Maximum Voltage : 12V = 12000mV double Ka = 3.0f / KP_I; @@ -1623,19 +1595,9 @@ //////////////////////////////////////////////////////////////////////////// ///////////////// Dead Zone Cancellation & Linearization ////////////////// //////////////////////////////////////////////////////////////////////////// - // Dead Zone Cancellation (Mechanical Valve dead-zone) - if (FLAG_VALVE_DEADZONE) { - if (VALVE_PWM_RAW > 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_PLUS * mV_PER_pulse; // unit: mV - else if (VALVE_PWM_RAW < 0) VALVE_PWM_RAW = VALVE_PWM_RAW + VALVE_DEADZONE_MINUS * mV_PER_pulse; // unit: mV - - VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW + (double)VALVE_CENTER * mV_PER_pulse; // unit: mV - - } else { - VALVE_PWM_VALVE_DZ = VALVE_PWM_RAW; - } // Output Voltage Linearization - double CUR_PWM_nonlin = VALVE_PWM_VALVE_DZ; // Unit : mV + double CUR_PWM_nonlin = (double)VALVE_PWM_RAW; // Unit : mV double CUR_PWM_lin = PWM_duty_byLT(CUR_PWM_nonlin); // -8000~8000 // Dead Zone Cancellation (Electrical dead-zone) @@ -1688,65 +1650,64 @@ TIM4->CCR2 = (PWM_ARR)*(1.0f-dtc_v); TIM4->CCR1 = (PWM_ARR)*(1.0f-dtc_w); - - if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { - // Position, Velocity, and Torque (ID:1200) - if (flag_data_request[0] == HIGH) { - if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator - 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 ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator - 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)); - } - } - - } + if (TMR2_COUNT_CAN_TX % (int) ((int) TMR_FREQ_5k/CAN_FREQ) == 0) { + // Position, Velocity, and Torque (ID:1200) + if (flag_data_request[0] == HIGH) { + if ((OPERATING_MODE & 0b01) == 0) { // Rotary Actuator + 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 ((OPERATING_MODE & 0b01) == 1) { // Linear Actuator + 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)); + } + } - + } + + // ID:1300 if (flag_data_request[1] == HIGH) { CAN_TX_TORQUE((int16_t) 7); //1300 } - // Reference Current, Current Current (ID:1700) + // Reference Current, Current Current (ID:1700) if (flag_data_request[2] == HIGH) { - - CAN_TX_SOMETHING((int16_t) (I_REF_fil / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (0), (int16_t) (0)); + + CAN_TX_SOMETHING((int16_t) (I_REF_fil_DZ / mA_PER_pulse), (int16_t) (cur.sen / mA_PER_pulse), (int16_t) (HOMEPOS_OFFSET), (int16_t) (0)); //double t_value = 0.0f; - // if(value>=(float) VALVE_CENTER) { - // t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - // } else { - // t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - // } - // double t_value_ref = 0.0f; - // if(valve_pos.ref>=(float) VALVE_CENTER) { - // t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); - // } else { - // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); - // } - // - // - // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 + // if(value>=(float) VALVE_CENTER) { + // t_value = 10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + // } else { + // t_value = -10000.0f*((double)value - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + // } + // double t_value_ref = 0.0f; + // if(valve_pos.ref>=(float) VALVE_CENTER) { + // t_value_ref = 10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MAX_POS - (double)VALVE_CENTER); + // } else { + // t_value_ref = -10000.0f*((double)valve_pos.ref - (double)VALVE_CENTER)/((double)VALVE_MIN_POS - (double)VALVE_CENTER); + // } + // + // + // CAN_TX_PRES((int16_t) (t_value), (int16_t) (t_value_ref)); // 1400 } //If it doesn't rest, below can can not work. for (can_rest = 0; can_rest < 10000; can_rest++) { ; } - + // ID:1500 if (flag_data_request[3] == HIGH) { //PWM CAN_TX_PWM((int16_t) (torq.ref)); //1500 } - + // ID:1600 if (flag_data_request[4] == HIGH) { //valve position