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.
main.cpp@15:8adff67fe707, 2017-06-14 (annotated)
- Committer:
- tonnyleonard
- Date:
- Wed Jun 14 01:23:53 2017 +0000
- Revision:
- 15:8adff67fe707
- Parent:
- 14:e5f5b345b2fe
- Child:
- 16:e423f891cfbc
Create encoder counter based on index counting, update readData() with new parsing options, adjust speed computation to avoid interrupts, clean code. Quasi final version.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tonnyleonard | 0:789510d98ade | 1 | /* |
tonnyleonard | 10:8862c8779b71 | 2 | * Nucleo STM32F4(or L4) quadrature decoder, ADCs and DACs |
tonnyleonard | 8:28ad0ba5a673 | 3 | * with serial communication over the USB virtual serial port |
tonnyleonard | 10:8862c8779b71 | 4 | |
tonnyleonard | 8:28ad0ba5a673 | 5 | * Developed for Elliptec X15 piezoelectric motor control, on a L432KC board |
tonnyleonard | 8:28ad0ba5a673 | 6 | * |
tonnyleonard | 0:789510d98ade | 7 | * Using STM32's counter peripherals to interface rotary encoders. |
tonnyleonard | 10:8862c8779b71 | 8 | * Encoders are supported on F4xx's TIM1,2,3,4,5. TIM2 & TIM5 have 32bit count, |
tonnyleonard | 8:28ad0ba5a673 | 9 | * others 16bit. |
tonnyleonard | 10:8862c8779b71 | 10 | * Take into account that on F4xx Mbed uses TIM5 for system timer, SPI needs TIM1, |
tonnyleonard | 8:28ad0ba5a673 | 11 | * others are used for PWM. |
tonnyleonard | 10:8862c8779b71 | 12 | * Check your platform's PeripheralPins.c & PeripheralNames.h if you need |
tonnyleonard | 8:28ad0ba5a673 | 13 | * both PWM & encoders. This project does not use PWM. |
tonnyleonard | 0:789510d98ade | 14 | * |
tonnyleonard | 10:8862c8779b71 | 15 | * On L432KC, for example, only TIM2 has 32bit count, others have 16bit. |
tonnyleonard | 8:28ad0ba5a673 | 16 | * However, mbed has TIM2 assigned by default to the system ticker |
tonnyleonard | 10:8862c8779b71 | 17 | * For L432KC to work with TIM2 encoder input one needs to reassign |
tonnyleonard | 10:8862c8779b71 | 18 | * the system ticker from TIM2 to TIM7, for example, |
tonnyleonard | 8:28ad0ba5a673 | 19 | * in mbed-dev/targets/TARGET_STM/TARGET_STM32L4/TARGET_STM32L432xC/device/hal_tick.h |
tonnyleonard | 8:28ad0ba5a673 | 20 | * |
tonnyleonard | 8:28ad0ba5a673 | 21 | * Edit HAL_TIM_Encoder_MspInitFx(Lx).cpp to suit your mcu & board's available |
tonnyleonard | 8:28ad0ba5a673 | 22 | * pinouts & pullups/downs. |
tonnyleonard | 0:789510d98ade | 23 | * |
tonnyleonard | 0:789510d98ade | 24 | * Thanks to: |
tonnyleonard | 8:28ad0ba5a673 | 25 | * David Lowe (for the quadrature encoder code) |
tonnyleonard | 8:28ad0ba5a673 | 26 | * https://developer.mbed.org/users/gregeric/code/Nucleo_Hello_Encoder/ |
tonnyleonard | 10:8862c8779b71 | 27 | * |
tonnyleonard | 9:4d736d29ce19 | 28 | * And Frederic Blanc |
tonnyleonard | 9:4d736d29ce19 | 29 | * https://developer.mbed.org/users/fblanc/code/AnalogIn_Diff/ |
tonnyleonard | 9:4d736d29ce19 | 30 | * |
tonnyleonard | 9:4d736d29ce19 | 31 | * And Eric Lewiston / STM32L4xx_HAL_Driver |
tonnyleonard | 9:4d736d29ce19 | 32 | * https://developer.mbed.org/users/EricLew/code/STM32L4xx_HAL_Driver/docs/tip/group__ADC__LL__EF__Configuration__Channels.html |
tonnyleonard | 0:789510d98ade | 33 | * |
tonnyleonard | 0:789510d98ade | 34 | * References: |
tonnyleonard | 8:28ad0ba5a673 | 35 | * http://www.st.com/resource/en/datasheet/stm32l432kc.pdf |
tonnyleonard | 8:28ad0ba5a673 | 36 | * https://developer.mbed.org/platforms/ST-Nucleo-L432KC/ |
tonnyleonard | 0:789510d98ade | 37 | * http://www.st.com/web/en/resource/technical/document/application_note/DM00042534.pdf |
tonnyleonard | 0:789510d98ade | 38 | * http://www.st.com/web/en/resource/technical/document/datasheet/DM00102166.pdf |
tonnyleonard | 7:7f59b69d8895 | 39 | * |
tonnyleonard | 8:28ad0ba5a673 | 40 | * Tonny-Leonard Farauanu, 2017 |
tonnyleonard | 0:789510d98ade | 41 | */ |
tonnyleonard | 0:789510d98ade | 42 | |
tonnyleonard | 0:789510d98ade | 43 | #include "mbed.h" |
tonnyleonard | 0:789510d98ade | 44 | #include "Encoder.h" |
tonnyleonard | 8:28ad0ba5a673 | 45 | |
tonnyleonard | 8:28ad0ba5a673 | 46 | //Defining the timer and its coresponding encoder |
tonnyleonard | 8:28ad0ba5a673 | 47 | TIM_HandleTypeDef timer2; |
tonnyleonard | 4:4f115819171f | 48 | TIM_Encoder_InitTypeDef encoder1; |
tonnyleonard | 8:28ad0ba5a673 | 49 | |
tonnyleonard | 15:8adff67fe707 | 50 | //Timer to be used for the speed computation function |
tonnyleonard | 15:8adff67fe707 | 51 | Timer timer1; |
tonnyleonard | 15:8adff67fe707 | 52 | |
tonnyleonard | 8:28ad0ba5a673 | 53 | //The input for the encoder's index channel |
tonnyleonard | 5:29372f6cb533 | 54 | InterruptIn event(PA_8); |
tonnyleonard | 8:28ad0ba5a673 | 55 | |
tonnyleonard | 8:28ad0ba5a673 | 56 | //LED1 to signal USB serial RX interrupt reading |
tonnyleonard | 8:28ad0ba5a673 | 57 | DigitalOut led1(LED1); |
tonnyleonard | 8:28ad0ba5a673 | 58 | |
tonnyleonard | 8:28ad0ba5a673 | 59 | //LED2 to signal the encoder's index impulses |
tonnyleonard | 10:8862c8779b71 | 60 | //only for feedback, to calibrate the position |
tonnyleonard | 8:28ad0ba5a673 | 61 | //of the AVAGO encoder with respect to the shaft |
tonnyleonard | 8:28ad0ba5a673 | 62 | DigitalOut led2(PB_4); |
tonnyleonard | 8:28ad0ba5a673 | 63 | |
tonnyleonard | 15:8adff67fe707 | 64 | //Relay port to power on and off the Digitally Controlled Power Source (DCPS) |
tonnyleonard | 12:8c355d78e081 | 65 | DigitalOut relay1(PB_5); |
tonnyleonard | 12:8c355d78e081 | 66 | |
tonnyleonard | 8:28ad0ba5a673 | 67 | //Defining the ADCs |
tonnyleonard | 15:8adff67fe707 | 68 | //For reading the Phase Comparator output |
tonnyleonard | 6:5d4c09973041 | 69 | AnalogIn adc1(PA_3); //ADC1_IN8 |
tonnyleonard | 15:8adff67fe707 | 70 | //For reading the DCPS output |
tonnyleonard | 15:8adff67fe707 | 71 | AnalogIn adc2(PA_4); //ADC1_IN9 |
tonnyleonard | 15:8adff67fe707 | 72 | //For reading the current value from the shunt monitor |
tonnyleonard | 6:5d4c09973041 | 73 | AnalogIn adc3(PA_6); //ADC1_IN11 |
tonnyleonard | 8:28ad0ba5a673 | 74 | |
tonnyleonard | 15:8adff67fe707 | 75 | //Defining the DAC for the input of DCPS |
tonnyleonard | 8:28ad0ba5a673 | 76 | //(DCPS - Digitally Controlled Power Source) |
tonnyleonard | 8:28ad0ba5a673 | 77 | //DAC1_OUT2 on pin PA_5 is at least twices as fast as DAC1_OUT1 on pin PA_4 |
tonnyleonard | 15:8adff67fe707 | 78 | AnalogOut dac1(PA_5); // DAC1_OUT2 |
tonnyleonard | 8:28ad0ba5a673 | 79 | |
tonnyleonard | 8:28ad0ba5a673 | 80 | //Defining the serial object to be used for communicating with Raspi |
tonnyleonard | 8:28ad0ba5a673 | 81 | Serial raspi(USBTX, USBRX); |
tonnyleonard | 10:8862c8779b71 | 82 | |
tonnyleonard | 15:8adff67fe707 | 83 | //Counter for the absolute position from start |
tonnyleonard | 15:8adff67fe707 | 84 | int32_t count1 = 0; |
tonnyleonard | 15:8adff67fe707 | 85 | //Counter for the index passes |
tonnyleonard | 15:8adff67fe707 | 86 | int32_t count2 = 0; |
tonnyleonard | 15:8adff67fe707 | 87 | //Records the wheel offset position to the index |
tonnyleonard | 15:8adff67fe707 | 88 | int32_t count3 = 0; |
tonnyleonard | 15:8adff67fe707 | 89 | //Counter recording absolute position at last index pulse |
tonnyleonard | 15:8adff67fe707 | 90 | int32_t count4 = 0; |
tonnyleonard | 15:8adff67fe707 | 91 | //Used to filter the first index pulse, so that the offset position |
tonnyleonard | 15:8adff67fe707 | 92 | //to the index may be recorded |
tonnyleonard | 15:8adff67fe707 | 93 | volatile bool adjustOffset = false; |
tonnyleonard | 14:e5f5b345b2fe | 94 | |
tonnyleonard | 15:8adff67fe707 | 95 | //Last data written to dac1 (using subunitary values) |
tonnyleonard | 15:8adff67fe707 | 96 | float dac_val = 0.0; |
tonnyleonard | 8:28ad0ba5a673 | 97 | |
tonnyleonard | 14:e5f5b345b2fe | 98 | //Array with adc1, adc2 & adc3 correction addition, |
tonnyleonard | 15:8adff67fe707 | 99 | //experimentally determined (hardware offset voltage dependent) |
tonnyleonard | 14:e5f5b345b2fe | 100 | const float adc_corr[] = { 0, 0.022f, 0.2995, 0.0267}; |
tonnyleonard | 15:8adff67fe707 | 101 | |
tonnyleonard | 15:8adff67fe707 | 102 | //Enable ADC reading and printing to the serial interface |
tonnyleonard | 15:8adff67fe707 | 103 | bool adc_en = true; |
tonnyleonard | 15:8adff67fe707 | 104 | |
tonnyleonard | 15:8adff67fe707 | 105 | //Enables speed computing and printing to the serial interface |
tonnyleonard | 15:8adff67fe707 | 106 | bool speed_en = true; |
tonnyleonard | 14:e5f5b345b2fe | 107 | |
tonnyleonard | 15:8adff67fe707 | 108 | //Enable position reading and printing to the serial interface |
tonnyleonard | 15:8adff67fe707 | 109 | bool pos_en = true; |
tonnyleonard | 8:28ad0ba5a673 | 110 | |
tonnyleonard | 15:8adff67fe707 | 111 | //Enable alternative position computation and printing to the serial interface, |
tonnyleonard | 15:8adff67fe707 | 112 | //relative to the encoder index pulses counter |
tonnyleonard | 15:8adff67fe707 | 113 | bool posIndex_en = true; |
tonnyleonard | 8:28ad0ba5a673 | 114 | |
tonnyleonard | 8:28ad0ba5a673 | 115 | //Function invoked by the encoder's index interrupt pulses |
tonnyleonard | 15:8adff67fe707 | 116 | //It counts the index pulses, incrementing or decrementing the number, |
tonnyleonard | 15:8adff67fe707 | 117 | //and registers the offset position of the wheel at start in count3; |
tonnyleonard | 15:8adff67fe707 | 118 | //The index counter is used to correct possible reading errors of the |
tonnyleonard | 15:8adff67fe707 | 119 | //encoder's counter |
tonnyleonard | 15:8adff67fe707 | 120 | //led2 is used for visual feedback. |
tonnyleonard | 7:7f59b69d8895 | 121 | void atint() |
tonnyleonard | 7:7f59b69d8895 | 122 | { |
tonnyleonard | 15:8adff67fe707 | 123 | count4 =__HAL_TIM_GET_COUNTER(&timer2); |
tonnyleonard | 7:7f59b69d8895 | 124 | if (__HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2)) { |
tonnyleonard | 15:8adff67fe707 | 125 | if (count2 == 0 && adjustOffset == false) { //catch first index pulse |
tonnyleonard | 15:8adff67fe707 | 126 | count3 = count4; |
tonnyleonard | 15:8adff67fe707 | 127 | adjustOffset = true; |
tonnyleonard | 5:29372f6cb533 | 128 | } |
tonnyleonard | 15:8adff67fe707 | 129 | count2--; |
tonnyleonard | 15:8adff67fe707 | 130 | led2 =!led2; |
tonnyleonard | 7:7f59b69d8895 | 131 | } else { |
tonnyleonard | 15:8adff67fe707 | 132 | if (count2 == 0 && adjustOffset == false) { //catch first index pulse |
tonnyleonard | 15:8adff67fe707 | 133 | count3 = count4; |
tonnyleonard | 15:8adff67fe707 | 134 | adjustOffset = true; |
tonnyleonard | 5:29372f6cb533 | 135 | } |
tonnyleonard | 15:8adff67fe707 | 136 | count2++; |
tonnyleonard | 15:8adff67fe707 | 137 | led2 =!led2; |
tonnyleonard | 5:29372f6cb533 | 138 | } |
tonnyleonard | 5:29372f6cb533 | 139 | } |
tonnyleonard | 15:8adff67fe707 | 140 | |
tonnyleonard | 15:8adff67fe707 | 141 | //Function for adaptive speed computation |
tonnyleonard | 14:e5f5b345b2fe | 142 | float speedRead() |
tonnyleonard | 14:e5f5b345b2fe | 143 | { |
tonnyleonard | 15:8adff67fe707 | 144 | //counter for the waiting time window |
tonnyleonard | 10:8862c8779b71 | 145 | uint16_t i = 0; |
tonnyleonard | 15:8adff67fe707 | 146 | //sample time for speed computation |
tonnyleonard | 10:8862c8779b71 | 147 | uint32_t deltaT; |
tonnyleonard | 15:8adff67fe707 | 148 | |
tonnyleonard | 15:8adff67fe707 | 149 | //Computer speed |
tonnyleonard | 10:8862c8779b71 | 150 | float speed; |
tonnyleonard | 15:8adff67fe707 | 151 | |
tonnyleonard | 15:8adff67fe707 | 152 | //First and second registered position |
tonnyleonard | 15:8adff67fe707 | 153 | int32_t pos1; |
tonnyleonard | 15:8adff67fe707 | 154 | int32_t pos2; |
tonnyleonard | 15:8adff67fe707 | 155 | //position difference to be used for speed computation |
tonnyleonard | 10:8862c8779b71 | 156 | int32_t pos; |
tonnyleonard | 15:8adff67fe707 | 157 | //Direction of rotation, read from the encoder, for the first |
tonnyleonard | 15:8adff67fe707 | 158 | //and the second registered positions |
tonnyleonard | 10:8862c8779b71 | 159 | int8_t sens1; |
tonnyleonard | 10:8862c8779b71 | 160 | int8_t sens2; |
tonnyleonard | 15:8adff67fe707 | 161 | |
tonnyleonard | 15:8adff67fe707 | 162 | timer1.start(); |
tonnyleonard | 10:8862c8779b71 | 163 | pos1=__HAL_TIM_GET_COUNTER(&timer2); |
tonnyleonard | 10:8862c8779b71 | 164 | sens1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
tonnyleonard | 15:8adff67fe707 | 165 | //Minumum waiting time for this project would be 100 microseconds, |
tonnyleonard | 15:8adff67fe707 | 166 | //for 10 kHz quadrature frequency. We will set it 100 times longer |
tonnyleonard | 15:8adff67fe707 | 167 | //wait_ms(10); |
tonnyleonard | 15:8adff67fe707 | 168 | // Avoiding wait(), since it uses interrupts and timing here |
tonnyleonard | 15:8adff67fe707 | 169 | // is not critical. This takes 10 ms, if not interrupted |
tonnyleonard | 15:8adff67fe707 | 170 | for (uint32_t j=0; j<199950; j++){} |
tonnyleonard | 10:8862c8779b71 | 171 | pos2=__HAL_TIM_GET_COUNTER(&timer2); |
tonnyleonard | 10:8862c8779b71 | 172 | sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
tonnyleonard | 14:e5f5b345b2fe | 173 | |
tonnyleonard | 10:8862c8779b71 | 174 | //The speed computation method adapts to slow/fast speeds, in order to |
tonnyleonard | 10:8862c8779b71 | 175 | //optimize the rapport between computation precision and time windows size |
tonnyleonard | 15:8adff67fe707 | 176 | while (pos2 == pos1 && i<49) { // The accepted max delay time is 0.5 seconds |
tonnyleonard | 15:8adff67fe707 | 177 | //wait_ms(10); |
tonnyleonard | 15:8adff67fe707 | 178 | // Avoiding wait(), since it uses interrupts and timing here |
tonnyleonard | 15:8adff67fe707 | 179 | // is not critical. This takes 10 ms, if not interrupted |
tonnyleonard | 15:8adff67fe707 | 180 | for (uint32_t j=0; j<199950; j++){} |
tonnyleonard | 10:8862c8779b71 | 181 | i++; |
tonnyleonard | 10:8862c8779b71 | 182 | pos2=__HAL_TIM_GET_COUNTER(&timer2); |
tonnyleonard | 10:8862c8779b71 | 183 | sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
tonnyleonard | 14:e5f5b345b2fe | 184 | } |
tonnyleonard | 10:8862c8779b71 | 185 | pos2=__HAL_TIM_GET_COUNTER(&timer2); |
tonnyleonard | 10:8862c8779b71 | 186 | sens2 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
tonnyleonard | 15:8adff67fe707 | 187 | timer1.stop(); |
tonnyleonard | 15:8adff67fe707 | 188 | deltaT = timer1.read_us(); |
tonnyleonard | 15:8adff67fe707 | 189 | timer1.reset(); |
tonnyleonard | 15:8adff67fe707 | 190 | if (sens1 == sens2) { |
tonnyleonard | 15:8adff67fe707 | 191 | pos = pos2 - pos1; |
tonnyleonard | 15:8adff67fe707 | 192 | } else { |
tonnyleonard | 15:8adff67fe707 | 193 | printf("E:Speed computation error, change of direction between readings!\r\n"); |
tonnyleonard | 15:8adff67fe707 | 194 | } |
tonnyleonard | 14:e5f5b345b2fe | 195 | |
tonnyleonard | 15:8adff67fe707 | 196 | //For debugging |
tonnyleonard | 15:8adff67fe707 | 197 | //printf("%lu microseconds, %ld steps: start:%lu stop%lu\r\n", deltaT, pos, pos1, pos2); |
tonnyleonard | 14:e5f5b345b2fe | 198 | if (deltaT > 0) { |
tonnyleonard | 15:8adff67fe707 | 199 | //speed computation in rot/s |
tonnyleonard | 11:7a9837acbea0 | 200 | speed = ((float) pos)*125.f/((float) deltaT); // (pulses/us)*1000000/8000 -> rot/s |
tonnyleonard | 14:e5f5b345b2fe | 201 | } else { |
tonnyleonard | 15:8adff67fe707 | 202 | printf("E:Error, time interval not greater than zero, speed not calculated!\r\n"); |
tonnyleonard | 14:e5f5b345b2fe | 203 | } |
tonnyleonard | 10:8862c8779b71 | 204 | return speed; |
tonnyleonard | 14:e5f5b345b2fe | 205 | } |
tonnyleonard | 0:789510d98ade | 206 | |
tonnyleonard | 15:8adff67fe707 | 207 | //Function attached to the serial RX interrupt event, it parses |
tonnyleonard | 15:8adff67fe707 | 208 | //the serial messages and invoques the corresponding commands |
tonnyleonard | 7:7f59b69d8895 | 209 | void readData(void) |
tonnyleonard | 7:7f59b69d8895 | 210 | { |
tonnyleonard | 15:8adff67fe707 | 211 | char message[50]; |
tonnyleonard | 8:28ad0ba5a673 | 212 | if(raspi.readable()) { |
tonnyleonard | 15:8adff67fe707 | 213 | // Signalling the beginning of serial read |
tonnyleonard | 8:28ad0ba5a673 | 214 | led1 = 1; |
tonnyleonard | 15:8adff67fe707 | 215 | int p1 = 0; |
tonnyleonard | 15:8adff67fe707 | 216 | |
tonnyleonard | 8:28ad0ba5a673 | 217 | raspi.scanf("%s", message); |
tonnyleonard | 15:8adff67fe707 | 218 | //Message received printed for debugging |
tonnyleonard | 15:8adff67fe707 | 219 | //printf("M:%d %s\r\n", strlen(message), message); |
tonnyleonard | 15:8adff67fe707 | 220 | |
tonnyleonard | 15:8adff67fe707 | 221 | if (strcmp(message, "adcOn") == 0) { |
tonnyleonard | 15:8adff67fe707 | 222 | adc_en = true; |
tonnyleonard | 15:8adff67fe707 | 223 | printf("M:ADC true\r\n"); |
tonnyleonard | 15:8adff67fe707 | 224 | } else if (strcmp(message, "adcOff") == 0) { |
tonnyleonard | 15:8adff67fe707 | 225 | adc_en = false; |
tonnyleonard | 15:8adff67fe707 | 226 | printf("M:ADC false\r\n"); |
tonnyleonard | 15:8adff67fe707 | 227 | } else if (p1=strstr(message, "dac=") != NULL) { |
tonnyleonard | 15:8adff67fe707 | 228 | //Writing the dac1 value read from serial |
tonnyleonard | 14:e5f5b345b2fe | 229 | //The DCPS has 1V offset, so we have to remove it |
tonnyleonard | 15:8adff67fe707 | 230 | dac_val = (atof(message+p1+3)-1.0f)/15.51; |
tonnyleonard | 15:8adff67fe707 | 231 | dac1.write(dac_val); |
tonnyleonard | 15:8adff67fe707 | 232 | printf("M:Value to write to DAC: %f\r\n", dac_val*3.3f); |
tonnyleonard | 10:8862c8779b71 | 233 | } else if (strcmp(message, "reset") == 0) { |
tonnyleonard | 15:8adff67fe707 | 234 | //encoder related counters reset command |
tonnyleonard | 10:8862c8779b71 | 235 | TIM2->CNT = 0x0000; |
tonnyleonard | 15:8adff67fe707 | 236 | count1 = 0; |
tonnyleonard | 15:8adff67fe707 | 237 | count2 = 0; |
tonnyleonard | 15:8adff67fe707 | 238 | count3 = 0; |
tonnyleonard | 15:8adff67fe707 | 239 | count4 = 0; |
tonnyleonard | 15:8adff67fe707 | 240 | adjustOffset = false; |
tonnyleonard | 15:8adff67fe707 | 241 | printf("M:Encoder counters reset!\r\n"); |
tonnyleonard | 15:8adff67fe707 | 242 | } else if (strcmp(message, "powerOn") == 0) { |
tonnyleonard | 15:8adff67fe707 | 243 | //command to power on the DCPS |
tonnyleonard | 12:8c355d78e081 | 244 | relay1.write(1); |
tonnyleonard | 15:8adff67fe707 | 245 | printf("M:DCPS on\r\n"); |
tonnyleonard | 15:8adff67fe707 | 246 | } else if (strcmp(message, "powerOff") == 0) { |
tonnyleonard | 15:8adff67fe707 | 247 | //command to power off the DCPS |
tonnyleonard | 12:8c355d78e081 | 248 | relay1.write(0); |
tonnyleonard | 15:8adff67fe707 | 249 | printf("M:DCPS off\r\n"); |
tonnyleonard | 15:8adff67fe707 | 250 | } else if (strcmp(message, "posOn") == 0) { |
tonnyleonard | 15:8adff67fe707 | 251 | //command to enable the encoder position notification |
tonnyleonard | 15:8adff67fe707 | 252 | pos_en = true; |
tonnyleonard | 15:8adff67fe707 | 253 | printf("M:Position notification enabled\r\n"); |
tonnyleonard | 15:8adff67fe707 | 254 | } else if (strcmp(message, "posOff") == 0) { |
tonnyleonard | 15:8adff67fe707 | 255 | //command to disable the encoder position notification |
tonnyleonard | 15:8adff67fe707 | 256 | pos_en = false; |
tonnyleonard | 15:8adff67fe707 | 257 | printf("M:Position notification disabled\r\n"); |
tonnyleonard | 15:8adff67fe707 | 258 | } else if (strcmp(message, "posIndexOn") == 0) { |
tonnyleonard | 15:8adff67fe707 | 259 | //command to enable the index related encoder position notification |
tonnyleonard | 15:8adff67fe707 | 260 | posIndex_en = true; |
tonnyleonard | 15:8adff67fe707 | 261 | printf("M:Index related position notification enabled\r\n"); |
tonnyleonard | 15:8adff67fe707 | 262 | } else if (strcmp(message, "posIndexOff") == 0) { |
tonnyleonard | 15:8adff67fe707 | 263 | //command to disable the index related encoder position notification |
tonnyleonard | 15:8adff67fe707 | 264 | posIndex_en = false; |
tonnyleonard | 15:8adff67fe707 | 265 | printf("M:Index related position notification disabled\r\n"); |
tonnyleonard | 15:8adff67fe707 | 266 | } else if (strcmp(message, "speedOn") == 0) { |
tonnyleonard | 15:8adff67fe707 | 267 | //command to enable speed computation and notification |
tonnyleonard | 15:8adff67fe707 | 268 | speed_en = true; |
tonnyleonard | 15:8adff67fe707 | 269 | printf("M:Speed enabled\r\n"); |
tonnyleonard | 15:8adff67fe707 | 270 | } else if (strcmp(message, "speedOff") == 0) { |
tonnyleonard | 15:8adff67fe707 | 271 | //command to disable speed computation and notification |
tonnyleonard | 15:8adff67fe707 | 272 | speed_en = false; |
tonnyleonard | 15:8adff67fe707 | 273 | printf("M:Speed disabled\r\n"); |
tonnyleonard | 10:8862c8779b71 | 274 | } |
tonnyleonard | 10:8862c8779b71 | 275 | |
tonnyleonard | 8:28ad0ba5a673 | 276 | } |
tonnyleonard | 15:8adff67fe707 | 277 | //Signalling the end of searial read |
tonnyleonard | 8:28ad0ba5a673 | 278 | led1 = 0; |
tonnyleonard | 8:28ad0ba5a673 | 279 | } |
tonnyleonard | 7:7f59b69d8895 | 280 | |
tonnyleonard | 15:8adff67fe707 | 281 | float ADC_read(AnalogIn adc) |
tonnyleonard | 14:e5f5b345b2fe | 282 | { |
tonnyleonard | 15:8adff67fe707 | 283 | |
tonnyleonard | 15:8adff67fe707 | 284 | float Voltage; |
tonnyleonard | 15:8adff67fe707 | 285 | float Voltage_total = 0.0; |
tonnyleonard | 15:8adff67fe707 | 286 | |
tonnyleonard | 15:8adff67fe707 | 287 | // Voltage is summed then averaged |
tonnyleonard | 15:8adff67fe707 | 288 | for (int i=0; i<100; i++) { // do 100 readings |
tonnyleonard | 14:e5f5b345b2fe | 289 | Voltage = adc.read(); |
tonnyleonard | 14:e5f5b345b2fe | 290 | |
tonnyleonard | 15:8adff67fe707 | 291 | Voltage_total = Voltage_total+ Voltage; |
tonnyleonard | 14:e5f5b345b2fe | 292 | //wait_us(10); |
tonnyleonard | 14:e5f5b345b2fe | 293 | } |
tonnyleonard | 15:8adff67fe707 | 294 | Voltage = Voltage_total/100.f; |
tonnyleonard | 15:8adff67fe707 | 295 | return Voltage; |
tonnyleonard | 14:e5f5b345b2fe | 296 | } |
tonnyleonard | 14:e5f5b345b2fe | 297 | |
tonnyleonard | 8:28ad0ba5a673 | 298 | //The main function |
tonnyleonard | 0:789510d98ade | 299 | int main() |
tonnyleonard | 0:789510d98ade | 300 | { |
tonnyleonard | 12:8c355d78e081 | 301 | //Power onn the DCPS |
tonnyleonard | 15:8adff67fe707 | 302 | relay1.write(0); |
tonnyleonard | 14:e5f5b345b2fe | 303 | |
tonnyleonard | 7:7f59b69d8895 | 304 | //counting on both A&B inputs (A at PA0, B at PA1), 4 ticks per cycle, |
tonnyleonard | 5:29372f6cb533 | 305 | //full 32-bit count |
tonnyleonard | 7:7f59b69d8895 | 306 | //For L432KC to work with TIM2 one needs to reassign the system ticker |
tonnyleonard | 10:8862c8779b71 | 307 | //from TIM2 to TIM7 in TARGET/../device/hal_tick.h\ |
tonnyleonard | 8:28ad0ba5a673 | 308 | //Initialise encoder |
tonnyleonard | 4:4f115819171f | 309 | EncoderInit(&encoder1, &timer2, TIM2, 0xffffffff, TIM_ENCODERMODE_TI12); |
tonnyleonard | 10:8862c8779b71 | 310 | |
tonnyleonard | 10:8862c8779b71 | 311 | //Define function to call for interrupt event rise |
tonnyleonard | 8:28ad0ba5a673 | 312 | //This is triggered by the encoder's index pulses |
tonnyleonard | 8:28ad0ba5a673 | 313 | //and it resets the encoder counter |
tonnyleonard | 5:29372f6cb533 | 314 | event.rise(&atint); |
tonnyleonard | 14:e5f5b345b2fe | 315 | |
tonnyleonard | 12:8c355d78e081 | 316 | //Set serial baud rate |
tonnyleonard | 12:8c355d78e081 | 317 | raspi.baud(115200); |
tonnyleonard | 14:e5f5b345b2fe | 318 | |
tonnyleonard | 8:28ad0ba5a673 | 319 | //Attach functin to call for serial interrupt event |
tonnyleonard | 8:28ad0ba5a673 | 320 | raspi.attach(&readData); |
tonnyleonard | 7:7f59b69d8895 | 321 | |
tonnyleonard | 8:28ad0ba5a673 | 322 | //Message to mark the initialisation of the program |
tonnyleonard | 15:8adff67fe707 | 323 | printf("M:\n\rSTM HAL encoder with ADC and DAC\n\r"); |
tonnyleonard | 15:8adff67fe707 | 324 | printf("M:Running at %u MHz\r\n\n", HAL_RCC_GetSysClockFreq()/1000000); |
tonnyleonard | 7:7f59b69d8895 | 325 | |
tonnyleonard | 8:28ad0ba5a673 | 326 | //The main loop |
tonnyleonard | 0:789510d98ade | 327 | while(1) { |
tonnyleonard | 10:8862c8779b71 | 328 | |
tonnyleonard | 8:28ad0ba5a673 | 329 | //Variable for the direction of the counter |
tonnyleonard | 4:4f115819171f | 330 | int8_t dir1; |
tonnyleonard | 10:8862c8779b71 | 331 | |
tonnyleonard | 15:8adff67fe707 | 332 | //Prints the timestamp in miliseconds |
tonnyleonard | 15:8adff67fe707 | 333 | printf("T:%u\r\n", us_ticker_read()/1000); |
tonnyleonard | 10:8862c8779b71 | 334 | |
tonnyleonard | 15:8adff67fe707 | 335 | if (pos_en) { |
tonnyleonard | 15:8adff67fe707 | 336 | //It gets the position and the direction of the encoder |
tonnyleonard | 15:8adff67fe707 | 337 | count1=__HAL_TIM_GET_COUNTER(&timer2); |
tonnyleonard | 15:8adff67fe707 | 338 | dir1 = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer2); |
tonnyleonard | 15:8adff67fe707 | 339 | printf("P:%ld S:%s C:%d\r\n", count1, dir1==0 ? "+":"-", count2); |
tonnyleonard | 15:8adff67fe707 | 340 | if (posIndex_en) { |
tonnyleonard | 15:8adff67fe707 | 341 | // Does not work if speed computation is enabled and uses wait(), |
tonnyleonard | 15:8adff67fe707 | 342 | // because speed computation may take too long and wait() |
tonnyleonard | 15:8adff67fe707 | 343 | // disables other interrupts, therefore the index pulse |
tonnyleonard | 15:8adff67fe707 | 344 | // interrupt as well. |
tonnyleonard | 15:8adff67fe707 | 345 | if (count2 > 1) { |
tonnyleonard | 15:8adff67fe707 | 346 | printf("PI:%ld S:%s C:%d\r\n", (count1-count4) + (count2-1)*8000 + count3, dir1==0 ? "+":"-", count2); |
tonnyleonard | 15:8adff67fe707 | 347 | } else if (count2 < -1) { |
tonnyleonard | 15:8adff67fe707 | 348 | printf("PI:%ld S:%s C:%d\r\n", (count1-count4) + (count2+1)*8000 + count3, dir1==0 ? "+":"-", count2); |
tonnyleonard | 15:8adff67fe707 | 349 | } else { |
tonnyleonard | 15:8adff67fe707 | 350 | printf("PI:%ld S:%s C:%d\r\n", (count1-count4) + count3, dir1==0 ? "+":"-", count2); |
tonnyleonard | 15:8adff67fe707 | 351 | } |
tonnyleonard | 15:8adff67fe707 | 352 | } |
tonnyleonard | 15:8adff67fe707 | 353 | } |
tonnyleonard | 6:5d4c09973041 | 354 | |
tonnyleonard | 15:8adff67fe707 | 355 | if (speed_en) { |
tonnyleonard | 15:8adff67fe707 | 356 | //Print speed |
tonnyleonard | 15:8adff67fe707 | 357 | printf("S:%f\r\n", speedRead()); |
tonnyleonard | 15:8adff67fe707 | 358 | } |
tonnyleonard | 14:e5f5b345b2fe | 359 | |
tonnyleonard | 15:8adff67fe707 | 360 | if (adc_en) { |
tonnyleonard | 15:8adff67fe707 | 361 | printf("Phase= %3.3f%V\r\n", (3.3f*ADC_read(adc1)+0.022f)*125.62f); |
tonnyleonard | 15:8adff67fe707 | 362 | printf("Voltage DCPS: %3.3f%V\r\n", (3.3f*ADC_read(adc2)+0.062f)/0.207f); |
tonnyleonard | 15:8adff67fe707 | 363 | printf("Average Current: %3.3f%mA\r\n", 3.3f*ADC_read(adc3)+0.022f); |
tonnyleonard | 10:8862c8779b71 | 364 | } |
tonnyleonard | 10:8862c8779b71 | 365 | |
tonnyleonard | 15:8adff67fe707 | 366 | wait(0.09); |
tonnyleonard | 0:789510d98ade | 367 | } |
tonnyleonard | 8:28ad0ba5a673 | 368 | |
tonnyleonard | 8:28ad0ba5a673 | 369 | } |