main.cpp

00001 /*
00002  * Nucleo STM32F4(or L4) quadrature decoder, ADCs and DACs
00003  * with serial communication over the USB virtual serial port
00004 
00005  * Developed for Elliptec X15 piezoelectric motor control, on a L432KC board
00006  *
00007  * Using STM32's counter peripherals to interface rotary encoders.
00008  * Encoders are supported on F4xx's TIM1,2,3,4,5. TIM2 & TIM5 have 32bit count,
00009  * others 16bit.
00010  * Take into account that on F4xx Mbed uses TIM5 for system timer, SPI needs TIM1,
00011  * others are used for PWM.
00012  * Check your platform's PeripheralPins.c & PeripheralNames.h if you need
00013  * both PWM & encoders. This project does not use PWM.
00014  *
00015  * On L432KC, for example, only TIM2 has 32bit count, others have 16bit.
00016  * However, mbed has TIM2 assigned by default to the system ticker
00017  * For L432KC to work with TIM2 encoder input one needs to reassign
00018  * the system ticker from TIM2 to TIM7, for example,
00019  * in mbed-dev/targets/TARGET_STM/TARGET_STM32L4/TARGET_STM32L432xC/device/hal_tick.h
00020  *
00021  * Edit HAL_TIM_Encoder_MspInitFx(Lx).cpp to suit your mcu & board's available
00022  *  pinouts & pullups/downs.
00023  *
00024  * Thanks to:
00025  * David Lowe (for the quadrature encoder code)
00026  * https://developer.mbed.org/users/gregeric/code/Nucleo_Hello_Encoder/
00027  *
00028  * And Frederic Blanc
00029  * https://developer.mbed.org/users/fblanc/code/AnalogIn_Diff/
00030  *
00031  * And Eric Lewiston /  STM32L4xx_HAL_Driver
00032  * https://developer.mbed.org/users/EricLew/code/STM32L4xx_HAL_Driver/docs/tip/group__ADC__LL__EF__Configuration__Channels.html
00033  *
00034  * References:
00035  * http://www.st.com/resource/en/datasheet/stm32l432kc.pdf
00036  * https://developer.mbed.org/platforms/ST-Nucleo-L432KC/
00037  * http://www.st.com/web/en/resource/technical/document/application_note/DM00042534.pdf
00038  * http://www.st.com/web/en/resource/technical/document/datasheet/DM00102166.pdf
00039  *
00040  * Tonny-Leonard Farauanu, 2017
00041  */
00042 
00043 #include "mbed.h"
00044 #include "Encoder.h"
00045 
00046 //Defining the timer and its coresponding encoder
00047 TIM_HandleTypeDef timer2;
00048 TIM_Encoder_InitTypeDef encoder1;
00049 
00050 //Timer to be used for the speed computation function
00051 Timer timer1;
00052 
00053 //The input for the encoder's index channel
00054 InterruptIn event(PA_8);
00055 
00056 //LED1 to signal USB serial RX interrupt reading
00057 DigitalOut led1(LED1);
00058 
00059 //LED2 to signal the encoder's index impulses
00060 //only for feedback, to calibrate the position
00061 //of the AVAGO encoder with respect to the shaft
00062 DigitalOut led2(PB_4);
00063 
00064 //Relay port to power on and off the Digitally Controlled Power Source (DCPS)
00065 DigitalOut relay1(PB_5);
00066 
00067 //Defining the ADCs
00068 //For reading the Phase Comparator output
00069 AnalogIn adc1(PA_3); //ADC1_IN8
00070 //For reading the DCPS output
00071 AnalogIn adc2(PA_4); //ADC1_IN9
00072 //For reading the current value from the shunt monitor
00073 AnalogIn adc3(PA_6); //ADC1_IN11
00074 
00075 //Defining the DAC for the input of DCPS
00076 //(DCPS - Digitally Controlled Power Source)
00077 //DAC1_OUT2 on pin PA_5 is at least twices as fast as DAC1_OUT1 on pin PA_4
00078 AnalogOut dac1(PA_5); // DAC1_OUT2
00079 
00080 //Defining the serial object to be used for communicating with Raspi
00081 Serial raspi(USBTX, USBRX);
00082 
00083 //Counter for the absolute position from start
00084 int32_t count1 = 0;
00085 //Counter for the index passes
00086 volatile int32_t count2 = 0;
00087 //Records the wheel offset position to the index
00088 volatile int32_t count3 = 0;
00089 //Counter recording absolute position at last index pulse
00090 volatile int32_t count4 = 0;
00091 //Used to filter the first index pulse, so that the offset position
00092 //to the index may be recorded
00093 volatile bool adjustOffset = false;
00094 
00095 //Last data written to dac1 (using subunitary values)
00096 float dac_val = 0.0;
00097 
00098 //Array with adc1, adc2 & adc3 correction addition,
00099 //experimentally determined (hardware offset voltage dependent)
00100 const float adc_corr[] = { 0, 0.022f, 0.022f, 0.022f};
00101 
00102 //Enable ADC reading and printing to the serial interface
00103 bool adc_en = true;
00104 
00105 //Enables speed computing and printing to the serial interface
00106 bool speed_en = true;
00107 
00108 //Enable position reading and printing to the serial interface
00109 bool pos_en = true;
00110 
00111 //Enable alternative position computation and printing to the serial interface,
00112 //relative to the encoder index pulses counter
00113 bool posIndex_en = true;
00114 
00115 //Function invoked by the encoder's index interrupt pulses
00116 //It counts the index pulses, incrementing or decrementing the number,
00117 //and registers the offset position of the wheel at start in count3;
00118 //The index counter is used to correct possible reading errors of the
00119 //encoder's counter
00120 //led2 is used for visual feedback.
00121 void atint()
00122 {
00123     count4 =__HAL_TIM_GET_COUNTER(&timer2);
00124     if (__HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2)) {
00125         if (count2 == 0 && adjustOffset == false) { //catch first index pulse
00126             count3 = count4;
00127             adjustOffset = true;
00128         }
00129         count2--;
00130         led2 =!led2;
00131     } else {
00132         if (count2 == 0 && adjustOffset == false) { //catch first index pulse
00133             count3 = count4;
00134             adjustOffset = true;
00135         }
00136         count2++;
00137         led2 =!led2;
00138     }
00139 }
00140 
00141 //Function for adaptive speed computation
00142 float speedRead()
00143 {
00144     //counter for the waiting time window
00145     uint16_t i = 0;
00146     //sample time for speed computation
00147     uint32_t deltaT;
00148 
00149     //Computer speed
00150     float speed;
00151 
00152     //First and second registered position
00153     int32_t pos1;
00154     int32_t pos2;
00155     //position difference to be used for speed computation
00156     int32_t pos;
00157     //Direction of rotation, read from the encoder, for the first
00158     //and the second registered positions
00159     int8_t sens1;
00160     int8_t sens2;
00161 
00162     timer1.start();
00163     pos1=__HAL_TIM_GET_COUNTER(&timer2);
00164     sens1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
00165     //Minumum waiting time for this project would be 100 microseconds,
00166     //for 10 kHz quadrature frequency. We will set it 100 times longer
00167     //wait_ms(10);
00168     // Avoiding wait(), since it uses interrupts and timing here
00169     // is not critical. This takes 10 ms, if not interrupted
00170     for (uint32_t j=0; j<199950; j++) {}
00171     pos2=__HAL_TIM_GET_COUNTER(&timer2);
00172     sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
00173 
00174     //The speed computation method adapts to slow/fast speeds, in order to
00175     //optimize the rapport between computation precision and time windows size
00176     while (pos2 == pos1 && i<9) { // The accepted max delay time is 0.1 seconds
00177         //wait_ms(10);
00178         // Avoiding wait(), since it uses interrupts and timing here
00179         // is not critical. This takes 10 ms, if not interrupted
00180         for (uint32_t j=0; j<199950; j++) {}
00181         i++;
00182         pos2=__HAL_TIM_GET_COUNTER(&timer2);
00183         sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
00184     }
00185     pos2=__HAL_TIM_GET_COUNTER(&timer2);
00186     sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
00187     timer1.stop();
00188     deltaT = timer1.read_us();
00189     timer1.reset();
00190     if (sens1 == sens2) {
00191         pos = pos2 - pos1;
00192     } else {
00193         printf("E:Speed computation error, change of direction between readings!\n");
00194     }
00195 
00196     if (deltaT > 0) {
00197         //speed computation in rot/s
00198         speed = ((float) pos)*125.f/((float) deltaT); // (pulses/us)*1000000/8000 -> rot/s
00199     } else {
00200         printf("E:Error, time interval not greater than zero, speed not calculated!\n");
00201     }
00202     return speed;
00203 }
00204 
00205 //Function attached to the serial RX interrupt event, it parses
00206 //the serial messages and invoques the corresponding commands
00207 void readData(void)
00208 {
00209     led1 = 1;
00210     char message[15];
00211     if (raspi.readable()) {
00212         // Signalling the beginning of serial read
00213         int i = 0;
00214         bool rx = true;
00215         while(rx && i<14) {
00216             message[i] = raspi.getc();
00217             if (message[i] == '\r') {
00218                 message[i] = NULL;
00219                 rx = false;
00220             }
00221             i++;
00222         }
00223         //Message received printed for debugging
00224         //printf("M:%d %s\n", strlen(message), message);
00225         int p1 = 0;
00226         if (strcmp(message, "adcOn") == 0) {
00227             adc_en = true;
00228             printf("M:ADC true\n");
00229         } else if (strcmp(message, "adcOff") == 0) {
00230             adc_en = false;
00231             printf("M:ADC false\n");
00232         } else if (p1=strstr(message, "v=") != NULL) {
00233             //Writing the dac1 value read from serial
00234             //The DCPS has 1V offset, so we have to remove it
00235             dac_val = (atof(message+p1+1)-1.f)/15.51;
00236             dac1.write(dac_val);
00237         } else if (strcmp(message, "reset") == 0) {
00238             //encoder related counters reset command
00239             TIM2->CNT = 0x0000;
00240             count1 = 0;
00241             count2 = 0;
00242             count3 = 0;
00243             count4 = 0;
00244             adjustOffset = false;
00245             printf("M:Encoder counters reset!\n");
00246         } else if (strcmp(message, "powerOn") == 0) {
00247             //command to power on the DCPS
00248             relay1.write(1);
00249             printf("M:DCPS on\n");
00250         } else if (strcmp(message, "powerOff") == 0) {
00251             //command to power off the DCPS
00252             relay1.write(0);
00253             printf("M:DCPS off\n");
00254         } else if (strcmp(message, "posOn") == 0) {
00255             //command to enable the encoder position notification
00256             pos_en = true;
00257             printf("M:Position notification enabled\n");
00258         } else if (strcmp(message, "posOff") == 0) {
00259             //command to disable the encoder position notification
00260             pos_en = false;
00261             printf("M:Position notification disabled\n");
00262         } else if (strcmp(message, "posIndexOn") == 0) {
00263             //command to enable the index related encoder position notification
00264             posIndex_en = true;
00265             printf("M:Index related position notification enabled\n");
00266         } else if (strcmp(message, "posIndexOff") == 0) {
00267             //command to disable the index related encoder position notification
00268             posIndex_en = false;
00269             printf("M:Index related position notification disabled\n");
00270         } else if (strcmp(message, "speedOn") == 0) {
00271             //command to enable speed computation and notification
00272             speed_en = true;
00273             printf("M:Speed enabled\n");
00274         } else if (strcmp(message, "speedOff") == 0) {
00275             //command to disable speed computation and notification
00276             speed_en = false;
00277             printf("M:Speed disabled\n");
00278         }
00279     }
00280     
00281     //Signalling the end of searial read
00282     led1 = 0;
00283 }
00284 
00285 //Function to compute the average of 1000 ADC readings
00286 //Current intensity measurement should be as precise
00287 //as possible, the time delay introduced my multiplying
00288 //the readings is tolerable for this project
00289 float ADC_read(AnalogIn adc)
00290 {
00291     float Voltage;
00292     float Voltage_total = 0.0;
00293 
00294     // Voltage is summed then averaged
00295     for (int i=0; i<1000; i++) { // do 1000 readings
00296         Voltage = adc.read();
00297         Voltage_total = Voltage_total+ Voltage;
00298     }
00299     Voltage = Voltage_total/1000.f;
00300     return Voltage;
00301 }
00302 
00303 //The main function
00304 int main()
00305 {
00306     //Make sure the DCPS is off
00307     relay1.write(0);
00308     //Set the DCPS output to (0.32x3.3)x4.7+1 = ~6V
00309     //These parameters are set for DCPS on load
00310     dac1.write(0.32);
00311 
00312     //counting on both A&B inputs (A at PA0, B at PA1), 4 ticks per cycle,
00313     //full 32-bit count
00314     //For L432KC to work with TIM2 one needs to reassign the system ticker
00315     //from TIM2 to TIM7 in TARGET/../device/hal_tick.h\
00316     //Initialise encoder
00317     EncoderInit(&encoder1, &timer2, TIM2, 0xffffffff, TIM_ENCODERMODE_TI12);
00318 
00319     //Define function to call for interrupt event rise
00320     //This is triggered by the encoder's index pulses
00321     //and it resets the encoder counter
00322     event.rise(&atint);
00323 
00324     //Set serial baud rate
00325     raspi.baud(921600);
00326 
00327     //Attach functin to call for serial interrupt event
00328     //The wait() function ensures that not false serial RX
00329     //interrupt is triggered at power up, because of initial
00330     //serial port install by the Raspi OS
00331     wait(1);
00332     raspi.attach(&readData, Serial::RxIrq);
00333 
00334     //Message to mark the initialisation of the program
00335     printf("M:STM HAL encoder with ADC and DAC\n");
00336     printf("M:Running at %u MHz\n", HAL_RCC_GetSysClockFreq()/1000000);
00337 
00338     //The main loop
00339     while(1) {
00340 
00341         //Variable for the direction of the counter
00342         int8_t dir1;
00343 
00344         //Prints the timestamp in miliseconds
00345         printf("T:%u", us_ticker_read()/1000);
00346 
00347         if (pos_en) {
00348             //It gets the position and the direction of the encoder
00349             count1=__HAL_TIM_GET_COUNTER(&timer2);
00350             dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2);
00351             printf("P:%ldD:%sC:%d", count1, dir1==0 ? "+":"-", count2);
00352             if (posIndex_en) {
00353                 if (count2 > 1) {
00354                     printf("Pi:%ld", (count1-count4) + (count2-1)*8000 + count3);
00355                 } else if (count2 < -1) {
00356                     printf("Pi:%ld", (count1-count4) + (count2+1)*8000 + count3);
00357                 } else {
00358                     printf("Pi:%ld", (count1-count4) + count3);
00359                 }
00360             }
00361         }
00362 
00363         if (speed_en) {
00364             //Print speed in rot/s
00365             printf("S:%.3f%", speedRead());
00366         }
00367 
00368         if (adc_en) {
00369             //Print phase shift in Degrees, DCPS output voltage in Volts
00370             // and current intensity in Ampers
00371             // The dividing factors are hardware dependant
00372             printf("F:%3.3fU:%3.3fI:%3.3f", (3.3f*ADC_read(adc1)+adc_corr[1])*125.62f,
00373                    (3.3f*ADC_read(adc2)+adc_corr[2])/0.207f, 
00374                     3.3f*ADC_read(adc3)+adc_corr[3]);
00375         }
00376         printf("\n");
00377 
00378         wait(0.005);
00379     }
00380 
00381 }