encoder and tachometer class for EW3XX single board computer
EW305sbc.cpp@4:19ee7eb16605, 2019-09-25 (annotated)
- Committer:
- lddevrie
- Date:
- Wed Sep 25 16:05:11 2019 +0000
- Revision:
- 4:19ee7eb16605
- Parent:
- 2:6cf351721d86
- Child:
- 5:58b13292286e
faster update rate on encoder reading (1000 Hz)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lddevrie | 0:1d5bf13d7dbb | 1 | //#include "mbed.h" |
lddevrie | 0:1d5bf13d7dbb | 2 | #include "EW305sbc.h" |
lddevrie | 0:1d5bf13d7dbb | 3 | |
lddevrie | 0:1d5bf13d7dbb | 4 | |
lddevrie | 0:1d5bf13d7dbb | 5 | EW305sbc::EW305sbc(int ch, int pulsesPerRev) |
lddevrie | 0:1d5bf13d7dbb | 6 | : ch_(ch), pulsesPerRev_(pulsesPerRev), spi(p5, p6, p7), ls7166_cs1(p19,1), ls7166_cs2(p20,1) |
lddevrie | 0:1d5bf13d7dbb | 7 | { |
lddevrie | 0:1d5bf13d7dbb | 8 | init(); |
lddevrie | 0:1d5bf13d7dbb | 9 | } |
lddevrie | 0:1d5bf13d7dbb | 10 | |
lddevrie | 0:1d5bf13d7dbb | 11 | void EW305sbc::init() |
lddevrie | 0:1d5bf13d7dbb | 12 | { |
lddevrie | 4:19ee7eb16605 | 13 | dt_ = 0.001; // 500 Hz, also must update lines 19 and 38 since dt_ is not working for some reason?? |
lddevrie | 0:1d5bf13d7dbb | 14 | speed_ = 0.0; // initial distance |
lddevrie | 0:1d5bf13d7dbb | 15 | countp_ = 0; |
lddevrie | 0:1d5bf13d7dbb | 16 | count_ = 0; |
lddevrie | 2:6cf351721d86 | 17 | angle_ = 0.0; |
lddevrie | 0:1d5bf13d7dbb | 18 | /** configure the rising edge to start the timer */ |
lddevrie | 4:19ee7eb16605 | 19 | updater.attach(callback(this, &EW305sbc::recalc), 0.001); // 125 Hz, syntax from https://os.mbed.com/forum/mbed/topic/1964/?page=1 |
lddevrie | 0:1d5bf13d7dbb | 20 | wait(.2); //delay at beginning for voltage settle purposes |
lddevrie | 0:1d5bf13d7dbb | 21 | |
lddevrie | 2:6cf351721d86 | 22 | // initialize channel ch_ encoder by |
lddevrie | 0:1d5bf13d7dbb | 23 | LS7366_reset_counter(ch_); |
lddevrie | 0:1d5bf13d7dbb | 24 | LS7366_quad_mode_x4(ch_); |
lddevrie | 0:1d5bf13d7dbb | 25 | LS7366_write_DTR(ch_,0); |
lddevrie | 0:1d5bf13d7dbb | 26 | } |
lddevrie | 0:1d5bf13d7dbb | 27 | |
lddevrie | 0:1d5bf13d7dbb | 28 | |
lddevrie | 0:1d5bf13d7dbb | 29 | void EW305sbc::recalc() |
lddevrie | 0:1d5bf13d7dbb | 30 | { |
lddevrie | 0:1d5bf13d7dbb | 31 | // Read encoder |
lddevrie | 1:b33d4964669b | 32 | count_ = LS7366_read_counter(ch_); // input is the encoder channel |
lddevrie | 2:6cf351721d86 | 33 | |
lddevrie | 2:6cf351721d86 | 34 | // convert count to angle |
lddevrie | 2:6cf351721d86 | 35 | angle_ = count_*2.0*3.14159/(4.0*pulsesPerRev_); |
lddevrie | 2:6cf351721d86 | 36 | |
lddevrie | 0:1d5bf13d7dbb | 37 | // Estimate speed (counts/sec) |
lddevrie | 4:19ee7eb16605 | 38 | speed_ = -(count_-countp_)/0.001*2.0*3.14159/(4.0*pulsesPerRev_); |
lddevrie | 0:1d5bf13d7dbb | 39 | |
lddevrie | 0:1d5bf13d7dbb | 40 | // Age variable |
lddevrie | 0:1d5bf13d7dbb | 41 | countp_ = count_; |
lddevrie | 0:1d5bf13d7dbb | 42 | } |
lddevrie | 0:1d5bf13d7dbb | 43 | |
lddevrie | 0:1d5bf13d7dbb | 44 | |
lddevrie | 0:1d5bf13d7dbb | 45 | int EW305sbc::getCount(void) |
lddevrie | 0:1d5bf13d7dbb | 46 | { |
lddevrie | 0:1d5bf13d7dbb | 47 | return count_; |
lddevrie | 0:1d5bf13d7dbb | 48 | } |
lddevrie | 0:1d5bf13d7dbb | 49 | |
lddevrie | 0:1d5bf13d7dbb | 50 | float EW305sbc::getSpeed(void) |
lddevrie | 0:1d5bf13d7dbb | 51 | { |
lddevrie | 0:1d5bf13d7dbb | 52 | return speed_; |
lddevrie | 0:1d5bf13d7dbb | 53 | } |
lddevrie | 0:1d5bf13d7dbb | 54 | |
lddevrie | 2:6cf351721d86 | 55 | float EW305sbc::getAngle(void) |
lddevrie | 2:6cf351721d86 | 56 | { |
lddevrie | 2:6cf351721d86 | 57 | return angle_; |
lddevrie | 2:6cf351721d86 | 58 | } |
lddevrie | 0:1d5bf13d7dbb | 59 | |
lddevrie | 0:1d5bf13d7dbb | 60 | |
lddevrie | 0:1d5bf13d7dbb | 61 | //---- Function Listing ------------------------------- |
lddevrie | 0:1d5bf13d7dbb | 62 | |
lddevrie | 0:1d5bf13d7dbb | 63 | //----- LS7366 Encoder/Counter Routines -------------------- |
lddevrie | 0:1d5bf13d7dbb | 64 | void EW305sbc::LS7366_cmd(int inst, int reg) |
lddevrie | 0:1d5bf13d7dbb | 65 | { |
lddevrie | 0:1d5bf13d7dbb | 66 | char cmd; |
lddevrie | 0:1d5bf13d7dbb | 67 | spi.format(8, 0); |
lddevrie | 0:1d5bf13d7dbb | 68 | spi.frequency(2000000); |
lddevrie | 0:1d5bf13d7dbb | 69 | cmd = (inst << 6) | (reg << 3); |
lddevrie | 0:1d5bf13d7dbb | 70 | // printf("\r\ncmd=0X%2X", cmd); |
lddevrie | 0:1d5bf13d7dbb | 71 | spi.write(cmd); |
lddevrie | 0:1d5bf13d7dbb | 72 | } |
lddevrie | 0:1d5bf13d7dbb | 73 | |
lddevrie | 0:1d5bf13d7dbb | 74 | long EW305sbc::LS7366_read_counter(int chan_num) |
lddevrie | 0:1d5bf13d7dbb | 75 | { |
lddevrie | 0:1d5bf13d7dbb | 76 | union bytes { |
lddevrie | 0:1d5bf13d7dbb | 77 | char byte_enc[4]; |
lddevrie | 0:1d5bf13d7dbb | 78 | long long_enc; |
lddevrie | 0:1d5bf13d7dbb | 79 | } counter; |
lddevrie | 0:1d5bf13d7dbb | 80 | |
lddevrie | 0:1d5bf13d7dbb | 81 | counter.long_enc = 0; |
lddevrie | 0:1d5bf13d7dbb | 82 | spi.format(8, 0); |
lddevrie | 0:1d5bf13d7dbb | 83 | spi.frequency(2000000); |
lddevrie | 0:1d5bf13d7dbb | 84 | |
lddevrie | 0:1d5bf13d7dbb | 85 | if(chan_num!=2) { |
lddevrie | 0:1d5bf13d7dbb | 86 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 87 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 88 | LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR |
lddevrie | 0:1d5bf13d7dbb | 89 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 90 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 91 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 92 | } else { |
lddevrie | 0:1d5bf13d7dbb | 93 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 94 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 95 | LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR |
lddevrie | 0:1d5bf13d7dbb | 96 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 97 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 98 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 99 | } |
lddevrie | 0:1d5bf13d7dbb | 100 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 101 | LS7366_cmd(RD,CNTR); //cmd = 0x60, READ from CNTR |
lddevrie | 0:1d5bf13d7dbb | 102 | counter.byte_enc[3] = spi.write(0x00); |
lddevrie | 0:1d5bf13d7dbb | 103 | counter.byte_enc[2] = spi.write(0x00); |
lddevrie | 0:1d5bf13d7dbb | 104 | counter.byte_enc[1] = spi.write(0x00); |
lddevrie | 0:1d5bf13d7dbb | 105 | counter.byte_enc[0] = spi.write(0x00); |
lddevrie | 0:1d5bf13d7dbb | 106 | |
lddevrie | 0:1d5bf13d7dbb | 107 | if(chan_num!=2) { |
lddevrie | 0:1d5bf13d7dbb | 108 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 109 | } else { |
lddevrie | 0:1d5bf13d7dbb | 110 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 111 | } |
lddevrie | 0:1d5bf13d7dbb | 112 | |
lddevrie | 0:1d5bf13d7dbb | 113 | return counter.long_enc; //return count |
lddevrie | 0:1d5bf13d7dbb | 114 | } |
lddevrie | 0:1d5bf13d7dbb | 115 | |
lddevrie | 0:1d5bf13d7dbb | 116 | void EW305sbc::LS7366_quad_mode_x4(int chan_num) |
lddevrie | 0:1d5bf13d7dbb | 117 | { |
lddevrie | 0:1d5bf13d7dbb | 118 | |
lddevrie | 0:1d5bf13d7dbb | 119 | spi.format(8, 0); |
lddevrie | 0:1d5bf13d7dbb | 120 | spi.frequency(2000000); |
lddevrie | 0:1d5bf13d7dbb | 121 | |
lddevrie | 0:1d5bf13d7dbb | 122 | if(chan_num!=2) { |
lddevrie | 0:1d5bf13d7dbb | 123 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 124 | } else { |
lddevrie | 0:1d5bf13d7dbb | 125 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 126 | } |
lddevrie | 0:1d5bf13d7dbb | 127 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 128 | LS7366_cmd(WR,MDR0);// Write to the MDR0 register |
lddevrie | 0:1d5bf13d7dbb | 129 | wait_us(1); |
lddevrie | 0:1d5bf13d7dbb | 130 | spi.write(0x03); // X4 quadrature count mode |
lddevrie | 0:1d5bf13d7dbb | 131 | if(chan_num!=2) { |
lddevrie | 0:1d5bf13d7dbb | 132 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 133 | } else { |
lddevrie | 0:1d5bf13d7dbb | 134 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 135 | } |
lddevrie | 0:1d5bf13d7dbb | 136 | } |
lddevrie | 0:1d5bf13d7dbb | 137 | |
lddevrie | 0:1d5bf13d7dbb | 138 | void EW305sbc::LS7366_reset_counter(int chan_num) |
lddevrie | 0:1d5bf13d7dbb | 139 | { |
lddevrie | 0:1d5bf13d7dbb | 140 | spi.format(8, 0); // set up SPI for 8 data bits, mode 0 |
lddevrie | 0:1d5bf13d7dbb | 141 | spi.frequency(2000000); // 2MHz SPI clock |
lddevrie | 0:1d5bf13d7dbb | 142 | |
lddevrie | 0:1d5bf13d7dbb | 143 | if(chan_num!=2) { // activate chip select |
lddevrie | 0:1d5bf13d7dbb | 144 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 145 | } else { |
lddevrie | 0:1d5bf13d7dbb | 146 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 147 | } |
lddevrie | 0:1d5bf13d7dbb | 148 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 149 | LS7366_cmd(CLR,CNTR); // Clear the counter register |
lddevrie | 0:1d5bf13d7dbb | 150 | if(chan_num!=2) { // de-activate chip select |
lddevrie | 0:1d5bf13d7dbb | 151 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 152 | } else { |
lddevrie | 0:1d5bf13d7dbb | 153 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 154 | } |
lddevrie | 0:1d5bf13d7dbb | 155 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 156 | |
lddevrie | 0:1d5bf13d7dbb | 157 | if(chan_num!=2) { // activate chip select |
lddevrie | 0:1d5bf13d7dbb | 158 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 159 | } else { |
lddevrie | 0:1d5bf13d7dbb | 160 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 161 | } |
lddevrie | 0:1d5bf13d7dbb | 162 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 163 | LS7366_cmd(LOAD,CNTR); // load counter reg |
lddevrie | 0:1d5bf13d7dbb | 164 | if(chan_num!=2) { // de-activate chip select |
lddevrie | 0:1d5bf13d7dbb | 165 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 166 | } else { |
lddevrie | 0:1d5bf13d7dbb | 167 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 168 | } |
lddevrie | 0:1d5bf13d7dbb | 169 | } |
lddevrie | 0:1d5bf13d7dbb | 170 | |
lddevrie | 0:1d5bf13d7dbb | 171 | void EW305sbc::LS7366_write_DTR(int chan_num, long enc_value) |
lddevrie | 0:1d5bf13d7dbb | 172 | { |
lddevrie | 0:1d5bf13d7dbb | 173 | union bytes { // Union to speed up byte writes |
lddevrie | 0:1d5bf13d7dbb | 174 | char byte_enc[4]; |
lddevrie | 0:1d5bf13d7dbb | 175 | long long_enc; |
lddevrie | 0:1d5bf13d7dbb | 176 | } counter; |
lddevrie | 0:1d5bf13d7dbb | 177 | |
lddevrie | 0:1d5bf13d7dbb | 178 | spi.format(8, 0); // set up SPI for 8 data bits, mode 0 |
lddevrie | 0:1d5bf13d7dbb | 179 | spi.frequency(2000000); // 2MHz SPI clock |
lddevrie | 0:1d5bf13d7dbb | 180 | |
lddevrie | 0:1d5bf13d7dbb | 181 | counter.long_enc = enc_value; // pass enc_value to Union |
lddevrie | 0:1d5bf13d7dbb | 182 | |
lddevrie | 0:1d5bf13d7dbb | 183 | if(chan_num!=2) { // activate chip select |
lddevrie | 0:1d5bf13d7dbb | 184 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 185 | } else { |
lddevrie | 0:1d5bf13d7dbb | 186 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 187 | } |
lddevrie | 0:1d5bf13d7dbb | 188 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 189 | LS7366_cmd(WR,DTR); // Write to the Data Transfer Register |
lddevrie | 0:1d5bf13d7dbb | 190 | spi.write(counter.byte_enc[3]); // Write the 32-bit encoder value |
lddevrie | 0:1d5bf13d7dbb | 191 | spi.write(counter.byte_enc[2]); |
lddevrie | 0:1d5bf13d7dbb | 192 | spi.write(counter.byte_enc[1]); |
lddevrie | 0:1d5bf13d7dbb | 193 | spi.write(counter.byte_enc[0]); |
lddevrie | 0:1d5bf13d7dbb | 194 | if(chan_num!=2) { // de-activate the chip select |
lddevrie | 0:1d5bf13d7dbb | 195 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 196 | } else { |
lddevrie | 0:1d5bf13d7dbb | 197 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 198 | } |
lddevrie | 0:1d5bf13d7dbb | 199 | |
lddevrie | 0:1d5bf13d7dbb | 200 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 201 | if(chan_num!=2) { // activate chip select |
lddevrie | 0:1d5bf13d7dbb | 202 | ls7166_cs1 = 0; |
lddevrie | 0:1d5bf13d7dbb | 203 | } else { |
lddevrie | 0:1d5bf13d7dbb | 204 | ls7166_cs2 = 0; |
lddevrie | 0:1d5bf13d7dbb | 205 | } |
lddevrie | 0:1d5bf13d7dbb | 206 | wait_us(1); // short delay |
lddevrie | 0:1d5bf13d7dbb | 207 | LS7366_cmd(LOAD,CNTR); // load command to the counter register from DTR |
lddevrie | 0:1d5bf13d7dbb | 208 | if(chan_num!=2) { // de-activate chip select |
lddevrie | 0:1d5bf13d7dbb | 209 | ls7166_cs1 = 1; |
lddevrie | 0:1d5bf13d7dbb | 210 | } else { |
lddevrie | 0:1d5bf13d7dbb | 211 | ls7166_cs2 = 1; |
lddevrie | 0:1d5bf13d7dbb | 212 | } |
lddevrie | 0:1d5bf13d7dbb | 213 | } |