Update util.c
This commit is contained in:
parent
aaeb44d0b6
commit
e508a2131a
20
Src/util.c
20
Src/util.c
|
@ -429,7 +429,7 @@ void calcAvgSpeed(void) {
|
||||||
* - release potentiometers to the resting postion
|
* - release potentiometers to the resting postion
|
||||||
* - press the power button to confirm or wait for the 20 sec timeout
|
* - press the power button to confirm or wait for the 20 sec timeout
|
||||||
*/
|
*/
|
||||||
void inputCalibLim(void) {
|
void adcCalibLim(void) {
|
||||||
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
|
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -451,6 +451,7 @@ void inputCalibLim(void) {
|
||||||
|
|
||||||
// Extract MIN, MAX and MID from ADC while the power button is not pressed
|
// Extract MIN, MAX and MID from ADC while the power button is not pressed
|
||||||
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout
|
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && input_cal_timeout++ < 4000) { // 20 sec timeout
|
||||||
|
readInput();
|
||||||
filtLowPass32(cmd1_in, FILTER, &input1_fixdt);
|
filtLowPass32(cmd1_in, FILTER, &input1_fixdt);
|
||||||
filtLowPass32(cmd2_in, FILTER, &input2_fixdt);
|
filtLowPass32(cmd2_in, FILTER, &input2_fixdt);
|
||||||
INPUT1_MID_temp = (uint16_t)CLAMP(input1_fixdt >> 16, 0, 4095); // convert fixed-point to integer
|
INPUT1_MID_temp = (uint16_t)CLAMP(input1_fixdt >> 16, 0, 4095); // convert fixed-point to integer
|
||||||
|
@ -500,26 +501,26 @@ void updateCurSpdLim(void) {
|
||||||
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
|
if (speedAvgAbs > 5) { // do not enter this mode if motors are spinning
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
#ifdef CONTROL_ADC
|
|
||||||
consoleLog("Torque and Speed limits update started... ");
|
consoleLog("Torque and Speed limits update started... ");
|
||||||
|
|
||||||
int32_t adc1_fixdt = adc_buffer.l_tx2 << 16;
|
int32_t input1_fixdt = cmd1_in << 16;
|
||||||
int32_t adc2_fixdt = adc_buffer.l_rx2 << 16;
|
int32_t input2_fixdt = cmd2_in << 16;
|
||||||
uint16_t cur_spd_timeout = 0;
|
uint16_t cur_spd_timeout = 0;
|
||||||
uint16_t cur_factor; // fixdt(0,16,16)
|
uint16_t cur_factor; // fixdt(0,16,16)
|
||||||
uint16_t spd_factor; // fixdt(0,16,16)
|
uint16_t spd_factor; // fixdt(0,16,16)
|
||||||
|
|
||||||
// Wait for the power button press
|
// Wait for the power button press
|
||||||
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout
|
while (!HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && cur_spd_timeout++ < 2000) { // 10 sec timeout
|
||||||
filtLowPass32(adc_buffer.l_tx2, FILTER, &adc1_fixdt);
|
readInput();
|
||||||
filtLowPass32(adc_buffer.l_rx2, FILTER, &adc2_fixdt);
|
filtLowPass32(cmd1_in, FILTER, &input1_fixdt);
|
||||||
|
filtLowPass32(cmd2_in, FILTER, &input2_fixdt);
|
||||||
HAL_Delay(5);
|
HAL_Delay(5);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Calculate scaling factors
|
// Calculate scaling factors
|
||||||
cur_factor = CLAMP((adc1_fixdt - (ADC1_MIN_CAL << 16)) / (ADC1_MAX_CAL - ADC1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A
|
cur_factor = CLAMP((input1_fixdt - (INPUT1_MIN_CAL << 16)) / (INPUT1_MAX_CAL - INPUT1_MIN_CAL), 6553, 65535); // ADC1, MIN_cur(10%) = 1.5 A
|
||||||
spd_factor = CLAMP((adc2_fixdt - (ADC2_MIN_CAL << 16)) / (ADC2_MAX_CAL - ADC2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm
|
spd_factor = CLAMP((input2_fixdt - (INPUT2_MIN_CAL << 16)) / (INPUT2_MAX_CAL - INPUT2_MIN_CAL), 3276, 65535); // ADC2, MIN_spd(5%) = 50 rpm
|
||||||
|
|
||||||
// Update maximum limits
|
// Update maximum limits
|
||||||
rtP_Left.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
|
rtP_Left.i_max = (int16_t)((I_MOT_MAX * A2BIT_CONV * cur_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
|
||||||
rtP_Left.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
|
rtP_Left.n_max = (int16_t)((N_MOT_MAX * spd_factor) >> 12); // fixdt(0,16,16) to fixdt(1,16,4)
|
||||||
|
@ -529,7 +530,6 @@ void updateCurSpdLim(void) {
|
||||||
cur_spd_valid = 1;
|
cur_spd_valid = 1;
|
||||||
consoleLog("OK\n");
|
consoleLog("OK\n");
|
||||||
|
|
||||||
#endif
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
|
|
Loading…
Reference in New Issue