//This duplicates functionalist in leg_ctrl
static void hallUpdateBEMF() {
//Back EMF measurements are made automatically by coordination of the ADC, PWM, and DMA.
//Copy to local variables. Not strictly neccesary, just for clarity.
//This **REQUIRES** that the divider on the battery & BEMF circuits have the same ratio.
hallbemf[0] = adcGetVBatt() - adcGetBEMFL();
hallbemf[1] = adcGetVBatt() - adcGetBEMFR();
//NOTE: at this point, we should have a proper correspondance between
// the order of all the structured variable; bemf[i] associated with
// pidObjs[i], bemfLast[i], etc.
// Any "jumbling" of the inputs can be done in the above assignments.
//Negative ADC measures mean nothing and should never happen anyway
if (hallbemf[0] < 0) {
hallbemf[0] = 0;
}
if (hallbemf[1] < 0) {
hallbemf[1] = 0;
}
//Apply median filter
int i;
for (i = 0; i < NUM_HALL_PIDS; i++) {
hallbemfHist[i][2] = hallbemfHist[i][1]; //rotate first
hallbemfHist[i][1] = hallbemfHist[i][0];
hallbemfHist[i][0] = hallbemf[i]; //include newest value
hallbemf[i] = medianFilter3(hallbemfHist[i]); //Apply median filter
}
// IIR filter on BEMF: y[n] = 0.2 * y[n-1] + 0.8 * x[n]
hallbemf[0] = (2 * (long) hallbemfLast[0] / 10) + 8 * (long) hallbemf[0] / 10;
hallbemf[1] = (2 * (long) hallbemfLast[1] / 10) + 8 * (long) hallbemf[1] / 10;
hallbemfLast[0] = hallbemf[0]; //bemfLast will not be used after here, OK to set
hallbemfLast[1] = hallbemf[1];
}
void updateBEMF(){
//Back EMF measurements are made automatically by coordination of the ADC, PWM, and DMA.
//Copy to local variables. Not strictly neccesary, just for clarity.
//This **REQUIRES** that the divider on the battery & BEMF circuits have the same ratio.
bemf[0] = adcGetVBatt() - adcGetBEMFL();
bemf[1] = adcGetVBatt() - adcGetBEMFR();
//NOTE: at this point, we should have a proper correspondance between
// the order of all the structured variable; bemf[i] associated with
// pidObjs[i], bemfLast[i], etc.
// Any "jumbling" of the inputs can be done in the above assignments.
//Negative ADC measures mean nothing and should never happen anyway
if (bemf[0] < 0) {
bemf[0] = 0;
}
if (bemf[1] < 0) {
bemf[1] = 0;
}
//Apply median filter
int i;
for (i = 0; i < NUM_MOTOR_PIDS; i++) {
bemfHist[i][2] = bemfHist[i][1]; //rotate first
bemfHist[i][1] = bemfHist[i][0];
bemfHist[i][0] = bemf[i]; //include newest value
bemf[i] = medianFilter3(bemfHist[i]); //Apply median filter
}
// IIR filter on BEMF: y[n] = 0.2 * y[n-1] + 0.8 * x[n]
bemf[0] = (2 * (long) bemfLast[0] / 10) + 8 * (long) bemf[0] / 10;
bemf[1] = (2 * (long) bemfLast[1] / 10) + 8 * (long) bemf[1] / 10;
bemfLast[0] = bemf[0]; //bemfLast will not be used after here, OK to set
bemfLast[1] = bemf[1];
//Simple indicator if a leg is "in motion", via the yellow LED.
//Not functionally necceasry; can be elimited to use the LED for something else.
if ((bemf[0] > 0) || (bemf[1] > 0)) {
LED_YELLOW = 1;
} else {
LED_YELLOW = 0;
}
}
