A program to monitor some parameters for a motor
Dependencies: mbed-dev BufferSerial
Thanks to David Lowe for https://developer.mbed.org/users/gregeric/code/Nucleo_Hello_Encoder/ which I adapted for the use of TIM2 32bit timer as an encoder reader on the Nucleo L432KC board.
Diff: main.cpp
- Revision:
- 15:8adff67fe707
- Parent:
- 14:e5f5b345b2fe
- Child:
- 16:e423f891cfbc
--- a/main.cpp Sat Jun 03 18:59:04 2017 +0000 +++ b/main.cpp Wed Jun 14 01:23:53 2017 +0000 @@ -42,12 +42,14 @@ #include "mbed.h" #include "Encoder.h" -#include "inttypes.h" // for PRIu32 encoding //Defining the timer and its coresponding encoder TIM_HandleTypeDef timer2; TIM_Encoder_InitTypeDef encoder1; +//Timer to be used for the speed computation function +Timer timer1; + //The input for the encoder's index channel InterruptIn event(PA_8); @@ -59,192 +61,245 @@ //of the AVAGO encoder with respect to the shaft DigitalOut led2(PB_4); -//Relay to power on and off the DCPS +//Relay port to power on and off the Digitally Controlled Power Source (DCPS) DigitalOut relay1(PB_5); -//AnalogIn vref(ADC_VREF); -//AnalogIn tempint(ADC_TEMP); - //Defining the ADCs +//For reading the Phase Comparator output AnalogIn adc1(PA_3); //ADC1_IN8 -AnalogIn adc2(PA_7); //ADC1_IN9 +//For reading the DCPS output +AnalogIn adc2(PA_4); //ADC1_IN9 +//For reading the current value from the shunt monitor AnalogIn adc3(PA_6); //ADC1_IN11 -//Defining the DAC for setting the half of the current amplitude -//and generate a quasi square signal for the PLL current phase input -AnalogOut dac1(PA_4); // DAC1_OUT1 - -//Defining the DAC for the Power Source +//Defining the DAC for the input of DCPS //(DCPS - Digitally Controlled Power Source) //DAC1_OUT2 on pin PA_5 is at least twices as fast as DAC1_OUT1 on pin PA_4 -AnalogOut dac2(PA_5); // DAC1_OUT2 +AnalogOut dac1(PA_5); // DAC1_OUT2 //Defining the serial object to be used for communicating with Raspi Serial raspi(USBTX, USBRX); -//Serial& raspi = get_stdio_serial(); -//Serial debug(PB_6,PA_10); // Serial1 tx rx - -int ADC_Count = 0; -float Voltage; -float Voltage_total; -float samples[1024]; +//Counter for the absolute position from start +int32_t count1 = 0; +//Counter for the index passes +int32_t count2 = 0; +//Records the wheel offset position to the index +int32_t count3 = 0; +//Counter recording absolute position at last index pulse +int32_t count4 = 0; +//Used to filter the first index pulse, so that the offset position +//to the index may be recorded +volatile bool adjustOffset = false; -uint16_t count1=0; -int16_t count2=0; -int32_t count3=0; -int16_t adjust=0; - -float speedLast=0; - -float dac_val=0; -float dac2_val=0; -float adc3_val=0; +//Last data written to dac1 (using subunitary values) +float dac_val = 0.0; //Array with adc1, adc2 & adc3 correction addition, -//experimentally determined +//experimentally determined (hardware offset voltage dependent) const float adc_corr[] = { 0, 0.022f, 0.2995, 0.0267}; - -int main() -{ - printf("%d\n", lookup("X0")); - return 0; -} + +//Enable ADC reading and printing to the serial interface +bool adc_en = true; + +//Enables speed computing and printing to the serial interface +bool speed_en = true; -volatile bool adc3_en = true; -int16_t lines = 4; +//Enable position reading and printing to the serial interface +bool pos_en = true; -//TIM1 to be used with the interrupt for the encoder's index pulses -Timer timer1; -Timer timer3; +//Enable alternative position computation and printing to the serial interface, +//relative to the encoder index pulses counter +bool posIndex_en = true; //Function invoked by the encoder's index interrupt pulses +//It counts the index pulses, incrementing or decrementing the number, +//and registers the offset position of the wheel at start in count3; +//The index counter is used to correct possible reading errors of the +//encoder's counter +//led2 is used for visual feedback. void atint() { - timer1.start(); + count4 =__HAL_TIM_GET_COUNTER(&timer2); if (__HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2)) { - adjust--; - if (adjust == -1) { - count2--; - count1 +=__HAL_TIM_GET_COUNTER(&timer2); - //TIM2->CNT = 0x0000; - adjust = 0; - led2 =!led2; + if (count2 == 0 && adjustOffset == false) { //catch first index pulse + count3 = count4; + adjustOffset = true; } + count2--; + led2 =!led2; } else { - adjust++; - if (adjust == 1) { - count2++; - count1 +=__HAL_TIM_GET_COUNTER(&timer2); - //TIM2->CNT = 0x0000; - adjust = 0; - led2 =!led2; + if (count2 == 0 && adjustOffset == false) { //catch first index pulse + count3 = count4; + adjustOffset = true; } + count2++; + led2 =!led2; } } + +//Function for adaptive speed computation float speedRead() { + //counter for the waiting time window uint16_t i = 0; + //sample time for speed computation uint32_t deltaT; + + //Computer speed float speed; - uint32_t pos1; - uint32_t pos2; + + //First and second registered position + int32_t pos1; + int32_t pos2; + //position difference to be used for speed computation int32_t pos; + //Direction of rotation, read from the encoder, for the first + //and the second registered positions int8_t sens1; int8_t sens2; - printf("Encoder speed reading!\r\n"); - timer3.start(); + + timer1.start(); pos1=__HAL_TIM_GET_COUNTER(&timer2); sens1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); - wait_ms(5); + //Minumum waiting time for this project would be 100 microseconds, + //for 10 kHz quadrature frequency. We will set it 100 times longer + //wait_ms(10); + // Avoiding wait(), since it uses interrupts and timing here + // is not critical. This takes 10 ms, if not interrupted + for (uint32_t j=0; j<199950; j++){} pos2=__HAL_TIM_GET_COUNTER(&timer2); sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); //The speed computation method adapts to slow/fast speeds, in order to //optimize the rapport between computation precision and time windows size - while (pos2 == pos1 && i<99) { // The accepted max delay time is 0.5 seconds - wait_ms(5); + while (pos2 == pos1 && i<49) { // The accepted max delay time is 0.5 seconds + //wait_ms(10); + // Avoiding wait(), since it uses interrupts and timing here + // is not critical. This takes 10 ms, if not interrupted + for (uint32_t j=0; j<199950; j++){} i++; pos2=__HAL_TIM_GET_COUNTER(&timer2); sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); } pos2=__HAL_TIM_GET_COUNTER(&timer2); sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); - timer3.stop(); - deltaT = timer3.read_us(); - timer3.reset(); - pos = (int32_t) pos2 - (int32_t) pos1; + timer1.stop(); + deltaT = timer1.read_us(); + timer1.reset(); + if (sens1 == sens2) { + pos = pos2 - pos1; + } else { + printf("E:Speed computation error, change of direction between readings!\r\n"); + } - printf("Time is %lu microseconds, position modified %ld %lu %lu\r\n", deltaT, pos, pos1, pos2); + //For debugging + //printf("%lu microseconds, %ld steps: start:%lu stop%lu\r\n", deltaT, pos, pos1, pos2); if (deltaT > 0) { + //speed computation in rot/s speed = ((float) pos)*125.f/((float) deltaT); // (pulses/us)*1000000/8000 -> rot/s } else { - printf("Error, time interval not greater than zero, speed not calculated!\r\n"); + printf("E:Error, time interval not greater than zero, speed not calculated!\r\n"); } - printf("The encoder speed is %f rot/s\r\n", speed); return speed; } -//Function attached to the serial RX interrupt event +//Function attached to the serial RX interrupt event, it parses +//the serial messages and invoques the corresponding commands void readData(void) { - char message[125]; + char message[50]; if(raspi.readable()) { - int p1=0; + // Signalling the beginning of serial read led1 = 1; + int p1 = 0; + raspi.scanf("%s", message); - printf("%d %s\r\n", strlen(message), message); - if (strcmp(message, "startAdc") == 0) { - adc3_en = true; - lines = 6; - printf("true\r\n"); - } else if (strcmp(message, "stopAdc") == 0) { - adc3_en = false; - lines = 4; - printf("false\r\n"); - } else if (p1=strstr(message, "DAC=") != NULL) { + //Message received printed for debugging + //printf("M:%d %s\r\n", strlen(message), message); + + if (strcmp(message, "adcOn") == 0) { + adc_en = true; + printf("M:ADC true\r\n"); + } else if (strcmp(message, "adcOff") == 0) { + adc_en = false; + printf("M:ADC false\r\n"); + } else if (p1=strstr(message, "dac=") != NULL) { + //Writing the dac1 value read from serial //The DCPS has 1V offset, so we have to remove it - dac_val = (atof(message+p1+3)-1.0f)/15.61; - dac2.write(dac_val); - printf("Value to write to DAC: %f\r\n", dac_val*3.3f); + dac_val = (atof(message+p1+3)-1.0f)/15.51; + dac1.write(dac_val); + printf("M:Value to write to DAC: %f\r\n", dac_val*3.3f); } else if (strcmp(message, "reset") == 0) { + //encoder related counters reset command TIM2->CNT = 0x0000; - printf("Encoder reset!\r\n"); - } else if (strcmp(message, "poweron") == 0) { + count1 = 0; + count2 = 0; + count3 = 0; + count4 = 0; + adjustOffset = false; + printf("M:Encoder counters reset!\r\n"); + } else if (strcmp(message, "powerOn") == 0) { + //command to power on the DCPS relay1.write(1); - printf("DCPS on\r\n"); - } else if (strcmp(message, "poweroff") == 0) { + printf("M:DCPS on\r\n"); + } else if (strcmp(message, "powerOff") == 0) { + //command to power off the DCPS relay1.write(0); - printf("DCPS off\r\n"); + printf("M:DCPS off\r\n"); + } else if (strcmp(message, "posOn") == 0) { + //command to enable the encoder position notification + pos_en = true; + printf("M:Position notification enabled\r\n"); + } else if (strcmp(message, "posOff") == 0) { + //command to disable the encoder position notification + pos_en = false; + printf("M:Position notification disabled\r\n"); + } else if (strcmp(message, "posIndexOn") == 0) { + //command to enable the index related encoder position notification + posIndex_en = true; + printf("M:Index related position notification enabled\r\n"); + } else if (strcmp(message, "posIndexOff") == 0) { + //command to disable the index related encoder position notification + posIndex_en = false; + printf("M:Index related position notification disabled\r\n"); + } else if (strcmp(message, "speedOn") == 0) { + //command to enable speed computation and notification + speed_en = true; + printf("M:Speed enabled\r\n"); + } else if (strcmp(message, "speedOff") == 0) { + //command to disable speed computation and notification + speed_en = false; + printf("M:Speed disabled\r\n"); } } + //Signalling the end of searial read led1 = 0; } -void ADC_read(AnalogIn adc) +float ADC_read(AnalogIn adc) { - ADC_Count++; - Voltage_total =0; - for (int i=0; i<100; i++) { // do 25 readings + + float Voltage; + float Voltage_total = 0.0; + + // Voltage is summed then averaged + for (int i=0; i<100; i++) { // do 100 readings Voltage = adc.read(); - Voltage_total = Voltage_total+ Voltage; //Note Vinput.read can be summed then averaged + Voltage_total = Voltage_total+ Voltage; //wait_us(10); } - Voltage=Voltage_total/100.f; - samples[ADC_Count] = Voltage; //Save averaged reading within an array - if (ADC_Count == 1023){ - ADC_Count = 0; - } - + Voltage = Voltage_total/100.f; + return Voltage; } //The main function int main() { //Power onn the DCPS - relay1.write(1); + relay1.write(0); //counting on both A&B inputs (A at PA0, B at PA1), 4 ticks per cycle, //full 32-bit count @@ -265,74 +320,50 @@ raspi.attach(&readData); //Message to mark the initialisation of the program - printf("\n\rSTM HAL encoder with ADC and DAC\n\r"); - printf("Running at %u MHz\r\n\n", HAL_RCC_GetSysClockFreq()/1000000); + printf("M:\n\rSTM HAL encoder with ADC and DAC\n\r"); + printf("M:Running at %u MHz\r\n\n", HAL_RCC_GetSysClockFreq()/1000000); - //printf("%" PRIu32 "\n", UINT32_MAX); - - int i = 0; //The main loop while(1) { - //Variable for the one loop encoder counter - uint32_t count3=0; - //Variable for the direction of the counter int8_t dir1; - //uint16_t voltage1=0; - - - //OK 401 411 446 NOK 030 - //count1=TIM2->CNT; - //dir1=TIM2->CR1&TIM_CR1_DIR; - - //It reads the ADC1 value converted from 12bit to 16bit resolution - //voltage1=adc1.read_u16(); - - //It resets the DAC1 - //dac1.free(); - dac2_val = dac2.read(); - - //It writes the DAC1 value as a 16bit number - //dac1.write_u16(count3*2); - - //It writes the DAC1 value as a subunitary float number - //to be multiplied with the max DAC_RANGE - - - //It gets the one loop position and the direction of the encoder - count3=__HAL_TIM_GET_COUNTER(&timer2); - dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); + //Prints the timestamp in miliseconds + printf("T:%u\r\n", us_ticker_read()/1000); - if (i >= 100) { - adc3_val = adc3.read(); - ADC_read(adc1); - ADC_read(adc2); - ADC_read(adc3); - //dac1.write(adc3_val); - - printf("%ld%s passes=%d\r\n", count3, dir1==0 ? "+":"-", count2); - printf("Voltage ADC1= %3.3f%V, DAC=%f\r\n", (3.3f*samples[ADC_Count-2]+0.022f), dac2_val*3.3f); - printf("VOUT: %f\r\n", dac2_val*15.61f+1.0f); - printf("Voltage ADC2: %3.3f%V\r\n", (3.3f*samples[ADC_Count-1]+0.062f)/0.207048458f); + if (pos_en) { + //It gets the position and the direction of the encoder + count1=__HAL_TIM_GET_COUNTER(&timer2); + dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); + printf("P:%ld S:%s C:%d\r\n", count1, dir1==0 ? "+":"-", count2); + if (posIndex_en) { + // Does not work if speed computation is enabled and uses wait(), + // because speed computation may take too long and wait() + // disables other interrupts, therefore the index pulse + // interrupt as well. + if (count2 > 1) { + printf("PI:%ld S:%s C:%d\r\n", (count1-count4) + (count2-1)*8000 + count3, dir1==0 ? "+":"-", count2); + } else if (count2 < -1) { + printf("PI:%ld S:%s C:%d\r\n", (count1-count4) + (count2+1)*8000 + count3, dir1==0 ? "+":"-", count2); + } else { + printf("PI:%ld S:%s C:%d\r\n", (count1-count4) + count3, dir1==0 ? "+":"-", count2); + } + } + } - //printf("Vref(f): %f, Vref : %u, Temperature : %u\r\n", - // vref.read(), vref.read_u16(), tempint.read_u16()); - if (adc3_en) { - printf("Voltage ADC3: %3.3f%V\r\n", adc3_val*3.3f); - printf("Average ADC3: %3.3f%V\r\n", (3.3f*samples[ADC_Count]+0.022f)/0.8245614f); - printf("DAC1 read: %3.3f%V\r\n", dac1.read()*3.3f); - printf("DAC2 read: %3.3f%V\r\n", dac2.read()*3.3f); - } - speedLast = speedRead(); - //printf("\033[%dA", lines); // Moves cursor up of #lines + if (speed_en) { + //Print speed + printf("S:%f\r\n", speedRead()); + } - i=0; + if (adc_en) { + printf("Phase= %3.3f%V\r\n", (3.3f*ADC_read(adc1)+0.022f)*125.62f); + printf("Voltage DCPS: %3.3f%V\r\n", (3.3f*ADC_read(adc2)+0.062f)/0.207f); + printf("Average Current: %3.3f%mA\r\n", 3.3f*ADC_read(adc3)+0.022f); } - i++; - wait(0.04); + wait(0.09); } } \ No newline at end of file